zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
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.
40501 Commits
10 Branches
0 Tags
227 MiB
 
 
 
 
 
 
Tree: 24b7c0c3b4
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '24b7c0c3b4'
${ noResults }
zr-v4/ArduCopter/zr_flight.cpp

480 lines
18 KiB
Raw Blame History

#if 1
#include "Copter.h"
// 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)
{
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);
}
void Copter::zr_SuperSlowLoop(){
static bool before_fly = true;
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){
zr_set_battery_capacity();
static uint32_t prx_init_time = AP_HAL::millis();
static bool init_prx = false;
if (init_prx == false ) {
uint32_t tnow = AP_HAL::millis();
if(tnow - prx_init_time > 5000){
init_prx = true;
init_proximity();
// gcs().send_text(MAV_SEVERITY_INFO, "--- init prx:%dms -> %dms ---",prx_init_time,tnow);
}
}
}
if(control_mode != Mode::Number::CIRCLE && control_mode != Mode::Number::AUTO && control_mode != Mode::Number::RTL){
circle_nav->parm_reset();
}
}
void Copter::zr_set_battery_capacity()
{
uint8_t bat_cnt;
float now_volt = battery.voltage();
int16_t bat_cycled = (int16_t)g.zr_bat_cycled;
float cycle_pst = (1 - bat_cycled * 0.0002);
switch (battery.get_batt_type(0))
{
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,now_volt,0,100);
battery.reset_remaining(1,bat_cnt * cycle_pst);
// 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,now_volt,0,100);
battery.reset_remaining(1,bat_cnt * cycle_pst);
// gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
break;
default:
break;
}
}
void Copter::do_avoid_action(){
float proxy_dist = 0;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
uint16_t rc10_in = RC_Channels::rc_channel(CH_6)->get_radio_in();
proxy_dist = (rc10_in - 1100)/900.0*50.0;
#else
AP_Proximity *proximity = AP::proximity();
if(proximity == nullptr)
return;
bool get_new_dist = proximity->get_horizontal_distance(0,proxy_dist);
bool data_failed = proximity->sensor_failed();
if(get_new_dist == false || data_failed){
proxy_dist = avoid.get_zr_avd_max() + 1.0;
}
#endif
avoid_area_detect(proxy_dist);
avoid_state(proxy_dist);
avoid_action(proxy_area,proxy_state,proxy_dist);
}
uint8_t Copter::avoid_area_detect(float dist)
{
static Proxy_Area area = PROXY_OUTRANGE;
static int8_t min_dist;
if(area == PROXY_OUTRANGE){
min_dist = 15;
}else{
min_dist = avoid.get_zr_avd_min();
}
if(dist < min_dist){
if(area != PROXY_TOO_CLOSE && area != PROXY_FIND_BARRIER){ // 前一个状态不是障碍物过近
area = PROXY_OUTRANGE;
}else{
area = PROXY_TOO_CLOSE;
}
}
else if(dist< avoid.get_zr_avd_trig()){
area = PROXY_TOO_CLOSE;
}
else if(dist<avoid.get_zr_avd_max()){
area = PROXY_FIND_BARRIER;
}
else {
area = PROXY_OUTRANGE;
}
if(proxy_area != area){
gcs().send_text(MAV_SEVERITY_INFO, "proxy_area:%d",area);
}
proxy_area = area;
return area;
}
uint8_t Copter::avoid_state(float dist)
{
Proxy_State state;
static float previous_dist = 0;
float delt_dist = dist - previous_dist;
previous_dist = dist;
if(delt_dist > 1)
state = PROXY_LEAVE;
else if(delt_dist < -1)
state = PROXY_APPROACH;
else
state = PROXY_HOLD;
proxy_state = state;
return state;
}
void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
{
static uint8_t delay_cnt;
// static Proxy_Action action;
Vector3f pos_neu_cm;
static int32_t max_climb_alt = 0;
int32_t turn_yaw;
static uint32_t rtl_wait_time = AP_HAL::millis();
switch (avoid_action_step)
{
case PROXY_WAIT:
if(control_mode == Mode::Number::LAND ){ // 降落模式不触发避障
break;
}
if(area == PROXY_FIND_BARRIER && state == PROXY_APPROACH)
avoid_action_step = PROXY_SLOW_DOWN;
if(area == PROXY_TOO_CLOSE)
avoid_action_step = PROXY_BRAKE;
break;
case PROXY_SLOW_DOWN:
if(state == PROXY_APPROACH){
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 发现障碍物，距离:%3.2fm",dist);
// last_prx_diff = dist;
}
if(area == PROXY_TOO_CLOSE)
avoid_action_step = PROXY_BRAKE;
break;
case PROXY_BRAKE:
if(state != PROXY_LEAVE){
gcs().send_text(MAV_SEVERITY_CRITICAL,"障碍物距离过近:%3.2fm，刹车",dist);
set_mode(Mode::Number::BRAKE,ModeReason::AVOIDANCE);
avoid_action_step = PROXY_DELAY;
delay_cnt = 0;
}
break;
case PROXY_DELAY:
if(delay_cnt > 30){
set_mode(Mode::Number::GUIDED,ModeReason::AVOIDANCE);
// turn_yaw = current_loc.get_bearing_to(ahrs.get_home());
// bool need_turn_yaw = abs(ahrs.yaw_sensor - turn_yaw)<4500;
// if(need_turn_yaw){
// avoid_action_step = PROXY_CLIMB;
max_climb_alt = current_loc.alt+avoid.get_zr_avd_max_climb();
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:准备爬升，Now:%.2f，MAX:%.2f，",current_loc.alt/100.0f,max_climb_alt/100.0f);
// }else{
// avoid_action_step = PROXY_RTL;
// max_climb_alt = current_loc.alt;
// gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:角度在安全范围:%d，直接返航",abs(ahrs.yaw_sensor - turn_yaw));
// }
avoid_action_step = PROXY_CLIMB;
delay_cnt = 0;
}
if(control_mode == Mode::Number::LOITER || control_mode == Mode::Number::ALT_HOLD ){
avoid_action_step = PROXY_WAIT;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:切换到人为控制");
}
delay_cnt+=1;
break;
case PROXY_CLIMB:
if (!current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:无法获取位置");
break;
}
pos_neu_cm.z += 1000;
turn_yaw = current_loc.get_bearing_to(ahrs.get_home());
mode_guided.set_destination(pos_neu_cm, true, turn_yaw, true, 200, false);
// mode_guided.set_destination(pos_neu_cm, false, 0, false, 0, false);
if(current_loc.alt > max_climb_alt){
avoid_action_step = PROXY_NEED_CTL;
rtl_wait_time = AP_HAL::millis();
gcs().send_text(MAV_SEVERITY_CRITICAL,"超出爬升高度限制:%.2f，请手动控制",current_loc.alt/100.0f);
}
if(area == PROXY_OUTRANGE){
avoid_action_step = PROXY_RTL;
max_climb_alt = current_loc.alt + 1000;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:已避开障碍，准备返航");
}
if(control_mode == Mode::Number::LOITER || control_mode == Mode::Number::ALT_HOLD ){
avoid_action_step = PROXY_WAIT;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:切换到人为控制");
}
break;
case PROXY_RTL:
if(current_loc.alt < max_climb_alt)
{
if (!current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:无法获取位置");
break;
}
pos_neu_cm.z += max_climb_alt - current_loc.alt + 500;
// turn_yaw = current_loc.get_bearing_to(ahrs.get_home());
mode_guided.set_destination(pos_neu_cm, false, 0, false, 0, false);
if(control_mode != Mode::Number::GUIDED){
avoid_action_step = PROXY_WAIT;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:切换到人为控制");
}
}else{
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:开始返航");
set_mode(Mode::Number::RTL,ModeReason::AVOIDANCE);
avoid_action_step = PROXY_WAIT;
}
break;
case PROXY_NEED_CTL:
{
uint32_t tnow = AP_HAL::millis();
if(area == PROXY_OUTRANGE){
avoid_action_step = PROXY_WAIT;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:人为控制，已离开障碍");
}
if(tnow - rtl_wait_time > (uint32_t)(g.zr_avd_wait * 1000)){
if(avoid.get_zr_mode() == 2){
avoid_action_step = PROXY_WAIT;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:等待超时，原地降落");
set_mode(Mode::Number::LAND,ModeReason::AVOIDANCE);
}else{
avoid_action_step = PROXY_RTL_ERR;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:等待超时，关闭避障返航");
set_mode(Mode::Number::RTL,ModeReason::AVOIDANCE);
}
}
}
break;
case PROXY_RTL_ERR:
{
if(control_mode == Mode::Number::LAND ){ // 如果切换到Land，则可以重新开始
avoid_action_step = PROXY_WAIT;
break;
}
}
default:
break;
}
}
/**
* @brief 读取航点，存到_flow_wp中，用于测流航点距离计算
*
*/
void Copter::add_flow_wp()
{
// Vector3f* _flow_wp; // 动态存储坐标
// uint16_t _flow_wp_points_count; // 动态存储计数
flow_measure.max_flow_wp = mode_auto.mission.num_commands();
AP_Mission::Mission_Command mission_cmd;
Vector2f pos_ne;
flow_measure._flow_wp_points_count = 0;
for(uint16_t i = 1; i < flow_measure.max_flow_wp - 1;i++)
{
if(mode_auto.mission.read_cmd_from_storage(i,mission_cmd)) { // 读取原本航点
if (mode_auto.mission.is_nav_cmd(mission_cmd)) { // 是否是Waypoint
if(mission_cmd.content.location.get_vector_xy_from_origin_NE(pos_ne)) //坐标转换
{
flow_measure._flow_wp[flow_measure._flow_wp_points_count] = pos_ne; // 存到动态存储中
flow_measure._flow_wp_points_count += 1;
}
}
}
}
}
/**
* @brief 计算当前飞机位置与最近的航点
*
*/
void Copter::get_mindist_flow_wp()
{
uint8_t min_dist_index;
float min_dist = 0;
Vector2f posNE;
Location loc;
if(!ahrs.get_position(loc)){
return;
}
if (!loc.get_vector_xy_from_origin_NE(posNE)) {
// not breached if we don't now where we are
return;
}
if(flow_measure.last_wp_update_time == 0 || flow_measure.last_wp_update_time != mode_auto.mission.last_change_time_ms()) // 航点更新
{
flow_measure.last_wp_update_time = mode_auto.mission.last_change_time_ms();
add_flow_wp();
gcs().send_text(MAV_SEVERITY_INFO, "Mission update:%d,wp num:%d",flow_measure.last_wp_update_time,flow_measure._flow_wp_points_count);
}
if(flow_measure._flow_wp_points_count > 2){
float wp_distance;
min_dist = norm(posNE.x-flow_measure._flow_wp[0].x,posNE.y-flow_measure._flow_wp[0].y); // 最小距离先复制第一个航电距离
min_dist_index = 1;
for(uint16_t i = 1; i < flow_measure._flow_wp_points_count;i++)
{
wp_distance = norm(posNE.x-flow_measure._flow_wp[i].x,posNE.y-flow_measure._flow_wp[i].y)/100.0;
if(min_dist > wp_distance){
min_dist_index = i ;
min_dist = wp_distance;
}
}
flow_measure.curr_mark.index = min_dist_index;
flow_measure.curr_mark.distance = min_dist;
if(flow_measure.curr_mark.index != flow_measure.last_mark.index){
flow_measure.last_mark.index = flow_measure.curr_mark.index;
// mode_auto.mission.set_current_cmd(flow_measure.curr_mark.index + 2);
flow_measure.set_wp_auto_index(flow_measure.curr_mark.index);
gcs().send_text(MAV_SEVERITY_INFO, "update index:#%d",flow_measure.curr_mark.index);
}
if(flow_measure.curr_mark.index != flow_measure.last_mark.index || fabs(flow_measure.last_mark.distance - flow_measure.curr_mark.distance) > 2){
// gcs().send_text(MAV_SEVERITY_INFO, "close to:#%d ,d:%.2f",flow_measure.curr_mark.index,flow_measure.curr_mark.distance);
flow_measure.last_mark.distance = flow_measure.curr_mark.distance;
}
}
}
void Copter::zr_mission_flow_mode()
{
// RC6 测流开关，RC9 计数开关
uint8_t measure_time = wp_nav->get_measure_time();
int32_t real_alt;
flow_measure.set_radar_mode(g.zr_flow_type); //设置雷达模式，单测流或者三合一等
flow_measure.set_radar_sens(g.zr_flow_sens); //设置雷达灵敏度
if(g.zr_flow_type == 2) // 单测流+指定水位高
real_alt = g.zr_flow_real_elevation; // 直接设置水位高度
else{
real_alt= gps.location().alt + gps.get_gps_delt_alt();
}
if(control_mode != Mode::Number::AUTO){
flow_measure.set_reach_flow_wp(false);
get_mindist_flow_wp();
}else{
if(flow_measure.flow_start_sw){
flow_measure.flow_start_sw = false;
gcs().send_text(MAV_SEVERITY_NOTICE, "NOTICE:自动测流模式，关闭手动测流");
}
}
flow_measure.flow_auto_measure(flow_measure.flow_start_sw, real_alt, measure_time);
// flow_measure.flow_auto_measure(1,1, real_alt, measure_time);
}
#endif