diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 697e16f7e2..614c593f14 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -145,6 +145,9 @@ void Blimp::full_rate_logging() if (should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) { Log_Write_Attitude(); } + if (should_log(MASK_LOG_PID)) { + Log_Write_PIDs(); + } } // ten_hz_logging_loop @@ -220,6 +223,14 @@ void Blimp::read_AHRS(void) { // we tell AHRS to skip INS update as we have already done it in fast_loop() ahrs.update(true); + + IGNORE_RETURN(ahrs.get_velocity_NED(vel_ned)); + IGNORE_RETURN(ahrs.get_relative_position_NED_home(pos_ned)); + + vel_yaw = ahrs.get_yaw_rate_earth(); + Vector2f vel_xy_filtd = vel_xy_filter.apply({vel_ned.x, vel_ned.y}); + vel_ned_filtd = {vel_xy_filtd.x, vel_xy_filtd.y, vel_z_filter.apply(vel_ned.z)}; + vel_yaw_filtd = vel_yaw_filter.apply(vel_yaw); } // read baro and log control tuning @@ -237,6 +248,21 @@ void Blimp::update_altitude() } } +//Conversions are in 2D so that up remains up in world frame when the blimp is not exactly level. +void Blimp::rotate_BF_to_NE(float &x, float &y) +{ + float ne_x = x*ahrs.cos_yaw() - y*ahrs.sin_yaw(); + float ne_y = x*ahrs.sin_yaw() + y*ahrs.cos_yaw(); + x = ne_x; + y = ne_y; +} + +void Blimp::rotate_NE_to_BF(float &x, float &y) +{ + float bf_x = x*ahrs.cos_yaw() + y*ahrs.sin_yaw(); + float bf_y = -x*ahrs.sin_yaw() + y*ahrs.cos_yaw(); + x = bf_x; + y = bf_y; } diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index e6dcce983b..f19060d438 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -49,6 +49,10 @@ #include // Battery monitor library #include #include +#include +#include +#include +#include // Configuration #include "defines.h" @@ -90,6 +94,8 @@ public: friend class Mode; friend class ModeManual; friend class ModeLand; + friend class ModeVelocity; + friend class ModeLoiter; friend class Fins; @@ -232,17 +238,27 @@ private: // 3D Location vectors // Current location of the vehicle (altitude is relative to home) Location current_loc; + Vector3f vel_ned; + Vector3f vel_ned_filtd; + + Vector3f pos_ned; + float vel_yaw; + float vel_yaw_filtd; + NotchFilterVector2f vel_xy_filter; + NotchFilterFloat vel_z_filter; + NotchFilterFloat vel_yaw_filter; // Inertial Navigation AP_InertialNav_NavEKF 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}; - // Attitude, Position and Waypoint navigation objects - // To-Do: move inertial nav up or other navigation variables down here - // AC_AttitudeControl_t *attitude_control; - // AC_PosControl *pos_control; - // AC_WPNav *wp_nav; - // AC_Loiter *loiter_nav; - + 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 // System Timers // -------------- @@ -314,6 +330,8 @@ private: void one_hz_loop(); void read_AHRS(void); void update_altitude(); + void rotate_NE_to_BF(float &x, float &y); + void rotate_BF_to_NE(float &x, float &y); // commands.cpp void update_home_from_EKF(); @@ -446,6 +464,8 @@ private: Mode *flightmode; ModeManual mode_manual; ModeLand mode_land; + ModeVelocity mode_velocity; + ModeLoiter mode_loiter; // mode.cpp Mode *mode_from_mode_num(const Mode::Number mode); diff --git a/Blimp/Fins.cpp b/Blimp/Fins.cpp index 631b678ee7..fef383bcba 100644 --- a/Blimp/Fins.cpp +++ b/Blimp/Fins.cpp @@ -80,6 +80,12 @@ void Fins::output() blimp.Write_FINI(right_out, front_out, down_out, yaw_out); + //Constrain after logging so as to still show when sub-optimal tuning is causing massive overshoots. + right_out = constrain_float(right_out, -1, 1); + front_out = constrain_float(front_out, -1, 1); + down_out = constrain_float(down_out, -1, 1); + yaw_out = constrain_float(yaw_out, -1, 1); + _time = AP_HAL::micros() * 1.0e-6; for (int8_t i=0; iget_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s(); + float pilot_rgt = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s(); + float pilot_dwn = 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(); + + if (g.simple_mode == 0){ + //If simple mode is disabled, input is in body-frame, thus needs to be rotated. + blimp.rotate_BF_to_NE(pilot_fwd, pilot_rgt); + } + target_pos.x += pilot_fwd; + target_pos.y += pilot_rgt; + target_pos.z += pilot_dwn; + 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); + float yaw_ef = blimp.ahrs.get_yaw(); + float target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef)); + blimp.pid_pos_yaw.set_target_rate(target_yaw); + blimp.pid_pos_yaw.set_actual_rate(yaw_ef); + + Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -g.max_vel_xy, g.max_vel_xy), + constrain_float(target_vel_ef.y, -g.max_vel_xy, g.max_vel_xy), + constrain_float(target_vel_ef.z, -g.max_vel_xy, g.max_vel_xy)}; + 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); + blimp.rotate_NE_to_BF(actuator.x, actuator.y); + float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c, blimp.vel_yaw_filtd); + + if(!(blimp.g.dis_mask & (1<<(2-1)))){ + motors->front_out = actuator.x; + } if(!(blimp.g.dis_mask & (1<<(1-1)))){ + motors->right_out = actuator.y; + } if(!(blimp.g.dis_mask & (1<<(3-1)))){ + motors->down_out = act_down; + } if(!(blimp.g.dis_mask & (1<<(4-1)))){ + motors->yaw_out = act_yaw; + } + + AP::logger().Write_PSC(target_pos*100.0f, blimp.pos_ned*100.0f, target_vel_ef_c*100.0f, blimp.vel_ned_filtd*100.0f, blimp.vel_ned*100.0f, target_yaw*100.0f, yaw_ef*100.0f); //last entries here are just for debugging + AP::logger().Write_PSCZ(target_pos.z*100.0f, blimp.pos_ned.z*100.0f, blimp.scheduler.get_loop_period_s()*100.0f, target_vel_ef_c.z*100.0f, blimp.vel_ned_filtd.z*100.0f, 0.0f, blimp.vel_ned.z*100.0f, blimp.vel_yaw*100.0f, blimp.vel_yaw_filtd*100.0f); +} diff --git a/Blimp/mode_velocity.cpp b/Blimp/mode_velocity.cpp new file mode 100644 index 0000000000..b4e531943b --- /dev/null +++ b/Blimp/mode_velocity.cpp @@ -0,0 +1,33 @@ +#include "Blimp.h" +/* + * Init and run calls for velocity flight mode + */ + +// Runs the main velocity controller +void ModeVelocity::run() +{ + 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; + blimp.rotate_BF_to_NE(target_vel.x, target_vel.y); + 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}); + blimp.rotate_NE_to_BF(actuator.x, actuator.y); + 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); + + if(!(blimp.g.dis_mask & (1<<(2-1)))){ + motors->front_out = actuator.x; + } if(!(blimp.g.dis_mask & (1<<(1-1)))){ + motors->right_out = actuator.y; + } if(!(blimp.g.dis_mask & (1<<(3-1)))){ + motors->down_out = act_down; + } if(!(blimp.g.dis_mask & (1<<(4-1)))){ + motors->yaw_out = act_yaw; + } + + AP::logger().Write_PSC({0,0,0}, blimp.pos_ned, target_vel*100.0f, blimp.vel_ned_filtd*100.0f, blimp.vel_ned*100.0f, 0, 0); + AP::logger().Write_PSCZ(0.0f, 0.0f, 0.0f, target_vel.z*100.0f, blimp.vel_ned_filtd.z*100.0f, 0.0f, blimp.vel_ned.z*100.0f, blimp.vel_yaw*100.0f, blimp.vel_yaw_filtd*100.0f); +} diff --git a/Blimp/system.cpp b/Blimp/system.cpp index 5ef5e11aa0..f4dcd8fa89 100644 --- a/Blimp/system.cpp +++ b/Blimp/system.cpp @@ -101,6 +101,11 @@ void Blimp::init_ardupilot() enable_motor_output(); } + //Initialise fin filters + vel_xy_filter.init(scheduler.get_loop_rate_hz(), motors->freq_hz, 0.5f, 15.0f); + vel_z_filter.init(scheduler.get_loop_rate_hz(), motors->freq_hz, 1.0f, 15.0f); + vel_yaw_filter.init(scheduler.get_loop_rate_hz(),motors->freq_hz, 5.0f, 15.0f); + // attempt to switch to MANUAL, if this fails then switch to Land if (!set_mode((enum Mode::Number)g.initial_mode.get(), ModeReason::INITIALISED)) { // set mode to MANUAL will trigger mode change notification to pilot