Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_DAL: support wheel odomotry

Browse Source
This commit is contained in:
Andrew Tridgell 2020-11-07 11:34:06 +11:00
parent 4a06641048
commit b8a3413f5e
3 changed files with 48 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_DAL/AP_DAL.cpp
View File

@ -277,6 +277,7 @@ void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawF
WRITE_REPLAY_BLOCK_IFCHANGD(ROFH, _ROFH, old);
}
// log external navigation data
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();
@ -292,6 +293,7 @@ void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float
WRITE_REPLAY_BLOCK_IFCHANGD(REPH, _REPH, old);
}
// log external velocity data
void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
end_frame();
@ -305,6 +307,20 @@ void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSta
}
// log wheel odomotry data
void AP_DAL::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
{
end_frame();
const log_RWOH old = _RWOH;
_RWOH.delAng = delAng;
_RWOH.delTime = delTime;
_RWOH.timeStamp_ms = timeStamp_ms;
_RWOH.posOffset = posOffset;
_RWOH.radius = radius;
WRITE_REPLAY_BLOCK_IFCHANGD(RWOH, _RWOH, old);
}
#if APM_BUILD_TYPE(APM_BUILD_Replay)
/*
handle frame message. This message triggers the EKF2/EKF3 updates and logging
@ -369,7 +385,7 @@ void AP_DAL::handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
}
/*
handle external position data
handle external velocity data
*/
void AP_DAL::handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
{
@ -377,6 +393,16 @@ void AP_DAL::handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
ekf2.writeExtNavVelData(_REVH.vel, _REVH.err, _REVH.timeStamp_ms, _REVH.delay_ms);
ekf3.writeExtNavVelData(_REVH.vel, _REVH.err, _REVH.timeStamp_ms, _REVH.delay_ms);
}
/*
handle wheel odomotry data
*/
void AP_DAL::handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
{
_RWOH = msg;
// note that EKF2 does not support wheel odomotry
ekf3.writeWheelOdom(msg.delAng, msg.delTime, msg.timeStamp_ms, msg.posOffset, msg.radius);
}
#endif // APM_BUILD_Replay
#include <stdio.h>

5
libraries/AP_DAL/AP_DAL.h
View File

@ -217,6 +217,9 @@ public:
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);
// log wheel odomotry data
void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius);
// Replay support:
#if APM_BUILD_TYPE(APM_BUILD_Replay)
void handle_message(const log_RFRH &msg) {
@ -291,6 +294,7 @@ public:
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);
void handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
#endif
// map core number for replay
@ -313,6 +317,7 @@ private:
struct log_ROFH _ROFH;
struct log_REPH _REPH;
struct log_REVH _REVH;
struct log_RWOH _RWOH;
// cached variables for speed:
uint32_t _micros;

18
libraries/AP_DAL/LogStructure.h
View File

@ -35,7 +35,8 @@
LOG_RMGI_MSG, \
LOG_ROFH_MSG, \
LOG_REPH_MSG, \
LOG_REVH_MSG
LOG_REVH_MSG, \
LOG_RWOH_MSG
// Replay Data Structures
struct log_RFRH {
@ -342,6 +343,17 @@ struct log_REVH {
uint8_t _end;
};
// @LoggerMessage: RWOH
// @Description: Replay wheel odometry data
struct log_RWOH {
float delAng;
float delTime;
uint32_t timeStamp_ms;
Vector3f posOffset;
float radius;
uint8_t _end;
};
#define RLOG_SIZE(sname) 3+offsetof(struct log_ ##sname,_end)
#define LOG_STRUCTURE_FROM_DAL \
@ -402,4 +414,6 @@ struct log_REVH {
{ 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", "------", "------" },
"REVH", "ffffIH", "VX,VY,VZ,Er,TS,D", "------", "------" }, \
{ LOG_RWOH_MSG, RLOG_SIZE(RWOH), \
"RWOH", "ffIffff", "DA,DT,TS,PX,PY,PZ,R", "-------", "-------" },