mavlink_parameters.c
Go to the documentation of this file.
00001 /*******************************************************************************
00002  
00003  Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch
00004  
00005  This program is free software: you can redistribute it and/or modify
00006  it under the terms of the GNU General Public License as published by
00007  the Free Software Foundation, either version 3 of the License, or
00008  (at your option) any later version.
00009  
00010  This program is distributed in the hope that it will be useful,
00011  but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  GNU General Public License for more details.
00014  
00015  You should have received a copy of the GNU General Public License
00016  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00017  
00018  ****************************************************************************/
00019 
00020 #include "mavlink_missionlib_data.h"
00021 #include "mavlink_parameters.h"
00022 #include "math.h" /* isinf / isnan checks */
00023 
00024 extern mavlink_system_t mavlink_system;
00025 extern mavlink_pm_storage pm;
00026 
00027 extern void mavlink_missionlib_send_message(mavlink_message_t* msg);
00028 extern void mavlink_missionlib_send_gcs_string(const char* string);
00029 
00030 void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t* msg)
00031 {
00032         switch (msg->msgid)
00033         {
00034                 case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
00035                         {
00036                                 // Start sending parameters
00037                                 pm.next_param = 0;
00038                                 mavlink_missionlib_send_gcs_string("PM SENDING LIST");
00039                         }
00040                         break;
00041                 case MAVLINK_MSG_ID_PARAM_SET:
00042                 {
00043                         mavlink_param_set_t set;
00044                         mavlink_msg_param_set_decode(msg, &set);
00045                         
00046                         // Check if this message is for this system
00047                         if (set.target_system == mavlink_system.sysid && set.target_component == mavlink_system.compid)
00048                         {
00049                                 char* key = set.param_id;
00050                                 
00051                                 for (uint16_t i = 0; i < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; i++)
00052                                 {
00053                                         bool match = true;
00054                                         for (uint16_t j = 0; j < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; j++)
00055                                         {
00056                                                 // Compare
00057                                                 if (pm.param_names[i][j] != key[j])
00058                                                 {
00059                                                         match = false;
00060                                                 }
00061                                                 
00062                                                 // End matching if null termination is reached
00063                                                 if (pm.param_names[i][j] == '\0')
00064                                                 {
00065                                                         break;
00066                                                 }
00067                                         }
00068                                         
00069                                         // Check if matched
00070                                         if (match)
00071                                         {
00072                                                 // Only write and emit changes if there is actually a difference
00073                                                 // AND only write if new value is NOT "not-a-number"
00074                                                 // AND is NOT infinity
00075                                                 if (pm.param_values[i] != set.param_value
00076                                                         && !isnan(set.param_value)
00077                                                         && !isinf(set.param_value))
00078                                                 {
00079                                                         pm.param_values[i] = set.param_value;
00080                                                         // Report back new value
00081 #ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00082                                                         mavlink_message_t tx_msg;
00083                                                         mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
00084                                                                                                                           mavlink_system.compid,
00085                                                                                                                           MAVLINK_COMM_0,
00086                                                                                                                           &tx_msg,
00087                                                                                                                           pm.param_names[i],
00088                                                                                                                           pm.param_values[i],
00089                                                                                                                           MAVLINK_TYPE_FLOAT,
00090                                                                                                                           pm.size,
00091                                                                                                                           i);
00092                                                         mavlink_missionlib_send_message(&tx_msg);
00093 #else
00094                                                         mavlink_msg_param_value_send(MAVLINK_COMM_0,
00095                                                                                                                  pm.param_names[i],
00096                                                                                                                  pm.param_values[i],
00097                                                                                                                  MAVLINK_TYPE_FLOAT,
00098                                                                                                                  pm.size,
00099                                                                                                                  i);
00100 #endif
00101                                                         
00102                                                         mavlink_missionlib_send_gcs_string("PM received param");
00103                                                 } // End valid value check
00104                                         } // End match check
00105                                 } // End for loop
00106                 } // End system ID check
00107                 
00108         } // End case
00109                         break;
00110                         
00111         } // End switch
00112         
00113 }
00114         
00122         void mavlink_pm_queued_send(void)
00123         {
00124                 //send parameters one by one
00125                 if (pm.next_param < pm.size)
00126                 {
00127                         //for (int i.. all active comm links)
00128 #ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00129                         mavlink_message_t tx_msg;
00130                         mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
00131                                                                                           mavlink_system.compid,
00132                                                                                           MAVLINK_COMM_0,
00133                                                                                           &tx_msg,
00134                                                                                           pm.param_names[pm.next_param],
00135                                                                                           pm.param_values[pm.next_param],
00136                                                                                           MAVLINK_TYPE_FLOAT,
00137                                                                                           pm.size,
00138                                                                                           pm.next_param);
00139                         mavlink_missionlib_send_message(&tx_msg);
00140 #else
00141                         mavlink_msg_param_value_send(MAVLINK_COMM_0,
00142                                                                                  pm.param_names[pm.next_param],
00143                                                                                  pm.param_values[pm.next_param],
00144                                                                                  MAV_DATA_TYPE_FLOAT,
00145                                                                                  pm.size,
00146                                                                                  pm.next_param);
00147 #endif
00148                         pm.next_param++;
00149                 }
00150         }


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57