diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index bf3e6f0b21..5943a7bdbb 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -257,6 +257,7 @@ int climb_rate; // m/s * 100 - For future implementation of controlled a float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? int last_ground_speed; // used to dampen navigation +byte wp_control; // used to control - navgation or loiter byte command_must_index; // current command memory location byte command_may_index; // current command memory location @@ -327,9 +328,6 @@ byte yaw_tracking = TRACK_NONE; // no tracking, point at next wp, or at a tar // Loiter management // ----------------- long saved_target_bearing; // deg * 100 -//int loiter_total; // deg : how many times to loiter * 360 -//int loiter_delta; // deg : how far we just turned -//int loiter_sum; // deg : how far we have turned around a waypoint long loiter_time; // millis : when we started LOITER mode long loiter_time_max; // millis : how long to stay in LOITER mode @@ -591,21 +589,6 @@ void medium_loop() // -------------------------------------------- update_navigation(); } - - // we call these regardless of GPS because of the rapid nature of the yaw sensor - // ----------------------------------------------------------------------------- - //if(wp_distance < 800){ // 8 meters - //if(g.rc_6.control_in > 500){ // 8 meters - //calc_loiter_nav(); - // calc_waypoint_nav(); - //}else{ - // calc_waypoint_nav(); - //} - invalid_nav = true; - - // replace with invalidater byte - //calc_waypoint_nav(); - break; // command processing @@ -946,7 +929,6 @@ void update_current_flight_mode(void) } break; - //case LOITER: case STABILIZE: // clear any AP naviagtion values nav_pitch = 0; @@ -1107,11 +1089,17 @@ void update_navigation() if(control_mode == AUTO || control_mode == GCS_AUTO){ verify_commands(); - // calc a rate dampened pitch to the target - calc_rate_nav(); + if(wp_control == LOITER_MODE){ + // calc a pitch to the target + calc_loiter_nav(); - // rotate that pitch to the copter frame of reference - calc_nav_output(); + } else { + // calc a rate dampened pitch to the target + calc_rate_nav(); + + // rotate that pitch to the copter frame of reference + calc_nav_output(); + } //limit our copter pitch - this will change if we go to a fully rate limited approach. limit_nav_pitch_roll(g.pitch_max.get()); @@ -1122,8 +1110,22 @@ void update_navigation() } }else{ + switch(control_mode){ + case LOITER: + // calc a pitch to the target + calc_loiter_nav(); + + // limit tilt + limit_nav_pitch_roll(g.pitch_max.get()); + break; + case RTL: + // calc a pitch to the target + calc_loiter_nav(); + + // limit tilt + limit_nav_pitch_roll(g.pitch_max.get()); update_crosstrack(); break; } @@ -1169,6 +1171,8 @@ void update_alt() // read barometer baro_alt = read_barometer(); + + // XXX temp removed fr debugging //filter out bad sonar reads //int temp = sonar.read(); @@ -1182,12 +1186,15 @@ void update_alt() update_sonar_light(sonar_alt > 100); // decide if we're using sonar - if (baro_alt < 1200){ + if ((baro_alt < 1200) || sonar_alt < 300){ // correct alt for angle of the sonar - sonar_alt = (float)sonar_alt * (cos_pitch_x * cos_roll_x); + float temp = cos_pitch_x * cos_roll_x; + temp = max(temp, 0.707); - if(sonar_alt < 500){ + sonar_alt = (float)sonar_alt * temp; + + if(sonar_alt < 400){ altitude_sensor = SONAR; } } diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index 73d5df347c..4617773954 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -193,11 +193,13 @@ bool verify_may() } /********************************************************************************/ -// Nav (Must) commands +// /********************************************************************************/ void do_RTL(void) { + // we need to change this. + wp_control = LOITER_MODE; control_mode = LOITER; Location temp = home; temp.alt = read_alt_to_hold(); @@ -216,8 +218,14 @@ void do_RTL(void) // Log_Write_Mode(control_mode); } +/********************************************************************************/ +// Nav (Must) commands +/********************************************************************************/ + void do_takeoff() { + wp_control = LOITER_MODE; + Location temp = current_loc; // next_command.alt is a relative altitude!!! @@ -233,6 +241,8 @@ void do_takeoff() void do_nav_wp() { + wp_control = WP_MODE; + // next_command.alt is a relative altitude!!! next_command.alt += home.alt; @@ -256,6 +266,8 @@ void do_nav_wp() void do_land() { + wp_control = LOITER_MODE; + Serial.println("dlnd "); // not really used right now, might be good for debugging @@ -276,6 +288,8 @@ void do_land() void do_loiter_unlimited() { + wp_control = LOITER_MODE; + //Serial.println("dloi "); if(next_command.lat == 0) set_next_WP(¤t_loc); @@ -286,6 +300,8 @@ void do_loiter_unlimited() void do_loiter_turns() { /* + wp_control = LOITER_MODE; + if(next_command.lat == 0) set_next_WP(¤t_loc); else @@ -298,6 +314,7 @@ void do_loiter_turns() void do_loiter_time() { /* + wp_control = LOITER_MODE; if(next_command.lat == 0) set_next_WP(¤t_loc); else @@ -393,6 +410,9 @@ bool verify_nav_wp() if(wp_verify_byte & NAV_LOCATION){ // we have reached our goal + // loiter at the WP + wp_control = LOITER_MODE; + if ((millis() - loiter_time) > loiter_time_max) { wp_verify_byte |= NAV_DELAY; //gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index da86f84aae..902df272c0 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -129,6 +129,9 @@ #define TRACK_NEXT_WP 2 #define TRACK_TARGET_WP 4 +#define LOITER_MODE 1 +#define WP_MODE 2 + // Waypoint options #define WP_OPTION_ALT_RELATIVE 1 #define WP_OPTION_ALT_CHANGE 2 diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 99cc3c68da..a652637d61 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -224,8 +224,6 @@ void update_crosstrack(void) crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); nav_bearing = wrap_360(nav_bearing); - }else{ - reset_crosstrack(); } }