From d5c5400e76a18b1a5693ed6e1e68dc22208b9070 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 13 Jun 2015 19:12:54 +1000 Subject: [PATCH] Plane: added support for ALTITUDE_WAIT mission command --- ArduPlane/Attitude.cpp | 45 ++++++++++++++++++++++++++++++++++++ ArduPlane/Plane.h | 12 ++++++++++ ArduPlane/commands_logic.cpp | 44 +++++++++++++++++++++++++++++++++++ 3 files changed, 101 insertions(+) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index a160f82be3..76f067b02e 100644 --- a/ArduPlane/Attitude.cpp +++ b/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. */ diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 94d95a5df9..2b09ae9737 100644 --- a/ArduPlane/Plane.h +++ b/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); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index b60b54fbed..870e1b0fa0 100644 --- a/ArduPlane/commands_logic.cpp +++ b/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 /********************************************************************************/