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

AP_DAL: support external pos and velocity data

Browse Source
This commit is contained in:
Andrew Tridgell 2020-11-07 10:57:42 +11:00
parent 7f0113b82d
commit c068da154a
3 changed files with 93 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_DAL/AP_DAL.cpp
View File

@ -277,6 +277,34 @@ void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawF
WRITE_REPLAY_BLOCK_IFCHANGD(ROFH, _ROFH, old);
}
void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
{
end_frame();
const log_REPH old = _REPH;
_REPH.pos = pos;
_REPH.quat = quat;
_REPH.posErr = posErr;
_REPH.angErr = angErr;
_REPH.timeStamp_ms = timeStamp_ms;
_REPH.delay_ms = delay_ms;
_REPH.resetTime_ms = resetTime_ms;
WRITE_REPLAY_BLOCK_IFCHANGD(REPH, _REPH, old);
}
void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
end_frame();
const log_REVH old = _REVH;
_REVH.vel = vel;
_REVH.err = err;
_REVH.timeStamp_ms = timeStamp_ms;
_REVH.delay_ms = delay_ms;
WRITE_REPLAY_BLOCK_IFCHANGD(REVH, _REVH, old);
}
#if APM_BUILD_TYPE(APM_BUILD_Replay)
/*
handle frame message. This message triggers the EKF2/EKF3 updates and logging
@ -329,6 +357,26 @@ void AP_DAL::handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
ekf2.writeOptFlowMeas(_ROFH.rawFlowQuality, _ROFH.rawFlowRates, _ROFH.rawGyroRates, _ROFH.msecFlowMeas, _ROFH.posOffset);
ekf3.writeOptFlowMeas(_ROFH.rawFlowQuality, _ROFH.rawFlowRates, _ROFH.rawGyroRates, _ROFH.msecFlowMeas, _ROFH.posOffset);
}
/*
handle external position data
*/
void AP_DAL::handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
{
_REPH = msg;
ekf2.writeExtNavData(_REPH.pos, _REPH.quat, _REPH.posErr, _REPH.angErr, _REPH.timeStamp_ms, _REPH.delay_ms, _REPH.resetTime_ms);
ekf3.writeExtNavData(_REPH.pos, _REPH.quat, _REPH.posErr, _REPH.angErr, _REPH.timeStamp_ms, _REPH.delay_ms, _REPH.resetTime_ms);
}
/*
handle external position data
*/
void AP_DAL::handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
{
_REVH = msg;
ekf2.writeExtNavVelData(_REVH.vel, _REVH.err, _REVH.timeStamp_ms, _REVH.delay_ms);
ekf3.writeExtNavVelData(_REVH.vel, _REVH.err, _REVH.timeStamp_ms, _REVH.delay_ms);
}
#endif // APM_BUILD_Replay
#include <stdio.h>

13
libraries/AP_DAL/AP_DAL.h
View File

@ -212,7 +212,11 @@ public:
// log optical flow data
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset);
// log external nav data
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
// Replay support:
#if APM_BUILD_TYPE(APM_BUILD_Replay)
void handle_message(const log_RFRH &msg) {
@ -285,6 +289,8 @@ public:
#endif
}
void handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
#endif
// map core number for replay
@ -298,10 +304,15 @@ private:
static AP_DAL *_singleton;
// framing structures
struct log_RFRH _RFRH;
struct log_RFRF _RFRF;
struct log_RFRN _RFRN;
// push-based sensor structures
struct log_ROFH _ROFH;
struct log_REPH _REPH;
struct log_REVH _REVH;
// cached variables for speed:
uint32_t _micros;

35
libraries/AP_DAL/LogStructure.h
View File

@ -3,6 +3,8 @@
#include <AP_Logger/LogStructure.h>
#include <AP_Math/vector3.h>
#include <AP_Math/vector2.h>
#include <AP_Math/matrix3.h>
#include <AP_Math/quaternion.h>
#define LOG_IDS_FROM_DAL \
LOG_RFRH_MSG, \
@ -31,7 +33,9 @@
LOG_RVOH_MSG, \
LOG_RMGH_MSG, \
LOG_RMGI_MSG, \
LOG_ROFH_MSG
LOG_ROFH_MSG, \
LOG_REPH_MSG, \
LOG_REVH_MSG
// Replay Data Structures
struct log_RFRH {
@ -315,6 +319,29 @@ struct log_ROFH {
uint8_t _end;
};
// @LoggerMessage: REPH
// @Description: Replay external position data
struct log_REPH {
Vector3f pos;
Quaternion quat;
float posErr;
float angErr;
uint32_t timeStamp_ms;
uint32_t resetTime_ms;
uint16_t delay_ms;
uint8_t _end;
};
// @LoggerMessage: REVH
// @Description: Replay external position data
struct log_REVH {
Vector3f vel;
float err;
uint32_t timeStamp_ms;
uint16_t delay_ms;
uint8_t _end;
};
#define RLOG_SIZE(sname) 3+offsetof(struct log_ ##sname,_end)
#define LOG_STRUCTURE_FROM_DAL \
@ -371,4 +398,8 @@ struct log_ROFH {
{ LOG_RVOH_MSG, RLOG_SIZE(RVOH), \
"RVOH", "fffIBBB", "OX,OY,OZ,Del,H,Ena,NPtr", "-------", "-------" }, \
{ LOG_ROFH_MSG, RLOG_SIZE(ROFH), \
"ROFH", "ffffIfffB", "FX,FY,GX,GY,Tms,PX,PY,PZ,Qual", "---------", "---------" },
"ROFH", "ffffIfffB", "FX,FY,GX,GY,Tms,PX,PY,PZ,Qual", "---------", "---------" }, \
{ LOG_REPH_MSG, RLOG_SIZE(REPH), \
"REPH", "fffffffffIIH", "PX,PY,PZ,Q1,Q2,Q3,Q4,PEr,AEr,TS,RT,D", "------------", "------------" }, \
{ LOG_REVH_MSG, RLOG_SIZE(REVH), \
"REVH", "ffffIH", "VX,VY,VZ,Er,TS,D", "------", "------" },