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

Blimp: Support changing update period

This commit is contained in:
Leonard Hall 2022-11-30 15:55:42 +10:30 committed by Randy Mackay
parent 5cadbc03f2
commit 2ceec6b17d
3 changed files with 23 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Blimp/Blimp.h
View File

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

22
Blimp/mode_loiter.cpp
View File

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

8
Blimp/mode_velocity.cpp
View File

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