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

AP_NavEKF2: support replay with external navigation data

This commit is contained in:
Andrew Tridgell 2020-11-07 10:58:10 +11:00
parent c068da154a
commit bec095d661
2 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -1274,6 +1274,8 @@ bool NavEKF2::use_compass(void) const
// posOffset is the XYZ flow sensor position in the body frame in m
void NavEKF2::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset)
{
AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset);
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset);
@ -1681,6 +1683,8 @@ void NavEKF2::getTimingStatistics(int8_t instance, struct ekf_timing &timing) co
*/
void NavEKF2::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
{
AP::dal().writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
@ -1739,7 +1743,7 @@ void NavEKF2::writeDefaultAirSpeed(float airspeed)
*/
void NavEKF2::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
// AP_DAL::log_writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
AP::dal().writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
if (core) {
for (uint8_t i=0; i<num_cores; i++) {

2
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -121,8 +121,6 @@ void NavEKF2_core::readRangeFinder(void)
// this needs to be called externally.
void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset)
{
AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset);
// The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
// The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
// A positive X rate is produced by a positive sensor rotation about the X axis