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.
164 lines
4.2 KiB
164 lines
4.2 KiB
#include "Copter.h" |
|
|
|
|
|
|
|
|
|
#ifdef USERHOOK_INIT |
|
void Copter::userhook_init() |
|
{ |
|
// put your initialisation code here |
|
// this will be called once at start-up |
|
if (x30t != nullptr) |
|
{ |
|
x30t->init(); |
|
} |
|
else |
|
{ |
|
if (x30t == nullptr) |
|
{ |
|
gcs().send_text(MAV_SEVERITY_INFO,"x30t is null"); |
|
x30t = new AP_Camera_Serial_X30T(); |
|
if (x30t == nullptr || !x30t->init()) |
|
{ |
|
delete x30t; |
|
x30t = nullptr; |
|
} |
|
} |
|
} |
|
} |
|
#endif |
|
|
|
#ifdef USERHOOK_FASTLOOP |
|
void Copter::userhook_FastLoop() |
|
{ |
|
// put your 100Hz code here |
|
} |
|
#endif |
|
|
|
#ifdef USERHOOK_50HZLOOP |
|
void Copter::userhook_50Hz() |
|
{ |
|
// put your 50Hz code here |
|
} |
|
#endif |
|
|
|
#ifdef USERHOOK_MEDIUMLOOP |
|
void Copter::userhook_MediumLoop() |
|
{ |
|
// put your 10Hz code here |
|
if (x30t != nullptr) |
|
{ |
|
int32_t lat = AP::gps().location().lat; |
|
int32_t lng = AP::gps().location().lat; |
|
int32_t alt = AP::gps().location().alt; |
|
|
|
uint8_t posdata[12]; |
|
posdata[0] = lat & 0xff; |
|
posdata[1] = (lat >> 8) & 0xff; |
|
posdata[2] = (lat >> 16) & 0xff; |
|
posdata[3] = (lat >> 24) & 0xff; |
|
|
|
posdata[4] = lng & 0xff; |
|
posdata[5] = (lng >> 8) & 0xff; |
|
posdata[6] = (lng >> 16) & 0xff; |
|
posdata[7] = (lng >> 24) & 0xff; |
|
|
|
posdata[8] = alt& 0xff; |
|
posdata[9] = (alt>> 8) & 0xff; |
|
posdata[10] =(alt >> 16) & 0xff; |
|
posdata[11] =(alt >> 24) & 0xff; |
|
|
|
x30t->sendPos(&posdata[0],X30T_MSG_TYPE_POS); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
} |
|
#endif |
|
|
|
#ifdef USERHOOK_SLOWLOOP |
|
void Copter::userhook_SlowLoop() |
|
{ |
|
// put your 3.3Hz code here |
|
/********** @Brown,for get camera pos **********/ |
|
static int32_t last_pos_lat; |
|
static int32_t last_pos_lng; |
|
|
|
camera.get_last_trigger_pos(last_pos_lat,last_pos_lng); |
|
|
|
// static uint8_t cnt; |
|
// cnt++; |
|
// if(cnt > 9){ |
|
// gcs().send_text(MAV_SEVERITY_INFO, "cam pos: %d,%d",last_pos_lat,last_pos_lng); |
|
// gcs().send_text(MAV_SEVERITY_INFO, "gps pos: %d,%d",current_loc.lat,current_loc.lng); |
|
// cnt = 0; |
|
// } |
|
// mission.set_last_trigger_pos(last_pos_lat,last_pos_lng); |
|
if(mode_auto.mission._type_resume && last_pos_lat!=0) |
|
// if(0) |
|
mode_auto.mission.auto_resume_mission((uint8_t)control_mode,motors->armed(),last_pos_lat,last_pos_lng); |
|
else |
|
mode_auto.mission.auto_resume_mission((uint8_t)control_mode,motors->armed(),current_loc.lat,current_loc.lng); |
|
/********** @Brown,for get camera pos **********/ |
|
} |
|
#endif |
|
|
|
#ifdef USERHOOK_SUPERSLOWLOOP |
|
static float batt_mah_teb[] = |
|
{ |
|
25.20,24.90,24.84,24.76,24.69,24.64,24.60,24.57,24.54,24.52, |
|
24.50,24.48,24.45,24.43,24.40,24.36,24.32,24.26,24.21,24.14, |
|
24.07,24.00,23.94,23.88,23.79,23.71,23.65,23.58,23.51,23.46, |
|
23.41,23.36,23.30,23.24,23.19,23.13,23.07,23.01,22.95,22.91, |
|
22.86,22.81,22.76,22.70,22.65,22.59,22.54,22.48,22.43,22.37, |
|
22.31,22.26,22.21,22.16,22.11,22.06,22.01,21.96,21.92,21.88, |
|
21.84,21.79,21.76,21.72,21.69,21.64,21.59,21.54,21.49,21.45, |
|
21.40,21.34,21.29,21.24,21.18,21.13,21.07,21.02,20.96,20.90, |
|
20.85,20.80,20.75,20.69,20.63,20.56,20.47,20.29,20.16,20.04, |
|
19.88,19.73,19.53,19.34,19.19,19.00,18.80,18.51,18.00,17.40, |
|
16.80 |
|
}; |
|
void Copter::userhook_SuperSlowLoop() |
|
{ |
|
// put your 1Hz code here |
|
static bool before_fly = true; |
|
if(motors->armed()) |
|
before_fly = false; |
|
if(before_fly){ |
|
uint8_t cnt,cacl_volt_pst; |
|
float delt_volt; |
|
for(cnt = 0; cnt<102; cnt++ ){ |
|
delt_volt = batt_mah_teb[cnt] - battery.voltage(); |
|
// delt_volt = batt_mah_teb[cnt] - test_volt; |
|
if(delt_volt <= 0) |
|
break; |
|
} |
|
cacl_volt_pst = 100 - cnt; |
|
battery.reset_remaining(1, cacl_volt_pst); |
|
} |
|
|
|
} |
|
#endif |
|
|
|
#ifdef USERHOOK_AUXSWITCH |
|
void Copter::userhook_auxSwitch1(uint8_t ch_flag) |
|
{ |
|
// put your aux switch #1 handler here (CHx_OPT = 47) |
|
} |
|
|
|
void Copter::userhook_auxSwitch2(uint8_t ch_flag) |
|
{ |
|
// put your aux switch #2 handler here (CHx_OPT = 48) |
|
} |
|
|
|
void Copter::userhook_auxSwitch3(uint8_t ch_flag) |
|
{ |
|
// put your aux switch #3 handler here (CHx_OPT = 49) |
|
} |
|
#endif
|
|
|