diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 266618e8fd..75c52636d8 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -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 diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index b77b1b4b8f..b42c7c57f8 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -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 { diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h index 6ca732e9d6..c8eb04f2a4 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -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????" },