You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
255 lines
9.1 KiB
255 lines
9.1 KiB
/* |
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
*/ |
|
#include "AC_ZR_App.h" |
|
|
|
|
|
AC_ZR_App *AC_ZR_App::_singleton; |
|
|
|
// table of user settable parameters |
|
const AP_Param::GroupInfo AC_ZR_App::var_info[] = { |
|
// @Param: _TYPE |
|
// @DisplayName: user type |
|
// @Description: parameters test |
|
// @Values: 0:None,1:other |
|
// @RebootRequired: True |
|
// @User: zrzk |
|
AP_GROUPINFO("_TYPE", 0, AC_ZR_App, _type, 1), |
|
|
|
AP_GROUPINFO("_SYS_TYPE", 1, AC_ZR_App, sysid_type, 0 ), |
|
AP_GROUPINFO("_SYS_ID", 2, AC_ZR_App, sysid_board_id, 98765432U), |
|
AP_GROUPINFO("_SYS_DL1", 3, AC_ZR_App, sysid_dl1, 0x8175), |
|
AP_GROUPINFO("_SYS_DL2", 4, AC_ZR_App, sysid_dl2, 0x6fda), |
|
AP_GROUPINFO("_SYS_DL3", 5, AC_ZR_App, sysid_dl3, 0xf38f), |
|
AP_GROUPINFO("_SYS_DL4", 6, AC_ZR_App, sysid_dl4, 0xbf48), |
|
|
|
// @Param: _SL_* |
|
// @DisplayName: ZR_SL_* |
|
// @Description: 降落减速设置，3个梯度 |
|
// @Values: 值单位cm和cm/s |
|
// @RebootRequired: True |
|
// @User: zrzk |
|
AP_GROUPINFO("_SL_ALT_HI", 7, AC_ZR_App, land_slow_alt_high, 3000), |
|
AP_GROUPINFO("_SL_ALT_MI", 8, AC_ZR_App, land_slow_alt_mid, 1500), |
|
AP_GROUPINFO("_SL_ALT_LO", 9, AC_ZR_App, land_slow_alt_low, 150), |
|
AP_GROUPINFO("_SL_SPD_HI", 10, AC_ZR_App, land_slow_speed_dn_high, 150), |
|
AP_GROUPINFO("_SL_SPD_MI", 11, AC_ZR_App, land_slow_speed_dn_mid, 50), |
|
AP_GROUPINFO("_SL_SPD_LO", 12, AC_ZR_App, land_slow_speed_dn_low, 30), |
|
|
|
AP_GROUPEND |
|
}; |
|
|
|
// constructor |
|
AC_ZR_App::AC_ZR_App() |
|
{ |
|
|
|
AP_Param::setup_object_defaults(this, var_info); |
|
|
|
if (_singleton != nullptr) { |
|
AP_HAL::panic("AC_ZR_App must be singleton"); |
|
} |
|
_singleton = this; |
|
} |
|
|
|
/** |
|
* @brief 从参数中解析注册到期时间 |
|
* |
|
* @param deadline 返回到期时间 |
|
*/ |
|
void AC_ZR_App::get_deadline_params(uint32_t &deadline) |
|
{ |
|
uint8_t tempbuf[8]; |
|
tempbuf[1] = sysid_dl1 & 0xff; |
|
tempbuf[0] = (sysid_dl1 & 0xff00) >> 8; |
|
// gcs().send_text(MAV_SEVERITY_INFO, "tempbuf0: %d ,tempbuf1:%d",tempbuf[0] ,tempbuf[1]); |
|
|
|
tempbuf[3] = sysid_dl2 & 0xff; |
|
tempbuf[2] = (sysid_dl2 & 0xff00) >> 8; |
|
//gcs().send_text(MAV_SEVERITY_INFO, "tempbuf2: %d ,tempbuf3:%d",tempbuf[2] ,tempbuf[3]); |
|
|
|
tempbuf[5] = sysid_dl3 & 0xff; |
|
tempbuf[4] = (sysid_dl3 & 0xff00) >> 8; |
|
//gcs().send_text(MAV_SEVERITY_INFO, "tempbuf4: %d ,tempbuf5:%d",tempbuf[4] ,tempbuf[5]); |
|
|
|
tempbuf[7] = sysid_dl4 & 0xff; |
|
tempbuf[6] = (sysid_dl4 & 0xff00) >> 8; |
|
//gcs().send_text(MAV_SEVERITY_INFO, "tempbuf6: %d ,tempbuf7:%d",tempbuf[6] ,tempbuf[7]); |
|
|
|
uint8_t out_data_temp[8]; |
|
|
|
Des des; |
|
uint8_t deskey_zr[8]= {90, 114, 90, 107, 85, 97, 86, 103}; //{ "ZrZkUaVg" }; |
|
uint8_t deskey_zhd[8]= {90, 114, 90, 104, 68, 85, 97, 86}; //{ "ZrZhDUaV" }; |
|
|
|
int8_t type = (int)ZR_FRAME_TYPE; |
|
|
|
if(type == 3 ){ // 中海达加密模式 |
|
des.des(&tempbuf[0], &deskey_zhd[0], 1, &out_data_temp[0]); |
|
}else{ |
|
des.des(&tempbuf[0], &deskey_zr[0], 1, &out_data_temp[0]); |
|
} |
|
|
|
// des.des(&tempbuf[0], &des.deskey[0], 1, &out_data_temp[0]); |
|
//gcs().send_text(MAV_SEVERITY_INFO, "out_data_temp6: %d ,out_data_temp7:%d",out_data_temp[6] ,out_data_temp[7]); |
|
if (out_data_temp[6] == 122 && out_data_temp[7] == 114) //'zr' |
|
{ |
|
deadline = 20000000 + ((out_data_temp[0] - 0x30) * 10 + (out_data_temp[1] - 0x30)) * 10000 + |
|
((out_data_temp[2] - 0x30) * 10 + (out_data_temp[3] - 0x30)) * 100 + |
|
((out_data_temp[4] - 0x30) * 10 + (out_data_temp[5] - 0x30)); |
|
} |
|
else |
|
{ |
|
deadline = 20200101; |
|
gcs().send_text(MAV_SEVERITY_INFO, "D_Error: The registration code is wrong!"); |
|
} |
|
} |
|
/** |
|
* @brief 检查是否注册时间是否正常 |
|
* |
|
* @param date 返回到期时间 |
|
* @return true 没到期 |
|
* @return false 到期 |
|
*/ |
|
bool AC_ZR_App::check_deadline(uint32_t &date) |
|
{ |
|
uint32_t deadline =0; |
|
const AP_GPS &gps = AP::gps(); |
|
if(gps.status() < 2){ // 室内测试没卫星的情况，允许解锁测试 |
|
return true; |
|
} |
|
|
|
get_deadline_params(deadline); |
|
date = deadline; |
|
uint32_t gps_date =0; |
|
if((gps.time_week() ==0)||(gps.time_week_ms() ==0)){ |
|
return false; |
|
} |
|
uint64_t timestamp = gps.istate_time_to_epoch_ms(gps.time_week(),gps.time_week_ms())/1000; |
|
//tick -timestamp - uint:s |
|
time_t tick = (time_t)timestamp; |
|
tm gpstime = *localtime(&tick); |
|
uint16_t year = gpstime.tm_year+1900; |
|
uint8_t month = gpstime.tm_mon+1; |
|
uint8_t day = gpstime.tm_mday; |
|
gps_date = year*10000+month*100+day; |
|
if (deadline>= gps_date) |
|
{ |
|
return true; |
|
} |
|
else |
|
{ |
|
return false; |
|
} |
|
} |
|
|
|
uint32_t AC_ZR_App::get_zr_sysid() |
|
{ |
|
return (uint32_t)sysid_board_id; |
|
} |
|
|
|
/** |
|
* @brief 降落根据高度梯度减速 |
|
* |
|
* @param alt_cm 当前飞机高度 |
|
* @param sys_speed_dn 系统设置的降落速度 |
|
* @return uint16_t 减速后速度 |
|
*/ |
|
uint16_t AC_ZR_App::get_land_deceleration(int32_t alt_cm,int16_t sys_speed_dn) |
|
{ |
|
int16_t speed_dn_now = sys_speed_dn; // 没到减速阶段则使用系统降落速度 |
|
|
|
if(alt_cm < land_slow_alt_low){ // 最低处减速 |
|
speed_dn_now = land_slow_speed_dn_low; |
|
} |
|
else if(alt_cm < land_slow_alt_mid){ // 中高处减速 |
|
speed_dn_now = land_slow_speed_dn_mid; |
|
} |
|
else if(alt_cm < land_slow_alt_high){ // 高处减速 |
|
speed_dn_now = land_slow_speed_dn_high; |
|
} |
|
|
|
return abs(speed_dn_now); |
|
} |
|
|
|
|
|
|
|
uint8_t AC_ZR_App::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; |
|
|
|
if (fabs(a[cacl_volt_pst] - value)<0.01) |
|
//if (abs(a[mid] - value) < 30) |
|
return cacl_volt_pst; |
|
else if (a[cacl_volt_pst] > value) |
|
return BinarySearch2f(a, value, low, cacl_volt_pst - 1); |
|
else |
|
return BinarySearch2f(a, value, cacl_volt_pst + 1, high); |
|
} |
|
|
|
|
|
uint8_t AC_ZR_App::get_battery_capacity(uint8_t type,float volt) |
|
{ |
|
uint8_t bat_cnt = 100; |
|
// float cycle_pst = (1 - bat_cycled * 0.0002); |
|
switch (type) |
|
{ |
|
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,volt,0,100); |
|
// 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,volt,0,100); |
|
// gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst); |
|
break; |
|
|
|
default: |
|
break; |
|
} |
|
return bat_cnt; |
|
} |