Skip to content

Commit

Permalink
AC_AttitudeControl: record latest gyro value and time used for logging
Browse files Browse the repository at this point in the history 
move Write_Rate() to AC_AttitudeControl
move RATE log structure to AC_AttitudeControl
  • Loading branch information
@andyp1per
andyp1per committed Sep 9, 2024
1 parent bbe3bbc commit ed9f731
Show file tree
Hide file tree
Showing 6 changed files with 109 additions and 14 deletions.
50 changes: 50 additions & 0 deletions libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,9 @@
#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;

Expand Down Expand Up @@ -1240,3 +1242,51 @@ 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
{
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
9 changes: 7 additions & 2 deletions libraries/AC_AttitudeControl/AC_AttitudeControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -428,8 +428,8 @@ class AC_AttitudeControl {
// purposes
void set_PD_scale_mult(const Vector3f &pd_scale) { _pd_scale *= pd_scale; }

// get the value of the PD scale that was used in the last loop, for logging
const Vector3f &get_PD_scale_logging(void) const { return _pd_scale_used; }
// write RATE message
void Write_Rate(const AC_PosControl &pos_control) const;

// User settable parameters
static const struct AP_Param::GroupInfo var_info[];
Expand Down Expand Up @@ -487,6 +487,11 @@ class AC_AttitudeControl {
AP_Float _land_pitch_mult;
AP_Float _land_yaw_mult;

// latest gyro value use by the rate_controller
Vector3f _rate_gyro;
// timestamp of the latest gyro value used by the rate controller
uint64_t _rate_gyro_time_us;

// Intersampling period in seconds
float _dt;

Expand Down
7 changes: 4 additions & 3 deletions libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -422,20 +422,21 @@ void AC_AttitudeControl_Heli::rate_controller_run()
{
_ang_vel_body += _sysid_ang_vel_body;

Vector3f gyro_latest = _ahrs.get_gyro_latest();
_rate_gyro = _ahrs.get_gyro_latest();
_rate_gyro_time_us = AP_HAL::micros64();

// call rate controllers and send output to motors object
// if using a flybar passthrough roll and pitch directly to motors
if (_flags_heli.flybar_passthrough) {
_motors.set_roll(_passthrough_roll / 4500.0f);
_motors.set_pitch(_passthrough_pitch / 4500.0f);
} else {
rate_bf_to_motor_roll_pitch(gyro_latest, _ang_vel_body.x, _ang_vel_body.y);
rate_bf_to_motor_roll_pitch(_rate_gyro, _ang_vel_body.x, _ang_vel_body.y);
}
if (_flags_heli.tail_passthrough) {
_motors.set_yaw(_passthrough_yaw / 4500.0f);
} else {
_motors.set_yaw(rate_target_to_motor_yaw(gyro_latest.z, _ang_vel_body.z));
_motors.set_yaw(rate_target_to_motor_yaw(_rate_gyro.z, _ang_vel_body.z));
}

_sysid_ang_vel_body.zero();
Expand Down
9 changes: 5 additions & 4 deletions libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -449,15 +449,16 @@ void AC_AttitudeControl_Multi::rate_controller_run()

_ang_vel_body += _sysid_ang_vel_body;

Vector3f gyro_latest = _ahrs.get_gyro_latest();
_rate_gyro = _ahrs.get_gyro_latest();
_rate_gyro_time_us = AP_HAL::micros64();

_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, _rate_gyro.x, _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
_motors.set_roll_ff(get_rate_roll_pid().get_ff());

_motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y);
_motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, _rate_gyro.y, _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y);
_motors.set_pitch_ff(get_rate_pitch_pid().get_ff());

_motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z);
_motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, _rate_gyro.z, _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z);
_motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);

_sysid_ang_vel_body.zero();
Expand Down
10 changes: 6 additions & 4 deletions libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -423,10 +423,12 @@ void AC_AttitudeControl_Sub::rate_controller_run()
// move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
update_throttle_rpy_mix();

Vector3f gyro_latest = _ahrs.get_gyro_latest();
_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll));
_motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch));
_motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw));
_rate_gyro = _ahrs.get_gyro_latest();
_rate_gyro_time_us = AP_HAL::micros64();

_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, _rate_gyro.x, _dt, _motors.limit.roll));
_motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, _rate_gyro.y, _dt, _motors.limit.pitch));
_motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, _rate_gyro.z, _dt, _motors.limit.yaw));

control_monitor_update();
}
Expand Down
38 changes: 37 additions & 1 deletion libraries/AC_AttitudeControl/LogStructure.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,40 @@ struct PACKED log_PSCx {
float accel;
};

// @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;
};

#define PSCx_FMT "Qffffffff"
#define PSCx_UNITS "smmnnnooo"
#define PSCx_MULTS "F00000000"
Expand All @@ -68,4 +102,6 @@ struct PACKED log_PSCx {
{ LOG_PSCE_MSG, sizeof(log_PSCx), \
"PSCE", PSCx_FMT, "TimeUS,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", PSCx_UNITS, PSCx_MULTS }, \
{ LOG_PSCD_MSG, sizeof(log_PSCx), \
"PSCD", PSCx_FMT, "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS }
"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 }

0 comments on commit ed9f731

Please sign in to comment.