Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: move Write_Rate() to AC_AttitudeControl 

move RATE log structure to AC_AttitudeControl
This commit is contained in:
Andy Piper 2024-08-22 20:20:02 +01:00 committed by Peter Hall
parent c40422cde3
commit 4f4e822d88
3 changed files with 0 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_AHRS/AP_AHRS_Logging.cpp
View File

@ -139,50 +139,4 @@ void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
AP::logger().WriteBlock(&pkt, sizeof(pkt)); AP::logger().WriteBlock(&pkt, sizeof(pkt));
} }
// Write a rate packet
void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl &attitude_control,
const AC_PosControl &pos_control) const
{
const Vector3f &rate_targets = attitude_control.rate_bf_targets();
const Vector3f &accel_target = pos_control.get_accel_target_cmss();
const auto timeus = AP_HAL::micros64();
const struct log_Rate pkt_rate{
LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
time_us : timeus,
control_roll : degrees(rate_targets.x),
roll : degrees(get_gyro().x),
roll_out : motors.get_roll()+motors.get_roll_ff(),
control_pitch : degrees(rate_targets.y),
pitch : degrees(get_gyro().y),
pitch_out : motors.get_pitch()+motors.get_pitch_ff(),
control_yaw : degrees(rate_targets.z),
yaw : degrees(get_gyro().z),
yaw_out : motors.get_yaw()+motors.get_yaw_ff(),
control_accel : (float)accel_target.z,
accel : (float)(-(get_accel_ef().z + GRAVITY_MSS) * 100.0f),
accel_out : motors.get_throttle(),
throttle_slew : motors.get_throttle_slew_rate()
};
AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate));
/*
log P/PD gain scale if not == 1.0
*/
const Vector3f &scale = attitude_control.get_last_angle_P_scale();
const Vector3f &pd_scale = attitude_control.get_PD_scale_logging();
if (scale != AC_AttitudeControl::VECTORF_111 || pd_scale != AC_AttitudeControl::VECTORF_111) {
const struct log_ATSC pkt_ATSC {
LOG_PACKET_HEADER_INIT(LOG_ATSC_MSG),
time_us : timeus,
scaleP_x : scale.x,
scaleP_y : scale.y,
scaleP_z : scale.z,
scalePD_x : pd_scale.x,
scalePD_y : pd_scale.y,
scalePD_z : pd_scale.z,
};
AP::logger().WriteBlock(&pkt_ATSC, sizeof(pkt_ATSC));
}
}
#endif // HAL_LOGGING_ENABLED #endif // HAL_LOGGING_ENABLED

3
libraries/AP_AHRS/AP_AHRS_View.h
View File

@ -174,9 +174,6 @@ public:
// Logging Functions // Logging Functions
void Write_AttitudeView(const Vector3f &targets) const; void Write_AttitudeView(const Vector3f &targets) const;
void Write_Rate(const class AP_Motors &motors,
const class AC_AttitudeControl &attitude_control,
const AC_PosControl &pos_control) const;
float roll; float roll;
float pitch; float pitch;

36
libraries/AP_AHRS/LogStructure.h
View File

@ -105,40 +105,6 @@ struct PACKED log_POS {
float rel_origin_alt; float rel_origin_alt;
}; };
// @LoggerMessage: RATE
// @Description: Desired and achieved vehicle attitude rates. Not logged in Fixed Wing Plane modes.
// @Field: TimeUS: Time since system startup
// @Field: RDes: vehicle desired roll rate
// @Field: R: achieved vehicle roll rate
// @Field: ROut: normalized output for Roll
// @Field: PDes: vehicle desired pitch rate
// @Field: P: vehicle pitch rate
// @Field: POut: normalized output for Pitch
// @Field: Y: achieved vehicle yaw rate
// @Field: YOut: normalized output for Yaw
// @Field: YDes: vehicle desired yaw rate
// @Field: ADes: desired vehicle vertical acceleration
// @Field: A: achieved vehicle vertical acceleration
// @Field: AOut: percentage of vertical thrust output current being used
// @Field: AOutSlew: vertical thrust output slew rate
struct PACKED log_Rate {
LOG_PACKET_HEADER;
uint64_t time_us;
float control_roll;
float roll;
float roll_out;
float control_pitch;
float pitch;
float pitch_out;
float control_yaw;
float yaw;
float yaw_out;
float control_accel;
float accel;
float accel_out;
float throttle_slew;
};
// @LoggerMessage: VSTB // @LoggerMessage: VSTB
// @Description: Log message for video stabilisation software such as Gyroflow // @Description: Log message for video stabilisation software such as Gyroflow
// @Field: TimeUS: Time since system startup // @Field: TimeUS: Time since system startup
@ -200,8 +166,6 @@ struct PACKED log_ATSC {
"ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s#DUm", "F-GGB" }, \ "ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s#DUm", "F-GGB" }, \
{ LOG_POS_MSG, sizeof(log_POS), \ { LOG_POS_MSG, sizeof(log_POS), \
"POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt", "sDUmmm", "FGG000" , true }, \ "POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt", "sDUmmm", "FGG000" , true }, \
{ LOG_RATE_MSG, sizeof(log_Rate), \
"RATE", "Qfffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut,AOutSlew", "skk-kk-kk-oo--", "F?????????BB--" , true }, \
{ LOG_ATSC_MSG, sizeof(log_ATSC), \ { LOG_ATSC_MSG, sizeof(log_ATSC), \
"ATSC", "Qffffff", "TimeUS,AngPScX,AngPScY,AngPScZ,PDScX,PDScY,PDScZ", "s------", "F000000" , true }, \ "ATSC", "Qffffff", "TimeUS,AngPScX,AngPScY,AngPScZ,PDScX,PDScY,PDScZ", "s------", "F000000" , true }, \
{ LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \ { LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \