Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-29 20:18:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Publish the magnetometer selection index

Browse Source
This commit is contained in:
Paul Riseborough 2015-11-08 20:42:09 +11:00 committed by Andrew Tridgell
parent 7294c8004b
commit f00b1ff22d
4 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -683,6 +683,15 @@ void NavEKF2::getMagXYZ(int8_t instance, Vector3f &magXYZ)
}
}
// return the magnetometer in use for the specified instance
uint8_t NavEKF2::getActiveMag(int8_t instance)
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
return core[instance].getActiveMag();
}
}
// Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid
bool NavEKF2::getMagOffsets(Vector3f &magOffsets) const

4
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -130,6 +130,10 @@ public:
// An out of range instance (eg -1) returns data for the the primary instance
void getMagXYZ(int8_t instance, Vector3f &magXYZ);
// return the magnetometer in use for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
uint8_t getActiveMag(int8_t instance);
// Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid
bool getMagOffsets(Vector3f &magOffsets) const;

5
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -341,6 +341,11 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
magXYZ = stateStruct.body_magfield*1000.0f;
}
// return the index for the active magnetometer
uint8_t NavEKF2_core::getActiveMag() const
{
return (uint8_t)magSelectIndex;
}
// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
void NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const

3
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -143,6 +143,9 @@ public:
// return body magnetic field estimates in measurement units / 1000
void getMagXYZ(Vector3f &magXYZ) const;
// return the index for the active magnetometer
uint8_t getActiveMag() const;
// Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid
bool getMagOffsets(Vector3f &magOffsets) const;