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

AP_Compass: Read current for compensation from the battery singleton

This commit is contained in:
Michael du Breuil 2018-03-01 14:42:31 -07:00 committed by Francisco Ferreira
parent 97fce557f5
commit c66d15efd7
3 changed files with 12 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Compass/AP_Compass.cpp
View File

@ -449,7 +449,6 @@ Compass::Compass(void) :
_compass_count(0),
_board_orientation(ROTATION_NONE),
_null_init_done(false),
_thr_or_curr(0.0f),
_hil_mode(false)
{
AP_Param::setup_object_defaults(this, var_info);
@ -1192,7 +1191,7 @@ void Compass::motor_compensation_type(const uint8_t comp_type)
{
if (_motor_comp_type <= AP_COMPASS_MOT_COMP_CURRENT && _motor_comp_type != (int8_t)comp_type) {
_motor_comp_type = (int8_t)comp_type;
_thr_or_curr = 0; // set current current or throttle to zero
_thr = 0; // set current throttle to zero
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
set_motor_compensation(i, Vector3f(0,0,0)); // clear out invalid compensation vectors
}

15
libraries/AP_Compass/AP_Compass.h
View File

@ -8,6 +8,7 @@
#include <AP_Math/AP_Math.h>
#include <AP_Param/AP_Param.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include "CompassCalibrator.h"
#include "AP_Compass_Backend.h"
@ -247,15 +248,7 @@ public:
/// @param thr_pct throttle expressed as a percentage from 0 to 1.0
void set_throttle(float thr_pct) {
if (_motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
_thr_or_curr = thr_pct;
}
}
/// Set the current used by system in amps
/// @param amps current flowing to the motors expressed in amps
void set_current(float amps) {
if (_motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
_thr_or_curr = amps;
_thr = thr_pct;
}
}
@ -415,8 +408,8 @@ private:
// 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
AP_Int8 _motor_comp_type;
// throttle expressed as a percentage from 0 ~ 1.0 or current expressed in amps
float _thr_or_curr;
// throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation
float _thr;
struct mag_state {
AP_Int8 external;

10
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -72,9 +72,13 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
if (_compass._per_motor.enabled() && i == 0) {
// per-motor correction is only valid for first compass
_compass._per_motor.compensate(state.motor_offset);
} else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE ||
_compass._motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
state.motor_offset = mot * _compass._thr_or_curr;
} else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
state.motor_offset = mot * _compass._thr;
} else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
AP_BattMonitor &battery = AP::battery();
if (battery.has_current()) {
state.motor_offset = mot * battery.current_amps();
}
}
/*