From a243039dc1526f1e7b3fbf0c9ab808b926f0f6e3 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Thu, 17 Feb 2011 07:25:31 +0000 Subject: [PATCH] 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 | 390 ---------------------------- ArduCopterMega/EEPROM.txt | 8 - ArduCopterMega/commands.pde | 26 +- ArduCopterMega/commands_process.pde | 4 +- ArduCopterMega/motors.pde | 14 +- ArduCopterMega/navigation.pde | 4 +- 6 files changed, 25 insertions(+), 421 deletions(-) delete mode 100644 ArduCopterMega/EEPROM.pde diff --git a/ArduCopterMega/EEPROM.pde b/ArduCopterMega/EEPROM.pde deleted file mode 100644 index 9231019d4f..0000000000 --- a/ArduCopterMega/EEPROM.pde +++ /dev/null @@ -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); -} diff --git a/ArduCopterMega/EEPROM.txt b/ArduCopterMega/EEPROM.txt index 6747a74f37..52687a7887 100644 --- a/ArduCopterMega/EEPROM.txt +++ b/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; -} /********************************************************************************/ diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 310bbd07d1..a43be84f46 100644 --- a/ArduCopterMega/commands.pde +++ b/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 // -------------------- diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index 9498a875b2..653844f0a1 100644 --- a/ArduCopterMega/commands_process.pde +++ b/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; } diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index cd28dbc350..80221a6048 100644 --- a/ArduCopterMega/motors.pde +++ b/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_2] = constrain(motor_out[CH_2], out_min, g.rc_3.radio_max); - motor_out[CH_3] = constrain(motor_out[CH_3], out_min, g.rc_3.radio_max); - motor_out[CH_4] = constrain(motor_out[CH_4], 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.get()); + 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.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_8] = constrain(motor_out[CH_8], 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.get()); } num++; diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index ddd2bf28cc..d887e0a1ab 100644 --- a/ArduCopterMega/navigation.pde +++ b/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 - target_altitude = next_WP.alt - ((wp_distance * offset_altitude) / (wp_totalDistance - 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 - waypoint_radius)); // stay within a certain range if(prev_WP.alt > next_WP.alt){