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

AP_AHRS: add throttle slew to RATE

Browse Source 
log PD scaling values
This commit is contained in:
Andy Piper 2022-07-16 10:38:12 +01:00 committed by Andrew Tridgell
parent 53b7f96a5d
commit 80fb33fb8b
2 changed files with 18 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_AHRS/AP_AHRS_Logging.cpp
View File

@ -160,21 +160,26 @@ void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl
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()
accel_out : motors.get_throttle(),
throttle_slew : motors.get_throttle_slew_rate()
};
AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate));
/*
log P gain scale if not == 1.0
log P/PD gain scale if not == 1.0
*/
const Vector3f &scale = attitude_control.get_angle_P_scale_logging();
if (!is_equal(scale.x,1.0f) || !is_equal(scale.y,1.0f) || !is_equal(scale.z,1.0f)) {
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));
}

12
libraries/AP_AHRS/LogStructure.h
View File

@ -123,6 +123,7 @@ struct PACKED log_POS {
// @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;
@ -138,6 +139,7 @@ struct PACKED log_Rate {
float control_accel;
float accel;
float accel_out;
float throttle_slew;
};
// @LoggerMessage: VSTB
@ -175,12 +177,18 @@ struct PACKED log_Video_Stabilisation {
// @Field: AngPScX: Angle P scale X
// @Field: AngPScY: Angle P scale Y
// @Field: AngPScZ: Angle P scale Z
// @Field: PDScX: PD scale X
// @Field: PDScY: PD scale Y
// @Field: PDScZ: PD scale Z
struct PACKED log_ATSC {
LOG_PACKET_HEADER;
uint64_t time_us;
float scaleP_x;
float scaleP_y;
float scaleP_z;
float scalePD_x;
float scalePD_y;
float scalePD_z;
};
@ -196,9 +204,9 @@ struct PACKED log_ATSC {
{ LOG_POS_MSG, sizeof(log_POS), \
"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 }, \
"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), \
"ATSC", "Qfff", "TimeUS,AngPScX,AngPScY,AngPScZ", "s---", "F000" , true }, \
"ATSC", "Qffffff", "TimeUS,AngPScX,AngPScY,AngPScZ,PDScX,PDScY,PDScZ", "s------", "F000000" , true }, \
{ LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \
"VSTB", "Qffffffffff", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,Q1,Q2,Q3,Q4", "sEEEooo????", "F000000????" },