Browse Source

GCS_MAVLink: handle param_set in GCS_MAVLINK base class

master
Peter Barker 8 years ago committed by Francisco Ferreira
parent
commit
50242178b3
  1. 2
      libraries/GCS_MAVLink/GCS.h
  2. 2
      libraries/GCS_MAVLink/GCS_Common.cpp
  3. 6
      libraries/GCS_MAVLink/GCS_Param.cpp

2
libraries/GCS_MAVLink/GCS.h

@ -260,7 +260,7 @@ protected: @@ -260,7 +260,7 @@ protected:
bool handle_mission_item(mavlink_message_t *msg, AP_Mission &mission);
void handle_common_param_message(mavlink_message_t *msg);
void handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash);
void handle_param_set(mavlink_message_t *msg);
void handle_param_request_list(mavlink_message_t *msg);
void handle_param_request_read(mavlink_message_t *msg);

2
libraries/GCS_MAVLink/GCS_Common.cpp

@ -1785,6 +1785,8 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg) @@ -1785,6 +1785,8 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
handle_setup_signing(msg);
break;
case MAVLINK_MSG_ID_PARAM_SET:
/* fall through */
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
handle_common_param_message(msg);
break;

6
libraries/GCS_MAVLink/GCS_Param.cpp

@ -235,7 +235,7 @@ void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg) @@ -235,7 +235,7 @@ void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg)
param_requests.push(req);
}
void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash)
void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg)
{
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
@ -269,6 +269,7 @@ void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *Data @@ -269,6 +269,7 @@ void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *Data
// save the change
vp->save(force_save);
DataFlash_Class *DataFlash = DataFlash_Class::instance();
if (DataFlash != nullptr) {
DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));
}
@ -440,6 +441,9 @@ void GCS_MAVLINK::send_parameter_reply(void) @@ -440,6 +441,9 @@ void GCS_MAVLINK::send_parameter_reply(void)
void GCS_MAVLINK::handle_common_param_message(mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_SET:
handle_param_set(msg);
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
handle_param_request_read(msg);
break;

Loading…
Cancel
Save