aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: allow for more than one compass in HIL/SITL

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-15 14:04:00 +10:00
parent a356ad1c79
commit 91b4ba3588
4 changed files with 30 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -27,9 +27,9 @@ extern const AP_HAL::HAL& hal;
// constructor
AP_Compass_HIL::AP_Compass_HIL(Compass &compass):
AP_Compass_Backend(compass),
_compass_instance(0)
AP_Compass_Backend(compass)
{
memset(_compass_instance, 0, sizeof(_compass_instance));
_compass._setup_earth_field();
}
@ -50,12 +50,18 @@ AP_Compass_Backend *AP_Compass_HIL::detect(Compass &compass)
bool
AP_Compass_HIL::init(void)
{
// register the compass instance in the frontend
_compass_instance = register_compass();
// register two compass instances
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
_compass_instance[i] = register_compass();
}
return true;
}
void AP_Compass_HIL::read()
{
publish_field(_compass._hil.field, _compass_instance);
for (uint8_t i=0; i<sizeof(_compass_instance)/sizeof(_compass_instance[0]); i++) {
if (_compass._hil.healthy[i]) {
publish_field(_compass._hil.field[_compass_instance[i]], _compass_instance[i]);
}
}
}

2
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -15,7 +15,7 @@ public:
static AP_Compass_Backend *detect(Compass &compass);
private:
uint8_t _compass_instance;
uint8_t _compass_instance[COMPASS_MAX_INSTANCES];
};
#endif

21
libraries/AP_Compass/Compass.cpp
View File

@ -568,7 +568,7 @@ bool Compass::configured(void)
// Update raw magnetometer values from HIL data
//
void Compass::setHIL(float roll, float pitch, float yaw)
void Compass::setHIL(uint8_t instance, float roll, float pitch, float yaw)
{
Matrix3f R;
@ -582,31 +582,34 @@ void Compass::setHIL(float roll, float pitch, float yaw)
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
_hil.field = R.mul_transpose(_hil.Bearth);
_hil.field[instance] = R.mul_transpose(_hil.Bearth);
// apply default board orientation for this compass type. This is
// a noop on most boards
_hil.field.rotate(MAG_BOARD_ORIENTATION);
_hil.field[instance].rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_hil.field.rotate((enum Rotation)_state[0].orientation.get());
_hil.field[instance].rotate((enum Rotation)_state[0].orientation.get());
if (!_state[0].external) {
// and add in AHRS_ORIENTATION setting if not an external compass
_hil.field.rotate(_board_orientation);
_hil.field[instance].rotate(_board_orientation);
}
_hil.healthy[instance] = true;
}
// Update raw magnetometer values from HIL mag vector
//
void Compass::setHIL(const Vector3f &mag)
void Compass::setHIL(uint8_t instance, const Vector3f &mag)
{
_hil.field = mag;
_hil.field[instance] = mag;
_hil.healthy[instance] = true;
_last_update_usec = hal.scheduler->micros();
}
const Vector3f& Compass::getHIL() const {
return _hil.field;
const Vector3f& Compass::getHIL(uint8_t instance) const
{
return _hil.field[instance];
}
// setup _Bearth

11
libraries/AP_Compass/Compass.h
View File

@ -35,7 +35,7 @@
maximum number of compass instances available on this platform. If more
than 1 then redundent sensors may be available
*/
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
#define COMPASS_MAX_INSTANCES 3
#define COMPASS_MAX_BACKEND 3
#else
@ -223,9 +223,9 @@ public:
uint8_t get_primary(void) const { return _primary; }
// HIL methods
void setHIL(float roll, float pitch, float yaw);
void setHIL(const Vector3f &mag);
const Vector3f& getHIL() const;
void setHIL(uint8_t instance, float roll, float pitch, float yaw);
void setHIL(uint8_t instance, const Vector3f &mag);
const Vector3f& getHIL(uint8_t instance) const;
void _setup_earth_field();
// enable HIL mode
@ -240,7 +240,8 @@ public:
struct {
Vector3f Bearth;
float last_declination;
Vector3f field;
bool healthy[COMPASS_MAX_INSTANCES];
Vector3f field[COMPASS_MAX_INSTANCES];
} _hil;
private: