zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

updated Param gen - won't compile yet. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1668 f9c3cf11-9bcb-44bc-f272-b75c42450872
master
jasonshort 14 years ago
parent
e14e77c151
commit
a243039dc1
6 changed files with 25 additions and 421 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 390
      ArduCopterMega/EEPROM.pde
  2. 8
      ArduCopterMega/EEPROM.txt
  3. 26
      ArduCopterMega/commands.pde
  4. 4
      ArduCopterMega/commands_process.pde
  5. 14
      ArduCopterMega/motors.pde
  6. 4
      ArduCopterMega/navigation.pde

390
ArduCopterMega/EEPROM.pde
Unescape View File

@ -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);
}

8
ArduCopterMega/EEPROM.txt
Unescape View File

@ -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;
}
/********************************************************************************/ /********************************************************************************/

26
ArduCopterMega/commands.pde
Unescape View File

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

4
ArduCopterMega/commands_process.pde
Unescape View File

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

14
ArduCopterMega/motors.pde
Unescape View File

@ -43,7 +43,7 @@ set_servos_4()
int out_min = g.rc_3.radio_min; int out_min = g.rc_3.radio_min;
// Throttle is 0 to 1000 only // 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) if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + 50; out_min = g.rc_3.radio_min + 50;
@ -141,14 +141,14 @@ set_servos_4()
// limit output so motors don't stop // 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) { 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++; num++;

4
ArduCopterMega/navigation.pde
Unescape View File

@ -131,8 +131,8 @@ void calc_distance_error()
void calc_altitude_error() void calc_altitude_error()
{ {
if(control_mode == AUTO && offset_altitude != 0) { 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 // stay within a certain range
if(prev_WP.alt > next_WP.alt){ if(prev_WP.alt > next_WP.alt){

Write Preview
Loading…
Cancel
Save