Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: SET_SENSORS_OFFSETS support for third compass

This commit is contained in:
Don Gagne 2016-03-14 10:56:26 -03:00 committed by Lucas De Marchi
parent 4743a34cf7
commit 97d361333d
1 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduCopter/GCS_Mavlink.cpp
View File

@ -1396,17 +1396,21 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
if (is_equal(packet.param1,2.0f)) {
// save first compass's offsets
copter.compass.set_and_save_offsets(0, packet.param2, packet.param3, packet.param4);
result = MAV_RESULT_ACCEPTED;
{
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) {
copter.compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4);
result = MAV_RESULT_ACCEPTED;
}
break;
}
if (is_equal(packet.param1,5.0f)) {
// save secondary compass's offsets
copter.compass.set_and_save_offsets(1, packet.param2, packet.param3, packet.param4);
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1,1.0f)) {