Skip to content

Commit

Permalink
Blimp: Fixup - parameter move
Browse files Browse the repository at this point in the history
  • Loading branch information
@MichelleRos
MichelleRos committed Apr 26, 2024
1 parent f388636 commit 1e3d966
Show file tree
Hide file tree
Showing 9 changed files with 81 additions and 546 deletions.
1 change: 0 additions & 1 deletion Blimp/Blimp.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -213,7 +213,6 @@ void Blimp::one_hz_loop()

AP_Notify::flags.flying = !ap.land_complete;

blimp.pid_pos_yaw.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
}

void Blimp::read_AHRS(void)
Expand Down
11 changes: 0 additions & 11 deletions Blimp/Blimp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -224,17 +224,6 @@ class Blimp : public AP_Vehicle
// Inertial Navigation
AP_InertialNav inertial_nav;

// Vel & pos PIDs
AC_PID pid_vel_x{3, 0.2, 0, 0, 0.2, 3, 3, 3}; //These are the defaults - P I D FF IMAX FiltHz FiltDHz DT
AC_PID pid_vel_y{3, 0.2, 0, 0, 0.2, 3, 3, 3};
AC_PID pid_vel_z{7, 1.5, 0, 0, 1, 3, 3, 3};
AC_PID pid_vel_yaw{3, 0.4, 0, 0, 0.2, 3, 3, 3};

AC_PID pid_pos_x{1, 0.05, 0, 0, 0.1, 3, 3, 3};
AC_PID pid_pos_y{1, 0.05, 0, 0, 0.1, 3, 3, 3};
AC_PID pid_pos_z{0.7, 0, 0, 0, 0, 3, 3, 3};
AC_PID pid_pos_yaw{1.2, 0.5, 0, 0, 2, 3, 3, 3}; //p, i, d, ff, imax, filt_t, filt_e, filt_d, dt, opt srmax, opt srtau

// System Timers
// --------------
// arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed.
Expand Down
16 changes: 8 additions & 8 deletions Blimp/GCS_Mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,28 +128,28 @@ void GCS_MAVLINK_Blimp::send_pid_tuning()
const AP_PIDInfo *pid_info = nullptr;
switch (axes[i]) {
case PID_SEND::VELX:
pid_info = &blimp.pid_vel_x.get_pid_info();
pid_info = &blimp.loiter->pid_vel_x.get_pid_info();
break;
case PID_SEND::VELY:
pid_info = &blimp.pid_vel_y.get_pid_info();
pid_info = &blimp.loiter->pid_vel_y.get_pid_info();
break;
case PID_SEND::VELZ:
pid_info = &blimp.pid_vel_z.get_pid_info();
pid_info = &blimp.loiter->pid_vel_z.get_pid_info();
break;
case PID_SEND::VELYAW:
pid_info = &blimp.pid_vel_yaw.get_pid_info();
pid_info = &blimp.loiter->pid_vel_yaw.get_pid_info();
break;
case PID_SEND::POSX:
pid_info = &blimp.pid_pos_x.get_pid_info();
pid_info = &blimp.loiter->pid_pos_x.get_pid_info();
break;
case PID_SEND::POSY:
pid_info = &blimp.pid_pos_y.get_pid_info();
pid_info = &blimp.loiter->pid_pos_y.get_pid_info();
break;
case PID_SEND::POSZ:
pid_info = &blimp.pid_pos_z.get_pid_info();
pid_info = &blimp.loiter->pid_pos_z.get_pid_info();
break;
case PID_SEND::POSYAW:
pid_info = &blimp.pid_pos_yaw.get_pid_info();
pid_info = &blimp.loiter->pid_pos_yaw.get_pid_info();
break;
default:
continue;
Expand Down
16 changes: 8 additions & 8 deletions Blimp/Log.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,14 +79,14 @@ struct PACKED log_Control_Tuning {
// Write PID packets
void Blimp::Log_Write_PIDs()
{
logger.Write_PID(LOG_PIVN_MSG, pid_vel_x.get_pid_info());
logger.Write_PID(LOG_PIVE_MSG, pid_vel_y.get_pid_info());
logger.Write_PID(LOG_PIVD_MSG, pid_vel_z.get_pid_info());
logger.Write_PID(LOG_PIVY_MSG, pid_vel_yaw.get_pid_info());
logger.Write_PID(LOG_PIDN_MSG, pid_pos_x.get_pid_info());
logger.Write_PID(LOG_PIDE_MSG, pid_pos_y.get_pid_info());
logger.Write_PID(LOG_PIDD_MSG, pid_pos_z.get_pid_info());
logger.Write_PID(LOG_PIDY_MSG, pid_pos_yaw.get_pid_info());
logger.Write_PID(LOG_PIVN_MSG, loiter->pid_vel_x.get_pid_info());
logger.Write_PID(LOG_PIVE_MSG, loiter->pid_vel_y.get_pid_info());
logger.Write_PID(LOG_PIVD_MSG, loiter->pid_vel_z.get_pid_info());
logger.Write_PID(LOG_PIVY_MSG, loiter->pid_vel_yaw.get_pid_info());
logger.Write_PID(LOG_PIDN_MSG, loiter->pid_pos_x.get_pid_info());
logger.Write_PID(LOG_PIDE_MSG, loiter->pid_pos_y.get_pid_info());
logger.Write_PID(LOG_PIDD_MSG, loiter->pid_pos_z.get_pid_info());
logger.Write_PID(LOG_PIDY_MSG, loiter->pid_pos_yaw.get_pid_info());
}

// Write an attitude packet
Expand Down
211 changes: 48 additions & 163 deletions Blimp/Loiter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,72 +3,33 @@
#include <AC_AttitudeControl/AC_PosControl.h>

const AP_Param::GroupInfo Loiter::var_info[] = {

// @Param: RP_DAMP_LIM
// @DisplayName: Roll/Pitch limit (in deg) before damping outputs. Zero means disabled.
// @Description: RP D
// @Range: 0 180
// @User: Standard
AP_GROUPINFO("RP_DAMP_LIM", 4, Loiter, rp_damp_lim, 0),

// @Param: RP_DAMP_OFF
// @DisplayName: Roll/Pitch limit (in deg) where outputs are fully cut off when _AMT is 1.
// @Description: RP D
// @Range: 0 180
// @User: Standard
AP_GROUPINFO("RP_DAMP_OFF", 5, Loiter, rp_damp_off, 0),

// @Param: RP_DAMP_AMT
// @DisplayName: Roll/Pitch output damping amount. Higher number means more damping.
// @Description: RP D
// @Range: 0 1
// @User: Standard
AP_GROUPINFO("RP_DAMP_AMT", 9, Loiter, rp_damp_amt, 1),

// @Param: RP_DAMP_LI2
// @DisplayName: Roll/Pitch rate limit (in deg/s) before damping outputs. Zero means disabled.
// @Description: RP D
// @Range: 0 180
AP_SUBGROUPINFO(pid_vel_x, "LOIT_VELX_", 0, Loiter, AC_PID),
AP_SUBGROUPINFO(pid_vel_y, "LOIT_VELY_", 1, Loiter, AC_PID),
AP_SUBGROUPINFO(pid_vel_z, "LOIT_VELZ_", 2, Loiter, AC_PID),
AP_SUBGROUPINFO(pid_vel_yaw, "LOIT_VELYAW_", 3, Loiter, AC_PID),
AP_SUBGROUPINFO(pid_pos_x, "LOIT_POSX_", 4, Loiter, AC_PID),
AP_SUBGROUPINFO(pid_pos_y, "LOIT_POSY_", 5, Loiter, AC_PID),
AP_SUBGROUPINFO(pid_pos_z, "LOIT_POSZ_", 6, Loiter, AC_PID),
AP_SUBGROUPINFO(pid_pos_yaw, "LOIT_POSYAW_", 7, Loiter, AC_PID),

// @Param: SCALER_SPD
// @DisplayName: Loiter scaler speed
// @Description: Factor for scaler filter, speed at which the scaler updates. Zero means immediate change (no filter). Higher number means slower change.
// @Range: 0 0.999
// @User: Advanced
AP_GROUPINFO("SCALER_SPD", 8, Loiter, scaler_spd, 0.99),

// @Param: LAG
// @DisplayName: Loiter lag
// @Description: Number of seconds' worth of travel that the actual position can be behind the target position.
// @Units: s
// @Range: 0 0.999
// @User: Standard
AP_GROUPINFO("RP_DAMP_LI2", 6, Loiter, rp_damp_lim2, 0),

// @Param: RP_DAMP_OF2
// @DisplayName: Roll/Pitch rate limit (in deg/s) where outputs are fully cut off when _AMT is 1.
// @Description: RP D
// @Range: 0 1
// @User: Standard
AP_GROUPINFO("RP_DAMP_OF2", 7, Loiter, rp_damp_off2, 0),

// @Param: RP_DAMP_AM2
// @DisplayName: Roll/Pitch output damping amount. Higher number means more damping.
// @Description: RP D
// @Range: 0 1
// @User: Standard
AP_GROUPINFO("RP_DAMP_AM2", 10, Loiter, rp_damp_amt2, 1),

// @Param: RP_DAMP_MSK
// @DisplayName: Roll/Pitch output damping mask for which outputs to affect
// @Description: RP D
// @Values: 15:All enabled, 3:Right & Front only
// @Bitmask: 0:Right,1:Front,2:Down,3:Yaw
// @User: Standard
AP_GROUPINFO("RP_DAMP_MSK", 8, Loiter, rp_damp_msk, 15),

//Higher number means slower change. Zero means immediate change (no filter).
AP_GROUPINFO("SCALER_SPD", 11, Loiter, scaler_spd, 0.99),

//Number of seconds' worth of travel that the actual position can be behind the target position.
AP_GROUPINFO("LAG", 12, Loiter, pos_lag, 1),

AP_GROUPINFO("BAT_MULT", 13, Loiter, bat_mult, 1),
AP_GROUPINFO("BAT_OFF", 14, Loiter, bat_off, 0),
AP_GROUPINFO("LAG", 9, Loiter, pos_lag, 1),

AP_GROUPEND
};

#define MA scaler_spd
#define MO (1-MA)

void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled)
{
const float dt = blimp.scheduler.get_last_loop_time_s();
Expand Down Expand Up @@ -96,18 +57,18 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled

Vector3f target_vel_ef;
if (!axes_disabled.x)
target_vel_ef.x = blimp.pid_pos_x.update_all(target_pos.x, blimp.pos_ned.x, dt, limit.x);
target_vel_ef.x = pid_pos_x.update_all(target_pos.x, blimp.pos_ned.x, dt, limit.x);
if (!axes_disabled.x)
target_vel_ef.y = blimp.pid_pos_y.update_all(target_pos.y, blimp.pos_ned.y, dt, limit.y);
target_vel_ef.y = pid_pos_y.update_all(target_pos.y, blimp.pos_ned.y, dt, limit.y);
if (!axes_disabled.z) {
target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt, limit.z);
target_vel_ef.z = pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt, limit.z);
}

float target_vel_yaw = 0;
if (!axes_disabled.yaw) {
target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef), dt, limit.yaw);
blimp.pid_pos_yaw.set_target_rate(target_yaw);
blimp.pid_pos_yaw.set_actual_rate(yaw_ef);
target_vel_yaw = pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef), dt, limit.yaw);
pid_pos_yaw.set_target_rate(target_yaw);
pid_pos_yaw.set_actual_rate(yaw_ef);
}

Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -blimp.g.max_vel_x, blimp.g.max_vel_x),
Expand All @@ -116,12 +77,10 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled
float target_vel_yaw_c = constrain_float(target_vel_yaw, -blimp.g.max_vel_yaw, blimp.g.max_vel_yaw);

if (!blimp.motors->armed()) {
blimp.pid_pos_x.set_integrator(0);
blimp.pid_pos_y.set_integrator(0);
blimp.pid_pos_z.set_integrator(0);
blimp.pid_pos_yaw.set_integrator(0);
target_pos = blimp.pos_ned;
target_yaw = blimp.ahrs.get_yaw();
pid_pos_x.set_integrator(0);
pid_pos_y.set_integrator(0);
pid_pos_z.set_integrator(0);
pid_pos_yaw.set_integrator(0);
}

#if HAL_LOGGING_ENABLED
Expand All @@ -133,8 +92,6 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled
run_vel(target_vel_ef_c, target_vel_yaw_c, axes_disabled, false);
}

//---------------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------
void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b axes_disabled, bool log)
{
const float dt = blimp.scheduler.get_last_loop_time_s();
Expand Down Expand Up @@ -169,90 +126,18 @@ void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b ax
}
}

float aroll = fabsf(blimp.ahrs.get_roll());
float apitch = fabsf(blimp.ahrs.get_pitch());
Vector3f agyro = blimp.ahrs.get_gyro();

//Damping by roll & pitch angle
if (rp_damp_off > 0 && (rp_damp_off > rp_damp_lim)) //Disable when unused, and also protect against divide by zero
{
float excessr = 0;
float excessp = 0;
if (fabsf(aroll) > radians(rp_damp_lim)) excessr = (fabsf(aroll)-radians(rp_damp_lim))/(radians(rp_damp_off)-radians(rp_damp_lim));
if (fabsf(apitch) > radians(rp_damp_lim)) excessp = (fabsf(apitch)-radians(rp_damp_lim))/(radians(rp_damp_off)-radians(rp_damp_lim));
/*
actual, lim, off
(a-lim)/(off-lim)
eg
21-20 / 30-20 = 1/10 = 0.1
25-20 / 30-20 = 5/10 = 0.5
30-20 / 30-20 = 10/10 = 1
Thus need 1-ans.
*/
float rp_scale = 0;
//Cut off if it's past rp_damp_off
if ((rp_damp_amt*(excessr+excessp)) >= 1) rp_scale = 0;
else rp_scale = 1 - rp_damp_amt*(excessr+excessp);

AP::logger().WriteStreaming("FIND", "TimeUS,er,ep,rps", "Qfff",
AP_HAL::micros64(),
excessr,
excessp,
rp_scale);
// gcs().send_named_float("EECS", rp_scale);

if (rp_damp_msk & (1<<(2-1))) scaler_x_n = scaler_x_n * rp_scale;
if (rp_damp_msk & (1<<(1-1))) scaler_y_n = scaler_y_n * rp_scale;
if (rp_damp_msk & (1<<(3-1))) scaler_z_n = scaler_z_n * rp_scale;
if (rp_damp_msk & (1<<(4-1))) scaler_yaw_n = scaler_yaw_n * rp_scale;
}

//Damping by roll & pitch rate
if (rp_damp_off2 > 0 && (rp_damp_off2 > rp_damp_lim2)) //Disable when unused, and also protect against divide by zero
{
float excessr2 = 0;
float excessp2 = 0;
if (fabsf(agyro.x) > radians(rp_damp_lim2)) excessr2 = (fabsf(agyro.x)-radians(rp_damp_lim2))/(radians(rp_damp_off2)-radians(rp_damp_lim2));
if (fabsf(agyro.y) > radians(rp_damp_lim2)) excessp2 = (fabsf(agyro.y)-radians(rp_damp_lim2))/(radians(rp_damp_off2)-radians(rp_damp_lim2));

float rp_scale2 = 0;
//Cut off if it's past rp_damp_off
if ((rp_damp_amt*(excessr2+excessp2)) >= 1) rp_scale2 = 0;
else rp_scale2 = 1 - rp_damp_amt*(excessr2+excessp2);

AP::logger().WriteStreaming("FIN2", "TimeUS,er,ep,rps", "Qfff",
AP_HAL::micros64(),
excessr2,
excessp2,
rp_scale2);

if (rp_damp_msk & (1<<(2-1))) scaler_x_n = scaler_x_n * rp_scale2;
if (rp_damp_msk & (1<<(1-1))) scaler_y_n = scaler_y_n * rp_scale2;
if (rp_damp_msk & (1<<(3-1))) scaler_z_n = scaler_z_n * rp_scale2;
if (rp_damp_msk & (1<<(4-1))) scaler_yaw_n = scaler_yaw_n * rp_scale2;
}

float scaler_bat = (blimp.battery.voltage() - bat_off) * bat_mult;
if (!is_zero(scaler_bat) && scaler_bat <= 1) {
scaler_x_n = scaler_x_n * scaler_bat;
scaler_y_n = scaler_y_n * scaler_bat;
scaler_z_n = scaler_z_n * scaler_bat;
scaler_yaw_n = scaler_yaw_n * scaler_bat;
}

scaler_x = scaler_x*MA + scaler_x_n*MO;
scaler_y = scaler_y*MA + scaler_y_n*MO;
scaler_z = scaler_z*MA + scaler_z_n*MO;
scaler_yaw = scaler_yaw*MA + scaler_yaw_n*MO;
scaler_x = scaler_x*scaler_spd + scaler_x_n*(1-scaler_spd);
scaler_y = scaler_y*scaler_spd + scaler_y_n*(1-scaler_spd);
scaler_z = scaler_z*scaler_spd + scaler_z_n*(1-scaler_spd);
scaler_yaw = scaler_yaw*scaler_spd + scaler_yaw_n*(1-scaler_spd);

#if HAL_LOGGING_ENABLED
AP::logger().WriteStreaming("BSC", "TimeUS,x,y,z,yaw,xn,yn,zn,yawn,b",
"Qfffffffff",
AP::logger().WriteStreaming("BSC", "TimeUS,x,y,z,yaw,xn,yn,zn,yawn",
"Qffffffff",
AP_HAL::micros64(),
scaler_x, scaler_y, scaler_z, scaler_yaw, scaler_x_n, scaler_y_n, scaler_z_n, scaler_yaw_n, scaler_bat);
scaler_x, scaler_y, scaler_z, scaler_yaw, scaler_x_n, scaler_y_n, scaler_z_n, scaler_yaw_n);
#endif
if (AP_HAL::millis() % blimp.g.stream_rate < 30){
gcs().send_named_float("BSCB", scaler_bat);
gcs().send_named_float("BSCXN", scaler_x_n);
gcs().send_named_float("BSCYN", scaler_y_n);
gcs().send_named_float("BSCZN", scaler_z_n);
Expand Down Expand Up @@ -289,31 +174,31 @@ void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b ax

Vector2f actuator;
if (!axes_disabled.x) {
actuator.x = blimp.pid_vel_x.update_all(target_vel_bf_c.x * scaler_x, vel_bf_filtd.x, dt, limit.x);
actuator.x = pid_vel_x.update_all(target_vel_bf_c.x * scaler_x, vel_bf_filtd.x, dt, limit.x);
}

if (!axes_disabled.y) {
actuator.y = blimp.pid_vel_y.update_all(target_vel_bf_c.y * scaler_y, vel_bf_filtd.y, dt, limit.y);
actuator.y = pid_vel_y.update_all(target_vel_bf_c.y * scaler_y, vel_bf_filtd.y, dt, limit.y);
}

float act_down = 0;
if (!axes_disabled.z) {
act_down = blimp.pid_vel_z.update_all(target_vel_bf_c.z * scaler_z, vel_bf_filtd.z, dt, limit.z);
act_down = pid_vel_z.update_all(target_vel_bf_c.z * scaler_z, vel_bf_filtd.z, dt, limit.z);
}

float act_yaw = 0;
if (!axes_disabled.yaw) {
act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yaw, blimp.vel_yaw_filtd, dt, limit.yaw);
act_yaw = pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yaw, blimp.vel_yaw_filtd, dt, limit.yaw);
}

if (!blimp.motors->armed()) {
blimp.pid_vel_x.set_integrator(0);
blimp.pid_vel_y.set_integrator(0);
blimp.pid_vel_z.set_integrator(0);
blimp.pid_vel_yaw.set_integrator(0);
pid_vel_x.set_integrator(0);
pid_vel_y.set_integrator(0);
pid_vel_z.set_integrator(0);
pid_vel_yaw.set_integrator(0);
}

// blimp.rotate_NE_to_BF(actuator); //Don't need this anymore because we're already in BF
//We're already in body-frame, so we can output directly.

if (zero.x) {
blimp.motors->front_out = 0;
Expand Down
12 changes: 12 additions & 0 deletions Blimp/Loiter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,18 @@ class Loiter
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];

// Vel & pos PIDs
//p, i, d, ff, imax, filt_T_hz, filt_E_hz, filt_D_hz
AC_PID pid_vel_x{3, 0.2, 0, 0, 0.2, 0, 0, 0};
AC_PID pid_vel_y{3, 0.2, 0, 0, 0.2, 0, 0, 0};
AC_PID pid_vel_z{7, 1.5, 0, 0, 1, 0, 0, 0};
AC_PID pid_vel_yaw{3, 0.4, 0, 0, 0.2, 0, 0, 0};

AC_PID pid_pos_x{1, 0.05, 0, 0, 0.1, 0, 0, 0};
AC_PID pid_pos_y{1, 0.05, 0, 0, 0.1, 0, 0, 0};
AC_PID pid_pos_z{0.7, 0, 0, 0, 0, 0, 0, 0};
AC_PID pid_pos_yaw{1.2, 0.5, 0, 0, 2, 0, 0, 0};

AP_Float rp_damp_lim;
AP_Float rp_damp_off;
AP_Float rp_damp_amt;
Expand Down
Loading

0 comments on commit 1e3d966

Please sign in to comment.