zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

起降流程优化，自动模式起飞悬停优化

celiu-4.0.17-rc8
孤帆远影Faraway 3 years ago committed by zbr
parent
cf8864e38a
commit
0acf4903a0
4 changed files with 91 additions and 48 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      ArduCopter/mode.cpp
  2. 1
      ArduCopter/mode.h
  3. 123
      ArduCopter/mode_auto.cpp
  4. 13
      ArduCopter/mode_rtl.cpp

2
ArduCopter/mode.cpp
Unescape View File

@ -848,6 +848,8 @@ bool Copter::update_updownStatus(const uint8_t status) @@ -848,6 +848,8 @@ bool Copter::update_updownStatus(const uint8_t status)
void Copter::update_updownStatus2(uint8_t status)
{
copter.updownStatus = (UpDownState)status;
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus2:%d",copter.updownStatus);
if (status == 3)
{
set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD);

1
ArduCopter/mode.h
Unescape View File

@ -509,6 +509,7 @@ private: @@ -509,6 +509,7 @@ private:
uint16_t wp_nav_index; // nav 航点计数
bool first_wp_dec; // nav 航点计数
uint8_t updown_state; // 起降状态
Location last_loc{};
};

123
ArduCopter/mode_auto.cpp
Unescape View File

@ -46,7 +46,9 @@ bool ModeAuto::init(bool ignore_checks) @@ -46,7 +46,9 @@ bool ModeAuto::init(bool ignore_checks)
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
wp_nav_index = 0; // nav 重新开始计数
updown_state = 0; // 起飞悬停状态清0
first_wp_dec = false;
gcs().send_text(MAV_SEVERITY_INFO, "auto init:up:%d,wp:%d",updown_state,wp_nav_index);
return true;
} else {
return false;
@ -145,8 +147,12 @@ void ModeAuto::takeoff_start(const Location& dest_loc) @@ -145,8 +147,12 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
Location dest(dest_loc);
// gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: dl:%d,%d,%d",dest_loc.lat,dest_loc.lng,dest_loc.alt);
// last_loc = dest_loc;
last_loc = dest_loc;
if (g.zr_tk_req_alt > 0 && updown_state == 0){
updown_state = 1;
dest.alt = g.zr_tk_req_alt;
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
}
if (!copter.current_loc.initialised()) {
// vehicle doesn't know where it is ATM. We should not
// initialise our takeoff destination without knowing this!
@ -227,6 +233,7 @@ void ModeAuto::wp_start(const Location& dest_loc) @@ -227,6 +233,7 @@ void ModeAuto::wp_start(const Location& dest_loc)
if (copter.updownStatus < UpDown_InMission)
{
copter.updownStatus = UpDown_InMission;
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
gcs().send_message(MSG_ZR_FLYING_STATUS);
}
}
@ -757,36 +764,53 @@ void ModeAuto::takeoff_run() @@ -757,36 +764,53 @@ void ModeAuto::takeoff_run()
{
if (motors->armed() || copter.ap.auto_armed)
{
int32_t alt_rev = 0;
if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_rev))
if(g.zr_tk_delay > 0 && g.zr_tk_req_alt > 0 && \
copter.updownStatus < UpDown_RequestClimb && \
updown_state == 2)
{
if (g.zr_tk_req_alt > 0 && alt_rev >= g.zr_tk_req_alt
&& copter.updownStatus < UpDown_RequestClimb)
{
copter.updownStatus = UpDown_RequestClimb;
countdown = g.zr_tk_delay;
gcs().send_message(MSG_ZR_FLYING_STATUS);
last_takeoff_request_time = AP_HAL::millis();
if(g.zr_tk_delay>0){
is_takeoff_delay_enable =true;
}else{
is_takeoff_delay_enable =false;
updown_state = 3;
copter.updownStatus = UpDown_RequestClimb;
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
gcs().send_message(MSG_ZR_FLYING_STATUS);
countdown = g.zr_tk_delay;
Vector3f stopping_point;
wp_nav->get_wp_stopping_point(stopping_point);
// initialise waypoint controller target to stopping point
wp_nav->set_wp_destination(stopping_point);
// hold yaw at current heading
auto_yaw.set_mode(AUTO_YAW_HOLD);
}else if(copter.position_ok() && copter.updownStatus == UpDown_RequestClimb ){
if ((AP_HAL::millis() - last_takeoff_request_time) > 1000){
last_takeoff_request_time = AP_HAL::millis();
if (countdown > 0){
gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: 继续任务倒计时(秒)：%d", countdown);
}
set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD);
}
else if (g.zr_tk_req_alt > 0 && alt_rev < g.zr_tk_req_alt
&& alt_rev > 0
&&(AP_HAL::millis()-last_time_send_updownload>1000))
{
last_time_send_updownload = AP_HAL::millis();
copter.updownStatus = UpDown_TakeOffClimb;
// gcs().send_message(MSG_ZR_FLYING_STATUS); //TODO
}
}
else if (countdown <= 0){
gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: 执行继续任务");
copter.updownStatus = UpDown_ContinueClimb;
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
gcs().send_message(MSG_ZR_FLYING_STATUS);
updown_state = 4;
takeoff_start(last_loc);
}
countdown--; // TODO Actually 1 second longer
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint and z-axis position controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
pos_control->update_z_controller();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0);
return;
}else if(copter.updownStatus > UpDown_RequestClimb){
updown_state = 4;
// gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: 执行继续任务");
}
}
if(g.zr_8_circle == 1){
const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
@ -933,8 +957,8 @@ void ModeAuto::circle_run() @@ -933,8 +957,8 @@ void ModeAuto::circle_run()
if(circle_init_yaw == false){ // 如果需要转向到下一个航点
if (abs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) { //转到角度
circle_init_yaw = true;
gcs().send_text(MAV_SEVERITY_INFO, "[%d]init ok:%d -- %d, %d",circle_init_yaw,initial_armed_bearing,ahrs.yaw_sensor,abs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)));
}else{ // 转向
gcs().send_text(MAV_SEVERITY_INFO, "circ init: y: %d -- %d ",initial_armed_bearing,ahrs.yaw_sensor);
}else{
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), initial_armed_bearing ,true);
return;
}
@ -1176,6 +1200,9 @@ Location ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_Command& @@ -1176,6 +1200,9 @@ Location ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_Command&
void ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd)
{
// Set wp navigation target to safe altitude above current position
// 如果是Mission Start进入Auto,是先执行这里 ，再执行初始化
wp_nav_index = 0; // nav 重新开始计数
updown_state = 0; // 起飞悬停状态清0
takeoff_start(cmd.content.location);
}
@ -1650,7 +1677,22 @@ bool ModeAuto::verify_takeoff() @@ -1650,7 +1677,22 @@ bool ModeAuto::verify_takeoff()
}else{ // 常规起飞判断，增加设置绕圈相关初始设置
copter.circle_nav->set_circle_8_finish(false);
first_wp_dec = false;
return reached_wp_dest;
if(g.zr_tk_req_alt > 0 ){
if(reached_wp_dest && updown_state <2){
updown_state = 2;
}else if(updown_state >= 4){
return reached_wp_dest;
}
static uint8_t last_state = 0;
if(last_state != updown_state){
last_state = updown_state;
}
return false;
}else{
return reached_wp_dest;
}
}
}
@ -1956,23 +1998,18 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) @@ -1956,23 +1998,18 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
// play a tone
AP_Notify::events.waypoint_complete = 1;
}
wp_nav_index += 1; // nav 计数，用于是否第一个航点绕8字判断
if(g.zr_8_circle == 2 && !first_wp_dec){
bool cyc_ret = copter.circle_nav->get_circle_8_finish();
if(cyc_ret){
// calculate stopping point
Vector3f stopping_point;
wp_nav->get_wp_stopping_point(stopping_point);
// initialise waypoint controller target to stopping point
wp_nav->set_wp_destination(stopping_point);
gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i,nav:%d",cmd.index,wp_nav_index);
first_wp_dec = true;
gcs().send_text(MAV_SEVERITY_INFO, "circle ok,Reached command #%i,nav:%d",cmd.index,wp_nav_index);
first_wp_dec = true;
wp_nav_index += 1; // nav 计数
}
return cyc_ret;
}else{
gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i,nav:%d",cmd.index,wp_nav_index);
wp_nav_index += 1; // nav 计数
return true;
}
}

13
ArduCopter/mode_rtl.cpp
Unescape View File

@ -99,8 +99,9 @@ if(g.zr_8_circle == 2){ @@ -99,8 +99,9 @@ if(g.zr_8_circle == 2){
if (copter.updownStatus < UpDown_RequestDescent)
{
copter.updownStatus = UpDown_RequestDescent;
countdown = g.zr_rtl_delay;
gcs().send_message(MSG_ZR_FLYING_STATUS);
countdown = g.zr_rtl_delay;
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
gcs().send_message(MSG_ZR_FLYING_STATUS);
last_decent_time = AP_HAL::millis();
if(g.zr_rtl_delay>0){
is_rtl_delay_enable = true;
@ -118,7 +119,8 @@ if(g.zr_8_circle == 2){ @@ -118,7 +119,8 @@ if(g.zr_8_circle == 2){
else if(countdown<=0){
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 开始着陆");
copter.updownStatus = UpDown_ContinueDescent;
gcs().send_message(MSG_ZR_FLYING_STATUS);
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
gcs().send_message(MSG_ZR_FLYING_STATUS);
_state = RTL_Land;
}
countdown --; //TODO shiji countdown+1 s
@ -326,8 +328,9 @@ void ModeRTL::loiterathome_run() @@ -326,8 +328,9 @@ void ModeRTL::loiterathome_run()
// rtl_descent_start - initialise descent to final alt
void ModeRTL::descent_start()
{
copter.updownStatus = UpDown_RTLDescent;
gcs().send_message(MSG_ZR_FLYING_STATUS);
copter.updownStatus = UpDown_RTLDescent;
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
gcs().send_message(MSG_ZR_FLYING_STATUS);
_state = RTL_FinalDescent;
_state_complete = false;

Write Preview
Loading…
Cancel
Save