Plane: added support for ALTITUDE_WAIT mission command

ArduPlane/Attitude.cpp

@@ -681,6 +681,45 @@ void Plane::flaperon_update(int8_t flap_percent)
     RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::k_flaperon2, ch2);
 }
 
+/*
+  setup servos for idle mode
+  Idle mode is used during balloon launch to keep servos still, apart
+  from occasional wiggle to prevent freezing up
+ */
+void Plane::set_servos_idle(void)
+{
+    RC_Channel_aux::output_ch_all();
+    if (auto_state.idle_wiggle_stage == 0) {
+        RC_Channel::output_trim_all();
+        return;
+    }
+    int16_t servo_value = 0;
+    // move over full range for 2 seconds
+    auto_state.idle_wiggle_stage += 2;
+    if (auto_state.idle_wiggle_stage < 50) {
+        servo_value = auto_state.idle_wiggle_stage * (4500 / 50);
+    } else if (auto_state.idle_wiggle_stage < 100) {
+        servo_value = (100 - auto_state.idle_wiggle_stage) * (4500 / 50);        
+    } else if (auto_state.idle_wiggle_stage < 150) {
+        servo_value = (100 - auto_state.idle_wiggle_stage) * (4500 / 50);        
+    } else if (auto_state.idle_wiggle_stage < 200) {
+        servo_value = (auto_state.idle_wiggle_stage-200) * (4500 / 50);        
+    } else {
+        auto_state.idle_wiggle_stage = 0;
+    }
+    channel_roll->servo_out = servo_value;
+    channel_pitch->servo_out = servo_value;
+    channel_rudder->servo_out = servo_value;
+    channel_roll->calc_pwm();
+    channel_pitch->calc_pwm();
+    channel_rudder->calc_pwm();
+    channel_roll->output();
+    channel_pitch->output();
+    channel_throttle->output();
+    channel_rudder->output();
+    channel_throttle->output_trim();
+}
+
 
 /*****************************************
 * Set the flight control servos based on the current calculated values
@@ -689,6 +728,12 @@ void Plane::set_servos(void)
 {
     int16_t last_throttle = channel_throttle->radio_out;
 
+    if (control_mode == AUTO && auto_state.idle_mode) {
+        // special handling for balloon launch
+        set_servos_idle();
+        return;
+    }
+
     /*
       see if we are doing ground steering.
      */

ArduPlane/Plane.h

@@ -434,6 +434,15 @@ private:
         // denotes if a go-around has been commanded for landing
         bool commanded_go_around:1;
 
+        // Altitude threshold to complete a takeoff command in autonomous modes.  Centimeters
+        // are we in idle mode? used for balloon launch to stop servo
+        // movement until altitude is reached
+        bool idle_mode:1;
+
+        // used to 'wiggle' servos in idle mode to prevent them freezing
+        // at high altitudes
+        uint8_t idle_wiggle_stage;
+
         // Altitude threshold to complete a takeoff command in autonomous
         // modes.  Centimeters above home
         int32_t takeoff_altitude_rel_cm;
@@ -730,6 +739,7 @@ private:
     bool verify_wait_delay();
     bool verify_change_alt();
     bool verify_within_distance();
+    bool verify_altitude_wait(const AP_Mission::Mission_Command &cmd);
     void do_loiter_at_location();
     void do_take_picture();
     void log_picture();
@@ -852,6 +862,7 @@ private:
     void terrain_update(void);
     void update_flight_mode(void);
     void stabilize();
+    void set_servos_idle(void);
     void set_servos();
     void update_aux();
     void determine_is_flying(void);
@@ -891,6 +902,7 @@ private:
     void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
     void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
     void do_loiter_time(const AP_Mission::Mission_Command& cmd);
+    void do_altitude_wait(const AP_Mission::Mission_Command& cmd);
     void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
     void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);

ArduPlane/commands_logic.cpp

@@ -24,6 +24,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
 
         // if a go around had been commanded, clear it now.
         auto_state.commanded_go_around = false;
+
+        // start non-idle
+        auto_state.idle_mode = false;
         
         gcs_send_text_fmt(PSTR("Executing nav command ID #%i"),cmd.id);
     } else {
@@ -68,6 +71,10 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         do_continue_and_change_alt(cmd);
         break;
 
+    case MAV_CMD_NAV_ALTITUDE_WAIT:
+        do_altitude_wait(cmd);
+        break;
+
     // Conditional commands
 
     case MAV_CMD_CONDITION_DELAY:
@@ -227,6 +234,9 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
     case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
         return verify_continue_and_change_alt();
 
+    case MAV_CMD_NAV_ALTITUDE_WAIT:
+        return verify_altitude_wait(cmd);
+
     // Conditional commands
 
     case MAV_CMD_CONDITION_DELAY:
@@ -364,6 +374,12 @@ void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
     reset_offset_altitude();
 }
 
+void Plane::do_altitude_wait(const AP_Mission::Mission_Command& cmd)
+{
+    // set all servos to trim until we reach altitude or descent speed
+    auto_state.idle_mode = true;
+}
+
 void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) 
 {
     //set target alt  
@@ -631,6 +647,34 @@ bool Plane::verify_continue_and_change_alt()
     return false;
 }
 
+/*
+  see if we have reached altitude or descent speed
+ */
+bool Plane::verify_altitude_wait(const AP_Mission::Mission_Command &cmd)
+{
+    if (current_loc.alt > cmd.content.altitude_wait.altitude*100.0f) {
+        gcs_send_text_P(SEVERITY_LOW,PSTR("Reached altitude"));
+        return true;
+    }
+    if (auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) {
+        gcs_send_text_fmt(PSTR("Reached descent rate %.1f m/s"), auto_state.sink_rate);
+        return true;        
+    }
+
+    // if requested, wiggle servos
+    if (cmd.content.altitude_wait.wiggle_time != 0) {
+        static uint32_t last_wiggle_ms;
+        if (auto_state.idle_wiggle_stage == 0 &&
+            hal.scheduler->millis() - last_wiggle_ms > cmd.content.altitude_wait.wiggle_time*1000) {
+            auto_state.idle_wiggle_stage = 1;
+            last_wiggle_ms = hal.scheduler->millis();
+        }
+        // idle_wiggle_stage is updated in set_servos_idle()
+    }
+
+    return false;
+}
+
 /********************************************************************************/
 //  Condition (May) commands
 /********************************************************************************/