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.
90 lines
2.9 KiB
90 lines
2.9 KiB
5 years ago
|
#if 1
|
||
|
#include "Copter.h"
|
||
|
|
||
|
|
||
|
void Copter::do_avoid_action(){
|
||
|
|
||
|
}
|
||
|
|
||
|
|
||
|
bool Copter::zr_radio_valid(){
|
||
|
if(!motors->armed()){
|
||
|
return true;
|
||
|
}
|
||
|
if (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::LAND){
|
||
|
if(abs(channel_roll->get_control_in())>750 || \
|
||
|
abs(channel_pitch->get_control_in())>750 || \
|
||
|
abs(channel_yaw->get_control_in())>750 || \
|
||
|
channel_throttle->get_control_in()<380 || \
|
||
|
channel_throttle->get_control_in()>590 )
|
||
|
{
|
||
|
// rc().set_disable_ch();
|
||
|
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 注意遥控通道是否在中位");
|
||
|
|
||
|
return false;
|
||
|
}
|
||
|
}
|
||
|
return true;
|
||
|
}
|
||
|
|
||
|
void Copter::zr_SlowLoop(){
|
||
|
|
||
|
/********** @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);
|
||
|
if(mode_auto.mission._type_resume && last_pos_lat!=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);
|
||
|
|
||
|
}
|
||
|
|
||
|
void Copter::zr_SuperSlowLoop(){
|
||
|
static bool before_fly = true;
|
||
|
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
|
||
|
};
|
||
|
|
||
|
if(motors->armed()){
|
||
|
before_fly = false;
|
||
|
relay.on(3);
|
||
|
|
||
|
}else{
|
||
|
relay.off(3);
|
||
|
// rc().set_enable_ch();
|
||
|
updownStatus =UpDown_TakeOffStart;
|
||
|
mavlink_zr_flying_status_t zr_flying_status_t;
|
||
|
zr_flying_status_t.updown_status = updownStatus;
|
||
|
//TODO UPDATE COUNTDOWN
|
||
|
gcs().update_zr_fly_status(&zr_flying_status_t);
|
||
|
}
|
||
|
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);
|
||
|
}
|
||
|
|
||
|
gcs().send_message(MSG_ZR_FLYING_STATUS);
|
||
|
|
||
|
}
|
||
|
#endif
|