Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: add video stab log message

Browse Source
This commit is contained in:
Iampete1 2021-12-15 19:00:26 +00:00 committed by Andrew Tridgell
parent cee0503812
commit efb04e7f30
3 changed files with 26 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_AHRS/AP_AHRS.h
View File

@ -432,6 +432,7 @@ public:
};
void Write_Origin(LogOriginType origin_type, const Location &loc) const;
void Write_POS(void) const;
void write_video_stabilisation() const;
// return a smoothed and corrected gyro vector in radians/second
// using the latest ins data (which may not have been consumed by

23
libraries/AP_AHRS/AP_AHRS_Logging.cpp
View File

@ -97,6 +97,29 @@ void AP_AHRS::Write_POS() const
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
// Write a packet for video stabilisation
void AP_AHRS::write_video_stabilisation() const
{
Quaternion current_attitude;
get_quat_body_to_ned(current_attitude);
Vector3f accel = get_accel() - get_accel_bias();
const struct log_Video_Stabilisation pkt {
LOG_PACKET_HEADER_INIT(LOG_VIDEO_STABILISATION_MSG),
time_us : AP_HAL::micros64(),
gyro_x : _gyro_estimate.x,
gyro_y : _gyro_estimate.y,
gyro_z : _gyro_estimate.z,
accel_x : accel.x,
accel_y : accel.y,
accel_z : accel.z,
Q1 : current_attitude.q1,
Q2 : current_attitude.q2,
Q3 : current_attitude.q3,
Q4 : current_attitude.q4,
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
// Write an attitude view packet
void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
{

4
libraries/AP_AHRS/LogStructure.h
View File

@ -153,7 +153,7 @@ struct PACKED log_Rate {
// @Field: Q3: Estimated attitude quaternion component 3
// @Field: Q4: Estimated attitude quaternion component 4
struct PACKED log_Gyroflow {
struct PACKED log_Video_Stabilisation {
LOG_PACKET_HEADER;
uint64_t time_us;
float gyro_x;
@ -182,6 +182,6 @@ struct PACKED log_Gyroflow {
"POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt", "sDUmmm", "FGG000" , true }, \
{ LOG_RATE_MSG, sizeof(log_Rate), \
"RATE", "Qffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut", "skk-kk-kk-oo-", "F?????????BB-" , true }, \
{ LOG_GYROFLOW_MSG, sizeof(log_Gyroflow), \
{ LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \
"VSTB", "Qffffffffff", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,Q1,Q2,Q3,Q4", "sEEEooo????", "F000000????" },