APMrover V2.0b - updates about Save_WP

Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>

APMrover2/APM_Config_Rover.h

@@ -12,7 +12,7 @@
 
 #define AIRSPEED_SENSOR	    DISABLED
 #define MAGNETOMETER	    ENABLED
-#define LOGGING_ENABLED	    DISABLED
+#define LOGGING_ENABLED	    ENABLED
 
 //////////////////////////////////////////////////////////////////////////////
 // Serial port speeds.
@@ -413,7 +413,7 @@
 // Also, set the value of USE_CURRENT_ALT in meters.  This is mainly intended to allow
 // users to start using the APM without running the WaypointWriter first.
 //
-#define WP_RADIUS_DEFAULT       3   // meters
+#define WP_RADIUS_DEFAULT       5   // meters
 #define LOITER_RADIUS_DEFAULT	10   // 60
 #define USE_CURRENT_ALT		TRUE
 #define ALT_HOLD_HOME		0

APMrover2/APMrover2.pde

@@ -16,7 +16,7 @@ License as published by the Free Software Foundation; either
 version 2.1 of the License, or (at your option) any later version.
 
 //
-// JLN updates: last update 2012-04-30
+// JLN updates: last update 2012-05-01
 //
 // DOLIST:
 //  
@@ -24,6 +24,7 @@ version 2.1 of the License, or (at your option) any later version.
 //-------------------------------------------------------------------------------------------------------------------------
 // Dev Startup : 2012-04-21
 //
+//  2012-04-30: Successfully tested in autonomous nav with a waypoints list recorded in live mode
 //  2012-04-30: Now a full version for APM v1 or APM v2 with magnetometer
 //  2012-04-27: Cosmetic changes
 //  2012-04-26: Only one PID (pidNavRoll) for steering the wheel with nav_roll
@@ -48,7 +49,7 @@ version 2.1 of the License, or (at your option) any later version.
 // Ch3: to the motor ESC
 // Ch4: not used
 //
-// more infos this experimental version: http://diydrones.com/profile/JeanLouisNaudin
+// more infos about this experimental version: http://diydrones.com/profile/JeanLouisNaudin
 // =======================================================================================================
 */
 

APMrover2/control_modes.pde

@@ -66,7 +66,10 @@ if (g.ch7_option == CH7_SAVE_WP){         // set to 1
 				if(control_mode == MANUAL){          // if SW7 is ON in MANUAL = Erase the Flight Plan
 					// reset the mission
 					CH7_wp_index = 0;
-					g.command_total.set_and_save(CH7_wp_index);                                       
+					g.command_total.set_and_save(CH7_wp_index);
+                                        g.command_total = 0;
+                                        g.command_index =0;
+                                        nav_command_index = 0;
                                         #if X_PLANE == ENABLED
                                                 Serial.printf_P(PSTR("*** RESET the FPL\n"));
                                         #endif
@@ -81,7 +84,10 @@ if (g.ch7_option == CH7_SAVE_WP){         // set to 1
     
                                   // store the index
                                   g.command_total.set_and_save(CH7_wp_index);
-                                  
+                                  g.command_total = CH7_wp_index;
+                                  g.command_index = CH7_wp_index;
+                                  nav_command_index = 0;
+                                   
         			  // save command
         			  set_cmd_with_index(current_loc, CH7_wp_index);
                                                                              

APMrover2/navigation.pde

@@ -63,7 +63,7 @@ static void navigate()
 // Disabled for now
 void calc_distance_error()
 {
-	distance_estimate 	+= (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error * .01));
+	distance_estimate 	+= (float)ground_speed * .0002 * cos(radians(bearing_error * .01));
 	distance_estimate 	-= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
 	wp_distance  		= max(distance_estimate,10);
 }
@@ -118,7 +118,7 @@ static long wrap_180(long error)
 
 static void calc_turn_radius()    // JLN update - adjut automaticaly the wp_radius Vs the speed and the turn angle
 {
-  wp_radius = g_gps->ground_speed * 150 / g.roll_limit.get();
+  wp_radius = ground_speed * 150 / g.roll_limit.get();
   //Serial.println(wp_radius, DEC);
 }
 

APMrover2/system.pde

@@ -349,8 +349,8 @@ static void set_mode(byte mode)
 			break;
 
 		case AUTO:
-                        reload_commands();
-			update_auto();
+                        change_command(1); // restart to the 1st stored wp
+			//update_auto();
 			break;
 
 		case RTL:
@@ -366,7 +366,7 @@ static void set_mode(byte mode)
 			break;
 
 		default:
-			//do_RTL();
+			do_RTL();
 			break;
 	}