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.
143 lines
5.1 KiB
143 lines
5.1 KiB
#if 1 |
|
#include "Copter.h" |
|
|
|
|
|
void Copter::do_avoid_action(){ |
|
|
|
} |
|
// bool Copter::close_to_EKF_origin(const Location& loc) |
|
// { |
|
// // check distance to EKF origin |
|
// const struct Location &ekf_origin = inertial_nav.get_origin(); |
|
// if (get_distance(ekf_origin, loc) < 15 && ekf_origin.alt < 5000) { |
|
// // gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: dist:%.2f,alt:%d",get_distance(ekf_origin, loc) ,ekf_origin.alt); |
|
// return true; |
|
// } |
|
|
|
// // close enough to origin |
|
// return false; |
|
// } |
|
|
|
bool Copter::zr_radio_valid(){ |
|
if(!motors->armed()){ |
|
return true; |
|
} |
|
if (!rc().ignore_ctrl_channel() && (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) |
|
{ |
|
// rc().set_disable_ch(); |
|
gcs().send_text(MAV_SEVERITY_INFO,"注意遥控通道是否在中位"); |
|
|
|
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); |
|
|
|
} |
|
|
|
uint8_t Copter::BinarySearch2f(float a[], float value, int low, int high) |
|
{ |
|
|
|
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); |
|
} |
|
|
|
void Copter::zr_SuperSlowLoop(){ |
|
static bool before_fly = true; |
|
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 |
|
}; |
|
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 |
|
}; |
|
|
|
if(motors->armed()){ |
|
if(before_fly){ |
|
before_fly = false; |
|
// camera.create_new_folder(10); |
|
gcs().send_text(MAV_SEVERITY_INFO,"%s",get_sysid_board_id()); |
|
} |
|
relay.on(3); |
|
gcs().send_message(MSG_ZR_FLYING_STATUS); |
|
// camera.set_in_arm_mode(true); |
|
|
|
}else{ |
|
relay.off(3); |
|
// camera.set_in_arm_mode(false); |
|
// rc().set_enable_ch(); |
|
updownStatus =UpDown_TakeOffStart; |
|
} |
|
if(before_fly){ |
|
cacl_volt_pst = 0; |
|
uint8_t bat_cnt; |
|
float now_volt = battery.voltage(); |
|
switch (battery.get_batt_type(0)) |
|
{ |
|
case 0: |
|
/* code */ |
|
break; |
|
case 1: |
|
bat_cnt = BinarySearch2f(batt_mah_teb,now_volt,0,100); |
|
battery.reset_remaining(1,bat_cnt); |
|
// gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst); |
|
break; |
|
case 2: |
|
bat_cnt = BinarySearch2f(batt_mah_teb_7s,now_volt,0,100); |
|
battery.reset_remaining(1,bat_cnt); |
|
// gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst); |
|
break; |
|
|
|
default: |
|
break; |
|
} |
|
} |
|
|
|
|
|
} |
|
#endif |