Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include "mavlink_missionlib_data.h"
00021 #include "mavlink_parameters.h"
00022 #include "math.h"
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
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
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
00057 if (pm.param_names[i][j] != key[j])
00058 {
00059 match = false;
00060 }
00061
00062
00063 if (pm.param_names[i][j] == '\0')
00064 {
00065 break;
00066 }
00067 }
00068
00069
00070 if (match)
00071 {
00072
00073
00074
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
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 }
00104 }
00105 }
00106 }
00107
00108 }
00109 break;
00110
00111 }
00112
00113 }
00114
00122 void mavlink_pm_queued_send(void)
00123 {
00124
00125 if (pm.next_param < pm.size)
00126 {
00127
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 }