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.
100 lines
2.5 KiB
100 lines
2.5 KiB
#include "Copter.h" |
|
|
|
#ifdef USERHOOK_INIT |
|
void Copter::userhook_init() |
|
{ |
|
// put your initialisation code here |
|
// this will be called once at start-up |
|
} |
|
#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 |
|
} |
|
#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 |
|
void Copter::userhook_SuperSlowLoop() |
|
{ |
|
// put your 1Hz code here |
|
// if(!motors->armed()) |
|
// { |
|
// camera.set_trigger_distance(0); // @Brown , stop take photos |
|
// } |
|
|
|
// float proximity_alt_diff; |
|
// AP_Proximity *proximity = AP::proximity(); |
|
// if (proximity && proximity->get_horizontal_distance(0,proximity_alt_diff)) { |
|
|
|
// if(proximity_alt_diff < 40.0) |
|
// { |
|
|
|
// if(control_mode != Mode::Number::BRAKE) |
|
// gcs().send_text(MAV_SEVERITY_INFO, "Dist: %f, stop",proximity_alt_diff); |
|
// control_mode = Mode::Number::BRAKE; |
|
|
|
// } |
|
|
|
// } |
|
} |
|
#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
|
|
|