From b47d4acd3106469db2cfddde27992185c01f92db Mon Sep 17 00:00:00 2001 From: yaozb Date: Sat, 8 Aug 2020 16:19:07 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E8=B5=B7=E9=A3=9E?= =?UTF-8?q?=E6=82=AC=E5=81=9C=E9=AB=98=E5=BA=A6=E6=9D=A5=E6=BA=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/mode_auto.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index b91f90ce0b..910965fb93 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -764,11 +764,9 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) 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.zr_tk_req_alt > 0 && alt_rev >= g.zr_tk_req_alt + { + if (g.zr_tk_req_alt > 0 && copter.current_loc.alt >= g.zr_tk_req_alt + && copter.updownStatus < UpDown_RequestClimb) { copter.updownStatus = UpDown_RequestClimb; @@ -785,7 +783,8 @@ void ModeAuto::takeoff_run() } set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD); } - else if (g.zr_tk_req_alt > 0 && alt_rev < g.zr_tk_req_alt && alt_rev > 0 + else if (g.zr_tk_req_alt > 0 && copter.current_loc.alt < g.zr_tk_req_alt + && copter.current_loc.alt > 0 &&(AP_HAL::millis()-last_time_send_updownload>500)) { last_time_send_updownload = AP_HAL::millis(); @@ -795,8 +794,8 @@ void ModeAuto::takeoff_run() zr_flying_status_t.countdown = 255; gcs().update_zr_fly_status(&zr_flying_status_t); } - } } + auto_takeoff_run(); } From 9d77fe9e908288ca0a0ff9e951a1bc80bf45bc96 Mon Sep 17 00:00:00 2001 From: yaozb Date: Sat, 8 Aug 2020 18:13:07 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E7=94=B5=E6=B1=A0=E5=8E=8B=E5=B7=AE?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E7=94=B5=E8=8A=AF=E6=95=B0=E7=9B=AE=E5=8F=82?= =?UTF-8?q?=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp | 5 +++-- libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp | 2 ++ libraries/AP_BattMonitor/AP_BattMonitor_Params.h | 1 + 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 1dbb30151e..31d4e2368c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -268,9 +268,10 @@ bool AP_BattMonitor_Backend::cells_dropout_voltage_checks(uint8_t &cell_min_inde { uint16_t cell_min = 65535; uint16_t cell_max = 0; - for (int k = 0; k < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; k++) + + for (int k = 0; k <_params._batt_cells_amount ; k++)//MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN { - if ((_state.cell_voltages.cells[k]!=65535)) //TODO if valid cell valtage =0 + if (_state.cell_voltages.cells[k]!=65535) //TODO if valid cell valtage =0 { if (_state.cell_voltages.cells[k] < cell_min) { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index eb4756c20d..5ae21589b0 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -169,6 +169,8 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Increment: 0.01 // @User: Standard AP_GROUPINFO("DROPOUT_V", 20, AP_BattMonitor_Params, _arming_dropout_voltage, 0), + + AP_GROUPINFO("CELL_TOTAL", 21, AP_BattMonitor_Params, _batt_cells_amount, 0), AP_GROUPEND diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index ae2e05c75b..18cca08943 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -57,4 +57,5 @@ public: 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 + AP_Int8 _batt_cells_amount; /// voltage level required to arm };