|
|
|
@ -2538,10 +2538,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_flash_bootloader(const mavlink_command_lo
@@ -2538,10 +2538,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_flash_bootloader(const mavlink_command_lo
|
|
|
|
|
|
|
|
|
|
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; |
|
|
|
|
} |
|
|
|
|
Compass &compass = AP::compass(); |
|
|
|
|
|
|
|
|
|
uint8_t compassNumber = -1; |
|
|
|
|
if (is_equal(packet.param1, 2.0f)) { |
|
|
|
@ -2554,7 +2551,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_set_sensor_offsets(const mavlin
@@ -2554,7 +2551,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_set_sensor_offsets(const mavlin
|
|
|
|
|
if (compassNumber == (uint8_t) -1) { |
|
|
|
|
return MAV_RESULT_FAILED; |
|
|
|
|
} |
|
|
|
|
compass->set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4); |
|
|
|
|
compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4); |
|
|
|
|
return MAV_RESULT_ACCEPTED; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -2640,11 +2637,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_calibration(const mavlink_comma
@@ -2640,11 +2637,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_calibration(const mavlink_comma
|
|
|
|
|
|
|
|
|
|
MAV_RESULT GCS_MAVLINK::handle_command_mag_cal(const mavlink_command_long_t &packet) |
|
|
|
|
{ |
|
|
|
|
Compass *compass = get_compass(); |
|
|
|
|
if (compass == nullptr) { |
|
|
|
|
return MAV_RESULT_UNSUPPORTED; |
|
|
|
|
} |
|
|
|
|
return compass->handle_mag_cal_command(packet); |
|
|
|
|
return AP::compass().handle_mag_cal_command(packet); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
MAV_RESULT GCS_MAVLINK::handle_command_request_autopilot_capabilities(const mavlink_command_long_t &packet) |
|
|
|
@ -2845,18 +2838,15 @@ void GCS_MAVLINK::handle_command_int(mavlink_message_t *msg)
@@ -2845,18 +2838,15 @@ void GCS_MAVLINK::handle_command_int(mavlink_message_t *msg)
|
|
|
|
|
|
|
|
|
|
bool GCS_MAVLINK::try_send_compass_message(const enum ap_message id) |
|
|
|
|
{ |
|
|
|
|
Compass *compass = get_compass(); |
|
|
|
|
if (compass == nullptr) { |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
Compass &compass = AP::compass(); |
|
|
|
|
bool ret = true; |
|
|
|
|
switch (id) { |
|
|
|
|
case MSG_MAG_CAL_PROGRESS: |
|
|
|
|
compass->send_mag_cal_progress(chan); |
|
|
|
|
compass.send_mag_cal_progress(chan); |
|
|
|
|
ret = true;; |
|
|
|
|
break; |
|
|
|
|
case MSG_MAG_CAL_REPORT: |
|
|
|
|
compass->send_mag_cal_report(chan); |
|
|
|
|
compass.send_mag_cal_report(chan); |
|
|
|
|
ret = true; |
|
|
|
|
break; |
|
|
|
|
default: |
|
|
|
|