diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index ae42848ed0..2d7e3e556c 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -124,16 +124,33 @@ 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 + // @Param: ZR_TK_REQ_ALT // @DisplayName: take off Altitude // @Description: // @Units: cm - // @Range: -1 5000 + // @Range:0 5000 // @Increment: 1 // @User: Standard - GSCALAR(takeoff_alt_req, "TAKEOFF_ALT_REQ", TAKEOFF_ALT_REQ), + GSCALAR(zr_tk_req_alt, "ZR_TK_REQ_ALT", TAKEOFF_ALT_REQ), + + // @Param: ZR_TK_DELAY + // @DisplayName: take off delay at ZR_TK_REQ_ALT + // @Description: + // @Units: second + // @Range: 0 360 + // @Increment: 1 + // @User: Standard + GSCALAR(zr_tk_delay, "ZR_TK_DELAY", ZR_TK_DELAY), + + // @Param: ZR_RTL_DELAY + // @DisplayName: rtl Altitude when at final decent alt + // @Description: + // @Units: second + // @Range: 0 360 + // @Increment: 1 + // @User: Standard + GSCALAR(zr_rtl_delay, "ZR_RTL_DELAY", ZR_RTL_DELAY), - GSCALAR(updown_countdown, "UPDOWN_COUNTDOWN", UPDOWN_COUNTDOWN), #if MODE_RTL_ENABLED == ENABLED // @Param: RTL_ALT // @DisplayName: RTL Altitude diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index ae0fce33b3..4149cccf99 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -386,8 +386,9 @@ public: k_param_land_lock_alt, k_param_land_lock_rpy, - k_param_takeoff_alt_req, - k_param_updown_countdown, + k_param_zr_tk_req_alt, + k_param_zr_tk_delay, + k_param_zr_rtl_delay, // the k_param_* space is 9-bits in size // 511: reserved }; @@ -496,8 +497,9 @@ public: AP_Float acro_balance_pitch; AP_Int8 acro_trainer; AP_Float acro_rp_expo; - AP_Int16 takeoff_alt_req; - AP_Int16 updown_countdown; + AP_Int16 zr_tk_req_alt; + AP_Int16 zr_tk_delay; + AP_Int16 zr_rtl_delay; // Note: keep initializers here in the same order as they are declared // above. Parameters() diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 4abfb10608..47330f83e3 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -84,10 +84,19 @@ void Copter::userhook_SuperSlowLoop() static bool before_fly = true; if(motors->armed()){ before_fly = false; +<<<<<<< HEAD relay.on(3); }else{ relay.off(3); +======= + }else{ + updownStatus =UpDown_TakeOffStart; + mavlink_zr_flying_status_t zr_flying_status_t; + zr_flying_status_t.updown_status = updownStatus; + //TODO UPDATE COUNTDOWN + gcs().update_zr_fly_status(&zr_flying_status_t); +>>>>>>> b3fbb47935ac0226cb0daf620677df256ab603c4 } if(before_fly){ uint8_t cnt,cacl_volt_pst; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index c5345e4476..8aea68fb63 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -537,7 +537,15 @@ #endif #ifndef UPDOWN_COUNTDOWN -# define UPDOWN_COUNTDOWN 20 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. +#define UPDOWN_COUNTDOWN 20 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. +#endif + +#ifndef ZR_TK_DELAY +#define ZR_TK_DELAY 5 +#endif + +#ifndef ZR_RTL_DELAY +#define ZR_RTL_DELAY 15 #endif // RTL Mode diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 661c446ef2..ff672091c8 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -133,7 +133,8 @@ protected: float &G_Dt; int16_t countdown ; uint32_t last_takeoff_request_time; - bool is_countdown_enable = true; + bool is_takeoff_delay_enable = true; + bool is_rtl_delay_enable = true; // note that we support two entirely different automatic takeoffs: // "user-takeoff", which is available in modes such as ALT_HOLD diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 9c4c2257e0..bb38f5fb0e 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -768,24 +768,24 @@ void ModeAuto::takeoff_run() 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 + if (g.zr_tk_req_alt > 0 && alt_rev >= g.zr_tk_req_alt && copter.updownStatus < UpDown_RequestClimb) { copter.updownStatus = UpDown_RequestClimb; - countdown = g.updown_countdown; + countdown = g.zr_tk_delay; mavlink_zr_flying_status_t zr_flying_status_t; zr_flying_status_t.updown_status = copter.updownStatus; zr_flying_status_t.countdown = countdown; gcs().update_zr_fly_status(&zr_flying_status_t); last_takeoff_request_time = AP_HAL::millis(); - if(g.updown_countdown>0){ - is_countdown_enable =true; + if(g.zr_tk_delay>0){ + is_takeoff_delay_enable =true; }else{ - is_countdown_enable =false; + is_takeoff_delay_enable =false; } set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD); } - else if (g.takeoff_alt_req > 0 && alt_rev < g.takeoff_alt_req && alt_rev > 0 + else if (g.zr_tk_req_alt > 0 && alt_rev < g.zr_tk_req_alt && alt_rev > 0 &&(AP_HAL::millis()-last_time_send_updownload>500)) { last_time_send_updownload = AP_HAL::millis(); diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index e4ebef37bb..9c0e5a6a66 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -24,13 +24,13 @@ bool ModeBrake::init(bool ignore_checks) _timeout_ms = 0; if(copter.updownStatus == UpDown_RequestClimb){ - countdown = g.updown_countdown; + countdown = g.zr_tk_delay; } - if(g.updown_countdown>0){ - is_countdown_enable =true; + if(g.zr_tk_delay>0){ + is_takeoff_delay_enable =true; }else{ - is_countdown_enable =false; + is_takeoff_delay_enable =false; } return true; } @@ -76,12 +76,12 @@ void ModeBrake::run() } } - if(copter.updownStatus == UpDown_RequestClimb&&is_countdown_enable) + if(copter.updownStatus == UpDown_RequestClimb&&is_takeoff_delay_enable) { if((AP_HAL::millis()-last_takeoff_request_time)>1000) { last_takeoff_request_time = AP_HAL::millis(); - countdown --; + // mavlink_zr_flying_status_t zr_flying_status_t; // zr_flying_status_t.updown_status = copter.updownStatus; // zr_flying_status_t.countdown = countdown; @@ -94,6 +94,7 @@ void ModeBrake::run() copter.updownStatus = UpDown_ContinueClimb; set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD); } + countdown --; //TODO Actually 1 second longer } } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index dbd23ffa63..929a998264 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -72,21 +72,20 @@ void ModeRTL::run(bool disarm_on_land) copter.updownStatus = UpDown_RequestDescent; mavlink_zr_flying_status_t zr_flying_status_t; zr_flying_status_t.updown_status = copter.updownStatus; - countdown = g.updown_countdown; + countdown = g.zr_rtl_delay; zr_flying_status_t.countdown = countdown; gcs().update_zr_fly_status(&zr_flying_status_t); last_decent_time = AP_HAL::millis(); - if(g.updown_countdown>0){ - is_countdown_enable = true; + if(g.zr_rtl_delay>0){ + is_rtl_delay_enable = true; }else{ - is_countdown_enable = false; + is_rtl_delay_enable = false; } } - else if(copter.updownStatus ==UpDown_RequestDescent&&is_countdown_enable) + else if(copter.updownStatus ==UpDown_RequestDescent&&is_rtl_delay_enable) { if((AP_HAL::millis()- last_decent_time)>=1000){ - last_decent_time = AP_HAL::millis(); - countdown --; + last_decent_time = AP_HAL::millis(); // mavlink_zr_flying_status_t zr_flying_status_t; // zr_flying_status_t.updown_status = copter.updownStatus; // zr_flying_status_t.countdown = countdown; @@ -99,6 +98,7 @@ void ModeRTL::run(bool disarm_on_land) copter.updownStatus = UpDown_ContinueDescent; _state = RTL_Land; } + countdown --; //TODO shiji countdown+1 s } } else if (copter.updownStatus == UpDown_ContinueDescent) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 9f572e1fbf..7fd090bd6d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -17,7 +17,9 @@ #include #include "AP_BattMonitor.h" #include "AP_BattMonitor_Backend.h" - +#include +#include +#include /* base class constructor. This incorporates initialisation as well. @@ -167,6 +169,10 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const bool fs_voltage_inversion = is_positive(_params._critical_voltage) && is_positive(_params._low_voltage) && (_params._low_voltage < _params._critical_voltage); + uint8_t cell_min_index; + uint8_t cell_max_index; + uint16_t dropout_voltage; + bool fs_cells_dropout_voltage = cells_dropout_voltage_checks(cell_min_index,cell_max_index,dropout_voltage); bool result = update_check(buflen, buffer, low_voltage, "low voltage failsafe"); result = result && update_check(buflen, buffer, low_capacity, "low capacity failsafe"); @@ -176,7 +182,9 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const result = result && update_check(buflen, buffer, below_arming_capacity, "below minimum arming capacity"); result = result && update_check(buflen, buffer, fs_capacity_inversion, "capacity failsafe critical > low"); result = result && update_check(buflen, buffer, fs_voltage_inversion, "voltage failsafe critical > low"); - + // char bat_message[60]; + // snprintf(bat_message,60,"电池压差过大,%d号与%d号电芯，压差 %f V",cell_max_index,cell_min_index,dropout_voltage/1000.0); + result = result && update_check(buflen, buffer, fs_cells_dropout_voltage,"电池压差过大"); return result; } @@ -245,3 +253,35 @@ bool AP_BattMonitor_Backend::reset_remaining(float percentage) return true; } + +bool AP_BattMonitor_Backend::cells_dropout_voltage_checks(uint8_t &cell_min_index, uint8_t &cell_max_index, uint16_t &dropout_voltage) const +{ + if (has_cell_voltages()&&(_params._arming_dropout_voltage>0)) + { + uint16_t cell_min = 65535; + uint16_t cell_max = 0; + for (int k = 0; k < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; k++) + { + if ((_state.cell_voltages.cells[k]!=65535) && _state.cell_voltages.cells[k] != 0) //TODO if valid cell valtage =0 + { + if (_state.cell_voltages.cells[k] < cell_min) + { + cell_min = _state.cell_voltages.cells[k]; + cell_min_index = k + 1; + } + + if (_state.cell_voltages.cells[k] > cell_max) + { + cell_max = _state.cell_voltages.cells[k]; + cell_max_index = k + 1; + } + } + } + dropout_voltage = abs(cell_max - cell_min); + if ((dropout_voltage/1000.0) >= _params._arming_dropout_voltage) //0.1v + { + return true; + } + } + return false; +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index c5799c37e0..43c8bea434 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -61,6 +61,7 @@ public: // returns false if we fail arming checks, in which case the buffer will be populated with a failure message bool arming_checks(char * buffer, size_t buflen) const; + bool cells_dropout_voltage_checks(uint8_t &cell_min_index, uint8_t &cell_max_index, uint16_t& dropout_voltage)const; // reset remaining percentage to given value virtual bool reset_remaining(float percentage); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index a2384f4381..eb4756c20d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -162,6 +162,14 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @User: Advanced AP_GROUPINFO("ARM_MAH", 19, AP_BattMonitor_Params, _arming_minimum_capacity, 0), + // @Param: DROPOUT_V + // @DisplayName: Required dropout valtage + // @Description: if the each cells max dropout valtage great than or equal to this value,cant`t arm the UAV, Set to 0 to disable this function. + // @Units: V + // @Increment: 0.01 + // @User: Standard + AP_GROUPINFO("DROPOUT_V", 20, AP_BattMonitor_Params, _arming_dropout_voltage, 0), + AP_GROUPEND }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 9dc078bff4..ae2e05c75b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -56,5 +56,5 @@ public: AP_Int8 _failsafe_critical_action; /// action to preform on a critical battery failsafe AP_Int32 _arming_minimum_capacity; /// capacity level required to arm AP_Float _arming_minimum_voltage; /// voltage level required to arm - + AP_Float _arming_dropout_voltage; /// voltage level required to arm }; diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 98f14d735a..64478a334d 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1384,6 +1384,8 @@ struct PACKED log_Arm_Disarm { "MODE", "QMBB", "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \ { LOG_RFND_MSG, sizeof(log_RFND), \ "RFND", "QBCBB", "TimeUS,Instance,Dist,Stat,Orient", "s#m--", "F-B--" }, \ + { LOG_RFND2_MSG, sizeof(log_RFND), \ + "RFN2", "QBCBB", "TimeUS,Instance,Dist,Stat,Orient", "s#m--", "F-B--" }, \ { LOG_MAV_STATS, sizeof(log_MAV_Stats), \ "DMS", "IIIIIBBBBBBBBB", "TimeMS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "C-------------" }, \ { LOG_BEACON_MSG, sizeof(log_Beacon), \ @@ -1772,7 +1774,7 @@ enum LogMessages : uint8_t { LOG_ARM_DISARM_MSG, LOG_OA_BENDYRULER_MSG, LOG_OA_DIJKSTRA_MSG, - + LOG_RFND2_MSG, _LOG_LAST_MSG_ }; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index ba7021b1df..477f22b6a9 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -676,16 +676,30 @@ void RangeFinder::Log_RFND() if (s == nullptr) { continue; } - - const struct log_RFND pkt = { + if (i == 0) + { + const struct log_RFND pkt = { LOG_PACKET_HEADER_INIT(LOG_RFND_MSG), - time_us : AP_HAL::micros64(), - instance : i, - dist : s->distance_cm(), - status : (uint8_t)s->status(), - orient : s->orientation(), - }; - AP::logger().WriteBlock(&pkt, sizeof(pkt)); + time_us : AP_HAL::micros64(), + instance : i, + dist : s->distance_cm(), + status : (uint8_t)s->status(), + orient : s->orientation(), + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); + } + else + { + const struct log_RFND pkt2 = { + LOG_PACKET_HEADER_INIT(LOG_RFND2_MSG), + time_us : AP_HAL::micros64(), + instance : i, + dist : s->distance_cm(), + status : (uint8_t)s->status(), + orient : s->orientation(), + }; + AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); + } } }