From 4662b3b18015580217f7fc24e2419f32ac28fc3f Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 30 Nov 2022 15:55:42 +1030 Subject: [PATCH] Blimp: Support changing update period --- Blimp/Blimp.h | 12 ++++++------ Blimp/mode_loiter.cpp | 22 ++++++++++++---------- Blimp/mode_velocity.cpp | 8 +++++--- 3 files changed, 23 insertions(+), 19 deletions(-) diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 1b633f35db..fff397191d 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -224,13 +224,13 @@ private: AP_InertialNav inertial_nav; // Vel & pos PIDs - AC_PID_2D pid_vel_xy{3, 0.2, 0, 0, 0.2, 3, 3, 0.02}; //These are the defaults - P I D FF IMAX FiltHz FiltDHz DT - AC_PID_Basic pid_vel_z{7, 1.5, 0, 0, 1, 3, 3, 0.02}; - AC_PID_Basic pid_vel_yaw{3, 0.4, 0, 0, 0.2, 3, 3, 0.02}; + AC_PID_2D pid_vel_xy{3, 0.2, 0, 0, 0.2, 3, 3}; //These are the defaults - P I D FF IMAX FiltHz FiltDHz DT + AC_PID_Basic pid_vel_z{7, 1.5, 0, 0, 1, 3, 3}; + AC_PID_Basic pid_vel_yaw{3, 0.4, 0, 0, 0.2, 3, 3}; - AC_PID_2D pid_pos_xy{1, 0.05, 0, 0, 0.1, 3, 3, 0.02}; - AC_PID_Basic pid_pos_z{0.7, 0, 0, 0, 0, 3, 3, 0.02}; - AC_PID pid_pos_yaw{1.2, 0.5, 0, 0, 2, 3, 3, 3, 0.02}; //p, i, d, ff, imax, filt_t, filt_e, filt_d, dt, opt srmax, opt srtau + AC_PID_2D pid_pos_xy{1, 0.05, 0, 0, 0.1, 3, 3}; + AC_PID_Basic pid_pos_z{0.7, 0, 0, 0, 0, 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 // -------------- diff --git a/Blimp/mode_loiter.cpp b/Blimp/mode_loiter.cpp index bec0629d80..71e2794c2b 100644 --- a/Blimp/mode_loiter.cpp +++ b/Blimp/mode_loiter.cpp @@ -14,11 +14,13 @@ //Runs the main loiter controller void ModeLoiter::run() { + const float dt = blimp.scheduler.get_last_loop_time_s(); + Vector3f pilot; - pilot.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s(); - pilot.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s(); - pilot.z = channel_down->get_control_in() / float(RC_SCALE) * g.max_pos_z * blimp.scheduler.get_loop_period_s(); - float pilot_yaw = channel_yaw->get_control_in() / float(RC_SCALE) * g.max_pos_yaw * blimp.scheduler.get_loop_period_s(); + pilot.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_pos_xy * dt; + pilot.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * dt; + pilot.z = channel_down->get_control_in() / float(RC_SCALE) * g.max_pos_z * dt; + float pilot_yaw = channel_yaw->get_control_in() / float(RC_SCALE) * g.max_pos_yaw * dt; if (g.simple_mode == 0){ //If simple mode is disabled, input is in body-frame, thus needs to be rotated. @@ -30,10 +32,10 @@ void ModeLoiter::run() target_yaw = wrap_PI(target_yaw + pilot_yaw); //Pos controller's output becomes target for velocity controller - Vector3f target_vel_ef{blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, {0,0,0}), 0}; - target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z); + Vector3f target_vel_ef{blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, dt, {0,0,0}), 0}; + target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt); float yaw_ef = blimp.ahrs.get_yaw(); - float target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef)); + float target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef), dt); blimp.pid_pos_yaw.set_target_rate(target_yaw); blimp.pid_pos_yaw.set_actual_rate(yaw_ef); @@ -42,10 +44,10 @@ void ModeLoiter::run() constrain_float(target_vel_ef.z, -g.max_vel_z, g.max_vel_z)}; float target_vel_yaw_c = constrain_float(target_vel_yaw, -g.max_vel_yaw, g.max_vel_yaw); - Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c, blimp.vel_ned_filtd, {0,0,0}); - float act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z, blimp.vel_ned_filtd.z); + Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c, blimp.vel_ned_filtd, dt, {0,0,0}); + float act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z, blimp.vel_ned_filtd.z, dt); blimp.rotate_NE_to_BF(actuator); - float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c, blimp.vel_yaw_filtd); + float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c, blimp.vel_yaw_filtd, dt); if(!(blimp.g.dis_mask & (1<<(2-1)))){ motors->front_out = actuator.x; diff --git a/Blimp/mode_velocity.cpp b/Blimp/mode_velocity.cpp index b099089756..bd3db6b922 100644 --- a/Blimp/mode_velocity.cpp +++ b/Blimp/mode_velocity.cpp @@ -6,6 +6,8 @@ // Runs the main velocity controller void ModeVelocity::run() { + const float dt = blimp.scheduler.get_last_loop_time_s(); + Vector3f target_vel; target_vel.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_vel_xy; target_vel.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_vel_xy; @@ -13,10 +15,10 @@ void ModeVelocity::run() target_vel.z = channel_down->get_control_in() / float(RC_SCALE) * g.max_vel_z; float target_vel_yaw = channel_yaw->get_control_in() / float(RC_SCALE) * g.max_vel_yaw; - Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel, blimp.vel_ned_filtd, {0,0,0}); + Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel, blimp.vel_ned_filtd, dt, {0,0,0}); blimp.rotate_NE_to_BF(actuator); - float act_down = blimp.pid_vel_z.update_all(target_vel.z, blimp.vel_ned_filtd.z); - float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw, blimp.vel_yaw_filtd); + float act_down = blimp.pid_vel_z.update_all(target_vel.z, blimp.vel_ned_filtd.z, dt); + float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw, blimp.vel_yaw_filtd, dt); if(!(blimp.g.dis_mask & (1<<(2-1)))){ motors->front_out = actuator.x;