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
Unified 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)
void Copter::update_updownStatus2(uint8_t status) void Copter::update_updownStatus2(uint8_t status)
{ {
copter.updownStatus = (UpDownState)status; copter.updownStatus = (UpDownState)status;
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus2:%d",copter.updownStatus);
if (status == 3) if (status == 3)
{ {
set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD); set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD);

1
ArduCopter/mode.h
Unescape View File

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

123
ArduCopter/mode_auto.cpp
Unescape View File

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

13
ArduCopter/mode_rtl.cpp
Unescape View File

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

Write Preview
Loading…
Cancel
Save