diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index a8b48fa2de..62ef997c82 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -686,7 +686,7 @@ private: void rotate_body_frame_to_NE(float &x, float &y); uint16_t get_pilot_speed_dn(); int32_t get_alt_above_ground_cm(); - + UpDownState updownStatus = UpDown_TakeOffStart; #if ADSB_ENABLED == ENABLED // avoidance_adsb.cpp void avoidance_adsb_update(void); @@ -808,6 +808,10 @@ private: // mode.cpp bool set_mode(Mode::Number mode, ModeReason reason); bool set_mode(const uint8_t new_mode, const ModeReason reason) override; + + void update_updownStatus2(uint8_t status) ; + bool update_updownStatus(const uint8_t status) override; + void update_flight_mode(); void notify_flight_mode(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index b13171c192..4a86d2bed3 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -122,6 +122,15 @@ const AP_Param::Info Copter::var_info[] = { // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:AccelZ GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), + // @Param: TAKEOFF_ALT_REQ + // @DisplayName: take off Altitude + // @Description: + // @Units: cm + // @Range: -1 5000 + // @Increment: 1 + // @User: Standard + GSCALAR(takeoff_alt_req, "TAKEOFF_ALT_REQ", TAKEOFF_ALT_REQ), + #if MODE_RTL_ENABLED == ENABLED // @Param: RTL_ALT // @DisplayName: RTL Altitude @@ -977,7 +986,6 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 0:Continue if in Auto on RC failsafe, 1:Continue if in Auto on GCS failsafe, 2:Continue if in Guided on RC failsafe, 3:Continue if landing on any failsafe, 4:Continue if in pilot controlled modes on GCS failsafe // @User: Advanced AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, 0), - #if MODE_AUTOROTATE_ENABLED == ENABLED // @Group: AROT_ // @Path: ../libraries/AC_Autorotation/AC_Autorotation.cpp diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index f0ce11d546..9c70540d16 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -386,6 +386,7 @@ public: k_param_land_lock_alt, k_param_land_lock_rpy, + k_param_takeoff_alt_req, // the k_param_* space is 9-bits in size // 511: reserved }; @@ -445,7 +446,7 @@ public: AP_Int8 land_lock_rpy; - + // Throttle // AP_Int8 failsafe_throttle; @@ -495,6 +496,8 @@ public: AP_Int8 acro_trainer; AP_Float acro_rp_expo; + AP_Int16 takeoff_alt_req; + // Note: keep initializers here in the same order as they are declared // above. Parameters() diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 12f66ba017..9bd19347bd 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -531,6 +531,11 @@ #define ACRO_THR_MID_DEFAULT 0.0f #endif + +#ifndef TAKEOFF_ALT_REQ + # define TAKEOFF_ALT_REQ 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. +#endif + // RTL Mode #ifndef RTL_ALT_FINAL # define RTL_ALT_FINAL 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 88dcfc6d84..e14c6eb991 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -122,10 +122,13 @@ enum RTLState { }; // enum UpDownState { + UpDown_TakeOffStart =0, UpDown_TakeOffClimb, + UpDown_RequestClimb, UpDown_ContinueClimb, UpDown_InMission, UpDown_RTLDescent, + UpDown_RequestDescent, UpDown_ContinueDescent, }; diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 41e0a2422f..00f30c0d77 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -812,6 +812,22 @@ bool Mode::set_mode(Mode::Number mode, ModeReason reason) return copter.set_mode(mode, reason); } +bool Copter::update_updownStatus(const uint8_t status) +{ + copter.update_updownStatus2(status); + return true; +} + +void Copter::update_updownStatus2(uint8_t status) +{ + copter.updownStatus = (UpDownState)status; + if (status == 3) + { + set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD); + gcs().update_zr_fly_status(status); + } +} + void Mode::set_land_complete(bool b) { return copter.set_land_complete(b); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 9a17bbd84e..73c6da0d99 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -479,7 +479,7 @@ private: // Delay Mission Scripting Command int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) uint32_t condition_start; - + uint32_t last_time_send_updownload; LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending) struct { @@ -1066,7 +1066,7 @@ private: uint32_t _loiter_start_time; bool terrain_following_allowed; - UpDownState _updownStatus = UpDown_TakeOffClimb; + uint32_t last_send_time; }; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 2c1c195646..885c8ec10c 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -235,6 +235,11 @@ void ModeAuto::wp_start(const Location& dest_loc) if (auto_yaw.mode() != AUTO_YAW_ROI) { auto_yaw.set_mode_to_default(false); } + if (copter.updownStatus < UpDown_InMission) + { + copter.updownStatus = UpDown_InMission; + gcs().update_zr_fly_status(copter.updownStatus); + } } // auto_land_start - initialises controller to implement a landing @@ -755,6 +760,27 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) // called by auto_run at 100hz or more void ModeAuto::takeoff_run() { + if (motors->armed() || copter.ap.auto_armed) + { + int32_t alt_rev = 0; + if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_rev)) + { + if (g.takeoff_alt_req > 0 && alt_rev >= g.takeoff_alt_req + && copter.updownStatus < UpDown_RequestClimb) + { + copter.updownStatus = UpDown_RequestClimb; + set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD); + gcs().update_zr_fly_status(copter.updownStatus); + } + else if (g.takeoff_alt_req > 0 && alt_rev < g.takeoff_alt_req && alt_rev > 0 + &&(AP_HAL::millis()-last_time_send_updownload>500)) + { + last_time_send_updownload = AP_HAL::millis(); + copter.updownStatus = UpDown_TakeOffClimb; + gcs().update_zr_fly_status(copter.updownStatus); + } + } + } auto_takeoff_run(); } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index c9eeb35623..f6160105de 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -66,14 +66,18 @@ void ModeRTL::run(bool disarm_on_land) } break; case RTL_FinalDescent: - // gcs().send_text(MAV_SEVERITY_INFO,"_updownStatus:%d,_state:%d",_state,_state); - if(_updownStatus!= UpDown_ContinueDescent){ - mavlink_zr_flying_status_t zr_flying_status_t; - zr_flying_status_t.updown_status = UpDown_ContinueDescent; - gcs().update_zr_fly_status(&zr_flying_status_t); - gcs().send_text(MAV_SEVERITY_INFO,"into RTL_FinalDescent"); - set_mode(Mode::Number::BRAKE,ModeReason::MISSION_END); + + if (copter.updownStatus < UpDown_RequestDescent) + { + copter.updownStatus = UpDown_RequestDescent; + gcs().update_zr_fly_status(copter.updownStatus); + } + + if (copter.updownStatus == UpDown_ContinueDescent) + { + _state = RTL_Land; } + break; case RTL_Land: // do nothing - rtl_land_run will take care of disarming motors @@ -262,8 +266,9 @@ void ModeRTL::loiterathome_run() // rtl_descent_start - initialise descent to final alt void ModeRTL::descent_start() { + copter.updownStatus = UpDown_RTLDescent; + gcs().update_zr_fly_status(copter.updownStatus); _state = RTL_FinalDescent; - // _updownStatus = UpDown_RTLDescent; _state_complete = false; // Set wp navigation target to above home @@ -401,7 +406,11 @@ void ModeRTL::land_run(bool disarm_on_land) motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); land_run_horizontal_control(); - land_run_vertical_control(); + land_run_vertical_control(); + + if(AP_HAL::millis()-last_send_time>=500){ + gcs().update_zr_fly_status(copter.updownStatus); + } } void ModeRTL::build_path() diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 6097ec5d17..c5d617c244 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -52,7 +52,7 @@ public: static AP_Vehicle *get_singleton(); bool virtual set_mode(const uint8_t new_mode, const ModeReason reason) = 0; - + bool virtual update_updownStatus(const uint8_t status) =0; /* common parameters for fixed wing aircraft */ diff --git a/libraries/AP_Vehicle/ModeReason.h b/libraries/AP_Vehicle/ModeReason.h index d0a4f56001..cf6a72db3a 100644 --- a/libraries/AP_Vehicle/ModeReason.h +++ b/libraries/AP_Vehicle/ModeReason.h @@ -53,4 +53,5 @@ enum class ModeReason : uint8_t { UNAVAILABLE, AUTOROTATION_START, AUTOROTATION_BAILOUT, + REQUEST_CMD, }; diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 950737a560..fc59abe118 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -219,7 +219,7 @@ public: void send_rpm() const; bool send_battgo_info(const mavlink_battgo_info_t *mavlink_battgo_info_t); bool send_battgo_history(const mavlink_battgo_history_t *mavlink_battgo_info_t); - bool send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t); + bool send_zr_flying_status(const uint8_t flying_status); //void send_serial_battgo(mavlink_battgo_info_t &battgo_info_t ); bool locked() const; @@ -838,7 +838,7 @@ public: void update_serial_battgo_info(mavlink_battgo_info_t *battgo_info_t ); void update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_t ); - void update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t ); + void update_zr_fly_status(uint8_t flying_status ); // minimum amount of time (in microseconds) that must remain in // the main scheduler loop before we are allowed to send any // mavlink messages. We want to prioritise the main flight diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 90dab65c3e..a3dd5c51e7 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1924,12 +1924,12 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_ } } -void GCS::update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t ) +void GCS::update_zr_fly_status(uint8_t flying_status) { - for (uint8_t i=0; isend_zr_flying_status(zr_flying_status_t); - } + for (uint8_t i = 0; i < num_gcs(); i++) + { + chan(i)->send_zr_flying_status(flying_status); + } } void GCS::send_mission_item_reached_message(uint16_t mission_index) @@ -4829,13 +4829,16 @@ void GCS_MAVLINK::manual_override(RC_Channel *c, int16_t value_in, const uint16_ void GCS_MAVLINK::handle_zr_fly_status(const mavlink_message_t &msg) { mavlink_zr_flying_status_t packet; - mavlink_msg_zr_flying_status_decode(&msg, &packet); - //TODO handle data; + mavlink_msg_zr_flying_status_decode(&msg, &packet); + if (packet.updown_status == 3 || packet.updown_status == 7) + { + AP::vehicle()->update_updownStatus(packet.updown_status); + } } -bool GCS_MAVLINK::send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t) +bool GCS_MAVLINK::send_zr_flying_status(const uint8_t flying_status) { - mavlink_msg_zr_flying_status_send_struct(chan, zr_flying_status_t); + mavlink_msg_zr_flying_status_send(chan, flying_status,0); return true; } diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp index ed781b50d4..82083fcd83 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp @@ -129,9 +129,10 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len) } const size_t written = mavlink_comm_port[chan]->write(buf, len); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - if (written < len) { - AP_HAL::panic("Short write on UART: %lu < %u", written, len); - } + // if (written < len) { + // AP_HAL::panic("Short write on UART: %lu < %u", written, len); + // } + (void)written; #else (void)written; #endif