Compare commits
8 Commits
| Author | SHA1 | Date |
|---|---|---|
 Brown.Z
|
5090a31c83 | 2 years ago |
|
Brown.Z
|
ec7d59334e | 3 years ago |
|
Brown.Z
|
18df2c6d2c | 3 years ago |
|
Brown.Z
|
80e93f3f71 | 3 years ago |
|
Brown.Z
|
050d98eb0e | 3 years ago |
|
Brown.Z
|
16b69e95b1 | 3 years ago |
|
zbr3550
|
c96c4654d5 | 3 years ago |
|
zbr3550
|
309fb6f12b | 3 years ago |
41 changed files with 1420 additions and 38 deletions
@ -0,0 +1,31 @@
@@ -0,0 +1,31 @@
|
||||
#include "Copter.h" |
||||
|
||||
|
||||
void Copter::zr_app_1hz() |
||||
{ |
||||
static bool before_fly = true; |
||||
if(motors->armed()){ |
||||
if(before_fly){ |
||||
before_fly = false; |
||||
} |
||||
relay.on(3); // 电子开关，保持通路
|
||||
// camera.set_in_arm_mode(true);
|
||||
}else{ |
||||
relay.off(3); |
||||
if(before_fly){ |
||||
uint8_t bat_cnt = zr_app.get_battery_capacity(1,battery.voltage()); |
||||
battery.reset_remaining(0,(float)bat_cnt ); |
||||
// gcs().send_text(MAV_SEVERITY_INFO, "bat:%.2f,pst%d,%.2f",battery.voltage(),bat_cnt,(float)bat_cnt);
|
||||
} |
||||
} |
||||
|
||||
} |
||||
|
||||
void Copter::zr_app_10hz() |
||||
{ |
||||
|
||||
|
||||
} |
||||
void Copter::zr_app_50hz(){ |
||||
|
||||
} |
||||
@ -0,0 +1,3 @@
@@ -0,0 +1,3 @@
|
||||
./waf configure --board Pixhawk4 |
||||
./waf copter |
||||
cp ./build/Pixhawk4/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/编译/pixhakw4-master.px4 |
||||
@ -0,0 +1,3 @@
@@ -0,0 +1,3 @@
|
||||
./waf configure --board cubeblack |
||||
./waf copter |
||||
cp ./build/CubeBlack/bin/arducopter.apj /mnt/e/_00-inbox/_temp/cubeblack.px4 |
||||
@ -0,0 +1,2 @@
@@ -0,0 +1,2 @@
|
||||
./waf copter |
||||
cp build/Pixhawk4/bin/arducopter.apj /mnt/hgfs/firmware/编译/c415.px4 |
||||
@ -0,0 +1,255 @@
@@ -0,0 +1,255 @@
|
||||
/*
|
||||
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; |
||||
} |
||||
@ -0,0 +1,70 @@
@@ -0,0 +1,70 @@
|
||||
/*
|
||||
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/>.
|
||||
*/ |
||||
#pragma once |
||||
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
#include <AP_Param/AP_Param.h> |
||||
#include <GCS_MAVLink/GCS.h> |
||||
#include <GCS_MAVLink/GCS_MAVLink.h> |
||||
|
||||
#include <sys/time.h> |
||||
#include <time.h> |
||||
#include "Des.h" |
||||
#ifndef ZR_FRAME_TYPE |
||||
#define ZR_FRAME_TYPE 0 |
||||
#endif |
||||
class AC_ZR_App |
||||
{ |
||||
public: |
||||
AC_ZR_App(); |
||||
/* Do not allow copies */ |
||||
AC_ZR_App(const AC_ZR_App &other) = delete; |
||||
AC_ZR_App &operator=(const AC_ZR_App&) = delete; |
||||
|
||||
static AC_ZR_App *get_singleton() { |
||||
return _singleton; |
||||
} |
||||
|
||||
void get_deadline_params(uint32_t &deadline); |
||||
bool check_deadline(uint32_t &date); |
||||
uint32_t get_zr_sysid(); |
||||
|
||||
uint16_t get_land_deceleration(int32_t alt_cm,int16_t sys_speed_dn); |
||||
uint8_t get_battery_capacity(uint8_t type,float volt); |
||||
|
||||
static const struct AP_Param::GroupInfo var_info[]; |
||||
|
||||
protected: |
||||
|
||||
AP_Int8 _type; |
||||
AP_Int8 sysid_type; // modify by @Brown
|
||||
AP_Int32 sysid_board_id; |
||||
AP_Int32 sysid_dl1; |
||||
AP_Int32 sysid_dl2; |
||||
AP_Int32 sysid_dl3; |
||||
AP_Int32 sysid_dl4; |
||||
|
||||
AP_Int16 land_slow_alt_high; |
||||
AP_Int16 land_slow_alt_mid; |
||||
AP_Int16 land_slow_alt_low; |
||||
AP_Int16 land_slow_speed_dn_high; |
||||
AP_Int16 land_slow_speed_dn_mid; |
||||
AP_Int16 land_slow_speed_dn_low; |
||||
|
||||
private: |
||||
static AC_ZR_App *_singleton; |
||||
uint8_t BinarySearch2f(float a[], float value, int low, int high); |
||||
|
||||
}; |
||||
@ -0,0 +1,328 @@
@@ -0,0 +1,328 @@
|
||||
|
||||
#include "Des.h" |
||||
|
||||
const uint8_t initial_tr[64] = |
||||
{ |
||||
57, 49, 41, 33, 25, 17, 9, 1, |
||||
59, 51, 43, 35, 27, 19, 11, 3, |
||||
61, 53, 45, 37, 29, 21, 13, 5, |
||||
63, 55, 47, 39, 31, 23, 15, 7, |
||||
56, 48, 40, 32, 24, 16, 8, 0, |
||||
58, 50, 42, 34, 26, 18, 10, 2, |
||||
60, 52, 44, 36, 28, 20, 12, 4, |
||||
62, 54, 46, 38, 30, 22, 14, 6}; |
||||
const uint8_t final_tr[64] = |
||||
{ |
||||
39, 7, 47, 15, 55, 23, 63, 31, |
||||
38, 6, 46, 14, 54, 22, 62, 30, |
||||
37, 5, 45, 13, 53, 21, 61, 29, |
||||
36, 4, 44, 12, 52, 20, 60, 28, |
||||
35, 3, 43, 11, 51, 19, 59, 27, |
||||
34, 2, 42, 10, 50, 18, 58, 26, |
||||
33, 1, 41, 9, 49, 17, 57, 25, |
||||
32, 0, 40, 8, 48, 16, 56, 24}; |
||||
const uint8_t swap[64] = |
||||
{ |
||||
33, 34, 35, 36, 37, 38, 39, 40, |
||||
41, 42, 43, 44, 45, 46, 47, 48, |
||||
49, 50, 51, 52, 53, 54, 55, 56, |
||||
57, 58, 59, 60, 61, 62, 63, 64, |
||||
1, 2, 3, 4, 5, 6, 7, 8, |
||||
9, 10, 11, 12, 13, 14, 15, 16, |
||||
17, 18, 19, 20, 21, 22, 23, 24, |
||||
25, 26, 27, 28, 29, 30, 31, 32}; |
||||
const uint8_t key_tr1[56] = |
||||
{ |
||||
56, 48, 40, 32, 24, 16, 8, |
||||
0, 57, 49, 41, 33, 25, 17, |
||||
9, 1, 58, 50, 42, 34, 26, |
||||
18, 10, 2, 59, 51, 43, 35, |
||||
62, 54, 46, 38, 30, 22, 14, |
||||
6, 61, 53, 45, 37, 29, 21, |
||||
13, 5, 60, 52, 44, 36, 28, |
||||
20, 12, 4, 27, 19, 11, 3}; |
||||
const uint8_t key_tr2[64] = |
||||
{ |
||||
0, 0, 13, 4, 16, 10, 23, 0, |
||||
0, 0, 2, 9, 27, 14, 5, 20, |
||||
0, 0, 22, 7, 18, 11, 3, 25, |
||||
0, 0, 15, 1, 6, 26, 19, 12, |
||||
0, 0, 40, 54, 51, 30, 36, 46, |
||||
0, 0, 29, 47, 39, 50, 44, 32, |
||||
0, 0, 43, 52, 48, 38, 55, 33, |
||||
0, 0, 45, 31, 41, 49, 35, 28}; |
||||
const uint8_t etr[64] = |
||||
{ |
||||
0, 0, 31, 4, 0, 1, 2, 3, |
||||
0, 0, 3, 8, 4, 5, 6, 7, |
||||
0, 0, 7, 12, 8, 9, 10, 11, |
||||
0, 0, 11, 16, 12, 13, 14, 15, |
||||
0, 0, 15, 20, 16, 17, 18, 19, |
||||
0, 0, 19, 24, 20, 21, 22, 23, |
||||
0, 0, 23, 28, 24, 25, 26, 27, |
||||
0, 0, 27, 0, 28, 29, 30, 31}; |
||||
const uint8_t ptr[32] = |
||||
{ |
||||
31, 14, 39, 44, 60, 23, 55, 36, |
||||
4, 30, 46, 53, 12, 37, 62, 21, |
||||
5, 15, 47, 29, 63, 54, 6, 20, |
||||
38, 28, 61, 13, 45, 22, 7, 52}; |
||||
const uint8_t s[8][64] = |
||||
{ |
||||
{14, 4, 13, 1, 2, 15, 11, 8, 3, 10, 6, 12, 5, 9, 0, 7, |
||||
0, 15, 7, 4, 14, 2, 13, 1, 10, 6, 12, 11, 9, 5, 3, 8, |
||||
4, 1, 14, 8, 13, 6, 2, 11, 15, 12, 9, 7, 3, 10, 5, 0, |
||||
15, 12, 8, 2, 4, 9, 1, 7, 5, 11, 3, 14, 10, 0, 6, 13}, |
||||
{15, 1, 8, 14, 6, 11, 3, 4, 9, 7, 2, 13, 12, 0, 5, 10, |
||||
3, 13, 4, 7, 15, 2, 8, 14, 12, 0, 1, 10, 6, 9, 11, 5, |
||||
0, 14, 7, 11, 10, 4, 13, 1, 5, 8, 12, 6, 9, 3, 2, 15, |
||||
13, 8, 10, 1, 3, 15, 4, 2, 11, 6, 7, 12, 0, 5, 14, 9}, |
||||
{10, 0, 9, 14, 6, 3, 15, 5, 1, 13, 12, 7, 11, 4, 2, 8, |
||||
13, 7, 0, 9, 3, 4, 6, 10, 2, 8, 5, 14, 12, 11, 15, 1, |
||||
13, 6, 4, 9, 8, 15, 3, 0, 11, 1, 2, 12, 5, 10, 14, 7, |
||||
1, 10, 13, 0, 6, 9, 8, 7, 4, 15, 14, 3, 11, 5, 2, 12}, |
||||
{7, 13, 14, 3, 0, 6, 9, 10, 1, 2, 8, 5, 11, 12, 4, 15, |
||||
13, 8, 11, 5, 6, 15, 0, 3, 4, 7, 2, 12, 1, 10, 14, 9, |
||||
10, 6, 9, 0, 12, 11, 7, 13, 15, 1, 3, 14, 5, 2, 8, 4, |
||||
3, 15, 0, 6, 10, 1, 13, 8, 9, 4, 5, 11, 12, 7, 2, 14}, |
||||
{2, 12, 4, 1, 7, 10, 11, 6, 8, 5, 3, 15, 13, 0, 14, 9, |
||||
14, 11, 2, 12, 4, 7, 13, 1, 5, 0, 15, 10, 3, 9, 8, 6, |
||||
4, 2, 1, 11, 10, 13, 7, 8, 15, 9, 12, 5, 6, 3, 0, 14, |
||||
11, 8, 12, 7, 1, 14, 2, 13, 6, 15, 0, 9, 10, 4, 5, 3}, |
||||
{12, 1, 10, 15, 9, 2, 6, 8, 0, 13, 3, 4, 14, 7, 5, 11, |
||||
10, 15, 4, 2, 7, 12, 9, 5, 6, 1, 13, 14, 0, 11, 3, 8, |
||||
9, 14, 15, 5, 2, 8, 12, 3, 7, 0, 4, 10, 1, 13, 11, 6, |
||||
4, 3, 2, 12, 9, 5, 15, 10, 11, 14, 1, 7, 6, 0, 8, 13}, |
||||
{4, 11, 2, 14, 15, 0, 8, 13, 3, 12, 9, 7, 5, 10, 6, 1, |
||||
13, 0, 11, 7, 4, 9, 1, 10, 14, 3, 5, 12, 2, 15, 8, 6, |
||||
1, 4, 11, 13, 12, 3, 7, 14, 10, 15, 6, 8, 0, 5, 9, 2, |
||||
6, 11, 13, 8, 1, 4, 10, 7, 9, 5, 0, 15, 14, 2, 3, 12}, |
||||
{13, 2, 8, 4, 6, 15, 11, 1, 10, 9, 3, 14, 5, 0, 12, 7, |
||||
1, 15, 13, 8, 10, 3, 7, 4, 12, 5, 6, 11, 0, 14, 9, 2, |
||||
7, 11, 4, 1, 9, 12, 14, 2, 0, 6, 10, 13, 15, 3, 5, 8, |
||||
2, 1, 14, 7, 4, 10, 8, 13, 15, 12, 9, 0, 3, 5, 6, 11}}; |
||||
const uint8_t rots[16] = |
||||
{ |
||||
1, 1, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 2, 2, 1}; |
||||
const uint8_t bit_msk[8] = |
||||
{ |
||||
0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01}; |
||||
|
||||
/************************************************************************/ |
||||
/* */ |
||||
/* Module title: des */ |
||||
/* Module type: des mainrutine */ |
||||
/* */ |
||||
/* Author: YXH */ |
||||
/* Date: 2012-07-13 */ |
||||
/* */ |
||||
/* Last changed by: YXH */ |
||||
/* Date: 2012-07-13 */ |
||||
/* */ |
||||
/* Functional Description: Encipher and decipher 64 bits string */ |
||||
/* according to a 64 bits key string */ |
||||
/* The string format is shown below */ |
||||
/* */ |
||||
/* input parameter 1: pointer to 64 bits input string */ |
||||
/* 2: pointer to 64 bits key string */ |
||||
/* 3: boolean if false indicating enciphering */ |
||||
/* if true dechiphering 1解密 */ |
||||
/* 4: pointer to a 64 bits output string */ |
||||
/************************************************************************/ |
||||
/* */ |
||||
/* msb lsb */ |
||||
/* bit bit */ |
||||
/* +-- -- -- -- -- -- -- --+ */ |
||||
/* addr !1st 8th! */ |
||||
/* +-- -- -- -- -- -- -- --+ */ |
||||
/* addr+1 !9th 16th! */ |
||||
/* +-- -- -- -- -- -- -- --+ */ |
||||
/* : : */ |
||||
/* : : */ |
||||
/* +-- -- -- -- -- -- -- --+ */ |
||||
/* addr+7 !57th 64th! */ |
||||
/* +-- -- -- -- -- -- -- --+ */ |
||||
/* */ |
||||
/************************************************************************/ |
||||
void Des::des(uint8_t* plain_strng, uint8_t* key, uint8_t d, uint8_t* ciph_strng) |
||||
{ |
||||
uint8_t a_str[8], b_str[8], x_str[8]; |
||||
uint8_t i, j, * pkey, temp; |
||||
for (i = 0; i < 8; ++i) |
||||
{ |
||||
if (key[i] != main_key[i]) |
||||
{ |
||||
compute_subkeys(key); |
||||
i = 7; |
||||
} |
||||
} |
||||
|
||||
transpose(plain_strng, a_str, initial_tr, 64); |
||||
for (i = 1; i < 17; ++i) |
||||
{ |
||||
for (j = 0; j < 8; ++j) { b_str[j] = a_str[j]; } |
||||
|
||||
if (!d) /*enchipher*/ |
||||
pkey = &sub_keys[i - 1][0]; |
||||
else /*dechipher*/ |
||||
pkey = &sub_keys[16 - i][0]; |
||||
|
||||
for (j = 0; j < 4; ++j) { a_str[j] = b_str[j + 4]; } |
||||
|
||||
f(pkey, a_str, x_str); |
||||
|
||||
for (j = 0; j < 4; ++j) { a_str[j + 4] = b_str[j] ^ x_str[j]; } |
||||
} |
||||
|
||||
temp = a_str[0]; a_str[0] = a_str[4]; a_str[4] = temp; |
||||
temp = a_str[1]; a_str[1] = a_str[5]; a_str[5] = temp; |
||||
temp = a_str[2]; a_str[2] = a_str[6]; a_str[6] = temp; |
||||
temp = a_str[3]; a_str[3] = a_str[7]; a_str[7] = temp; |
||||
transpose(a_str, ciph_strng, final_tr, 64); |
||||
} |
||||
/************************************************************************/ |
||||
/* */ |
||||
/* Module title: transpose */ |
||||
/* Module type: des subrutine */ |
||||
/* */ |
||||
/* Author: YXH */ |
||||
/* Date: 2012-07-13 */ |
||||
/* */ |
||||
/* Last changed by: YXH */ |
||||
/* Date: 2012-07-13 */ |
||||
/* */ |
||||
/* Functional Description: Permute n bits in a string, according */ |
||||
/* to a table describing the new order. */ |
||||
/* 0 < n <= 64 */ |
||||
/* */ |
||||
/* input parameter 1: pointer to first byte in input string */ |
||||
/* 2: pointer to first byte in output string */ |
||||
/* 3: pointer to table describing new order */ |
||||
/* 4: number of bits to be permuted */ |
||||
/************************************************************************/ |
||||
void Des:: transpose(uint8_t* idata, uint8_t* odata, const uint8_t* tbl, uint8_t n) |
||||
{ |
||||
const uint8_t* tab_adr; |
||||
int i, bi_idx; |
||||
tab_adr = &bit_msk[0]; |
||||
i = 0; |
||||
|
||||
do |
||||
{ |
||||
odata[i++] = 0; |
||||
} while (i < 8); |
||||
i = 0; |
||||
do |
||||
{ |
||||
bi_idx = *tbl++; |
||||
if (idata[bi_idx >> 3] & tab_adr[bi_idx & 7]) |
||||
{ |
||||
odata[i >> 3] |= tab_adr[i & 7]; |
||||
} |
||||
} while (++i < n); |
||||
} |
||||
/************************************************************************/ |
||||
/* */ |
||||
/* Module title: rotate_l */ |
||||
/* Module type: des subrutine */ |
||||
/* */ |
||||
/* Author: YXH */ |
||||
/* Date: 2012-07-13 */ |
||||
/* */ |
||||
/* Last changed by: YXH */ |
||||
/* Date: 2012-07-13 */ |
||||
/* */ |
||||
/* Functional Description: rotate 2 concatenated strings of 28 */ |
||||
/* bits one position to the left. */ |
||||
/* */ |
||||
/* input parameter 1: pointer to first byte in key string */ |
||||
/* */ |
||||
/************************************************************************/ |
||||
void Des::rotate_l(uint8_t* key) |
||||
{ |
||||
uint8_t str_x[8]; |
||||
uint8_t i; |
||||
for (i = 0; i < 8; ++i) str_x[i] = key[i]; |
||||
for (i = 0; i < 7; ++i) |
||||
{ |
||||
key[i] = (key[i] << 1); |
||||
if ((i < 6) && ((str_x[i + 1] & 128) == 128)) |
||||
key[i] |= 1; |
||||
} |
||||
if (str_x[0] & 0x80) |
||||
key[3] |= 0x10; |
||||
else |
||||
key[3] &= ~0x10; |
||||
if (str_x[3] & 0x08) |
||||
key[6] |= 0x01; |
||||
else |
||||
key[6] &= ~0x01; |
||||
} |
||||
/************************************************************************/ |
||||
/* */ |
||||
/* Module title: compute_subkeys */ |
||||
/* Module type: des subrutine */ |
||||
/* */ |
||||
/* Author: YXH */ |
||||
/* Date: 2012-07-13 */ |
||||
/* */ |
||||
/* Last changed by: YXH */ |
||||
/* Date: 2012-07-13 */ |
||||
/* */ |
||||
/* Functional Description: Computes the 16 sub keys for use in the */ |
||||
/* DES algorithm */ |
||||
/* */ |
||||
/* input parameter 1: pointer to first byte in key string */ |
||||
/* output : fills the array sub_keys[16][8] with */ |
||||
/* sub keys and stores the input key in */ |
||||
/* main_key[8] */ |
||||
/************************************************************************/ |
||||
void Des::compute_subkeys(uint8_t* key) |
||||
{ |
||||
uint8_t i, j, ikey[8], okey[8]; |
||||
for (i = 0; i < 8; ++i) |
||||
{ |
||||
main_key[i] = key[i]; |
||||
} |
||||
|
||||
transpose(key, ikey, key_tr1, 56); |
||||
|
||||
for (i = 0; i < 16; ++i) |
||||
{ |
||||
for (j = 0; j < rots[i]; ++j) { rotate_l(ikey); } |
||||
transpose(ikey, okey, key_tr2, 64); |
||||
for (j = 0; j < 8; ++j) |
||||
{ |
||||
sub_keys[i][j] = okey[j]; |
||||
} |
||||
} |
||||
} |
||||
/************************************************************************/ |
||||
/* */ |
||||
/* Module title: f */ |
||||
/* Module type: des subrutine */ |
||||
/* */ |
||||
/* Author: YXH */ |
||||
/* Date: 2012-07-13 */ |
||||
/* */ |
||||
/* Last changed by: YXH */ |
||||
/* Date: 2012-07-13 */ |
||||
/* */ |
||||
/* Functional Description: The chipher function */ |
||||
/* */ |
||||
/* input parameter 1: pointer to first byte in key string */ |
||||
/* 2: pointer to a 32 bit input string */ |
||||
/* 3: pointer to a 32 bit output string */ |
||||
/************************************************************************/ |
||||
void Des::f(uint8_t* skey, uint8_t* a_str, uint8_t* x_str) |
||||
{ |
||||
uint8_t e_str[8], y_str[8], z_str[8]; |
||||
uint8_t k; |
||||
transpose(a_str, e_str, etr, 64); |
||||
for (k = 0; k < 8; ++k) |
||||
{ |
||||
y_str[k] = (e_str[k] ^ skey[k]) & 63; |
||||
z_str[k] = s[k][y_str[k]]; |
||||
} |
||||
transpose(z_str, x_str, ptr, 32); |
||||
} |
||||
@ -0,0 +1,22 @@
@@ -0,0 +1,22 @@
|
||||
#pragma once |
||||
|
||||
#include <stdint.h> |
||||
|
||||
class Des |
||||
{ |
||||
private: |
||||
/* data */ |
||||
public: |
||||
// uint8_t deskey[8] = {90, 114, 90, 107, 85, 97, 86, 103}; //{ "ZrZkUaVg" };
|
||||
// uint8_t deskey[8] = {90, 114, 90, 104, 68, 85, 97, 86}; //{ "ZrZhDUaV" };
|
||||
uint8_t DES_Encrypt_key[8]; |
||||
uint8_t DES_Decrypt_key[8]; |
||||
uint8_t sub_keys[16][8]; //sub_keys[16][8]
|
||||
uint8_t main_key[8]; |
||||
void des(uint8_t *, uint8_t *, uint8_t, uint8_t *); |
||||
void FLASH_Read_KEYS(uint8_t key_index); |
||||
void transpose(uint8_t *, uint8_t *, const uint8_t *, uint8_t); |
||||
void rotate_l(uint8_t *); |
||||
void compute_subkeys(uint8_t *); |
||||
void f(uint8_t *, uint8_t *, uint8_t *); |
||||
}; |
||||
@ -0,0 +1,250 @@
@@ -0,0 +1,250 @@
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
|
||||
#if HAL_CANMANAGER_ENABLED |
||||
|
||||
#include <GCS_MAVLink/GCS.h> |
||||
#include "AP_Proximity_UAVCAN.h" |
||||
#include <AP_CANManager/AP_CANManager.h> |
||||
#include <AP_UAVCAN/AP_UAVCAN.h> |
||||
|
||||
#include <zrzk/equipment/range_sensor/Proximity.hpp> |
||||
#define PRX_DEBUG 0 |
||||
#if PRX_DEBUG |
||||
#include <GCS_MAVLink/GCS.h> |
||||
# define Prx_Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_INFO, "prx-%d: " fmt "", __LINE__, ## args) |
||||
#else |
||||
# define Prx_Debug(fmt, args ...) |
||||
#endif |
||||
|
||||
extern const AP_HAL::HAL &hal; |
||||
|
||||
#define debug_proximity_uavcan(level_debug, can_driver, fmt, args...) \ |
||||
do \
|
||||
{ \
|
||||
if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) \
|
||||
{ \
|
||||
hal.console->printf(fmt, ##args); \
|
||||
} \
|
||||
} while (0) |
||||
|
||||
HAL_Semaphore AP_Proximity_UAVCAN::_sem_registry; |
||||
UC_REGISTRY_BINDER(ProximityCb, zrzk::equipment::range_sensor::Proximity); |
||||
|
||||
uint8_t AP_Proximity_UAVCAN::_node_id = 0; |
||||
AP_Proximity_UAVCAN* AP_Proximity_UAVCAN::_driver = nullptr; |
||||
AP_UAVCAN* AP_Proximity_UAVCAN::_ap_uavcan = nullptr; |
||||
|
||||
AP_Proximity_UAVCAN::DetectedModules AP_Proximity_UAVCAN::_detected_modules[] = {0}; |
||||
|
||||
AP_Proximity_UAVCAN::AP_Proximity_UAVCAN(AP_Proximity &_frontend, |
||||
AP_Proximity::Proximity_State &_state) :
|
||||
AP_Proximity_Backend(_frontend, _state) |
||||
{ |
||||
_driver = this; |
||||
} |
||||
|
||||
void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan) |
||||
{ |
||||
if (ap_uavcan == nullptr) |
||||
{ |
||||
Prx_Debug("ap_uavcan:nullptr" ); |
||||
return; |
||||
} |
||||
|
||||
Prx_Debug("ap_uavcan:subscribe_msgs"); |
||||
auto *node = ap_uavcan->get_node(); |
||||
|
||||
uavcan::Subscriber<zrzk::equipment::range_sensor::Proximity, ProximityCb> *proximity_listener; |
||||
proximity_listener = new uavcan::Subscriber<zrzk::equipment::range_sensor::Proximity, ProximityCb>(*node); |
||||
// Register method to handle incoming RangeFinder measurement
|
||||
const int proximity_listener_res = proximity_listener->start(ProximityCb(ap_uavcan, &handle_proximity_data_trampoline)); |
||||
if (proximity_listener_res < 0) |
||||
{ |
||||
AP_HAL::panic("UAVCAN RangeFinder subscriber start problem\n\r"); |
||||
return; |
||||
} |
||||
} |
||||
|
||||
AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend, |
||||
AP_Proximity::Proximity_State &_state) |
||||
{ |
||||
WITH_SEMAPHORE(_sem_registry); |
||||
|
||||
AP_Proximity_UAVCAN *backend = nullptr; |
||||
Prx_Debug("---------- AP_Proximity_UAVCAN ----------"); |
||||
backend = new AP_Proximity_UAVCAN(_frontend, _state); |
||||
if (backend != nullptr) |
||||
{ |
||||
Prx_Debug("backend:%d",(int)backend); |
||||
}else{ |
||||
|
||||
Prx_Debug("faild,backend"); |
||||
} |
||||
|
||||
Prx_Debug("---------- AP_Proximity_UAVCAN ----------\n"); |
||||
return backend; |
||||
} |
||||
|
||||
AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id) |
||||
{ |
||||
if (ap_uavcan == nullptr) |
||||
{ |
||||
return nullptr; |
||||
} |
||||
Prx_Debug("get_uavcan:%d",node_id); |
||||
for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) |
||||
{ |
||||
if (_detected_modules[i].driver != nullptr && |
||||
_detected_modules[i].ap_uavcan == ap_uavcan&& |
||||
_detected_modules[i].node_id == node_id)
|
||||
{ |
||||
return _detected_modules[i].driver; |
||||
} |
||||
} |
||||
|
||||
bool detected = false; |
||||
for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) |
||||
{ |
||||
if (_detected_modules[i].ap_uavcan == ap_uavcan && |
||||
_detected_modules[i].node_id == node_id) |
||||
{ |
||||
// detected
|
||||
detected = true; |
||||
break; |
||||
} |
||||
} |
||||
|
||||
if (!detected) |
||||
{ |
||||
for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) |
||||
{ |
||||
if (_detected_modules[i].ap_uavcan == nullptr) |
||||
{ |
||||
_detected_modules[i].ap_uavcan = ap_uavcan; |
||||
_detected_modules[i].node_id = node_id; |
||||
break; |
||||
} |
||||
} |
||||
} |
||||
|
||||
return nullptr; |
||||
} |
||||
|
||||
float AP_Proximity_UAVCAN::distance_max() const |
||||
{ |
||||
return 40.0f; |
||||
} |
||||
|
||||
float AP_Proximity_UAVCAN::distance_min() const |
||||
{ |
||||
return 0.20f; |
||||
} |
||||
|
||||
void AP_Proximity_UAVCAN::update(void) |
||||
{ |
||||
WITH_SEMAPHORE(_sem_proximity); |
||||
|
||||
// update_sector_data(0, _interim_data.d1_cm); // d1
|
||||
// update_sector_data(45, _interim_data.d2_cm); // d8
|
||||
// update_sector_data(90, _interim_data.d3_cm); // d7
|
||||
// update_sector_data(135, _interim_data.d4_cm); // d6
|
||||
// update_sector_data(180, _interim_data.d5_cm); // d5
|
||||
// update_sector_data(225, _interim_data.d6_cm); // d4
|
||||
// update_sector_data(270, _interim_data.d7_cm); // d3
|
||||
// update_sector_data(315, _interim_data.d8_cm); // d2
|
||||
|
||||
update_sector_data(0, _interim_data.d1_cm); // d1
|
||||
update_sector_data(45, _interim_data.d2_cm); // d8
|
||||
update_sector_data(315, _interim_data.d3_cm); // d7
|
||||
|
||||
if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_UAVCAN_TIMEOUT_MS)) |
||||
{ |
||||
set_status(AP_Proximity::Status::NoData); |
||||
// gcs().send_text(MAV_SEVERITY_INFO, "NoData\r\n");
|
||||
} |
||||
else |
||||
{ |
||||
set_status(AP_Proximity::Status::Good); |
||||
} |
||||
} |
||||
|
||||
void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb) |
||||
{ |
||||
|
||||
if (_driver == nullptr) { |
||||
Prx_Debug("----nullptr : ap:%d,id:%d ----",(int)_ap_uavcan,_node_id); |
||||
|
||||
return; |
||||
} |
||||
|
||||
if (_ap_uavcan == nullptr) { |
||||
_ap_uavcan = ap_uavcan; |
||||
_node_id = node_id; |
||||
Prx_Debug("---- uavcan: ap:%d,id:%d ----",(int)_ap_uavcan,_node_id); |
||||
} |
||||
Prx_Debug("handle_proximity: ap:%d,id:%d ,%d,addr:%d",(int)_ap_uavcan,_node_id,cb.msg->d0,cb.msg->sensor_id); |
||||
if (_ap_uavcan == ap_uavcan && _node_id == node_id) { |
||||
WITH_SEMAPHORE(_sem_registry); |
||||
_driver->_interim_data.d1_cm = cb.msg->d0; |
||||
_driver->_interim_data.d2_cm = cb.msg->d45; |
||||
_driver->_interim_data.d3_cm = cb.msg->d90; |
||||
_driver->_interim_data.d4_cm = cb.msg->d135; |
||||
_driver->_interim_data.d5_cm = cb.msg->d180; |
||||
_driver->_interim_data.d6_cm = cb.msg->d225; |
||||
_driver->_interim_data.d7_cm = cb.msg->d270; |
||||
_driver->_interim_data.d8_cm = cb.msg->d315; |
||||
_driver->_last_distance_received_ms = AP_HAL::millis(); |
||||
} |
||||
/*
|
||||
AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan,node_id); |
||||
if (driver == nullptr) |
||||
{ |
||||
return; |
||||
} |
||||
driver->handle_proximity_data(cb); |
||||
*/ |
||||
} |
||||
|
||||
void AP_Proximity_UAVCAN::handle_proximity_data(const ProximityCb &cb) |
||||
{ |
||||
|
||||
WITH_SEMAPHORE(_sem_proximity); |
||||
|
||||
_interim_data.d1_cm = cb.msg->d0; |
||||
_interim_data.d2_cm = cb.msg->d45; |
||||
_interim_data.d3_cm = cb.msg->d90; |
||||
_interim_data.d4_cm = cb.msg->d135; |
||||
_interim_data.d5_cm = cb.msg->d180; |
||||
_interim_data.d6_cm = cb.msg->d225; |
||||
_interim_data.d7_cm = cb.msg->d270; |
||||
_interim_data.d8_cm = cb.msg->d315; |
||||
_last_distance_received_ms = AP_HAL::millis(); |
||||
// _last_sample_time_ms = AP_HAL::millis();
|
||||
} |
||||
|
||||
void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distance_cm) |
||||
{ |
||||
// Get location on 3-D boundary based on angle to the object
|
||||
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg); |
||||
if ((distance_cm != 0xffff) && !ignore_reading(angle_deg, distance_cm * 0.01f, false)) { |
||||
boundary.set_face_attributes(face, angle_deg, ((float) distance_cm) / 100); |
||||
// update OA database
|
||||
database_push(angle_deg, ((float) distance_cm) / 100); |
||||
} else { |
||||
boundary.reset_face(face); |
||||
} |
||||
_last_distance_received_ms = AP_HAL::millis(); |
||||
|
||||
|
||||
/*
|
||||
uint8_t sector; |
||||
if(convert_angle_to_sector(angle_deg,sector)){ |
||||
_angle[sector] = angle_deg; |
||||
_distance[sector] = ((float)distance_cm) / 100; |
||||
_distance_valid[sector] = (_distance[sector] > distance_min() && _distance[sector] < distance_max()); |
||||
update_boundary_for_sector(sector, true); |
||||
} |
||||
*/ |
||||
} |
||||
|
||||
#endif |
||||
@ -0,0 +1,72 @@
@@ -0,0 +1,72 @@
|
||||
#pragma once |
||||
|
||||
#include "AP_Proximity_Backend.h" |
||||
|
||||
#if HAL_CANMANAGER_ENABLED |
||||
#include <AP_UAVCAN/AP_UAVCAN.h> |
||||
|
||||
class ProximityCb; |
||||
|
||||
#define PROXIMITY_UAVCAN_TIMEOUT_MS 300 |
||||
class AP_Proximity_UAVCAN : public AP_Proximity_Backend |
||||
{ |
||||
public: |
||||
AP_Proximity_UAVCAN(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); |
||||
|
||||
void update(void) override; |
||||
// get maximum and minimum distances (in meters) of sensor
|
||||
float distance_max() const override; |
||||
|
||||
float distance_min() const override; |
||||
|
||||
static void subscribe_msgs(AP_UAVCAN *ap_uavcan); |
||||
|
||||
static AP_Proximity_Backend *probe(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); |
||||
|
||||
static void handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb); |
||||
|
||||
//uint8_t _instance;
|
||||
//AP_Proximity::Proximity_State _status;
|
||||
|
||||
private: |
||||
static AP_Proximity_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id); |
||||
|
||||
static AP_Proximity_UAVCAN* _driver; |
||||
// inline void register_sensor()
|
||||
// {
|
||||
// // instance = frontend.register_sensor();
|
||||
// }
|
||||
|
||||
uint32_t _last_distance_received_ms; |
||||
void handle_proximity_data(const ProximityCb &cb); |
||||
struct Proximity_Data |
||||
{ |
||||
uint16_t d1_cm; |
||||
uint16_t d2_cm; |
||||
uint16_t d3_cm; |
||||
uint16_t d4_cm; |
||||
uint16_t d5_cm; |
||||
uint16_t d6_cm; |
||||
uint16_t d7_cm; |
||||
uint16_t d8_cm; |
||||
} _interim_data; |
||||
void update_sector_data(int16_t angle_deg, uint16_t distance_cm); |
||||
|
||||
uint16_t handle_data_valid(uint16_t data); |
||||
|
||||
static struct DetectedModules |
||||
{ |
||||
AP_UAVCAN *ap_uavcan; |
||||
uint8_t node_id; |
||||
AP_Proximity_UAVCAN *driver; |
||||
} _detected_modules[PROXIMITY_MAX_INSTANCES]; |
||||
|
||||
static HAL_Semaphore _sem_registry; |
||||
HAL_Semaphore _sem_proximity; |
||||
|
||||
static AP_UAVCAN* _ap_uavcan; |
||||
static uint8_t _node_id; |
||||
|
||||
|
||||
}; |
||||
#endif //HAL_WITH_UAVCAN
|
||||
@ -0,0 +1,4 @@
@@ -0,0 +1,4 @@
|
||||
uint8 rgb_mode # RGBLED mode |
||||
uint8 rgb_r # brightness of red light |
||||
uint8 rgb_g # brightness of red green |
||||
uint8 rgb_b # brightness of red blue |
||||
@ -0,0 +1,23 @@
@@ -0,0 +1,23 @@
|
||||
uint8 sensor_id |
||||
|
||||
uint5 SENSOR_TYPE_UNDEFINED = 0 |
||||
uint5 SENSOR_TYPE_SONAR = 1 |
||||
uint5 SENSOR_TYPE_LIDAR = 2 |
||||
uint5 SENSOR_TYPE_RADAR = 3 |
||||
uint5 sensor_type |
||||
|
||||
uint3 READING_TYPE_UNDEFINED = 0 # Range is unknown |
||||
uint3 READING_TYPE_VALID_RANGE = 1 # Range field contains valid distance |
||||
uint3 READING_TYPE_TOO_CLOSE = 2 # Range field contains min range for the sensor |
||||
uint3 READING_TYPE_TOO_FAR = 3 # Range field contains max range for the sensor |
||||
uint3 reading_type |
||||
|
||||
uint16 d0 # Meters distance_cm |
||||
uint16 d45 |
||||
uint16 d90 |
||||
uint16 d135 |
||||
uint16 d180 |
||||
uint16 d225 |
||||
uint16 d270 |
||||
uint16 d315 |
||||
|
||||
@ -1 +1 @@
@@ -1 +1 @@
|
||||
Subproject commit a2e2f0bd9ed3fc8dc04149d3292376c1e0451e27 |
||||
Subproject commit 60c48d29c63875362cd3c4ee2ff7f08271c5a43f |
||||
Loading…
Reference in new issue