Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: bug fix to GUIDED mode. Now checks that desired altitude was reached before deciding the waypoint is complete.

Browse Source
This commit is contained in:
rmackay9 2012-12-22 14:04:47 +09:00 committed by Andrew Tridgell
parent e850ab7ccd
commit e77b54c7dc
5 changed files with 12 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/GCS_Mavlink.pde
View File

@ -1605,9 +1605,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
set_mode(GUIDED);
// make any new wp uploaded instant (in case we are already in Guided mode)
set_next_WP(&guided_WP);
// verify we recevied the command
mavlink_msg_mission_ack_send(
chan,

12
ArduCopter/commands_logic.pde
View File

@ -268,16 +268,17 @@ static void do_nav_wp()
// this is our bitmask to verify we have met all conditions to move on
wp_verify_byte = 0;
// if no alt requirement in the waypoint, set the altitude achieved bit of wp_verify_byte
if((next_WP.options & WP_OPTION_ALT_REQUIRED) == false) {
wp_verify_byte |= NAV_ALTITUDE;
}
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0;
// this is the delay, stored in seconds and expanded to millis
loiter_time_max = command_nav_queue.p1 * 1000;
if((next_WP.options & WP_OPTION_ALT_REQUIRED) == false) {
wp_verify_byte |= NAV_ALTITUDE;
}
// reset control of yaw to default
if( g.yaw_override_behaviour == YAW_OVERRIDE_BEHAVIOUR_AT_NEXT_WAYPOINT ) {
set_yaw_mode(AUTO_YAW);
@ -411,8 +412,7 @@ static bool verify_nav_wp()
}
}
if(wp_verify_byte >= 7) {
//if(wp_verify_byte & NAV_LOCATION){
if( wp_verify_byte == (NAV_LOCATION | NAV_ALTITUDE | NAV_DELAY) ) {
gcs_send_text_fmt(PSTR("Reached Command #%i"),command_nav_index);
wp_verify_byte = 0;
copter_leds_nav_blink = 15; // Cause the CopterLEDs to blink three times to indicate waypoint reached

4
ArduCopter/defines.h
View File

@ -179,8 +179,8 @@
#define CH6_AHRS_KP 31 // accelerometer effect on roll/pitch angle (0=low)
#define CH6_INAV_TC 32 // inertial navigation baro/accel and gps/accel time constant (1.5 = strong baro/gps correction on accel estimatehas very strong does not correct accel estimate, 7 = very weak correction)
// nav byte mask
// -------------
// nav byte mask used with wp_verify_byte variable
// -----------------------------------------------
#define NAV_LOCATION 1
#define NAV_ALTITUDE 2
#define NAV_DELAY 4

8
ArduCopter/navigation.pde
View File

@ -179,12 +179,8 @@ static void run_navigation_contollers()
break;
case GUIDED:
wp_control = WP_MODE;
// check if we are close to point > loiter
wp_verify_byte = 0;
verify_nav_wp();
if (wp_control != WP_MODE) {
// switch to loiter once we've reached the target location and altitude
if(verify_nav_wp()) {
set_mode(LOITER);
}
update_nav_wp();

3
ArduCopter/system.pde
View File

@ -476,7 +476,8 @@ static void set_mode(uint8_t mode)
set_yaw_mode(GUIDED_YAW);
set_roll_pitch_mode(GUIDED_RP);
set_throttle_mode(GUIDED_THR);
next_WP = current_loc;
wp_control = WP_MODE;
wp_verify_byte = 0;
set_next_WP(&guided_WP);
break;