From cceb612d0cf207c8f3dbe1aa0cb3090cbc9f5bc8 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Thu, 24 Jun 2021 00:17:34 +1000 Subject: [PATCH] Blimp: Fix mode land, manual and fins class --- Blimp/Fins.cpp | 16 ++++++---------- Blimp/Fins.h | 12 ++---------- Blimp/mode_land.cpp | 7 +++++-- Blimp/mode_manual.cpp | 8 ++++---- 4 files changed, 17 insertions(+), 26 deletions(-) diff --git a/Blimp/Fins.cpp b/Blimp/Fins.cpp index b11e6dbdcb..2fba13b01c 100644 --- a/Blimp/Fins.cpp +++ b/Blimp/Fins.cpp @@ -34,9 +34,9 @@ Fins::Fins(uint16_t loop_rate) : void Fins::setup_fins() { - //amp r f d y,off r f d y right, front, down, yaw - add_fin(0, 0, -1, 0.5, 0, 0, 0, -0.5, 0); //Back(?) - add_fin(1, 0, 1, 0.5, 0, 0, 0, -0.5, 0); //Front(?) + //fin # r f d y, r f d y right, front, down, yaw for amplitude then for offset + add_fin(0, 0, 1, 0.5, 0, 0, 0, 0.5, 0); //Back + add_fin(1, 0, -1, 0.5, 0, 0, 0, 0.5, 0); //Front add_fin(2, -1, 0, 0, 0.5, 0, 0, 0, 0.5); //Right add_fin(3, 1, 0, 0, 0.5, 0, 0, 0, -0.5); //Left @@ -78,25 +78,21 @@ void Fins::output() yaw_out = 0; } - right_out /= RC_SCALE; - front_out /= RC_SCALE; - down_out /= RC_SCALE; - yaw_out /= RC_SCALE; _time = AP_HAL::micros() * 1.0e-6; for (int8_t i=0; i 0.0f) { + if (fmaxf(0,_right_amp_factor[i]*right_out) > 0.0f) { _num_added++; } - if (max(0,_front_amp_factor[i]*front_out) > 0.0f) { + if (fmaxf(0,_front_amp_factor[i]*front_out) > 0.0f) { _num_added++; } if (fabsf(_down_amp_factor[i]*down_out) > 0.0f) { diff --git a/Blimp/Fins.h b/Blimp/Fins.h index 1de1e1c019..fcd277b832 100644 --- a/Blimp/Fins.h +++ b/Blimp/Fins.h @@ -84,15 +84,6 @@ public: bool _interlock; // 1 if the motor interlock is enabled (i.e. motors run), 0 if disabled (motors don't run) bool _initialised_ok; // 1 if initialisation was successful - float max(float one, float two) - { - if (one >= two) { - return one; - } else { - return two; - } - } - void output_min(); void add_fin(int8_t fin_num, float right_amp_fac, float front_amp_fac, float yaw_amp_fac, float down_amp_fac, @@ -104,7 +95,8 @@ public: float get_throttle() { - return 0.1f; //TODO + //Only for Mavlink - essentially just an indicator of how hard the fins are working. + return fmaxf(fmaxf(fabsf(down_out),fabsf(front_out)), fmaxf(fabsf(right_out),fabsf(yaw_out))); } void rc_write(uint8_t chan, uint16_t pwm); diff --git a/Blimp/mode_land.cpp b/Blimp/mode_land.cpp index 521351311e..f6e334839e 100644 --- a/Blimp/mode_land.cpp +++ b/Blimp/mode_land.cpp @@ -6,8 +6,11 @@ // Runs the main land controller void ModeLand::run() { - //stop moving - + //Stop moving + motors->right_out = 0; + motors->front_out = 0; + motors->yaw_out = 0; + motors->down_out = 0; } // set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts diff --git a/Blimp/mode_manual.cpp b/Blimp/mode_manual.cpp index 063353ded3..1183b4bd7a 100644 --- a/Blimp/mode_manual.cpp +++ b/Blimp/mode_manual.cpp @@ -6,8 +6,8 @@ // Runs the main manual controller void ModeManual::run() { - motors->right_out = channel_right->get_control_in(); - motors->front_out = channel_front->get_control_in(); - motors->yaw_out = channel_yaw->get_control_in(); - motors->down_out = channel_down->get_control_in(); + motors->right_out = channel_right->get_control_in() / float(RC_SCALE); + motors->front_out = channel_front->get_control_in() / float(RC_SCALE); + motors->yaw_out = channel_yaw->get_control_in() / float(RC_SCALE); + motors->down_out = channel_down->get_control_in() / float(RC_SCALE); }