aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: move handling of PREFLIGHT_SET_SENSOR_OFFSETS up

Browse Source
This commit is contained in:
Peter Barker 2017-07-17 11:04:32 +10:00 committed by Francisco Ferreira
parent c430b3affe
commit 06647cde2c
2 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -276,6 +276,7 @@ protected:
bool telemetry_delayed() const;
virtual uint32_t telem_delay() const = 0;
MAV_RESULT handle_command_preflight_set_sensor_offsets(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_long_message(mavlink_command_long_t &packet);

27
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1835,6 +1835,28 @@ void GCS_MAVLINK::handle_common_mission_message(mavlink_message_t *msg)
}
}
MAV_RESULT GCS_MAVLINK::handle_command_preflight_set_sensor_offsets(const mavlink_command_long_t &packet)
{
Compass *compass = get_compass();
if (compass == nullptr) {
return MAV_RESULT_UNSUPPORTED;
}
uint8_t compassNumber = -1;
if (is_equal(packet.param1, 2.0f)) {
compassNumber = 0;
} else if (is_equal(packet.param1, 5.0f)) {
compassNumber = 1;
} else if (is_equal(packet.param1, 6.0f)) {
compassNumber = 2;
}
if (compassNumber == (uint8_t) -1) {
return MAV_RESULT_FAILED;
}
compass->set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4);
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK::handle_command_mag_cal(const mavlink_command_long_t &packet)
{
Compass *compass = get_compass();
@ -1861,6 +1883,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_message(mavlink_command_long_t &pack
result = handle_rc_bind(packet);
break;
case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: {
result = handle_command_preflight_set_sensor_offsets(packet);
break;
}
case MAV_CMD_DO_SET_SERVO:
/* fall through */
case MAV_CMD_DO_REPEAT_SERVO: