updated Param gen - won't compile yet.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1668 f9c3cf11-9bcb-44bc-f272-b75c42450872

ArduCopterMega/EEPROM.pde

@@ -1,390 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
-//	************************************************************************************
-//	This function gets critical data from EEPROM to get us underway if restarting in air
-//	************************************************************************************
-
-// Macro functions
-// ---------------
-void read_EEPROM_startup(void)
-{
-	read_EEPROM_PID();
-	read_EEPROM_frame();
-	read_EEPROM_throttle();
-	read_EEPROM_logs();
-	read_EEPROM_flight_modes();
-	read_EEPROM_waypoint_info();
-	imu.load_gyro_eeprom();
-	imu.load_accel_eeprom();
-	read_EEPROM_alt_RTL();
-	read_EEPROM_current();
-	read_EEPROM_nav();
-	// magnatometer
-	read_EEPROM_compass();
-	read_EEPROM_compass_declination();
-	read_EEPROM_compass_offset();
-}
-
-void read_EEPROM_airstart_critical(void)
-{	
-	// Read the home location
-	//-----------------------	
-	home = get_wp_with_index(0);
-		
-	// Read pressure sensor values
-	//----------------------------
-	read_EEPROM_pressure();
-}
-
-void save_EEPROM_groundstart(void)
-{
-	rc_1.save_trim();
-	rc_2.save_trim();
-	rc_3.save_trim();
-	rc_4.save_trim();
-	rc_5.save_trim();
-	rc_6.save_trim();
-	rc_7.save_trim();
-	rc_8.save_trim();
-	
-	// pressure sensor data saved by init_home
-}
-
-/********************************************************************************/
-
-long read_alt_to_hold()
-{	
-	read_EEPROM_alt_RTL();
-	if(alt_to_hold == -1)
-		return current_loc.alt;
-	else
-		return alt_to_hold + home.alt;
-}
-
-/********************************************************************************/
-
-void save_EEPROM_alt_RTL(void)
-{
-	eeprom_write_dword((uint32_t *)EE_ALT_HOLD_HOME, alt_to_hold);
-}
-
-void read_EEPROM_alt_RTL(void)
-{
-	alt_to_hold = eeprom_read_dword((const uint32_t *)	EE_ALT_HOLD_HOME);
-}
-
-/********************************************************************************/
-
-void read_EEPROM_waypoint_info(void)
-{
-	wp_total 		= eeprom_read_byte((uint8_t *) EE_WP_TOTAL);				
-	wp_radius 		= eeprom_read_byte((uint8_t *) EE_WP_RADIUS);
-	loiter_radius 	= eeprom_read_byte((uint8_t *) EE_LOITER_RADIUS);
-}
-
-void save_EEPROM_waypoint_info(void)
-{
-	eeprom_write_byte((uint8_t *)	EE_WP_TOTAL, 		wp_total);
-	eeprom_write_byte((uint8_t *)	EE_WP_RADIUS, 		wp_radius);
-	eeprom_write_byte((uint8_t *)	EE_LOITER_RADIUS, 	loiter_radius);
-}
-
-/********************************************************************************/
-
-void read_EEPROM_nav(void)
-{
-	// for nav estimation
-	distance_gain 		= read_EE_compressed_float(EE_DISTANCE_GAIN, 4);
-	altitude_gain 		= read_EE_compressed_float(EE_ALTITUDE_GAIN, 4);
-
-	// stored as degree * 100
-	x_track_gain 		= read_EE_compressed_float(EE_XTRACK_GAIN, 4);
-	x_track_angle 		= eeprom_read_word((uint16_t *)	EE_XTRACK_ANGLE) * 100;
-	pitch_max 			= eeprom_read_word((uint16_t *) EE_PITCH_MAX);	// stored as degress * 100
-}
-
-void save_EEPROM_nav(void)
-{
-	// for nav estimation
-	write_EE_compressed_float(altitude_gain, 	EE_ALTITUDE_GAIN, 	4);
-	write_EE_compressed_float(distance_gain, 	EE_DISTANCE_GAIN, 	4);
-	write_EE_compressed_float(x_track_gain, 	EE_XTRACK_GAIN, 	4);
-	
-	// stored as degrees
- 	eeprom_write_word((uint16_t *)	EE_XTRACK_ANGLE, x_track_angle / 100);
-
-	// stored as degrees
-	eeprom_write_word((uint16_t *)	EE_PITCH_MAX, pitch_max);
-}
-
-/********************************************************************************/
-
-void read_EEPROM_PID(void)
-{
-	pid_acro_rate_roll.load_gains();
-	pid_acro_rate_pitch.load_gains();
-	pid_acro_rate_yaw.load_gains();
-	
-	pid_stabilize_roll.load_gains();
-	pid_stabilize_pitch.load_gains();
-	pid_yaw.load_gains();
-	
-	pid_nav_lon.load_gains();
-	pid_nav_lat.load_gains();
-	pid_baro_throttle.load_gains();
-	pid_sonar_throttle.load_gains();
-	
-	// roll pitch
-	stabilize_dampener 			= read_EE_compressed_float(EE_STAB_DAMPENER, 4);
-
-	// yaw
-	hold_yaw_dampener			= read_EE_compressed_float(EE_HOLD_YAW_DAMPENER, 4);
- 	init_pids();
-}
-
-void save_EEPROM_PID(void)
-{
-	pid_acro_rate_roll.save_gains(); 
-	pid_acro_rate_pitch.save_gains();
-	pid_acro_rate_yaw.save_gains();
-	
-	pid_stabilize_roll.save_gains();
-	pid_stabilize_pitch.save_gains();
-	pid_yaw.save_gains();
-	
-	pid_nav_lon.save_gains();
-	pid_nav_lat.save_gains();
-	pid_baro_throttle.save_gains();
-	pid_sonar_throttle.save_gains();
-
-	// roll pitch
-	write_EE_compressed_float(stabilize_dampener,	EE_STAB_DAMPENER, 4);
-
-	// yaw
-	write_EE_compressed_float(hold_yaw_dampener, 	EE_HOLD_YAW_DAMPENER, 4);
-}
-
-/********************************************************************************/
-
-void save_EEPROM_frame(void)
-{
-	eeprom_write_byte((uint8_t *)EE_FRAME, frame_type);
-}
-
-void read_EEPROM_frame(void)
-{
-	frame_type 	= eeprom_read_byte((uint8_t *)	EE_FRAME);
-}
-
-/********************************************************************************/
-
-void save_EEPROM_throttle_cruise(void)
-{
-	eeprom_write_word((uint16_t *)EE_THROTTLE_CRUISE, throttle_cruise);
-}
-
-void read_EEPROM_throttle_cruise(void)
-{
-	throttle_cruise 	= eeprom_read_word((uint16_t *)	EE_THROTTLE_CRUISE);
-}
-
-/********************************************************************************/
-
-void save_EEPROM_mag_declination(void)
-{
-	write_EE_compressed_float(mag_declination, 	EE_MAG_DECLINATION, 1);
-}
-
-void read_EEPROM_compass_declination(void)
-{
-	mag_declination 	= read_EE_compressed_float(EE_MAG_DECLINATION, 1);
-}
-
-/********************************************************************************/
-
-void save_EEPROM_current(void)
-{
-	eeprom_write_byte((uint8_t *)	EE_CURRENT_SENSOR, 	current_enabled);
-	eeprom_write_word((uint16_t *)	EE_CURRENT_MAH, 	milliamp_hours);
-	
-}
-
-void read_EEPROM_current(void)
-{
-	current_enabled	= eeprom_read_byte((uint8_t *) EE_CURRENT_SENSOR);
-	milliamp_hours	= eeprom_read_word((uint16_t *) EE_CURRENT_MAH);
-}
-
-/********************************************************************************/
-
-void save_EEPROM_mag_offset(void)
-{
-	write_EE_compressed_float(mag_offset_x, 	EE_MAG_X, 2);
-	write_EE_compressed_float(mag_offset_y, 	EE_MAG_Y, 2);
-	write_EE_compressed_float(mag_offset_z, 	EE_MAG_Z, 2);
-}
-
-void read_EEPROM_compass_offset(void)
-{
-	mag_offset_x 	= read_EE_compressed_float(EE_MAG_X, 2);
-	mag_offset_y 	= read_EE_compressed_float(EE_MAG_Y, 2);
-	mag_offset_z 	= read_EE_compressed_float(EE_MAG_Z, 2);
-}
-
-/********************************************************************************/
-
-void read_EEPROM_compass(void)
-{
-	compass_enabled	= eeprom_read_byte((uint8_t *) EE_COMPASS);
-}
-
-void save_EEPROM_mag(void)
-{
-	eeprom_write_byte((uint8_t *)	EE_COMPASS, 	compass_enabled);
-}
-
-/********************************************************************************/
-
-void save_command_index(void)
-{
-	eeprom_write_byte((uint8_t *) EE_WP_INDEX, command_must_index);
-}
-
-void read_command_index(void)
-{
-	wp_index = command_must_index 	= eeprom_read_byte((uint8_t *) EE_WP_INDEX);
-}
-
-/********************************************************************************/
-
-void save_EEPROM_pressure(void)
-{
-	eeprom_write_dword((uint32_t *)EE_ABS_PRESS_GND, 	abs_pressure_ground);
-	eeprom_write_word((uint16_t *)EE_GND_TEMP, 			ground_temperature);
-}
-
-void read_EEPROM_pressure(void)
-{
-	abs_pressure_ground = eeprom_read_dword((uint32_t *) 	EE_ABS_PRESS_GND);
-	// Better than zero for an air start value
-	abs_pressure	 	= abs_pressure_ground;
-	ground_temperature 	= eeprom_read_word((uint16_t *) 	EE_GND_TEMP);
-}
-
-/********************************************************************************/
-
-void read_EEPROM_radio(void)
-{
-	rc_1.load_eeprom();
-	rc_2.load_eeprom();
-	rc_3.load_eeprom();
-	rc_4.load_eeprom();
-	rc_5.load_eeprom();
-	rc_6.load_eeprom();
-	rc_7.load_eeprom();
-	rc_8.load_eeprom();
-}
-
-void save_EEPROM_radio(void)
-{	
-	rc_1.save_eeprom();
-	rc_2.save_eeprom();
-	rc_3.save_eeprom();
-	rc_4.save_eeprom();
-	rc_5.save_eeprom();
-	rc_6.save_eeprom();
-	rc_7.save_eeprom();
-	rc_8.save_eeprom();
-}
-
-/********************************************************************************/
-// configs are the basics
-void read_EEPROM_throttle(void)
-{
-	throttle_min 				= eeprom_read_word((uint16_t *) 	EE_THROTTLE_MIN);
-	throttle_max 				= eeprom_read_word((uint16_t *) 	EE_THROTTLE_MAX);
-	read_EEPROM_throttle_cruise();
-	throttle_failsafe_enabled 	= eeprom_read_byte((byte *) 	EE_THROTTLE_FAILSAFE);
-	throttle_failsafe_action 	= eeprom_read_byte((byte *) 	EE_THROTTLE_FAILSAFE_ACTION);
-	throttle_failsafe_value 	= eeprom_read_word((uint16_t *) EE_THROTTLE_FS_VALUE);
-}
-
-void save_EEPROM_throttle(void)
-{
-	eeprom_write_word((uint16_t *) 	EE_THROTTLE_MIN, 			throttle_min);
-	eeprom_write_word((uint16_t *) 	EE_THROTTLE_MAX, 			throttle_max);
-	save_EEPROM_throttle_cruise();
-	eeprom_write_byte((byte *) 		EE_THROTTLE_FAILSAFE,		throttle_failsafe_enabled);
-	eeprom_write_byte((byte *) 		EE_THROTTLE_FAILSAFE_ACTION,throttle_failsafe_action);
-	eeprom_write_word((uint16_t *) 	EE_THROTTLE_FS_VALUE,		throttle_failsafe_value);
-}
-
-/********************************************************************************/
-
-void read_EEPROM_logs(void)
-{
-	log_bitmask 				= eeprom_read_word((uint16_t *) EE_LOG_BITMASK);
-}
-
-void save_EEPROM_logs(void)
-{
-	eeprom_write_word((uint16_t *) 	EE_LOG_BITMASK,				log_bitmask);
-}
-
-/********************************************************************************/
-
-void read_EEPROM_flight_modes(void)
-{
-	// Read Radio min/max settings
-	eeprom_read_block((void*)&flight_modes, (const void*)EE_FLIGHT_MODES, sizeof(flight_modes));
-}
-
-void save_EEPROM_flight_modes(void)
-{
-	// Save Radio min/max settings
- 	eeprom_write_block((const void *)&flight_modes, (void *)EE_FLIGHT_MODES, sizeof(flight_modes));
-}
-
-/********************************************************************************/
-
-float
-read_EE_float(int address)
-{
-	union {
-		byte bytes[4];
-		float value;
-	} _floatOut;
-	
-	for (int i = 0; i < 4; i++) 
-		_floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i));
-	return _floatOut.value;
-}
-
-void write_EE_float(float value, int address)
-{
-	union {
-		byte bytes[4];
-		float value;
-	} _floatIn;
-	
-	_floatIn.value = value;
-	for (int i = 0; i < 4; i++) 
-		eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
-}
-
-/********************************************************************************/
-
-float
-read_EE_compressed_float(int address, byte places)
-{
-	float scale = pow(10, places);
-	
-	int temp 	= eeprom_read_word((uint16_t *) address);
-	return ((float)temp / scale);
-}
-
-void write_EE_compressed_float(float value, int address, byte places)
-{
-	float scale = pow(10, places);
-	int temp 	= value * scale;
-	eeprom_write_word((uint16_t *) 	address, temp);
-}

ArduCopterMega/EEPROM.txt

@@ -51,14 +51,6 @@ void save_EEPROM_groundstart(void)
 
 /********************************************************************************/
 
-long read_alt_to_hold()
-{	
-	read_EEPROM_alt_RTL();
-	if(g.RTL_altitude == -1)
-		return current_loc.alt;
-	else
-		return g.RTL_altitude + home.alt;
-}
 
 /********************************************************************************/
 

ArduCopterMega/commands.pde

@@ -3,7 +3,7 @@
 void init_commands()
 {
 	read_EEPROM_waypoint_info();
-	g.waypoint_index 			= 0;
+    g.waypoint_index.set_and_save(0);
 	command_must_index	= 0;
 	command_may_index	= 0;
 	next_command.id 	= CMD_BLANK;
@@ -24,14 +24,6 @@ void reload_commands()
 	decrement_WP_index();
 }
 
-long read_alt_to_hold()
-{	
-	read_EEPROM_alt_RTL();
-	if(g.RTL_altitude == -1)
-		return current_loc.alt;
-	else
-		return g.RTL_altitude + home.alt;
-}
 
 // Getters
 // -------
@@ -87,19 +79,29 @@ void set_wp_with_index(struct Location temp, int i)
 void increment_WP_index()
 {
 	if(g.waypoint_index < g.waypoint_total){
-		g.waypoint_index++;
+        g.waypoint_index.set_and_save(g.waypoint_index + 1);
 		Serial.printf_P(PSTR("WP index is incremented to %d\n"),g.waypoint_index);
 	}else{
 		Serial.printf_P(PSTR("WP Failed to incremented WP index of %d\n"),g.waypoint_index);
 	}
+    SendDebugln(g.waypoint_index,DEC);
 }
+
 void decrement_WP_index()
 {
 	if(g.waypoint_index > 0){
-		g.waypoint_index--;
+        g.waypoint_index.set_and_save(g.waypoint_index - 1);
 	}
 }
 
+long read_alt_to_hold()
+{	
+	read_EEPROM_alt_RTL();
+	if(g.RTL_altitude == -1)
+		return current_loc.alt;
+	else
+		return g.RTL_altitude + home.alt;
+}
 
 //********************************************************************************
 //This function sets the waypoint and modes for Return to Launch
@@ -114,7 +116,7 @@ return_to_launch(void)
 
 	// home is WP 0
 	// ------------
-	g.waypoint_index = 0;
+	g.waypoint_index.set_and_save(0);
 	
 	// Loads WP from Memory
 	// --------------------

ArduCopterMega/commands_process.pde

@@ -414,7 +414,7 @@ void verify_must()
 			break;
 		
 		case CMD_WAYPOINT:	// reach a waypoint
-			if ((wp_distance > 0) && (wp_distance <= wp_radius)) {
+			if ((wp_distance > 0) && (wp_distance <= waypoint_radius)) {
 				Serial.print("MSG REACHED_WAYPOINT #");
 				Serial.println(command_must_index,DEC);
 				// clear the command queue;
@@ -435,7 +435,7 @@ void verify_must()
 			break;
 			
 		case CMD_RTL:
-			if (wp_distance <= wp_radius) {
+			if (wp_distance <= waypoint_radius) {
 				//Serial.println("REACHED_HOME");
 				command_must_index 	= 0;
 			}

ArduCopterMega/motors.pde

@@ -43,7 +43,7 @@ set_servos_4()
 		int out_min = g.rc_3.radio_min;
 		
 		// Throttle is 0 to 1000 only
-		g.rc_3.servo_out 	= constrain(g.rc_3.servo_out, 0, 1000);	
+		g.rc_3.servo_out 	= constrain(g.rc_3.servo_out.get(), 0, 1000);	
 
 		if(g.rc_3.servo_out > 0)
 			out_min = g.rc_3.radio_min + 50;
@@ -141,14 +141,14 @@ set_servos_4()
 		
 		
 		// limit output so motors don't stop
-		motor_out[CH_1]		= constrain(motor_out[CH_1], 	out_min, g.rc_3.radio_max);
+		motor_out[CH_1]		= constrain(motor_out[CH_1], 	out_min, g.rc_3.radio_max.get());
-		motor_out[CH_2]		= constrain(motor_out[CH_2], 	out_min, g.rc_3.radio_max);
+		motor_out[CH_2]		= constrain(motor_out[CH_2], 	out_min, g.rc_3.radio_max.get());
-		motor_out[CH_3]		= constrain(motor_out[CH_3], 	out_min, g.rc_3.radio_max);
+		motor_out[CH_3]		= constrain(motor_out[CH_3], 	out_min, g.rc_3.radio_max.get());
-		motor_out[CH_4] 	= constrain(motor_out[CH_4], 	out_min, g.rc_3.radio_max);
+		motor_out[CH_4] 	= constrain(motor_out[CH_4], 	out_min, g.rc_3.radio_max.get());
 				
 		if (frame_type == HEXA_FRAME) {
-			motor_out[CH_7]		= constrain(motor_out[CH_7], 	out_min, g.rc_3.radio_max);
+			motor_out[CH_7]		= constrain(motor_out[CH_7], 	out_min, g.rc_3.radio_max.get());
-			motor_out[CH_8]		= constrain(motor_out[CH_8], 	out_min, g.rc_3.radio_max);
+			motor_out[CH_8]		= constrain(motor_out[CH_8], 	out_min, g.rc_3.radio_max.get());
 		}
 		
 		num++;

ArduCopterMega/navigation.pde

@@ -131,8 +131,8 @@ void calc_distance_error()
 void calc_altitude_error() 
 {
 	if(control_mode == AUTO && offset_altitude != 0) {
-		// limit climb rates - we draw a straight line between first location and edge of wp_radius
+		// limit climb rates - we draw a straight line between first location and edge of waypoint_radius
-		target_altitude = next_WP.alt - ((wp_distance * offset_altitude) / (wp_totalDistance - wp_radius));
+		target_altitude = next_WP.alt - ((wp_distance * offset_altitude) / (wp_totalDistance - waypoint_radius));
 		
 		// stay within a certain range
 		if(prev_WP.alt > next_WP.alt){