mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_AccelCal: limit COMMAND_ACKs accepted for vehicle pose confirmation
This commit is contained in:
parent
be21091619
commit
4272b2c264
@ -367,6 +367,26 @@ void AP_AccelCal::handle_command_ack(const mavlink_command_ack_t &packet)
|
|||||||
if (!_waiting_for_mavlink_ack) {
|
if (!_waiting_for_mavlink_ack) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
// this is support for the old, non-accelcal-specific calibration.
|
||||||
|
// The GCS is expected to send back a COMMAND_ACK when the vehicle
|
||||||
|
// is posed, but we placed no constraints on the result code or
|
||||||
|
// the command field in the ack packet. That meant that any ACK
|
||||||
|
// would move the cal process forward - and since we don't even
|
||||||
|
// check the source system/component here the process could easily
|
||||||
|
// fail due to other ACKs floating around the mavlink network.
|
||||||
|
// GCSs should be moved to using the non-gcs-snoop method. As a
|
||||||
|
// round-up:
|
||||||
|
// MAVProxy: command=1-6 depending on pose, result=1
|
||||||
|
// QGC: command=0, result=1
|
||||||
|
// MissionPlanner: uses new ACCELCAL_VEHICLE_POS
|
||||||
|
if (packet.command > 6) {
|
||||||
|
// not an acknowledgement for a vehicle position
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (packet.result != MAV_RESULT_TEMPORARILY_REJECTED) {
|
||||||
|
// not an acknowledgement for a vehicle position
|
||||||
|
return;
|
||||||
|
}
|
||||||
_waiting_for_mavlink_ack = false;
|
_waiting_for_mavlink_ack = false;
|
||||||
_start_collect_sample = true;
|
_start_collect_sample = true;
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user