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

AC_AttitudeControl: introduce ANG log message for high resolution attitude logging 

Move RATE message to AC_AttitudeControl_Logging.cpp
This commit is contained in:
Andy Piper 2024-09-10 11:52:38 +01:00 committed by Andrew Tridgell
parent 202de8cc40
commit 71e2b756af
4 changed files with 105 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -1,9 +1,7 @@
#include "AC_AttitudeControl.h"
#include "AC_PosControl.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_Logger/AP_Logger.h>
extern const AP_HAL::HAL& hal;
@ -1263,51 +1261,3 @@ void AC_AttitudeControl::get_rpy_srate(float &roll_srate, float &pitch_srate, fl
pitch_srate = get_rate_pitch_pid().get_pid_info().slew_rate;
yaw_srate = get_rate_yaw_pid().get_pid_info().slew_rate;
}
#if HAL_LOGGING_ENABLED
// Write a rate packet
void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const
{
const Vector3f &rate_targets = rate_bf_targets();
const Vector3f &accel_target = pos_control.get_accel_target_cmss();
const Vector3f &gyro_rate = _rate_gyro;
const struct log_Rate pkt_rate{
LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
time_us : _rate_gyro_time_us,
control_roll : degrees(rate_targets.x),
roll : degrees(gyro_rate.x),
roll_out : _motors.get_roll()+_motors.get_roll_ff(),
control_pitch : degrees(rate_targets.y),
pitch : degrees(gyro_rate.y),
pitch_out : _motors.get_pitch()+_motors.get_pitch_ff(),
control_yaw : degrees(rate_targets.z),
yaw : degrees(gyro_rate.z),
yaw_out : _motors.get_yaw()+_motors.get_yaw_ff(),
control_accel : (float)accel_target.z,
accel : (float)(-(_ahrs.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 = get_last_angle_P_scale();
const Vector3f &pd_scale = _pd_scale_used;
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 : _rate_gyro_time_us,
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

3
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -434,6 +434,9 @@ public:
// write RATE message
void Write_Rate(const AC_PosControl &pos_control) const;
// write ANG message
void Write_ANG() const;
// User settable parameters
static const struct AP_Param::GroupInfo var_info[];

75
libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp Normal file
View File

@ -0,0 +1,75 @@
#include <AP_Logger/AP_Logger_config.h>
#if HAL_LOGGING_ENABLED
#include "AC_AttitudeControl.h"
#include "AC_PosControl.h"
#include <AP_Logger/AP_Logger.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include "LogStructure.h"
// Write an ANG packet
void AC_AttitudeControl::Write_ANG() const
{
Vector3f targets = get_att_target_euler_rad() * RAD_TO_DEG;
const struct log_ANG pkt{
LOG_PACKET_HEADER_INIT(LOG_ANG_MSG),
time_us : AP::scheduler().get_loop_start_time_us(),
control_roll : targets.x,
roll : degrees(_ahrs.roll),
control_pitch : targets.y,
pitch : degrees(_ahrs.pitch),
control_yaw : wrap_360(targets.z),
yaw : wrap_360(degrees(_ahrs.yaw)),
sensor_dt : AP::scheduler().get_last_loop_time_s()
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
// Write a rate packet
void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const
{
const Vector3f &rate_targets = rate_bf_targets();
const Vector3f &accel_target = pos_control.get_accel_target_cmss();
const Vector3f &gyro_rate = _rate_gyro;
const struct log_Rate pkt_rate{
LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
time_us : _rate_gyro_time_us,
control_roll : degrees(rate_targets.x),
roll : degrees(gyro_rate.x),
roll_out : _motors.get_roll()+_motors.get_roll_ff(),
control_pitch : degrees(rate_targets.y),
pitch : degrees(gyro_rate.y),
pitch_out : _motors.get_pitch()+_motors.get_pitch_ff(),
control_yaw : degrees(rate_targets.z),
yaw : degrees(gyro_rate.z),
yaw_out : _motors.get_yaw()+_motors.get_yaw_ff(),
control_accel : (float)accel_target.z,
accel : (float)(-(_ahrs.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 = get_last_angle_P_scale();
const Vector3f &pd_scale = _pd_scale_used;
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 : _rate_gyro_time_us,
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

29
libraries/AC_AttitudeControl/LogStructure.h
View File

@ -5,7 +5,8 @@
#define LOG_IDS_FROM_AC_ATTITUDECONTROL \
LOG_PSCN_MSG, \
LOG_PSCE_MSG, \
LOG_PSCD_MSG
LOG_PSCD_MSG, \
LOG_ANG_MSG
// @LoggerMessage: PSCN
// @Description: Position Control North
@ -92,6 +93,28 @@ struct PACKED log_Rate {
float throttle_slew;
};
// @LoggerMessage: ANG
// @Description: Attitude control attitude
// @Field: TimeUS: Timestamp of the current Attitude loop
// @Field: DesRoll: vehicle desired roll
// @Field: Roll: achieved vehicle roll
// @Field: DesPitch: vehicle desired pitch
// @Field: Pitch: achieved vehicle pitch
// @Field: DesYaw: vehicle desired yaw
// @Field: Yaw: achieved vehicle yaw
// @Field: Dt: attitude delta time
struct PACKED log_ANG {
LOG_PACKET_HEADER;
uint64_t time_us;
float control_roll;
float roll;
float control_pitch;
float pitch;
float control_yaw;
float yaw;
float sensor_dt;
};
#define PSCx_FMT "Qffffffff"
#define PSCx_UNITS "smmnnnooo"
#define PSCx_MULTS "F00000000"
@ -104,4 +127,6 @@ struct PACKED log_Rate {
{ LOG_PSCD_MSG, sizeof(log_PSCx), \
"PSCD", PSCx_FMT, "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS }, \
{ 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 }
"RATE", "Qfffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut,AOutSlew", "skk-kk-kk-oo--", "F?????????BB--" , true }, \
{ LOG_ANG_MSG, sizeof(log_ANG),\
"ANG", "Qfffffff", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,Dt", "sddddhhs", "F0000000" , true }