diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index a5089fdac9..ff53270067 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -53,7 +53,7 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = { FAST_TASK_CLASS(AP_InertialSensor, &blimp.ins, update), // send outputs to the motors library immediately FAST_TASK(motors_output), - // run EKF state estimator (expensive) + // run EKF state estimator (expensive) FAST_TASK(read_AHRS), // Inertial Nav FAST_TASK(read_inertia), diff --git a/Blimp/Fins.h b/Blimp/Fins.h index a362530055..6e22558038 100644 --- a/Blimp/Fins.h +++ b/Blimp/Fins.h @@ -71,7 +71,7 @@ protected: int8_t _num_added; -//MIR This should probably become private in future. + //MIR This should probably become private in future. public: float right_out; //input right movement, negative for left, +1 to -1 float front_out; //input front/forwards movement, negative for backwards, +1 to -1 diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 9f395b4d47..931dfed52f 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -52,8 +52,8 @@ void GCS_MAVLINK_Blimp::send_position_target_global_int() } static constexpr uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000; static constexpr uint16_t TYPE_MASK = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | - POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | - POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | POSITION_TARGET_TYPEMASK_LAST_BYTE; + POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | POSITION_TARGET_TYPEMASK_LAST_BYTE; mavlink_msg_position_target_global_int_send( chan, diff --git a/Blimp/Log.cpp b/Blimp/Log.cpp index 5e4df3e406..cda6756196 100644 --- a/Blimp/Log.cpp +++ b/Blimp/Log.cpp @@ -30,7 +30,7 @@ struct PACKED log_FINO { //Write a fin input packet void Blimp::Write_FINI(float right, float front, float down, float yaw) { - const struct log_FINI pkt{ + const struct log_FINI pkt { LOG_PACKET_HEADER_INIT(LOG_FINI_MSG), time_us : AP_HAL::micros64(), Right : right, @@ -44,7 +44,7 @@ void Blimp::Write_FINI(float right, float front, float down, float yaw) //Write a fin output packet void Blimp::Write_FINO(float *amp, float *off) { - const struct log_FINO pkt{ + const struct log_FINO pkt { LOG_PACKET_HEADER_INIT(LOG_FINO_MSG), time_us : AP_HAL::micros64(), Fin1_Amp : amp[0], @@ -248,8 +248,10 @@ const struct LogStructure Blimp::log_structure[] = { // @Field: D: Down // @Field: Y: Yaw - { LOG_FINI_MSG, sizeof(log_FINI), - "FINI", "Qffff", "TimeUS,R,F,D,Y", "s----", "F----" }, + { + LOG_FINI_MSG, sizeof(log_FINI), + "FINI", "Qffff", "TimeUS,R,F,D,Y", "s----", "F----" + }, // @LoggerMessage: FINO // @Description: Fin output @@ -263,8 +265,10 @@ const struct LogStructure Blimp::log_structure[] = { // @Field: F4A: Fin 4 Amplitude // @Field: F4O: Fin 4 Offset - { LOG_FINO_MSG, sizeof(log_FINO), - "FINO", "Qffffffff", "TimeUS,F1A,F1O,F2A,F2O,F3A,F3O,F4A,F4O", "s--------", "F--------" }, + { + LOG_FINO_MSG, sizeof(log_FINO), + "FINO", "Qffffffff", "TimeUS,F1A,F1O,F2A,F2O,F3A,F3O,F4A,F4O", "s--------", "F--------" + }, // @LoggerMessage: PIDD,PIVN,PIVE,PIVD,PIVY // @Description: Proportional/Integral/Derivative gain values @@ -279,16 +283,26 @@ const struct LogStructure Blimp::log_structure[] = { // @Field: Dmod: scaler applied to D gain to reduce limit cycling // @Field: SRate: slew rate // @Field: Limit: 1 if I term is limited due to output saturation - { LOG_PIDD_MSG, sizeof(log_PID), - "PIDD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, - { LOG_PIVN_MSG, sizeof(log_PID), - "PIVN", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, - { LOG_PIVE_MSG, sizeof(log_PID), - "PIVE", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, - { LOG_PIVD_MSG, sizeof(log_PID), - "PIVD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, - { LOG_PIVY_MSG, sizeof(log_PID), - "PIVY", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, + { + LOG_PIDD_MSG, sizeof(log_PID), + "PIDD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS + }, + { + LOG_PIVN_MSG, sizeof(log_PID), + "PIVN", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS + }, + { + LOG_PIVE_MSG, sizeof(log_PID), + "PIVE", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS + }, + { + LOG_PIVD_MSG, sizeof(log_PID), + "PIVD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS + }, + { + LOG_PIVY_MSG, sizeof(log_PID), + "PIVY", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS + }, // @LoggerMessage: PTUN // @Description: Parameter Tuning information diff --git a/Blimp/Loiter.cpp b/Blimp/Loiter.cpp index 453028debd..8c6b5d5e7a 100644 --- a/Blimp/Loiter.cpp +++ b/Blimp/Loiter.cpp @@ -9,14 +9,20 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled float scaler_xz_n; float xz_out = fabsf(blimp.motors->front_out) + fabsf(blimp.motors->down_out); - if (xz_out > 1) scaler_xz_n = 1 / xz_out; - else scaler_xz_n = 1; + if (xz_out > 1) { + scaler_xz_n = 1 / xz_out; + } else { + scaler_xz_n = 1; + } scaler_xz = scaler_xz*MA + scaler_xz_n*MO; float scaler_yyaw_n; float yyaw_out = fabsf(blimp.motors->right_out) + fabsf(blimp.motors->yaw_out); - if (yyaw_out > 1) scaler_yyaw_n = 1 / yyaw_out; - else scaler_yyaw_n = 1; + if (yyaw_out > 1) { + scaler_yyaw_n = 1 / yyaw_out; + } else { + scaler_yyaw_n = 1; + } scaler_yyaw = scaler_yyaw*MA + scaler_yyaw_n*MO; AP::logger().WriteStreaming("BSC", "TimeUS,xz,yyaw,xzn,yyawn", @@ -29,17 +35,27 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled float err_yaw = wrap_PI(target_yaw - yaw_ef); Vector4b zero; - if((fabsf(err_xyz.x) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(2-1)))) zero.x = true; - if((fabsf(err_xyz.y) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(1-1)))) zero.y = true; - if((fabsf(err_xyz.z) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(3-1)))) zero.z = true; - if((fabsf(err_yaw) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(4-1)))) zero.yaw = true; + if ((fabsf(err_xyz.x) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(2-1)))) { + zero.x = true; + } + if ((fabsf(err_xyz.y) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(1-1)))) { + zero.y = true; + } + if ((fabsf(err_xyz.z) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(3-1)))) { + zero.z = true; + } + if ((fabsf(err_yaw) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(4-1)))) { + zero.yaw = true; + } //Disabled means "don't update PIDs or output anything at all". Zero means actually output zero thrust. I term is limited in either case." Vector4b limit = zero || axes_disabled; Vector3f target_vel_ef; - if(!axes_disabled.x && !axes_disabled.y) target_vel_ef = {blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, dt, {(float)limit.x, (float)limit.y, (float)limit.z}), 0}; - if(!axes_disabled.z) target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt, limit.z); + if (!axes_disabled.x && !axes_disabled.y) target_vel_ef = {blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, dt, {(float)limit.x, (float)limit.y, (float)limit.z}), 0}; + if (!axes_disabled.z) { + target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt, limit.z); + } float target_vel_yaw = 0; if (!axes_disabled.yaw) { @@ -59,10 +75,14 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled Vector2f actuator; if (!axes_disabled.x && !axes_disabled.y) actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c_scaled_xy, vel_ned_filtd_scaled_xy, dt, {(float)limit.x, (float)limit.y}); float act_down = 0; - if(!axes_disabled.z) act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z * scaler_xz, blimp.vel_ned_filtd.z * scaler_xz, dt, limit.z); + if (!axes_disabled.z) { + act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z * scaler_xz, blimp.vel_ned_filtd.z * scaler_xz, dt, limit.z); + } blimp.rotate_NE_to_BF(actuator); float act_yaw = 0; - if(!axes_disabled.yaw) act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yyaw, blimp.vel_yaw_filtd * scaler_yyaw, dt, limit.yaw); + if (!axes_disabled.yaw) { + act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yyaw, blimp.vel_yaw_filtd * scaler_yyaw, dt, limit.yaw); + } if (!blimp.motors->armed()) { blimp.pid_pos_xy.set_integrator(Vector2f(0,0)); @@ -110,10 +130,18 @@ void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b ax const float dt = blimp.scheduler.get_last_loop_time_s(); Vector4b zero; - if(!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(2-1)))) zero.x = true; - if(!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(1-1)))) zero.y = true; - if(!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(3-1)))) zero.z = true; - if(!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(4-1)))) zero.yaw = true; + if (!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(2-1)))) { + zero.x = true; + } + if (!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(1-1)))) { + zero.y = true; + } + if (!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(3-1)))) { + zero.z = true; + } + if (!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(4-1)))) { + zero.yaw = true; + } //Disabled means "don't update PIDs or output anything at all". Zero means actually output zero thrust. I term is limited in either case." Vector4b limit = zero || axes_disabled; @@ -128,10 +156,14 @@ void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b ax Vector2f actuator; if (!axes_disabled.x && !axes_disabled.y) actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c_scaled_xy, vel_ned_filtd_scaled_xy, dt, {(float)limit.x, (float)limit.y}); float act_down = 0; - if(!axes_disabled.z) act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z * scaler_xz, blimp.vel_ned_filtd.z * scaler_xz, dt, limit.z); + if (!axes_disabled.z) { + act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z * scaler_xz, blimp.vel_ned_filtd.z * scaler_xz, dt, limit.z); + } blimp.rotate_NE_to_BF(actuator); float act_yaw = 0; - if(!axes_disabled.yaw) act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yyaw, blimp.vel_yaw_filtd * scaler_yyaw, dt, limit.yaw); + if (!axes_disabled.yaw) { + act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yyaw, blimp.vel_yaw_filtd * scaler_yyaw, dt, limit.yaw); + } if (!blimp.motors->armed()) { blimp.pid_vel_xy.set_integrator(Vector2f(0,0)); diff --git a/Blimp/mode.cpp b/Blimp/mode.cpp index 23f1328487..b7d80bd906 100644 --- a/Blimp/mode.cpp +++ b/Blimp/mode.cpp @@ -140,7 +140,7 @@ void Blimp::update_flight_mode() // exit_mode - high level call to organise cleanup as a flight mode is exited void Blimp::exit_mode(Mode *&old_flightmode, - Mode *&new_flightmode){} + Mode *&new_flightmode) {} // notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device void Blimp::notify_flight_mode() diff --git a/Blimp/mode_loiter.cpp b/Blimp/mode_loiter.cpp index 83fc19d443..725a702480 100644 --- a/Blimp/mode_loiter.cpp +++ b/Blimp/mode_loiter.cpp @@ -32,10 +32,18 @@ void ModeLoiter::run() blimp.rotate_BF_to_NE(pilot.xy()); } - if(fabsf(target_pos.x-blimp.pos_ned.x) < (g.max_pos_xy*POS_LAG)) target_pos.x += pilot.x; - if(fabsf(target_pos.y-blimp.pos_ned.y) < (g.max_pos_xy*POS_LAG)) target_pos.y += pilot.y; - if(fabsf(target_pos.z-blimp.pos_ned.z) < (g.max_pos_z*POS_LAG)) target_pos.z += pilot.z; - if(fabsf(wrap_PI(target_yaw-ahrs.get_yaw())) < (g.max_pos_yaw*POS_LAG)) target_yaw = wrap_PI(target_yaw + pilot_yaw); + if (fabsf(target_pos.x-blimp.pos_ned.x) < (g.max_pos_xy*POS_LAG)) { + target_pos.x += pilot.x; + } + if (fabsf(target_pos.y-blimp.pos_ned.y) < (g.max_pos_xy*POS_LAG)) { + target_pos.y += pilot.y; + } + if (fabsf(target_pos.z-blimp.pos_ned.z) < (g.max_pos_z*POS_LAG)) { + target_pos.z += pilot.z; + } + if (fabsf(wrap_PI(target_yaw-ahrs.get_yaw())) < (g.max_pos_yaw*POS_LAG)) { + target_yaw = wrap_PI(target_yaw + pilot_yaw); + } blimp.loiter->run(target_pos, target_yaw, Vector4b{false,false,false,false}); }