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

AP_DAL: added body frame odomotry

Browse Source
This commit is contained in:
Andrew Tridgell 2020-11-07 16:58:32 +11:00
parent 6c4e552a0b
commit 9635231088
3 changed files with 50 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_DAL/AP_DAL.cpp
View File

@ -321,6 +321,19 @@ void AP_DAL::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms,
WRITE_REPLAY_BLOCK_IFCHANGD(RWOH, _RWOH, old);
}
void AP_DAL::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
{
end_frame();
const log_RBOH old = _RBOH;
_RBOH.quality = quality;
_RBOH.delPos = delPos;
_RBOH.delAng = delAng;
_RBOH.delTime = delTime;
_RBOH.timeStamp_ms = timeStamp_ms;
WRITE_REPLAY_BLOCK_IFCHANGD(RBOH, _RBOH, old);
}
#if APM_BUILD_TYPE(APM_BUILD_Replay)
/*
handle frame message. This message triggers the EKF2/EKF3 updates and logging
@ -370,8 +383,8 @@ void AP_DAL::handle_message(const log_RFRF &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
void AP_DAL::handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
{
_ROFH = msg;
ekf2.writeOptFlowMeas(_ROFH.rawFlowQuality, _ROFH.rawFlowRates, _ROFH.rawGyroRates, _ROFH.msecFlowMeas, _ROFH.posOffset);
ekf3.writeOptFlowMeas(_ROFH.rawFlowQuality, _ROFH.rawFlowRates, _ROFH.rawGyroRates, _ROFH.msecFlowMeas, _ROFH.posOffset);
ekf2.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset);
ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset);
}
/*
@ -380,8 +393,8 @@ void AP_DAL::handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
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);
ekf2.writeExtNavData(msg.pos, msg.quat, msg.posErr, msg.angErr, msg.timeStamp_ms, msg.delay_ms, msg.resetTime_ms);
ekf3.writeExtNavData(msg.pos, msg.quat, msg.posErr, msg.angErr, msg.timeStamp_ms, msg.delay_ms, msg.resetTime_ms);
}
/*
@ -390,8 +403,8 @@ void AP_DAL::handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
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);
ekf2.writeExtNavVelData(msg.vel, msg.err, msg.timeStamp_ms, msg.delay_ms);
ekf3.writeExtNavVelData(msg.vel, msg.err, msg.timeStamp_ms, msg.delay_ms);
}
/*
@ -403,6 +416,16 @@ void AP_DAL::handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
// note that EKF2 does not support wheel odomotry
ekf3.writeWheelOdom(msg.delAng, msg.delTime, msg.timeStamp_ms, msg.posOffset, msg.radius);
}
/*
handle body frame odomotry
*/
void AP_DAL::handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
{
_RBOH = msg;
// note that EKF2 does not support body frame odomotry
ekf3.writeBodyFrameOdom(msg.quality, msg.delPos, msg.delAng, msg.delTime, msg.timeStamp_ms, msg.delay_ms, msg.posOffset);
}
#endif // APM_BUILD_Replay
#include <stdio.h>

3
libraries/AP_DAL/AP_DAL.h
View File

@ -219,6 +219,7 @@ public:
// log wheel odomotry data
void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius);
void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset);
// Replay support:
#if APM_BUILD_TYPE(APM_BUILD_Replay)
@ -295,6 +296,7 @@ public:
void handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
#endif
// map core number for replay
@ -318,6 +320,7 @@ private:
struct log_REPH _REPH;
struct log_REVH _REVH;
struct log_RWOH _RWOH;
struct log_RBOH _RBOH;
// cached variables for speed:
uint32_t _micros;

20
libraries/AP_DAL/LogStructure.h
View File

@ -36,7 +36,8 @@
LOG_ROFH_MSG, \
LOG_REPH_MSG, \
LOG_REVH_MSG, \
LOG_RWOH_MSG
LOG_RWOH_MSG, \
LOG_RBOH_MSG
// Replay Data Structures
struct log_RFRH {
@ -354,6 +355,19 @@ struct log_RWOH {
uint8_t _end;
};
// @LoggerMessage: RBOH
// @Description: Replay body odometry data
struct log_RBOH {
float quality;
Vector3f delPos;
Vector3f delAng;
float delTime;
uint32_t timeStamp_ms;
Vector3f posOffset;
uint16_t delay_ms;
uint8_t _end;
};
#define RLOG_SIZE(sname) 3+offsetof(struct log_ ##sname,_end)
#define LOG_STRUCTURE_FROM_DAL \
@ -416,4 +430,6 @@ struct log_RWOH {
{ LOG_REVH_MSG, RLOG_SIZE(REVH), \
"REVH", "ffffIH", "VX,VY,VZ,Er,TS,D", "------", "------" }, \
{ LOG_RWOH_MSG, RLOG_SIZE(RWOH), \
"RWOH", "ffIffff", "DA,DT,TS,PX,PY,PZ,R", "-------", "-------" },
"RWOH", "ffIffff", "DA,DT,TS,PX,PY,PZ,R", "-------", "-------" }, \
{ LOG_RBOH_MSG, RLOG_SIZE(RBOH), \
"RBOH", "ffffffffIfffH", "Q,DPX,DPY,DPZ,DAX,DAY,DAZ,DT,TS,OX,OY,OZ,D", "-------------", "-------------" },