/* send one parameter, assume lock on global_data_parameter_storage */ void mavlink_pm_send_one_parameter(uint16_t next_param) { if (next_param < global_data_parameter_storage->pm.size) { static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; mavlink_message_t tx_msg; strncpy((char *)name_buf, global_data_parameter_storage->pm.param_names[next_param], MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); mavlink_msg_param_value_pack_chan(mavlink_system.sysid, mavlink_system.compid, MAVLINK_COMM_0, &tx_msg, name_buf, global_data_parameter_storage->pm.param_values[next_param], MAVLINK_TYPE_FLOAT, global_data_parameter_storage->pm.size, next_param); mavlink_missionlib_send_message(&tx_msg); // mavlink_msg_param_value_send(MAVLINK_COMM_0, // name_buf, // global_data_parameter_storage->pm.param_values[next_param], // MAVLINK_TYPE_FLOAT, // global_data_parameter_storage->pm.size, // next_param); } }
/** * @brief Send low-priority messages at a maximum rate of xx Hertz * * This function sends messages at a lower rate to not exceed the wireless * bandwidth. It sends one message each time it is called until the buffer is empty. * Call this function with xx Hertz to increase/decrease the bandwidth. */ 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++; } }
int Mavlink::mavlink_pm_send_param(param_t param) { if (param == PARAM_INVALID) { return 1; } /* buffers for param transmission */ char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; float val_buf; mavlink_message_t tx_msg; /* query parameter type */ param_type_t type = param_type(param); /* copy parameter name */ strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* * Map onboard parameter type to MAVLink type, * endianess matches (both little endian) */ uint8_t mavlink_type; if (type == PARAM_TYPE_INT32) { mavlink_type = MAVLINK_TYPE_INT32_T; } else if (type == PARAM_TYPE_FLOAT) { mavlink_type = MAVLINK_TYPE_FLOAT; } else { mavlink_type = MAVLINK_TYPE_FLOAT; } /* * get param value, since MAVLink encodes float and int params in the same * space during transmission, copy param onto float val_buf */ int ret; if ((ret = param_get(param, &val_buf)) != OK) { return ret; } mavlink_msg_param_value_pack_chan(mavlink_system.sysid, mavlink_system.compid, _channel, &tx_msg, name_buf, val_buf, mavlink_type, param_count(), param_get_index(param)); send_message(&tx_msg); return OK; }
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 }