Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: convert param to new custom rotation

Browse Source
This commit is contained in:
Iampete1 2021-11-05 16:13:55 +00:00 committed by Andrew Tridgell
parent 480cc3fa46
commit 6c48c346c1
1 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_Compass/AP_Compass.cpp
View File

@ -7,6 +7,7 @@
#include <AP_Logger/AP_Logger.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
#include <AP_CustomRotations/AP_CustomRotations.h>
#include "AP_Compass_SITL.h"
#include "AP_Compass_AK8963.h"
@ -640,6 +641,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Increment: 1
// @RebootRequired: True
// @User: Advanced
// index 49
// @Param: CUS_PIT
@ -650,6 +652,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Increment: 1
// @RebootRequired: True
// @User: Advanced
// index 50
// @Param: CUS_YAW
@ -660,7 +663,9 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Increment: 1
// @RebootRequired: True
// @User: Advanced
// index 51
AP_GROUPEND
};
@ -688,6 +693,30 @@ void Compass::init()
return;
}
// convert to new custom rotation method
// PARAMETER_CONVERSION - Added: Nov-2021
#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_state[i].orientation != ROTATION_CUSTOM_OLD) {
continue;
}
_state[i].orientation.set_and_save(ROTATION_CUSTOM_2);
AP_Param::ConversionInfo info;
if (AP_Param::find_top_level_key_by_pointer(this, info.old_key)) {
info.type = AP_PARAM_FLOAT;
float rpy[3] = {};
AP_Float rpy_param;
for (info.old_group_element=49; info.old_group_element<=51; info.old_group_element++) {
if (AP_Param::find_old_parameter(&info, &rpy_param)) {
rpy[info.old_group_element-49] = rpy_param.get();
}
}
AP::custom_rotations().convert(ROTATION_CUSTOM_2, rpy[0], rpy[1], rpy[2]);
}
break;
}
#endif // !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
#if COMPASS_MAX_INSTANCES > 1
// Look if there was a primary compass setup in previous version
// if so and the the primary compass is not set in current setup