Browse Source

MAVLink app: Send newest param hash for every param change to notify GCS about change

This should help keeping the GCS up to date about asynchronous changes on the autopilot.
sbg
Lorenz Meier 7 years ago
parent
commit
fba592b5fc
  1. 16
      src/modules/mavlink/mavlink_main.cpp
  2. 2
      src/modules/mavlink/mavlink_main.h
  3. 2
      src/modules/mavlink/mavlink_parameters.cpp

16
src/modules/mavlink/mavlink_main.cpp

@ -2190,9 +2190,23 @@ Mavlink::task_main(int argc, char *argv[]) @@ -2190,9 +2190,23 @@ Mavlink::task_main(int argc, char *argv[])
update_rate_mult();
if (param_sub->update(&param_time, nullptr)) {
/* param update backoff: update only after 50 ms after the last change */
param_sub->update(&param_time, nullptr);
if (param_time + 100 * 1000 > hrt_absolute_time()) {
/* parameters updated */
mavlink_update_system();
/* send out param cache value */
uint32_t hash = param_hash_check();
/* build the one-off response message */
mavlink_param_value_t param_value;
param_value.param_count = param_count_used();
param_value.param_index = -1;
strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
param_value.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&param_value.param_value, &hash, sizeof(hash));
mavlink_msg_param_value_send_struct(get_channel(), &param_value);
}
check_radio_config();

2
src/modules/mavlink/mavlink_main.h

@ -77,6 +77,8 @@ enum Protocol { @@ -77,6 +77,8 @@ enum Protocol {
TCP,
};
#define HASH_PARAM "_HASH_CHECK"
class Mavlink
{

2
src/modules/mavlink/mavlink_parameters.cpp

@ -47,8 +47,6 @@ @@ -47,8 +47,6 @@
#include "mavlink_parameters.h"
#include "mavlink_main.h"
#define HASH_PARAM "_HASH_CHECK"
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) :
_send_all_index(-1),
_uavcan_open_request_list(nullptr),

Loading…
Cancel
Save