Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-20 11:28:58 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[mavlink] parse PARAM_SET

Browse Source
This commit is contained in:
Felix Ruess
2014-12-27 14:16:58 +01:00
parent 0f0f11856c
commit 1469f456bc
1 changed files with 59 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/datalink/mavlink.c
+59 -20
View File
@@ -44,7 +44,10 @@
mavlink_system_t mavlink_system;
static uint8_t mavlink_params_idx = NB_SETTING; /**< Transmitting parameters index */
static char mavlink_params[NB_SETTING][16] = SETTINGS; /**< Transmitting parameter names */
/** mavlink parameter names.
* 16 chars + 1 NULL termination.
*/
static char mavlink_params[NB_SETTING][16+1] = SETTINGS;
static uint8_t custom_version[8]; /**< first 8 bytes (16 chars) of GIT SHA1 */
static inline void mavlink_send_heartbeat(void);
@@ -93,12 +96,36 @@ void mavlink_periodic(void)
RunOnceEvery(32, mavlink_send_autopilot_version());
}
static int16_t settings_idx_from_param_id(char *param_id)
{
int i, j;
int16_t settings_idx = -1;
// Go trough all the settings to search the ID
for (i = 0; i < NB_SETTING; i++) {
for (j = 0; j < 16; j++) {
if (mavlink_params[i][j] != param_id[j]) {
break;
}
if (mavlink_params[i][j] == '\0') {
settings_idx = i;
return settings_idx;
}
}
if (mavlink_params[i][j] == '\0') {
break;
}
}
return settings_idx;
}
/**
* Event MAVLink calls
*/
void mavlink_event(void)
{
int i, j;
mavlink_message_t msg;
mavlink_status_t status;
@@ -151,24 +178,7 @@ void mavlink_event(void)
// First check param_index and search for the ID if needed
if (cmd.param_index == -1) {
// Go trough all the settings to search the ID
for (i = 0; i < NB_SETTING; i++) {
for (j = 0; j < 16; j++) {
if (mavlink_params[i][j] != cmd.param_id[j]) {
break;
}
if (mavlink_params[i][j] == '\0') {
cmd.param_index = i;
break;
}
}
if (mavlink_params[i][j] == '\0') {
break;
}
}
cmd.param_index = settings_idx_from_param_id(cmd.param_id);
}
mavlink_msg_param_value_send(MAVLINK_COMM_0,
@@ -182,6 +192,35 @@ void mavlink_event(void)
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 ((uint8_t) set.target_system == AC_ID) {
int16_t idx = settings_idx_from_param_id(set.param_id);
// setting found
if (idx >= 0) {
// Only write if new value is NOT "not-a-number"
// AND is NOT infinity
if (set.param_type == MAV_PARAM_TYPE_REAL32 &&
!isnan(set.param_value) && !isinf(set.param_value)) {
DlSetting(idx, set.param_value);
// Report back new value
mavlink_msg_param_value_send(MAVLINK_COMM_0,
mavlink_params[idx],
settings_get_value(idx),
MAV_PARAM_TYPE_REAL32,
NB_SETTING,
idx);
MAVLink(SendMessage());
}
}
}
}
break;
default:
//Do nothing
//printf("Received message with id: %d\r\n", msg.msgid);