Browse Source

GCS_MAVLink: support MAV_CMD_BATTERY_RESET command

mission-4.1.18
Andrew Tridgell 6 years ago
parent
commit
f28df4dff1
  1. 1
      libraries/GCS_MAVLink/GCS.h
  2. 14
      libraries/GCS_MAVLink/GCS_Common.cpp

1
libraries/GCS_MAVLink/GCS.h

@ -619,6 +619,7 @@ protected: @@ -619,6 +619,7 @@ protected:
MAV_RESULT handle_command_preflight_can(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_battery_reset(const mavlink_command_long_t &packet);
void handle_command_long(mavlink_message_t* msg);
MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet);

14
libraries/GCS_MAVLink/GCS_Common.cpp

@ -3728,7 +3728,15 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_can(const mavlink_command_long_ @@ -3728,7 +3728,15 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_can(const mavlink_command_long_
#endif
}
MAV_RESULT GCS_MAVLINK::handle_command_battery_reset(const mavlink_command_long_t &packet)
{
const uint16_t battery_mask = packet.param1;
const float percentage = packet.param2;
if (AP::battery().reset_remaining(battery_mask, percentage)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
}
MAV_RESULT GCS_MAVLINK::handle_command_mag_cal(const mavlink_command_long_t &packet)
{
@ -3921,6 +3929,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t @@ -3921,6 +3929,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
result = handle_command_preflight_calibration(packet);
break;
case MAV_CMD_BATTERY_RESET:
result = handle_command_battery_reset(packet);
break;
case MAV_CMD_PREFLIGHT_UAVCAN:
result = handle_command_preflight_can(packet);
break;

Loading…
Cancel
Save