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

AP_AHRS: added ATSC logging 

log scale factors for angle P scaling when not == 1.0
This commit is contained in:
Andrew Tridgell 2022-10-17 12:49:02 +11:00
parent bd1be3b41c
commit d398e567a3
2 changed files with 35 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_AHRS/AP_AHRS_Logging.cpp
View File

@ -145,9 +145,10 @@ void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl
{
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 : AP_HAL::micros64(),
time_us : timeus,
control_roll : degrees(rate_targets.x),
roll : degrees(get_gyro().x),
roll_out : motors.get_roll()+motors.get_roll_ff(),
@ -162,4 +163,19 @@ void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl
accel_out : motors.get_throttle()
};
AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate));
/*
log P 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 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,
};
AP::logger().WriteBlock(&pkt_ATSC, sizeof(pkt_ATSC));
}
}

19
libraries/AP_AHRS/LogStructure.h
View File

@ -8,7 +8,8 @@
LOG_ATTITUDE_MSG, \
LOG_ORGN_MSG, \
LOG_POS_MSG, \
LOG_RATE_MSG
LOG_RATE_MSG, \
LOG_ATSC_MSG
// @LoggerMessage: AHR2
// @Description: Backup AHRS data
@ -168,6 +169,20 @@ struct PACKED log_Video_Stabilisation {
float Q4;
};
// @LoggerMessage: ATSC
// @Description: Scale factors for attitude controller
// @Field: TimeUS: Time since system startup
// @Field: AngPScX: Angle P scale X
// @Field: AngPScY: Angle P scale Y
// @Field: AngPScZ: Angle P scale Z
struct PACKED log_ATSC {
LOG_PACKET_HEADER;
uint64_t time_us;
float scaleP_x;
float scaleP_y;
float scaleP_z;
};
#define LOG_STRUCTURE_FROM_AHRS \
{ LOG_AHR2_MSG, sizeof(log_AHRS), \
@ -182,6 +197,8 @@ struct PACKED log_Video_Stabilisation {
"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_ATSC_MSG, sizeof(log_ATSC), \
"ATSC", "Qfff", "TimeUS,AngPScX,AngPScY,AngPScZ", "s---", "F000" , true }, \
{ LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \
"VSTB", "Qffffffffff", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,Q1,Q2,Q3,Q4", "sEEEooo????", "F000000????" },