From 0e2cb4a00c15c1760788f8d747f01ae4f8ad3fbb Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Tue, 22 Jun 2021 16:36:11 +1000 Subject: [PATCH] Blimp: Code cleanups --- Blimp/AP_Arming.cpp | 21 +++++++++-------- Blimp/Blimp.cpp | 26 ++++++++++----------- Blimp/Blimp.h | 51 +++++++++++++++++------------------------ Blimp/Fins.h | 2 ++ Blimp/GCS_Mavlink.h | 6 ++--- Blimp/Log.cpp | 4 ++-- Blimp/Parameters.cpp | 19 +++++++-------- Blimp/Parameters.h | 39 +++++++++++++++---------------- Blimp/mode.h | 6 +---- Blimp/mode_loiter.cpp | 40 +++++++++++++++++--------------- Blimp/mode_velocity.cpp | 21 +++++++++-------- 11 files changed, 114 insertions(+), 121 deletions(-) diff --git a/Blimp/AP_Arming.cpp b/Blimp/AP_Arming.cpp index 81d589a4ea..dc3e3a21d2 100644 --- a/Blimp/AP_Arming.cpp +++ b/Blimp/AP_Arming.cpp @@ -40,14 +40,6 @@ bool AP_Arming_Blimp::run_pre_arm_checks(bool display_failure) return false; } - // check if motor interlock aux switch is in use - // if it is, switch needs to be in disabled position to arm - // otherwise exit immediately. This check to be repeated, - // as state can change at any time. - if (blimp.ap.using_interlock && blimp.ap.motor_interlock_switch) { - check_failed(display_failure, "Motor Interlock Enabled"); - } - // if pre arm checks are disabled run only the mandatory checks if (checks_to_perform == 0) { return mandatory_checks(display_failure); @@ -155,7 +147,7 @@ bool AP_Arming_Blimp::parameter_checks(bool display_failure) } } } - + return true; } @@ -273,6 +265,15 @@ bool AP_Arming_Blimp::mandatory_gps_checks(bool display_failure) return true; } + // check for GPS glitch (as reported by EKF) + nav_filter_status filt_status; + if (ahrs.get_filter_status(filt_status)) { + if (filt_status.flags.gps_glitching) { + check_failed(display_failure, "GPS glitching"); + return false; + } + } + // if we got here all must be ok return true; } @@ -383,7 +384,7 @@ bool AP_Arming_Blimp::arm(const AP_Arming::Method method, const bool do_arming_c blimp.arming_altitude_m = blimp.inertial_nav.get_altitude() * 0.01; } - // enable gps velocity based centrefugal force compensation + // enable gps velocity based centrifugal force compensation ahrs.set_correct_centrifugal(true); hal.util->set_soft_armed(true); diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 614c593f14..3f1ff9041b 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -142,7 +142,7 @@ void Blimp::update_batt_compass(void) // Full rate logging of attitude, rate and pid loops void Blimp::full_rate_logging() { - if (should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) { + if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); } if (should_log(MASK_LOG_PID)) { @@ -155,7 +155,7 @@ void Blimp::full_rate_logging() void Blimp::ten_hz_logging_loop() { // log attitude data if we're not already logging at the higher rate - if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) { + if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); } // log EKF attitude data @@ -223,7 +223,7 @@ 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)); @@ -249,20 +249,20 @@ 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) +void Blimp::rotate_BF_to_NE(Vector2f &vec) { - 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; + float ne_x = vec.x*ahrs.cos_yaw() - vec.y*ahrs.sin_yaw(); + float ne_y = vec.x*ahrs.sin_yaw() + vec.y*ahrs.cos_yaw(); + vec.x = ne_x; + vec.y = ne_y; } -void Blimp::rotate_NE_to_BF(float &x, float &y) +void Blimp::rotate_NE_to_BF(Vector2f &vec) { - 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; + float bf_x = vec.x*ahrs.cos_yaw() + vec.y*ahrs.sin_yaw(); + float bf_y = -vec.x*ahrs.sin_yaw() + vec.y*ahrs.cos_yaw(); + vec.x = bf_x; + vec.y = bf_y; } diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index f19060d438..53034366e5 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -150,35 +150,26 @@ private: // Documentation of Globals: typedef union { struct { - uint8_t unused1 : 1; // 0 - uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully - uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed - uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised - uint8_t logging_started : 1; // 6 // true if logging has started - uint8_t land_complete : 1; // 7 // true if we have detected a landing - uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio - uint8_t usb_connected_unused : 1; // 9 // UNUSED - uint8_t rc_receiver_present : 1; // 10 // true if we have an rc receiver present (i.e. if we've ever received an update - uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration - uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test - uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes - uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete) - uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock - uint8_t system_time_set_unused : 1; // 16 // true if the system time has been set from the GPS - uint8_t gps_glitching : 1; // 17 // true if GPS glitching is affecting navigation accuracy - uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use - uint8_t land_repo_active : 1; // 21 // true if the pilot is overriding the landing position - uint8_t motor_interlock_switch : 1; // 22 // true if pilot is requesting motor interlock enable - uint8_t in_arming_delay : 1; // 23 // true while we are armed but waiting to spin motors - uint8_t initialised_params : 1; // 24 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done - uint8_t unused3 : 1; // 25 // was compass_init_location; true when the compass's initial location has been set - uint8_t unused2 : 1; // 26 // aux switch rc_override is allowed - uint8_t unused4 : 1; // 27 // was "we armed using a arming switch" + uint8_t pre_arm_rc_check : 1; // 1 // true if rc input pre-arm checks have been completed successfully + uint8_t pre_arm_check : 1; // 2 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed + uint8_t auto_armed : 1; // 3 // stops auto missions from beginning until throttle is raised + uint8_t logging_started : 1; // 4 // true if logging has started + uint8_t land_complete : 1; // 5 // true if we have detected a landing + uint8_t new_radio_frame : 1; // 6 // Set true if we have new PWM data to act on from the Radio + uint8_t rc_receiver_present : 1; // 7 // true if we have an rc receiver present (i.e. if we've ever received an update + uint8_t compass_mot : 1; // 8 // true if we are currently performing compassmot calibration + uint8_t motor_test : 1; // 9 // true if we are currently performing the motors test + uint8_t initialised : 1; // 10 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes + uint8_t land_complete_maybe : 1; // 11 // true if we may have landed (less strict version of land_complete) + uint8_t throttle_zero : 1; // 12 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down + uint8_t gps_glitching : 1; // 13 // true if GPS glitching is affecting navigation accuracy + uint8_t in_arming_delay : 1; // 14 // true while we are armed but waiting to spin motors + uint8_t initialised_params : 1; // 15 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done }; uint32_t value; } ap_t; - ap_t ap; //MIR Set of general variables + ap_t ap; static_assert(sizeof(uint32_t) == sizeof(ap), "ap_t must be uint32_t"); @@ -240,7 +231,7 @@ private: Location current_loc; Vector3f vel_ned; Vector3f vel_ned_filtd; - + Vector3f pos_ned; float vel_yaw; float vel_yaw_filtd; @@ -250,14 +241,14 @@ private: // 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}; 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_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 @@ -330,8 +321,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); + void rotate_NE_to_BF(Vector2f &vec); + void rotate_BF_to_NE(Vector2f &vec); // commands.cpp void update_home_from_EKF(); diff --git a/Blimp/Fins.h b/Blimp/Fins.h index fcd277b832..be39b30ae1 100644 --- a/Blimp/Fins.h +++ b/Blimp/Fins.h @@ -96,6 +96,8 @@ public: float get_throttle() { //Only for Mavlink - essentially just an indicator of how hard the fins are working. + //Note that this is the unconstrained version, so if the higher level control gives too high input, + //throttle will be displayed as more than 100. return fmaxf(fmaxf(fabsf(down_out),fabsf(front_out)), fmaxf(fabsf(right_out),fabsf(yaw_out))); } diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 3544020d49..8658dd0698 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -67,10 +67,10 @@ private: //This is 1-indexed, unlike most enums for consistency with the mavlink PID_TUNING enums. enum PID_SEND : uint8_t { - VELX = 1, - VELY = 2, + VELX = 1, + VELY = 2, VELZ = 3, - VELYAW = 4, + VELYAW = 4, POSX = 5, POSY = 6, POSZ = 7, diff --git a/Blimp/Log.cpp b/Blimp/Log.cpp index b4c2dad526..dc5915e7a8 100644 --- a/Blimp/Log.cpp +++ b/Blimp/Log.cpp @@ -355,10 +355,10 @@ const struct LogStructure Blimp::log_structure[] = { // @Field: R: Right // @Field: F: Front // @Field: D: Down - // @Field: Y: Yaw + // @Field: Y: Yaw { LOG_FINI_MSG, sizeof(log_FINI), - "FINI", "Qffff", "TimeUS,R,F,D,Y", "s----", "F----" }, + "FINI", "Qffff", "TimeUS,R,F,D,Y", "s----", "F----" }, // @LoggerMessage: FINO // @Description: Fin output diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index 7aa4e2130b..c08effc51c 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -252,42 +252,42 @@ const AP_Param::Info Blimp::var_info[] = { // @Param: MAX_VEL_XY // @DisplayName: Max XY Velocity // @Description: Sets the maximum XY velocity, in m/s - // @Values: + // @Range: 0.2 5 // @User: Standard GSCALAR(max_vel_xy, "MAX_VEL_XY", 0.5), // @Param: MAX_VEL_Z // @DisplayName: Max Z Velocity // @Description: Sets the maximum Z velocity, in m/s - // @Values: + // @Range: 0.2 5 // @User: Standard GSCALAR(max_vel_z, "MAX_VEL_Z", 0.4), - + // @Param: MAX_VEL_YAW // @DisplayName: Max yaw Velocity // @Description: Sets the maximum yaw velocity, in rad/s - // @Values: + // @Range: 0.2 5 // @User: Standard GSCALAR(max_vel_yaw, "MAX_VEL_YAW", 0.5), // @Param: MAX_POS_XY // @DisplayName: Max XY Position change // @Description: Sets the maximum XY position change, in m/s - // @Values: + // @Range: 0.1 5 // @User: Standard GSCALAR(max_pos_xy, "MAX_POS_XY", 0.2), // @Param: MAX_POS_Z // @DisplayName: Max Z Position change // @Description: Sets the maximum Z position change, in m/s - // @Values: + // @Range: 0.1 5 // @User: Standard GSCALAR(max_pos_z, "MAX_POS_Z", 0.15), // @Param: MAX_POS_YAW // @DisplayName: Max Yaw Position change // @Description: Sets the maximum Yaw position change, in rad/s - // @Values: + // @Range: 0.1 5 // @User: Standard GSCALAR(max_pos_yaw, "MAX_POS_YAW", 0.3), @@ -306,9 +306,6 @@ const AP_Param::Info Blimp::var_info[] = { // @User: Standard GSCALAR(dis_mask, "DIS_MASK", 0), - GSCALAR(notch_bw, "NOTCH_BW", 2), - GSCALAR(notch_att, "NOTCH_ATT", 15), - // @Param: RC_SPEED // @DisplayName: ESC Update Speed // @Description: This is the speed in Hertz that your ESCs will receive updates @@ -756,7 +753,7 @@ const AP_Param::Info Blimp::var_info[] = { // @Param: POSYAW_SMAX // @DisplayName: Yaw slew rate limit - // @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. + // @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. // @Range: 0 200 // @Increment: 0.5 // @User: Advanced diff --git a/Blimp/Parameters.h b/Blimp/Parameters.h index ce25a4769d..1f9d329606 100644 --- a/Blimp/Parameters.h +++ b/Blimp/Parameters.h @@ -67,10 +67,10 @@ public: k_param_parachute, // Landing gear object - k_param_landinggear, // 18 + k_param_landinggear, // Input Management object - k_param_input_manager, // 19 + k_param_input_manager, // Misc k_param_gps_hdop_good, @@ -84,19 +84,30 @@ public: k_param_log_bitmask, k_param_throttle_filt, k_param_throttle_behavior, - k_param_pilot_takeoff_alt, // 64 + k_param_pilot_takeoff_alt, // AP_ADSB Library - k_param_adsb, // 72 - k_param_notify, // 73 + k_param_adsb, + k_param_notify, //PID Controllers - k_param_pid_vel_xy, - k_param_pid_pos_xy, + k_param_pid_vel_xy = 32, k_param_pid_vel_z, k_param_pid_vel_yaw, + k_param_pid_pos_xy, k_param_pid_pos_z, k_param_pid_pos_yaw, + + //Position & Velocity controller params + k_param_max_vel_xy = 50, + k_param_max_vel_z, + k_param_max_vel_yaw, + k_param_max_pos_xy, + k_param_max_pos_z, + k_param_max_pos_yaw, + k_param_simple_mode, + k_param_dis_mask, + // // 90: misc2 // @@ -180,18 +191,8 @@ public: // 220: Misc // k_param_fs_ekf_action = 220, - k_param_max_vel_xy, k_param_arming, - k_param_max_vel_z, - k_param_simple_mode, - k_param_dis_mask, //225 - k_param_notch_bw, - k_param_notch_att, - k_param_max_vel_yaw, - k_param_max_pos_xy, - k_param_max_pos_z, - k_param_max_pos_yaw, - + k_param_logger = 253, // 253 - Logging Group k_param_vehicle = 257, // vehicle common block of parameters @@ -252,8 +253,6 @@ public: AP_Int8 simple_mode; AP_Int16 dis_mask; - AP_Float notch_bw; - AP_Float notch_att; AP_Int8 rtl_alt_type; diff --git a/Blimp/mode.h b/Blimp/mode.h index acf60d724b..604a7e9717 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -16,7 +16,7 @@ public: MANUAL = 0, // manual control LAND = 1, // currently just stops moving VELOCITY = 2, // velocity mode - LOITER = 3, // loiter mode (position hold) + LOITER = 3, // loiter mode (position hold) }; // constructor @@ -47,10 +47,6 @@ public: { return false; } - virtual bool logs_attitude() const - { - return false; - } // return a string for this flightmode virtual const char *name() const = 0; diff --git a/Blimp/mode_loiter.cpp b/Blimp/mode_loiter.cpp index e2fce1b53c..550fd6793a 100644 --- a/Blimp/mode_loiter.cpp +++ b/Blimp/mode_loiter.cpp @@ -14,18 +14,19 @@ //Runs the main loiter controller void ModeLoiter::run() { - float pilot_fwd = channel_front->get_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(); + 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(); 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); + blimp.rotate_BF_to_NE(pilot.xy()); } - target_pos.x += pilot_fwd; - target_pos.y += pilot_rgt; - target_pos.z += pilot_dwn; + target_pos.x += pilot.x; + target_pos.y += pilot.y; + target_pos.z += pilot.z; target_yaw = wrap_PI(target_yaw + pilot_yaw); //Pos controller's output becomes target for velocity controller @@ -38,22 +39,25 @@ void ModeLoiter::run() 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); + 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); - blimp.rotate_NE_to_BF(actuator.x, actuator.y); + blimp.rotate_NE_to_BF(actuator); 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; + 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 diff --git a/Blimp/mode_velocity.cpp b/Blimp/mode_velocity.cpp index b4e531943b..b6de4dc8f4 100644 --- a/Blimp/mode_velocity.cpp +++ b/Blimp/mode_velocity.cpp @@ -9,23 +9,26 @@ 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); + blimp.rotate_BF_to_NE(target_vel.xy()); 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); + 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); 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; + 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);