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

AP_Compass: added COMPASS_BAT_MASK for which batteries to compensate with 

this allows selection of batteries that need to be used for compass
motor compensation with current
This commit is contained in:
Andrew Tridgell 2021-10-15 11:37:37 +11:00
parent b0aa456daa
commit c6e60ef168
3 changed files with 20 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_Compass/AP_Compass.cpp
View File

@ -152,6 +152,13 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Increment: 1 // @Increment: 1
// @User: Advanced // @User: Advanced
AP_GROUPINFO("MOT", 7, Compass, _state._priv_instance[0].motor_compensation, 0), AP_GROUPINFO("MOT", 7, Compass, _state._priv_instance[0].motor_compensation, 0),
// @Param: BAT_MASK
// @DisplayName: Motor interference compensation battery index
// @Description: The mask battery numbers that are used for motor compensation of compasses. Total current is sum of current from the selected batteries
// @Bitmask: 0:FirstBattery,1:2ndBattery,2:3rdBattery,3:4thBattery,4:5thBattery,5:6thBattery,6:7thBattery,7:8thBattery,8:9thBattery
// @User: Advanced
AP_GROUPINFO("BAT_MASK", 52, Compass, _motor_comp_batmask, 1),
#endif #endif
// @Param: ORIENT // @Param: ORIENT

3
libraries/AP_Compass/AP_Compass.h
View File

@ -470,6 +470,9 @@ private:
// 0 = disabled, 1 = enabled for throttle, 2 = enabled for current // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
AP_Int8 _motor_comp_type; AP_Int8 _motor_comp_type;
// motor compensation battery mask
AP_Int16 _motor_comp_batmask;
// automatic compass orientation on calibration // automatic compass orientation on calibration
AP_Int8 _rotate_auto; AP_Int8 _rotate_auto;

11
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -103,11 +103,18 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
state.motor_offset = mot * _compass._thr; state.motor_offset = mot * _compass._thr;
} else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) { } else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
AP_BattMonitor &battery = AP::battery(); AP_BattMonitor &battery = AP::battery();
float current_sum = 0;
for (uint8_t b=0; b<AP_BATT_MONITOR_MAX_INSTANCES; b++) {
const uint16_t mask = 1U<<b;
if (mask & uint16_t(_compass._motor_comp_batmask)) {
float current; float current;
if (battery.current_amps(current)) { if (battery.current_amps(current, b)) {
state.motor_offset = mot * current; current_sum += current;
} }
} }
}
state.motor_offset = mot * current_sum;
}
/* /*
we apply the motor offsets after we apply the eliptical we apply the motor offsets after we apply the eliptical