mavlink_parameters.c

Go to the documentation of this file.

/*******************************************************************************
 
 Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch
 
 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 ****************************************************************************/

#include "mavlink_missionlib_data.h"
#include "mavlink_parameters.h"
#include "math.h" /* isinf / isnan checks */

extern mavlink_system_t mavlink_system;
extern mavlink_pm_storage pm;

extern void mavlink_missionlib_send_message(mavlink_message_t* msg);
extern void mavlink_missionlib_send_gcs_string(const char* string);

void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t* msg)
{
        switch (msg->msgid)
        {
                case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
                        {
                                // Start sending parameters
                                pm.next_param = 0;
                                mavlink_missionlib_send_gcs_string("PM SENDING LIST");
                        }
                        break;
                case MAVLINK_MSG_ID_PARAM_SET:
                {
                        mavlink_param_set_t set;
                        mavlink_msg_param_set_decode(msg, &set);
                        
                        // Check if this message is for this system
                        if (set.target_system == mavlink_system.sysid && set.target_component == mavlink_system.compid)
                        {
                                char* key = set.param_id;
                                
                                for (uint16_t i = 0; i < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; i++)
                                {
                                        bool match = true;
                                        for (uint16_t j = 0; j < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; j++)
                                        {
                                                // Compare
                                                if (pm.param_names[i][j] != key[j])
                                                {
                                                        match = false;
                                                }
                                                
                                                // End matching if null termination is reached
                                                if (pm.param_names[i][j] == '\0')
                                                {
                                                        break;
                                                }
                                        }
                                        
                                        // Check if matched
                                        if (match)
                                        {
                                                // Only write and emit changes if there is actually a difference
                                                // AND only write if new value is NOT "not-a-number"
                                                // AND is NOT infinity
                                                if (pm.param_values[i] != set.param_value
                                                        && !isnan(set.param_value)
                                                        && !isinf(set.param_value))
                                                {
                                                        pm.param_values[i] = set.param_value;
                                                        // Report back new value
#ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS
                                                        mavlink_message_t tx_msg;
                                                        mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
                                                                                                                          mavlink_system.compid,
                                                                                                                          MAVLINK_COMM_0,
                                                                                                                          &tx_msg,
                                                                                                                          pm.param_names[i],
                                                                                                                          pm.param_values[i],
                                                                                                                          MAVLINK_TYPE_FLOAT,
                                                                                                                          pm.size,
                                                                                                                          i);
                                                        mavlink_missionlib_send_message(&tx_msg);
#else
                                                        mavlink_msg_param_value_send(MAVLINK_COMM_0,
                                                                                                                 pm.param_names[i],
                                                                                                                 pm.param_values[i],
                                                                                                                 MAVLINK_TYPE_FLOAT,
                                                                                                                 pm.size,
                                                                                                                 i);
#endif
                                                        
                                                        mavlink_missionlib_send_gcs_string("PM received param");
                                                } // End valid value check
                                        } // End match check
                                } // End for loop
                } // End system ID check
                
        } // End case
                        break;
                        
        } // End switch
        
}
        
        void mavlink_pm_queued_send(void)
        {
                //send parameters one by one
                if (pm.next_param < pm.size)
                {
                        //for (int i.. all active comm links)
#ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS
                        mavlink_message_t tx_msg;
                        mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
                                                                                          mavlink_system.compid,
                                                                                          MAVLINK_COMM_0,
                                                                                          &tx_msg,
                                                                                          pm.param_names[pm.next_param],
                                                                                          pm.param_values[pm.next_param],
                                                                                          MAVLINK_TYPE_FLOAT,
                                                                                          pm.size,
                                                                                          pm.next_param);
                        mavlink_missionlib_send_message(&tx_msg);
#else
                        mavlink_msg_param_value_send(MAVLINK_COMM_0,
                                                                                 pm.param_names[pm.next_param],
                                                                                 pm.param_values[pm.next_param],
                                                                                 MAV_DATA_TYPE_FLOAT,
                                                                                 pm.size,
                                                                                 pm.next_param);
#endif
                        pm.next_param++;
                }
        }