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.
331 lines
8.9 KiB
331 lines
8.9 KiB
/* |
|
* @Author: your name |
|
* @Date: 2020-10-29 14:13:48 |
|
* @LastEditTime: 2020-10-29 14:19:30 |
|
* @LastEditors: your name |
|
* @Description: In User Settings Edit |
|
* @FilePath: /zr-v4/ArduCopter/UserCode.cpp |
|
*/ |
|
#include "Copter.h" |
|
|
|
#if HAL_WITH_UAVCAN |
|
#include <AP_BoardConfig/AP_BoardConfig_CAN.h> |
|
#include <AP_UAVCAN/AP_UAVCAN.h> |
|
//#include<uavcan/equipment/camera_gimbal/Status.hpp> |
|
#endif |
|
|
|
#define CAM_DEBUG 0 |
|
|
|
#ifdef USERHOOK_INIT |
|
void Copter::userhook_init() |
|
{ |
|
// put your initialisation code here |
|
// this will be called once at start-up |
|
|
|
relay.on(1); |
|
set_mode(Mode::Number::LOITER, ModeReason::STARTUP); |
|
// gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: version: ZRUAV v4.0.7-rc1"); |
|
} |
|
#endif |
|
|
|
#ifdef USERHOOK_FASTLOOP |
|
void Copter::userhook_FastLoop() |
|
{ |
|
// put your 100Hz code here |
|
takeoff_crash_detect(); |
|
zr_app_10hz(); |
|
} |
|
#endif |
|
|
|
#ifdef USERHOOK_50HZLOOP |
|
void Copter::userhook_50Hz() |
|
{ |
|
// put your 50Hz code here |
|
zr_app_50hz(); |
|
} |
|
#endif |
|
|
|
#ifdef USERHOOK_MEDIUMLOOP |
|
void Copter::userhook_MediumLoop() |
|
{ |
|
// put your 10Hz code here |
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
if(current_loc.alt > avoid.get_zr_avd_alt()) |
|
do_avoid_action(); |
|
#else |
|
bool enable_avoid = false; |
|
static bool last_flag = false; |
|
|
|
if(avoid.get_zr_mode() > 0 && (control_mode == Mode::Number::BRAKE || control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::GUIDED || control_mode == Mode::Number::CIRCLE)){ |
|
if(far_from_home){ |
|
enable_avoid = true; |
|
} |
|
}else if(control_mode == Mode::Number::LOITER && !avoid.get_avoid_action() ){ |
|
if(avoid.get_zr_mode() == 3){ // 1: 避不开障碍直接返航，2：避不开降落，3：避不开返航+可以Loiter测试功能 |
|
if(far_from_home){ |
|
enable_avoid = true; |
|
} |
|
} |
|
} |
|
if(avoid.get_zr_mode() > 0 && last_flag != enable_avoid){ |
|
if(enable_avoid){ |
|
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 开启自动避障!"); |
|
}else{ |
|
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 关闭自动避障!"); |
|
} |
|
last_flag = enable_avoid; |
|
} |
|
if(enable_avoid){ |
|
do_avoid_action(); |
|
}else{ |
|
avoid_action_step = PROXY_WAIT; |
|
} |
|
#endif |
|
} |
|
#endif |
|
|
|
#ifdef USERHOOK_SLOWLOOP |
|
void Copter::userhook_SlowLoop() |
|
{ |
|
// put your 3.3Hz code here |
|
zr_SlowLoop(); |
|
|
|
} |
|
#endif |
|
|
|
#ifdef USERHOOK_SUPERSLOWLOOP |
|
|
|
void Copter::userhook_SuperSlowLoop() |
|
{ |
|
// put your 1Hz code here |
|
zr_SuperSlowLoop(); |
|
zr_set_uav_state_to_uavcan(); |
|
zr_camera_mkdir(); |
|
if(current_loc.alt < avoid.get_zr_avd_alt()){ // 飞行高度小于避障启用高度 |
|
if(home_distance() < abs(avoid.get_zr_avd_on_dist())){ // 距离Home点小于启用距离 |
|
far_from_home = false; |
|
}else{ |
|
far_from_home = true; |
|
} |
|
alt_too_low = true; |
|
}else{ |
|
far_from_home = true; |
|
alt_too_low = false; |
|
} |
|
avoid.set_enable_aviod(far_from_home); // 远离Home点才启用避障 |
|
/// 手动避障开启提示 |
|
static bool last_manual_avoid = avoid.get_avoid_action(); |
|
if(last_manual_avoid != avoid.get_avoid_action()){ |
|
if(avoid.get_avoid_action()){ |
|
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 开启手动避障!"); |
|
}else{ |
|
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 关闭手动避障!"); |
|
} |
|
last_manual_avoid = avoid.get_avoid_action(); |
|
} |
|
#if CAM_DEBUG |
|
if(in_debug_mode == 2 || control_mode==Mode::Number::ACRO){ |
|
camera.take_picture(); |
|
} |
|
#endif |
|
|
|
} |
|
#endif |
|
|
|
#ifdef USERHOOK_AUXSWITCH |
|
void Copter::userhook_auxSwitch1(uint8_t ch_flag) |
|
{ |
|
// put your aux switch #1 handler here (CHx_OPT = 47) |
|
|
|
// switch (ch_flag) { |
|
// case 2: { |
|
// relay.on(2); |
|
// break; |
|
// } |
|
// case 0: { |
|
// relay.off(2); |
|
// break; |
|
// } |
|
// } |
|
switch (ch_flag) { |
|
case 2: { |
|
// relay.on(2); |
|
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 2!"); |
|
in_debug_mode = 1; |
|
break; |
|
} |
|
// case 1: { |
|
// // relay.on(2); |
|
// gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 1!"); |
|
// in_debug_mode = 1; |
|
// break; |
|
// } |
|
case 0: { |
|
// relay.off(2); |
|
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode off!"); |
|
in_debug_mode = 0; |
|
break; |
|
} |
|
} |
|
#if CAM_DEBUG |
|
switch (ch_flag) { |
|
case 2: { |
|
// relay.on(2); |
|
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 2!"); |
|
in_debug_mode = 2; |
|
break; |
|
} |
|
// case 1: { |
|
// // relay.on(2); |
|
// gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 1!"); |
|
// in_debug_mode = 1; |
|
// break; |
|
// } |
|
case 0: { |
|
// relay.off(2); |
|
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode off!"); |
|
in_debug_mode = 0; |
|
break; |
|
} |
|
} |
|
#endif |
|
} |
|
|
|
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) |
|
} |
|
|
|
void Copter::zr_set_uav_state_to_uavcan() |
|
{ |
|
#if HAL_WITH_UAVCAN |
|
uint8_t can_num_drivers = AP::can().get_num_drivers(); |
|
for (uint8_t i = 0; i < can_num_drivers; i++) |
|
{ |
|
AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); |
|
if (uavcan != nullptr) |
|
{ |
|
zr_uav_state_data_t data{0}; |
|
data.armd = motors->armed(); |
|
data.fly_status = copter.updownStatus; |
|
data.gps_state = AP::gps().status(); |
|
data.wp_number = g.hardware_flag&0xFF;//硬件版本 |
|
uavcan->set_zr_uav_state(data); |
|
} |
|
} |
|
#endif |
|
} |
|
|
|
|
|
void Copter::zr_camera_mkdir() |
|
{ |
|
|
|
if (motors->armed()) |
|
{ |
|
switch(mkdir_step) |
|
{ |
|
case 0: |
|
zr_mkdir_in_takeoff(); |
|
break; |
|
case 1: //起飞已经新建文件夹,判断是否返航或者降落 是新建文件夹 |
|
if (control_mode == Mode::Number::LAND) |
|
{ |
|
AP_Camera *cam = AP::camera(); |
|
if (cam != nullptr) |
|
{ |
|
cam->create_new_folder(9); |
|
mkdir_step = 2; |
|
} |
|
} |
|
break; |
|
case 2: |
|
break; |
|
default: |
|
break; |
|
} |
|
} |
|
else |
|
{ |
|
mkdir_step = 0; |
|
} |
|
} |
|
|
|
void Copter::zr_mkdir_in_takeoff() |
|
{ |
|
Location loc; |
|
if (AP::ahrs().get_position(loc)) |
|
{ |
|
int32_t alt_res; |
|
if (loc.relative_alt) |
|
{ |
|
alt_res = loc.alt; |
|
} |
|
else |
|
{ |
|
alt_res = loc.alt - AP::ahrs().get_home().alt; |
|
} |
|
if (alt_res > 1000) //相对高度>10m 新建文件夹 |
|
{ |
|
mkdir_step = 1; |
|
AP_Camera *cam = AP::camera(); |
|
if (cam != nullptr) |
|
{ |
|
cam->create_new_folder(10); |
|
} |
|
} |
|
} |
|
} |
|
|
|
void Copter::takeoff_crash_detect() |
|
{ |
|
|
|
static uint16_t crash_counter; // number of iterations vehicle may have been crashed |
|
// return immediately if disarmed, or crash checking disabled |
|
if (!motors->armed() || g.fs_crash_check == 0) { |
|
crash_counter = 0; |
|
return; |
|
} |
|
|
|
if(g.zr_land_lock_att && rangefinder_alt_ok() && g.land_lock_alt \ |
|
&& current_loc.alt < g.land_slow_2nd_alt \ |
|
&& rangefinder_state.alt_cm_filt.get() < g.land_lock_alt){ |
|
|
|
// check for lean angle over 15 degrees 角度判断，角度小于稳定飞行值退出 |
|
const float lean_angle_deg = degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch())); |
|
if (lean_angle_deg > float(g.zr_takeoff_prt_deg)) { |
|
crash_counter +=1 ; |
|
}else{ |
|
crash_counter = 0; |
|
} |
|
|
|
// // check for angle error over 30 degrees |
|
const float angle_error = attitude_control->get_att_error_angle_deg(); |
|
if (angle_error > float(g.zr_takeoff_prt_deg)/2.0) { |
|
crash_counter +=1 ; |
|
}else{ |
|
crash_counter = 0; |
|
} |
|
// check if crashing for 2 seconds |
|
AP::logger().Write("TKPT", "TimeUS,Ldeg,Aerr,Alt", "Qfff", |
|
AP_HAL::micros64(), |
|
lean_angle_deg, |
|
angle_error, |
|
rangefinder_state.alt_cm_filt.get()); |
|
|
|
if (crash_counter >= (g.zr_takeoff_prt_ps)) { |
|
AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH); |
|
// keep logging even if disarmed: |
|
AP::logger().set_force_log_disarmed(true); |
|
// send message to gcs |
|
gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Takeoff crash protect");//Crash: Disarming |
|
// disarm motors |
|
arming.disarm(); |
|
} |
|
} |
|
} |
|
#endif
|
|
|