aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: Beeping when first reaching waypoint while in holding there

Browse Source
This commit is contained in:
apinxiko 2018-06-04 12:42:43 +09:00 committed by Randy Mackay
parent 5df4b9f6fd
commit e440583a7c
1 changed files with 19 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
ArduCopter/mode_auto.cpp
View File

@ -565,12 +565,12 @@ void Copter::ModeAuto::exit_mission()
// play a tone // play a tone
AP_Notify::events.mission_complete = 1; AP_Notify::events.mission_complete = 1;
// if we are not on the ground switch to loiter or land // if we are not on the ground switch to loiter or land
if(!ap.land_complete) { if (!ap.land_complete) {
// try to enter loiter but if that fails land // try to enter loiter but if that fails land
if(!loiter_start()) { if (!loiter_start()) {
set_mode(LAND, MODE_REASON_MISSION_END); set_mode(LAND, MODE_REASON_MISSION_END);
} }
}else{ } else {
// if we've landed it's safe to disarm // if we've landed it's safe to disarm
copter.init_disarm_motors(); copter.init_disarm_motors();
} }
@ -645,7 +645,7 @@ Return true if we do not recognize the command so that we move on to the next co
bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
{ {
switch(cmd.id) { switch (cmd.id) {
// //
// navigation commands // navigation commands
// //
@ -919,7 +919,7 @@ void Copter::ModeAuto::loiter_run()
// accept pilot input of yaw // accept pilot input of yaw
float target_yaw_rate = 0; float target_yaw_rate = 0;
if(!copter.failsafe.radio) { if (!copter.failsafe.radio) {
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
} }
@ -1112,7 +1112,7 @@ void Copter::ModeAuto::do_land(const AP_Mission::Mission_Command& cmd)
Location_Class target_loc = terrain_adjusted_location(cmd); Location_Class target_loc = terrain_adjusted_location(cmd);
wp_start(target_loc); wp_start(target_loc);
}else{ } else {
// set landing state // set landing state
land_state = LandStateType_Descending; land_state = LandStateType_Descending;
@ -1267,7 +1267,7 @@ void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
// if the next nav command is a waypoint set end type to spline or straight // if the next nav command is a waypoint set end type to spline or straight
if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT) { if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT) {
seg_end_type = AC_WPNav::SEGMENT_END_STRAIGHT; seg_end_type = AC_WPNav::SEGMENT_END_STRAIGHT;
}else if (temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) { } else if (temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) {
seg_end_type = AC_WPNav::SEGMENT_END_SPLINE; seg_end_type = AC_WPNav::SEGMENT_END_SPLINE;
} }
} }
@ -1354,7 +1354,7 @@ void Copter::ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd) void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
{ {
if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) { if (cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
copter.set_home_to_current_location(false); copter.set_home_to_current_location(false);
} else { } else {
copter.set_home(cmd.content.location, false); copter.set_home(cmd.content.location, false);
@ -1374,7 +1374,7 @@ void Copter::ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd) void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
{ {
#if MOUNT == ENABLED #if MOUNT == ENABLED
if(!copter.camera_mount.has_pan_control()) { if (!copter.camera_mount.has_pan_control()) {
auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,0); auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,0);
} }
copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw); copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);
@ -1728,7 +1728,7 @@ bool Copter::ModeAuto::verify_loiter_time()
} }
// start our loiter timer // start our loiter timer
if( loiter_time == 0 ) { if ( loiter_time == 0 ) {
loiter_time = millis(); loiter_time = millis();
} }
@ -1782,35 +1782,32 @@ bool Copter::ModeAuto::verify_yaw()
bool Copter::ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) bool Copter::ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{ {
// check if we have reached the waypoint // check if we have reached the waypoint
if( !copter.wp_nav->reached_wp_destination() ) { if ( !copter.wp_nav->reached_wp_destination() ) {
return false; return false;
} }
// start timer if necessary // start timer if necessary
if(loiter_time == 0) { if (loiter_time == 0) {
loiter_time = millis(); loiter_time = millis();
if(loiter_time_max > 0) { if (loiter_time_max > 0) {
// play a tone // play a tone
AP_Notify::events.waypoint_complete = 1; AP_Notify::events.waypoint_complete = 1;
gcs().send_text(MAV_SEVERITY_INFO, "Delay in arrival at command #%i",cmd.index);
} }
} }
// check if timer has run out // check if timer has run out
if (((millis() - loiter_time) / 1000) >= loiter_time_max) { if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
if(loiter_time_max == 0) { if (loiter_time_max == 0) {
// play a tone // play a tone
AP_Notify::events.waypoint_complete = 1; AP_Notify::events.waypoint_complete = 1;
} }
gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index); gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
return true; return true;
}else{ } else {
return false; return false;
} }
} }
// verify_circle - check if we have circled the point enough // verify_circle - check if we have circled the point enough
bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
{ {
@ -1841,18 +1838,16 @@ bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
return fabsf(copter.circle_nav->get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1); return fabsf(copter.circle_nav->get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
} }
// verify_spline_wp - check if we have reached the next way point using spline // verify_spline_wp - check if we have reached the next way point using spline
bool Copter::ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd) bool Copter::ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
{ {
// check if we have reached the waypoint // check if we have reached the waypoint
if( !copter.wp_nav->reached_wp_destination() ) { if ( !copter.wp_nav->reached_wp_destination() ) {
return false; return false;
} }
// start timer if necessary // start timer if necessary
if(loiter_time == 0) { if (loiter_time == 0) {
loiter_time = millis(); loiter_time = millis();
} }
@ -1860,7 +1855,7 @@ bool Copter::ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
if (((millis() - loiter_time) / 1000) >= loiter_time_max) { if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index); gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
return true; return true;
}else{ } else {
return false; return false;
} }
} }