diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index dc9c4256d1..b0e240114c 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -17,6 +17,7 @@ #define STABILIZE_PITCH_P 0.75 //#define STABILIZE_DAMPENER 0.1 +#define NAV_TEST 0 // 0 = traditional, 1 = rate controlled //#define CHANNEL_6_TUNING CH6_NONE #define CHANNEL_6_TUNING CH6_PMAX diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 5943a7bdbb..27b09d4bca 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -899,9 +899,6 @@ void update_current_flight_mode(void) nav_pitch = 0; nav_roll = 0; - // Output Pitch, Roll, Yaw and Throttle - // ------------------------------------ - // Yaw control output_manual_yaw(); @@ -1171,7 +1168,6 @@ void update_alt() // read barometer baro_alt = read_barometer(); - // XXX temp removed fr debugging //filter out bad sonar reads //int temp = sonar.read(); diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 5a5233b0c2..ab2eb915a5 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -108,11 +108,9 @@ void increment_WP_index() { if (g.waypoint_index < g.waypoint_total) { g.waypoint_index.set_and_save(g.waypoint_index + 1); - SendDebug("MSG WP index is incremented to "); - }else{ - //SendDebug("MSG Failed to increment WP index of "); - // This message is used excessively at the end of a mission + //SendDebug("MSG WP index is incremented to "); } + SendDebugln(g.waypoint_index,DEC); } diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index 4617773954..88aa3ae551 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -106,11 +106,7 @@ void handle_no_commands() { // we don't want to RTL. Maybe this will change in the future. RTL is kinda dangerous. // use landing commands - return; /* - if (command_must_ID) - return; - switch (control_mode){ default: //set_mode(RTL); @@ -226,6 +222,7 @@ void do_takeoff() { wp_control = LOITER_MODE; + // Start with current location Location temp = current_loc; // next_command.alt is a relative altitude!!! @@ -233,9 +230,7 @@ void do_takeoff() takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction - //Serial.print("dt "); - //Serial.println(temp.alt, DEC); - + // Set our waypoint set_next_WP(&temp); } diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index 4d0babe51c..c7bcb31bd0 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -18,13 +18,16 @@ void update_commands(void) next_command = get_command_with_index(g.waypoint_index + 1); if(next_command.id == NO_COMMAND){ + // if no commands were available from EEPROM // -------------------------------------------- + if (command_must_ID == NO_COMMAND) + handle_no_commands(); - handle_no_commands(); gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!")); } else { + // A command was loaded from EEPROM // -------------------------------------------- diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 2db3e390d4..50f9cd7419 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -368,33 +368,31 @@ // Navigation control gains // #ifndef NAV_LOITER_P -# define NAV_LOITER_P 2.0 +# define NAV_LOITER_P 2.5 // upped to be a bit more aggressive #endif #ifndef NAV_LOITER_I -# define NAV_LOITER_I 0.1 +# define NAV_LOITER_I 0.15 // upped a bit to deal with wind faster #endif #ifndef NAV_LOITER_D # define NAV_LOITER_D 0.00 #endif #ifndef NAV_LOITER_IMAX -# define NAV_LOITER_IMAX 250 // 250 Lat and Longtitude +# define NAV_LOITER_IMAX 20 // 20° #endif - #ifndef NAV_WP_P # define NAV_WP_P 4.0 #endif #ifndef NAV_WP_I -# define NAV_WP_I 0.0 +# define NAV_WP_I 0.0 // leave 0 #endif #ifndef NAV_WP_D -# define NAV_WP_D 15 +# define NAV_WP_D 15 // not sure about at all #endif #ifndef NAV_WP_IMAX # define NAV_WP_IMAX 20 // 20 degrees #endif - ////////////////////////////////////////////////////////////////////////////// // Throttle control gains // diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index a652637d61..0173cc5199 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -112,6 +112,20 @@ void calc_simple_nav() nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° } +void calc_nav_output() +{ + // get the sin and cos of the bearing error - rotated 90° + sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100)); + cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100)); + + // rotate the vector + nav_roll = (float)nav_lat * cos_nav_x; + nav_pitch = -(float)nav_lat * sin_nav_y; +} +#define WAYPOINT_SPEED 450 + +#if NAV_TEST == 0 + void calc_rate_nav() { // calc distance error @@ -133,22 +147,10 @@ void calc_rate_nav() } -void calc_nav_output() -{ - // get the sin and cos of the bearing error - rotated 90° - sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100)); - cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100)); - - // rotate the vector - nav_roll = (float)nav_lat * cos_nav_x; - nav_pitch = -(float)nav_lat * sin_nav_y; -} - -#define WAYPOINT_SPEED 450 - +#else // called after we get GPS read -void calc_rate_nav2() +void calc_rate_nav() { // change to rate error // we want to be going 450cm/s @@ -175,6 +177,7 @@ void calc_rate_nav2() nav_lat = constrain(nav_lat, -3800, 3800); // +- 20m max error } +#endif void calc_bearing_error() { @@ -220,7 +223,7 @@ void update_crosstrack(void) { // Crosstrack Error // ---------------- - if (cross_track_test() < 9000) { // If we are too far off or too close we don't do track following + if (cross_track_test() < 5000) { // If we are too far off or too close we don't do track following 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);