GCS_MAVLink: remove handling of PREFLIGHT_SET_SENSOR_OFFSETS

This was the old offboard-calibration code.

We've checked MAVProxy, QGC and MissionPlanner and they're not using this code.

The onboard calibration stuff is better.
This commit is contained in:
Peter Barker 2023-10-19 09:22:43 +11:00 committed by Peter Barker
parent 740b939aa5
commit 2412ba495d
2 changed files with 0 additions and 25 deletions

View File

@ -626,7 +626,6 @@ protected:
virtual uint32_t telem_delay() const = 0;
MAV_RESULT handle_command_run_prearm_checks(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_preflight_set_sensor_offsets(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_flash_bootloader(const mavlink_command_long_t &packet);
// generally this should not be overridden; Plane overrides it to ensure

View File

@ -4327,25 +4327,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_flash_bootloader(const mavlink_command_lo
return MAV_RESULT_FAILED;
}
MAV_RESULT GCS_MAVLINK::handle_command_preflight_set_sensor_offsets(const mavlink_command_long_t &packet)
{
Compass &compass = AP::compass();
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, Vector3f(packet.param2, packet.param3, packet.param4));
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration_baro(const mavlink_message_t &msg)
{
// fast barometer calibration
@ -4800,11 +4781,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
result = handle_command_flash_bootloader(packet);
break;
case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: {
result = handle_command_preflight_set_sensor_offsets(packet);
break;
}
default:
result = try_command_long_as_command_int(packet, msg);
break;