From 9a35132e95bd605e12920251d1e3f6e6bc75547f Mon Sep 17 00:00:00 2001 From: zbr Date: Sat, 2 Oct 2021 16:46:50 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=AE=B0=E5=BD=95=E7=94=B5?= =?UTF-8?q?=E6=B1=A0=E5=BE=AA=E7=8E=AF=E7=9A=84=E5=8F=82=E6=95=B0,?= =?UTF-8?q?=E7=94=B5=E6=B1=A0=E6=8D=9F=E8=80=97=E8=AE=A1=E7=AE=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Copter.h | 2 +- ArduCopter/Parameters.cpp | 26 ++++++- ArduCopter/Parameters.h | 6 +- ArduCopter/zr_flight.cpp | 109 ++++++++++++++------------- libraries/GCS_MAVLink/GCS_Common.cpp | 9 ++- modules/mavlink | 2 +- 6 files changed, 94 insertions(+), 60 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 4adac592ac..2ef3e573af 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -1057,11 +1057,11 @@ public: uint8_t BinarySearch2f(float a[], float value, int low, int high); int8_t in_debug_mode; - uint8_t cacl_volt_pst; uint8_t mkdir_step; Mode::Number get_before_mode() const { return before_mode; }; bool far_from_home; Proxy_Action avoid_action_step; + void zr_set_battery_capacity(); }; extern Copter copter; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 5cb46e8579..79a80c9b08 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -146,7 +146,7 @@ const AP_Param::Info Copter::var_info[] = { GSCALAR(zr_use_rc, "ZR_USE_RC", 1), GSCALAR(zr_reg_date, "ZR_REG_DATE", 1), GSCALAR(zr_avd_wait, "ZR_AVD_WAIT", 60), // 避障无法绕过，等待人为控制延时,单位s - + GSCALAR(zr_bat_cycled, "ZR_BAT_CYCL", 1), // 电池循环数 // @Param: ZR_RTL_DELAY // @DisplayName: rtl Altitude when at final decent alt // @Description: @@ -1722,12 +1722,30 @@ const char* Copter::get_sysid_board_id(void) int32_t deadline = 0; get_deadline_params(deadline); - gcs().send_text(MAV_SEVERITY_INFO, "date:%ld",deadline); - if(g.zr_reg_date != deadline){ - gcs().send_text(MAV_SEVERITY_INFO, "reload date:%ld -> %ld",(int32_t)g.zr_reg_date,deadline); + gcs().send_text(MAV_SEVERITY_INFO, "Date:%ld",deadline); + if((int32_t)g.zr_reg_date != deadline){ + // gcs().send_text(MAV_SEVERITY_INFO, "reload date:%ld->%ld",(int32_t)g.zr_reg_date,deadline); g.zr_reg_date.set_and_save_ifchanged(deadline); } + enum ap_var_type ptype; + AP_Int16 *bat_parm = (AP_Int16 *)AP_Param::find("ZR_BAT_CYCL", &ptype); + if (bat_parm != nullptr) + { + if (bat_parm->configured_in_storage()) + { + if (ptype == AP_PARAM_INT16) + { + gcs().send_text(MAV_SEVERITY_INFO, "Battery cycle: %d", (int16_t)g.zr_bat_cycled); + } + } + else + { + gcs().send_text(MAV_SEVERITY_INFO, "Battery not configured"); + AP_Param::set_and_save_by_name("ZR_BAT_CYCL", 1); + } + } + return buf; } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 88fbbc2768..4cdce02128 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -397,6 +397,8 @@ public: k_param_zr_reg_date, k_param_zr_avd_wait, k_param_zr_8_circle, + k_param_zr_bat_cycled, + // the k_param_* space is 9-bits in size // 511: reserved }; @@ -505,7 +507,7 @@ public: AP_Float acro_yaw_p; AP_Float acro_balance_roll; AP_Float acro_balance_pitch; - AP_Int8 acro_trainer; + AP_Int8 acro_trainer; AP_Float acro_rp_expo; AP_Int32 zr_tk_req_alt; AP_Int16 zr_tk_delay; @@ -514,6 +516,8 @@ public: AP_Int32 zr_reg_date; AP_Int16 zr_avd_wait; AP_Int8 zr_8_circle; + AP_Int16 zr_bat_cycled; + // Note: keep initializers here in the same order as they are declared // above. Parameters() diff --git a/ArduCopter/zr_flight.cpp b/ArduCopter/zr_flight.cpp index db500c7af5..dba940d9eb 100644 --- a/ArduCopter/zr_flight.cpp +++ b/ArduCopter/zr_flight.cpp @@ -49,7 +49,7 @@ void Copter::zr_SlowLoop(){ uint8_t Copter::BinarySearch2f(float a[], float value, int low, int high) { - + static uint8_t cacl_volt_pst = 0; if (low > high) return cacl_volt_pst; cacl_volt_pst = (low + high) / 2; @@ -65,34 +65,6 @@ uint8_t Copter::BinarySearch2f(float a[], float value, int low, int high) void Copter::zr_SuperSlowLoop(){ static bool before_fly = true; - static float batt_mah_teb[] = - { - 16.80, 17.40, 18.00, 18.51, 18.80, 19.00, 19.19, 19.34, 19.53, 19.73, - 19.88, 20.04, 20.16, 20.29, 20.47, 20.56, 20.63, 20.69, 20.75, 20.80, - 20.85, 20.90, 20.96, 21.02, 21.07, 21.13, 21.18, 21.24, 21.29, 21.34, - 21.40, 21.45, 21.49, 21.54, 21.59, 21.64, 21.69, 21.72, 21.76, 21.79, - 21.84, 21.88, 21.92, 21.96, 22.01, 22.06, 22.11, 22.16, 22.21, 22.26, - 22.31, 22.37, 22.43, 22.48, 22.54, 22.59, 22.65, 22.70, 22.76, 22.81, - 22.86, 22.91, 22.95, 23.01, 23.07, 23.13, 23.19, 23.24, 23.30, 23.36, - 23.41, 23.46, 23.51, 23.58, 23.65, 23.71, 23.79, 23.88, 23.94, 24.00, - 24.07, 24.14, 24.21, 24.26, 24.32, 24.36, 24.40, 24.43, 24.45, 24.48, - 24.50, 24.52, 24.54, 24.57, 24.60, 24.64, 24.69, 24.76, 24.84, 24.90, - 25.20 - }; - static float batt_mah_teb_7s[] = - { - 21.60, 21.80, 22.04, 22.28, 22.52, 22.74, 22.95, 23.19, 23.32, 23.52, - 23.65, 23.81, 23.94, 24.02, 24.10, 24.18, 24.22, 24.25, 24.30, 24.39, - 24.46, 24.52, 24.60, 24.67, 24.74, 24.80, 24.86, 24.93, 24.97, 25.03, - 25.08, 25.13, 25.18, 25.23, 25.27, 25.32, 25.36, 25.41, 25.45, 25.50, - 25.54, 25.58, 25.63, 25.69, 25.74, 25.79, 25.85, 25.91, 25.98, 26.05, - 26.10, 26.16, 26.21, 26.28, 26.35, 26.40, 26.47, 26.54, 26.61, 26.67, - 26.74, 26.79, 26.85, 26.93, 27.00, 27.07, 27.13, 27.19, 27.26, 27.32, - 27.38, 27.46, 27.52, 27.59, 27.66, 27.75, 27.80, 27.87, 27.94, 28.03, - 28.10, 28.16, 28.21, 28.28, 28.33, 28.38, 28.43, 28.46, 28.50, 28.53, - 28.56, 28.59, 28.61, 28.64, 28.67, 28.71, 28.77, 28.86, 28.97, 29.13, - 29.30 - }; if(motors->armed()){ if(before_fly){ @@ -111,28 +83,63 @@ void Copter::zr_SuperSlowLoop(){ updownStatus =UpDown_TakeOffStart; } if(before_fly){ - cacl_volt_pst = 0; - uint8_t bat_cnt; - float now_volt = battery.voltage(); - switch (battery.get_batt_type(0)) - { - case 0: - /* code */ - break; - case 1: - bat_cnt = BinarySearch2f(batt_mah_teb,now_volt,0,100); - battery.reset_remaining(1,bat_cnt); - // gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst); - break; - case 2: - bat_cnt = BinarySearch2f(batt_mah_teb_7s,now_volt,0,100); - battery.reset_remaining(1,bat_cnt); - // gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst); - break; - - default: - break; - } + zr_set_battery_capacity(); + } +} + +void Copter::zr_set_battery_capacity() +{ + uint8_t bat_cnt; + float now_volt = battery.voltage(); + int16_t bat_cycled = (int16_t)g.zr_bat_cycled; + float cycle_pst = (1 - bat_cycled * 0.0002); + switch (battery.get_batt_type(0)) + { + case 0: + /* code */ + break; + case 1: + + static float batt_mah_teb[] = + { + 16.80, 17.40, 18.00, 18.51, 18.80, 19.00, 19.19, 19.34, 19.53, 19.73, + 19.88, 20.04, 20.16, 20.29, 20.47, 20.56, 20.63, 20.69, 20.75, 20.80, + 20.85, 20.90, 20.96, 21.02, 21.07, 21.13, 21.18, 21.24, 21.29, 21.34, + 21.40, 21.45, 21.49, 21.54, 21.59, 21.64, 21.69, 21.72, 21.76, 21.79, + 21.84, 21.88, 21.92, 21.96, 22.01, 22.06, 22.11, 22.16, 22.21, 22.26, + 22.31, 22.37, 22.43, 22.48, 22.54, 22.59, 22.65, 22.70, 22.76, 22.81, + 22.86, 22.91, 22.95, 23.01, 23.07, 23.13, 23.19, 23.24, 23.30, 23.36, + 23.41, 23.46, 23.51, 23.58, 23.65, 23.71, 23.79, 23.88, 23.94, 24.00, + 24.07, 24.14, 24.21, 24.26, 24.32, 24.36, 24.40, 24.43, 24.45, 24.48, + 24.50, 24.52, 24.54, 24.57, 24.60, 24.64, 24.69, 24.76, 24.84, 24.90, + 25.20 + }; + bat_cnt = BinarySearch2f(batt_mah_teb,now_volt,0,100); + battery.reset_remaining(1,bat_cnt * cycle_pst); + // gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst); + break; + case 2: + static float batt_mah_teb_7s[] = + { + 21.60, 21.80, 22.04, 22.28, 22.52, 22.74, 22.95, 23.19, 23.32, 23.52, + 23.65, 23.81, 23.94, 24.02, 24.10, 24.18, 24.22, 24.25, 24.30, 24.39, + 24.46, 24.52, 24.60, 24.67, 24.74, 24.80, 24.86, 24.93, 24.97, 25.03, + 25.08, 25.13, 25.18, 25.23, 25.27, 25.32, 25.36, 25.41, 25.45, 25.50, + 25.54, 25.58, 25.63, 25.69, 25.74, 25.79, 25.85, 25.91, 25.98, 26.05, + 26.10, 26.16, 26.21, 26.28, 26.35, 26.40, 26.47, 26.54, 26.61, 26.67, + 26.74, 26.79, 26.85, 26.93, 27.00, 27.07, 27.13, 27.19, 27.26, 27.32, + 27.38, 27.46, 27.52, 27.59, 27.66, 27.75, 27.80, 27.87, 27.94, 28.03, + 28.10, 28.16, 28.21, 28.28, 28.33, 28.38, 28.43, 28.46, 28.50, 28.53, + 28.56, 28.59, 28.61, 28.64, 28.67, 28.71, 28.77, 28.86, 28.97, 29.13, + 29.30 + }; + bat_cnt = BinarySearch2f(batt_mah_teb_7s,now_volt,0,100); + battery.reset_remaining(1,bat_cnt * cycle_pst); + // gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst); + break; + + default: + break; } } diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 65455e8cdb..cf80c21df1 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1924,8 +1924,13 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_ { for (uint8_t i=0; isend_battgo_history(battgo_history_t); - } + chan(i)->send_battgo_history(battgo_history_t); + } + enum ap_var_type ptype; + AP_Int16 *bat_parm = (AP_Int16 *)AP_Param::find("ZR_BAT_CYCL", &ptype); + if (bat_parm != nullptr && bat_parm->configured_in_storage() && ptype == AP_PARAM_INT16) { + bat_parm->set_and_save_ifchanged(battgo_history_t->batt_cycled_time); + } } void GCS::send_mission_item_reached_message(uint16_t mission_index) diff --git a/modules/mavlink b/modules/mavlink index 6d62818f97..5fc095d170 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 6d62818f975c25ffc507904658d8510d3de53c80 +Subproject commit 5fc095d17043557589ecc6a28d62fb234081fc87