mirror of https://github.com/ArduPilot/ardupilot
updated Param gen - won't compile yet.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1668 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
49bef1a2bf
commit
149de91eb7
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
// --------------------
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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++;
|
||||
|
|
|
|||
|
|
@ -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){
|
||||
|
|
|
|||
Loading…
Reference in New Issue