Blimp: Fix mode land, manual and fins class

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
+    //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(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(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<NUM_FINS; i++) {
-        _amp[i] =  max(0,_right_amp_factor[i]*right_out) + max(0,_front_amp_factor[i]*front_out) +
+        _amp[i] =  fmaxf(0,_right_amp_factor[i]*right_out) + fmaxf(0,_front_amp_factor[i]*front_out) +
                    fabsf(_down_amp_factor[i]*down_out) + fabsf(_yaw_amp_factor[i]*yaw_out);
         _off[i] = _right_off_factor[i]*right_out + _front_off_factor[i]*front_out +
                   _down_off_factor[i]*down_out + _yaw_off_factor[i]*yaw_out;
         _freq[i] = 1;
 
         _num_added = 0;
-        if (max(0,_right_amp_factor[i]*right_out) > 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) {

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

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

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->right_out = channel_right->get_control_in() / float(RC_SCALE);
-    motors->front_out = channel_front->get_control_in();
+    motors->front_out = channel_front->get_control_in() / float(RC_SCALE);
-    motors->yaw_out = channel_yaw->get_control_in();
+    motors->yaw_out = channel_yaw->get_control_in() / float(RC_SCALE);
-    motors->down_out = channel_down->get_control_in();
+    motors->down_out = channel_down->get_control_in() / float(RC_SCALE);
 }