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

AP_NavEKF3: Add interface for yaw angle measurements

This commit is contained in:
priseborough 2017-05-28 21:25:07 +10:00 committed by Andrew Tridgell
parent a2e75876bf
commit 87c7649d09
4 changed files with 49 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1226,6 +1226,16 @@ void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &raw
}
}
// write yaw angle sensor measurements
void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)
{
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeEulerYawAngle(yawAngle, yawAngleErr, timeStamp_ms, type);
}
}
}
// return data for debugging optical flow fusion
void NavEKF3::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov,
float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const

14
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -267,6 +267,20 @@ public:
bool getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
float &offsetHigh, float &offsetLow, Vector3f &posNED) const;
/*
* Writes the measurement from a yaw angle sensor
*
* yawAngle: Yaw angle of the vehicle relative to true north in radians where a positive angle is
* produced by a RH rotation about the Z body axis. The Yaw rotation is the first rotation in a
* 321 (ZYX) or a 312 (ZXY) rotation sequence as specified by the 'type' argument.
* yawAngleErr is the 1SD accuracy of the yaw angle measurement in radians.
* timeStamp_ms: System time in msec when the yaw measurement was taken. This time stamp must include
* all measurement lag and transmission delays.
* type: An integer specifying Euler rotation order used to define the yaw angle.
* type = 1 specifies a 312 (ZXY) rotation order, type = 2 specifies a 321 (ZYX) rotation order.
*/
void writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
void setTakeoffExpected(bool val);

11
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -865,6 +865,17 @@ void NavEKF3_core::readRngBcnData()
}
/********************************************************
* Independant yaw sensor measurements *
********************************************************/
void NavEKF3_core::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)
{
//TODO
}
/*
update timing statistics structure
*/

14
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -258,6 +258,20 @@ public:
bool getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
float &offsetHigh, float &offsetLow, Vector3f &posNED);
/*
* Writes the measurement from a yaw angle sensor
*
* yawAngle: Yaw angle of the vehicle relative to true north in radians where a positive angle is
* produced by a RH rotation about the Z body axis. The Yaw rotation is the first rotation in a
* 321 (ZYX) or a 312 (ZXY) rotation sequence as specified by the 'type' argument.
* yawAngleErr is the 1SD accuracy of the yaw angle measurement in radians.
* timeStamp_ms: System time in msec when the yaw measurement was taken. This time stamp must include
* all measurement lag and transmission delays.
* type: An integer specifying Euler rotation order used to define the yaw angle.
* type = 1 specifies a 312 (ZXY) rotation order, type = 2 specifies a 321 (ZYX) rotation order.
*/
void writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
void setTakeoffExpected(bool val);