From 12d54e7e7297d4e3ea7fd3dc3dd3c6cb44fe58b8 Mon Sep 17 00:00:00 2001 From: CaranchoEngineering Date: Fri, 20 Aug 2010 06:12:44 +0000 Subject: [PATCH] moved default value config to UserConfig.h, added transmitter cal code, critical part currently commented out until testing completed. git-svn-id: https://arducopter.googlecode.com/svn/trunk@249 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- Arducopter/ArduCopter.h | 173 +----------------------- Arducopter/Arducopter.pde | 15 +-- Arducopter/DCM.pde | 6 +- Arducopter/Functions.pde | 39 +++--- Arducopter/Navigation.pde | 4 +- Arducopter/SerialCom.pde | 74 +++-------- Arducopter/UserConfig.h | 267 +++++++++++++++++++++++++++++++++++++- 7 files changed, 314 insertions(+), 264 deletions(-) diff --git a/Arducopter/ArduCopter.h b/Arducopter/ArduCopter.h index 2edd33db48..adab275e7e 100644 --- a/Arducopter/ArduCopter.h +++ b/Arducopter/ArduCopter.h @@ -1,5 +1,5 @@ /* - ArduCopter 1.3 - Aug 2010 + ArduCopter 1.3 - August 2010 www.ArduCopter.com Copyright (c) 2010. All rights reserved. An Open Source Arduino based multicopter. @@ -19,7 +19,6 @@ */ #include "WProgram.h" -#include "UserConfig.h" /*******************************************************************/ // ArduPilot Mega specific hardware and software settings @@ -244,172 +243,4 @@ long tlmTimer = 0; // Arming/Disarming uint8_t Arming_counter=0; -uint8_t Disarming_counter=0; - - - -/*****************************************************/ -// APM Specific Memory variables - -// Following variables stored in EEPROM -float KP_QUAD_ROLL; -float KD_QUAD_ROLL; -float KI_QUAD_ROLL; -float KP_QUAD_PITCH; -float KD_QUAD_PITCH; -float KI_QUAD_PITCH; -float KP_QUAD_YAW; -float KD_QUAD_YAW; -float KI_QUAD_YAW; -float STABLE_MODE_KP_RATE; -float KP_GPS_ROLL; -float KD_GPS_ROLL; -float KI_GPS_ROLL; -float KP_GPS_PITCH; -float KD_GPS_PITCH; -float KI_GPS_PITCH; -float GPS_MAX_ANGLE; -float KP_ALTITUDE; -float KD_ALTITUDE; -float KI_ALTITUDE; -int acc_offset_x; -int acc_offset_y; -int acc_offset_z; -int gyro_offset_roll; -int gyro_offset_pitch; -int gyro_offset_yaw; -float Kp_ROLLPITCH; -float Ki_ROLLPITCH; -float Kp_YAW; -float Ki_YAW; -float GEOG_CORRECTION_FACTOR; -int MAGNETOMETER; -float Kp_RateRoll; -float Ki_RateRoll; -float Kd_RateRoll; -float Kp_RatePitch; -float Ki_RatePitch; -float Kd_RatePitch; -float Kp_RateYaw; -float Ki_RateYaw; -float Kd_RateYaw; -float xmitFactor; - -// EEPROM storage addresses -#define KP_QUAD_ROLL_ADR 0 -#define KD_QUAD_ROLL_ADR 4 -#define KI_QUAD_ROLL_ADR 8 -#define KP_QUAD_PITCH_ADR 12 -#define KD_QUAD_PITCH_ADR 16 -#define KI_QUAD_PITCH_ADR 20 -#define KP_QUAD_YAW_ADR 24 -#define KD_QUAD_YAW_ADR 28 -#define KI_QUAD_YAW_ADR 32 -#define STABLE_MODE_KP_RATE_ADR 36 -#define KP_GPS_ROLL_ADR 40 -#define KD_GPS_ROLL_ADR 44 -#define KI_GPS_ROLL_ADR 48 -#define KP_GPS_PITCH_ADR 52 -#define KD_GPS_PITCH_ADR 56 -#define KI_GPS_PITCH_ADR 60 -#define GPS_MAX_ANGLE_ADR 64 -#define KP_ALTITUDE_ADR 68 -#define KD_ALTITUDE_ADR 72 -#define KI_ALTITUDE_ADR 76 -#define acc_offset_x_ADR 80 -#define acc_offset_y_ADR 84 -#define acc_offset_z_ADR 88 -#define gyro_offset_roll_ADR 92 -#define gyro_offset_pitch_ADR 96 -#define gyro_offset_yaw_ADR 100 -#define Kp_ROLLPITCH_ADR 104 -#define Ki_ROLLPITCH_ADR 108 -#define Kp_YAW_ADR 112 -#define Ki_YAW_ADR 116 -#define GEOG_CORRECTION_FACTOR_ADR 120 -#define MAGNETOMETER_ADR 124 -#define XMITFACTOR_ADR 128 -#define KP_RATEROLL_ADR 132 -#define KI_RATEROLL_ADR 136 -#define KD_RATEROLL_ADR 140 -#define KP_RATEPITCH_ADR 144 -#define KI_RATEPITCH_ADR 148 -#define KD_RATEPITCH_ADR 152 -#define KP_RATEYAW_ADR 156 -#define KI_RATEYAW_ADR 160 -#define KD_RATEYAW_ADR 164 -#define CHROLL_MID 168 -#define CHPITCH_MID 172 -#define CHYAW_MID 176 - -// Utilities for writing and reading from the EEPROM -float readEEPROM(int address) { - union floatStore { - byte floatByte[4]; - float floatVal; - } floatOut; - - for (int i = 0; i < 4; i++) - floatOut.floatByte[i] = EEPROM.read(address + i); - return floatOut.floatVal; -} - -void writeEEPROM(float value, int address) { - union floatStore { - byte floatByte[4]; - float floatVal; - } floatIn; - - floatIn.floatVal = value; - for (int i = 0; i < 4; i++) - EEPROM.write(address + i, floatIn.floatByte[i]); -} - -void readUserConfig() { - KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR); - KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR); - KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR); - KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR); - KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR); - KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR); - KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR); - KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR); - KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR); - STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR); - KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR); - KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR); - KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR); - KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR); - KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR); - KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR); - GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR); - KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR); - KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR); - KI_ALTITUDE = readEEPROM(KI_ALTITUDE_ADR); - acc_offset_x = readEEPROM(acc_offset_x_ADR); - acc_offset_y = readEEPROM(acc_offset_y_ADR); - acc_offset_z = readEEPROM(acc_offset_z_ADR); - gyro_offset_roll = readEEPROM(gyro_offset_roll_ADR); - gyro_offset_pitch = readEEPROM(gyro_offset_pitch_ADR); - gyro_offset_yaw = readEEPROM(gyro_offset_yaw_ADR); - Kp_ROLLPITCH = readEEPROM(Kp_ROLLPITCH_ADR); - Ki_ROLLPITCH = readEEPROM(Ki_ROLLPITCH_ADR); - Kp_YAW = readEEPROM(Kp_YAW_ADR); - Ki_YAW = readEEPROM(Ki_YAW_ADR); - GEOG_CORRECTION_FACTOR = readEEPROM(GEOG_CORRECTION_FACTOR_ADR); - MAGNETOMETER = readEEPROM(MAGNETOMETER_ADR); - Kp_RateRoll = readEEPROM(KP_RATEROLL_ADR); - Ki_RateRoll = readEEPROM(KI_RATEROLL_ADR); - Kd_RateRoll = readEEPROM(KD_RATEROLL_ADR); - Kp_RatePitch = readEEPROM(KP_RATEPITCH_ADR); - Ki_RatePitch = readEEPROM(KI_RATEPITCH_ADR); - Kd_RatePitch = readEEPROM(KD_RATEPITCH_ADR); - Kp_RateYaw = readEEPROM(KP_RATEYAW_ADR); - Ki_RateYaw = readEEPROM(KI_RATEYAW_ADR); - Kd_RateYaw = readEEPROM(KD_RATEYAW_ADR); - xmitFactor = readEEPROM(XMITFACTOR_ADR); - roll_mid = readEEPROM(CHROLL_MID); - pitch_mid = readEEPROM(CHPITCH_MID); - yaw_mid = readEEPROM(CHYAW_MID); - -} +uint8_t Disarming_counter=0; diff --git a/Arducopter/Arducopter.pde b/Arducopter/Arducopter.pde index c3ba3e3932..42cc804592 100644 --- a/Arducopter/Arducopter.pde +++ b/Arducopter/Arducopter.pde @@ -37,8 +37,7 @@ Red LED Off = No GPS Fix Green LED blink slow = Motors armed, Stable mode - Green LED blink rapid = Motors armed, Acro mode - + Green LED blink rapid = Motors armed, Acro mode */ /* User definable modules */ @@ -444,12 +443,12 @@ void loop(){ { // Commands from radio Rx... // Stick position defines the desired angle in roll, pitch and yaw - ch_roll = channel_filter(APM_RC.InputCh(0), ch_roll); - ch_pitch = channel_filter(APM_RC.InputCh(1), ch_pitch); - ch_throttle = channel_filter(APM_RC.InputCh(2), ch_throttle); - ch_yaw = channel_filter(APM_RC.InputCh(3), ch_yaw); - ch_aux = APM_RC.InputCh(4); - ch_aux2 = APM_RC.InputCh(5); + ch_roll = channel_filter(APM_RC.InputCh(0), ch_roll) * ch_roll_slope + ch_roll_offset; + ch_pitch = channel_filter(APM_RC.InputCh(1), ch_pitch) * ch_pitch_slope + ch_pitch_offset; + ch_throttle = channel_filter(APM_RC.InputCh(2), ch_throttle) * ch_throttle_slope + ch_throttle_offset; + ch_yaw = channel_filter(APM_RC.InputCh(3), ch_yaw) * ch_yaw_slope + ch_yaw_offset; + ch_aux = APM_RC.InputCh(4) * ch_aux_slope + ch_aux_offset; + ch_aux2 = APM_RC.InputCh(5) * ch_aux2_slope + ch_aux2_offset; command_rx_roll_old = command_rx_roll; command_rx_roll = (ch_roll-CHANN_CENTER) / 12.0; command_rx_roll_diff = command_rx_roll - command_rx_roll_old; diff --git a/Arducopter/DCM.pde b/Arducopter/DCM.pde index 927dc67cb3..169e4c8117 100644 --- a/Arducopter/DCM.pde +++ b/Arducopter/DCM.pde @@ -1,5 +1,5 @@ /* - ArduCopter v1.3 - Aug 2010 + ArduCopter v1.3 - August 2010 www.ArduCopter.com Copyright (c) 2010. All rights reserved. An Open Source Arduino based multicopter. @@ -16,7 +16,7 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . - */ +*/ /* ******* ADC functions ********************* */ @@ -244,4 +244,4 @@ void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) } } - + diff --git a/Arducopter/Functions.pde b/Arducopter/Functions.pde index 76a715e1be..c367d703a5 100644 --- a/Arducopter/Functions.pde +++ b/Arducopter/Functions.pde @@ -1,22 +1,22 @@ /* - ArduCopter 1.3 - Aug 2010 - www.ArduCopter.com - Copyright (c) 2010. All rights reserved. - An Open Source Arduino based multicopter. - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ + ArduCopter 1.3 - August 2010 + www.ArduCopter.com + Copyright (c) 2010. All rights reserved. + An Open Source Arduino based multicopter. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ void RadioCalibration() { long command_timer; @@ -131,7 +131,6 @@ void RadioCalibration() { else Serial.println("Command timeout, returning normal mode...."); } - void comma() { Serial.print(','); } @@ -143,4 +142,4 @@ void comma() { - + diff --git a/Arducopter/Navigation.pde b/Arducopter/Navigation.pde index abc1844359..767daf85b2 100644 --- a/Arducopter/Navigation.pde +++ b/Arducopter/Navigation.pde @@ -1,5 +1,5 @@ /* - ArduCopter v1.3 - Aug 2010 + ArduCopter v1.3 - August 2010 www.ArduCopter.com Copyright (c) 2010. All rights reserved. An Open Source Arduino based multicopter. @@ -16,7 +16,7 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . - */ +*/ /* ************************************************************ */ /* GPS based Position control */ diff --git a/Arducopter/SerialCom.pde b/Arducopter/SerialCom.pde index d86e7a1bb6..8478f6def9 100644 --- a/Arducopter/SerialCom.pde +++ b/Arducopter/SerialCom.pde @@ -1,5 +1,5 @@ /* - ArduCopter v1.2 - June 2010 + ArduCopter v1.3 - August 2010 www.ArduCopter.com Copyright (c) 2010. All rights reserved. An Open Source Arduino based multicopter. @@ -16,7 +16,7 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . - */ +*/ void readSerialCommand() { // Check for serial message @@ -134,52 +134,22 @@ void readSerialCommand() { writeEEPROM(yaw_mid, CHYAW_MID); break; case 'Y': // Initialize EEPROM with default values - KP_QUAD_ROLL = 1.8; - KD_QUAD_ROLL = 0.4; //0.48 - KI_QUAD_ROLL = 0.30; //0.4 - KP_QUAD_PITCH = 1.8; - KD_QUAD_PITCH = 0.4; //0.48 - KI_QUAD_PITCH = 0.30; //0.4 - KP_QUAD_YAW = 3.6; - KD_QUAD_YAW = 1.2; - KI_QUAD_YAW = 0.15; - STABLE_MODE_KP_RATE = 0.2; // New param for stable mode - KP_GPS_ROLL = 0.02; - KD_GPS_ROLL = 0.006; - KI_GPS_ROLL = 0.008; - KP_GPS_PITCH = 0.02; - KD_GPS_PITCH = 0.006; - KI_GPS_PITCH = 0.008; - GPS_MAX_ANGLE = 18; - KP_ALTITUDE = 0.8; - KD_ALTITUDE = 0.2; - KI_ALTITUDE = 0.2; - acc_offset_x = 2073; - acc_offset_y = 2056; - acc_offset_z = 2010; - gyro_offset_roll = 1659; - gyro_offset_pitch = 1618; - gyro_offset_yaw = 1673; - Kp_ROLLPITCH = 0.0014; - Ki_ROLLPITCH = 0.00000015; - Kp_YAW = 1.2; - Ki_YAW = 0.00005; - GEOG_CORRECTION_FACTOR = 0.87; - MAGNETOMETER = 0; - Kp_RateRoll = 0.6; - Ki_RateRoll = 0.1; - Kd_RateRoll = -0.8; - Kp_RatePitch = 0.6; - Ki_RatePitch = 0.1; - Kd_RatePitch = -0.8; - Kp_RateYaw = 1.6; - Ki_RateYaw = 0.3; - Kd_RateYaw = 0; - xmitFactor = 0.8; - roll_mid = 1500; - pitch_mid = 1500; - yaw_mid = 1500; + setUserConfig(); break; + case '1': // Receive transmitter calibration values + ch_roll_slope = readFloatSerial(); + ch_roll_offset = readFloatSerial(); + ch_pitch_slope = readFloatSerial(); + ch_pitch_offset = readFloatSerial(); + ch_yaw_slope = readFloatSerial(); + ch_yaw_offset = readFloatSerial(); + ch_throttle_slope = readFloatSerial(); + ch_throttle_offset = readFloatSerial(); + ch_aux_slope = readFloatSerial(); + ch_aux_offset = readFloatSerial(); + ch_aux2_slope = readFloatSerial(); + ch_aux2_offset = readFloatSerial(); + break; } } } @@ -399,7 +369,6 @@ void sendSerialTelemetry() { Serial.print(pitch_mid); // Pitch MID value comma(); Serial.println(yaw_mid); // Yaw MID Value - break; case 'V': // Spare break; @@ -434,11 +403,4 @@ float readFloatSerial() { } while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128)); return atof(data); -} - -/* -void comma() { - Serial.print(','); - } - */ - +} diff --git a/Arducopter/UserConfig.h b/Arducopter/UserConfig.h index a04b9132e3..25a325708b 100644 --- a/Arducopter/UserConfig.h +++ b/Arducopter/UserConfig.h @@ -1,5 +1,5 @@ /* - ArduCopter v1.3 - Aug 2010 + ArduCopter v1.3 - August 2010 www.ArduCopter.com Copyright (c) 2010. All rights reserved. An Open Source Arduino based multicopter. @@ -16,15 +16,13 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . - */ +*/ /* ************************************************************* TODO: - - move all user definable variables from main pde to here - comment variables properly - ************************************************************* */ @@ -68,3 +66,264 @@ TODO: #define CHANN_CENTER 1500 // Channel center, legacy #define MIN_THROTTLE 1040 // Throttle pulse width at minimun... +// Following variables stored in EEPROM +float KP_QUAD_ROLL; +float KD_QUAD_ROLL; +float KI_QUAD_ROLL; +float KP_QUAD_PITCH; +float KD_QUAD_PITCH; +float KI_QUAD_PITCH; +float KP_QUAD_YAW; +float KD_QUAD_YAW; +float KI_QUAD_YAW; +float STABLE_MODE_KP_RATE; +float KP_GPS_ROLL; +float KD_GPS_ROLL; +float KI_GPS_ROLL; +float KP_GPS_PITCH; +float KD_GPS_PITCH; +float KI_GPS_PITCH; +float GPS_MAX_ANGLE; +float KP_ALTITUDE; +float KD_ALTITUDE; +float KI_ALTITUDE; +int acc_offset_x; +int acc_offset_y; +int acc_offset_z; +int gyro_offset_roll; +int gyro_offset_pitch; +int gyro_offset_yaw; +float Kp_ROLLPITCH; +float Ki_ROLLPITCH; +float Kp_YAW; +float Ki_YAW; +float GEOG_CORRECTION_FACTOR; +int MAGNETOMETER; +float Kp_RateRoll; +float Ki_RateRoll; +float Kd_RateRoll; +float Kp_RatePitch; +float Ki_RatePitch; +float Kd_RatePitch; +float Kp_RateYaw; +float Ki_RateYaw; +float Kd_RateYaw; +float xmitFactor; +float ch_roll_slope = 1; +float ch_pitch_slope = 1; +float ch_throttle_slope = 1; +float ch_yaw_slope = 1; +float ch_aux_slope = 1; +float ch_aux2_slope = 1; +float ch_roll_offset = 0; +float ch_pitch_offset = 0; +float ch_throttle_offset = 0; +float ch_yaw_offset = 0; +float ch_aux_offset = 0; +float ch_aux2_offset = 0; + +// This function call contains the default values that are set to the ArduCopter +// when a "Default EEPROM Value" command is sent through serial interface +void setUserConfig() { + KP_QUAD_ROLL = 1.8; + KD_QUAD_ROLL = 0.4; //0.48 + KI_QUAD_ROLL = 0.30; //0.4 + KP_QUAD_PITCH = 1.8; + KD_QUAD_PITCH = 0.4; //0.48 + KI_QUAD_PITCH = 0.30; //0.4 + KP_QUAD_YAW = 3.6; + KD_QUAD_YAW = 1.2; + KI_QUAD_YAW = 0.15; + STABLE_MODE_KP_RATE = 0.2; // New param for stable mode + KP_GPS_ROLL = 0.012; + KD_GPS_ROLL = 0.005; + KI_GPS_ROLL = 0.004; + KP_GPS_PITCH = 0.012; + KD_GPS_PITCH = 0.005; + KI_GPS_PITCH = 0.004; + GPS_MAX_ANGLE = 10; + KP_ALTITUDE = 0.8; + KD_ALTITUDE = 0.2; + KI_ALTITUDE = 0.2; + acc_offset_x = 2073; + acc_offset_y = 2056; + acc_offset_z = 2010; + gyro_offset_roll = 1659; + gyro_offset_pitch = 1618; + gyro_offset_yaw = 1673; + Kp_ROLLPITCH = 0.0014; + Ki_ROLLPITCH = 0.00000015; + Kp_YAW = 1.2; + Ki_YAW = 0.00005; + GEOG_CORRECTION_FACTOR = 0.87; + MAGNETOMETER = 0; + Kp_RateRoll = 0.6; + Ki_RateRoll = 0.1; + Kd_RateRoll = -0.8; + Kp_RatePitch = 0.6; + Ki_RatePitch = 0.1; + Kd_RatePitch = -0.8; + Kp_RateYaw = 1.6; + Ki_RateYaw = 0.3; + Kd_RateYaw = 0; + xmitFactor = 0.8; + roll_mid = 1500; + pitch_mid = 1500; + yaw_mid = 1500; + ch_roll_slope = 1; + ch_pitch_slope = 1; + ch_throttle_slope = 1; + ch_yaw_slope = 1; + ch_aux_slope = 1; + ch_aux2_slope = 1; + ch_roll_offset = 0; + ch_pitch_offset = 0; + ch_throttle_offset = 0; + ch_yaw_offset = 0; + ch_aux_offset = 0; + ch_aux2_offset = 0; +} + +// EEPROM storage addresses +#define KP_QUAD_ROLL_ADR 0 +#define KD_QUAD_ROLL_ADR 4 +#define KI_QUAD_ROLL_ADR 8 +#define KP_QUAD_PITCH_ADR 12 +#define KD_QUAD_PITCH_ADR 16 +#define KI_QUAD_PITCH_ADR 20 +#define KP_QUAD_YAW_ADR 24 +#define KD_QUAD_YAW_ADR 28 +#define KI_QUAD_YAW_ADR 32 +#define STABLE_MODE_KP_RATE_ADR 36 +#define KP_GPS_ROLL_ADR 40 +#define KD_GPS_ROLL_ADR 44 +#define KI_GPS_ROLL_ADR 48 +#define KP_GPS_PITCH_ADR 52 +#define KD_GPS_PITCH_ADR 56 +#define KI_GPS_PITCH_ADR 60 +#define GPS_MAX_ANGLE_ADR 64 +#define KP_ALTITUDE_ADR 68 +#define KD_ALTITUDE_ADR 72 +#define KI_ALTITUDE_ADR 76 +#define acc_offset_x_ADR 80 +#define acc_offset_y_ADR 84 +#define acc_offset_z_ADR 88 +#define gyro_offset_roll_ADR 92 +#define gyro_offset_pitch_ADR 96 +#define gyro_offset_yaw_ADR 100 +#define Kp_ROLLPITCH_ADR 104 +#define Ki_ROLLPITCH_ADR 108 +#define Kp_YAW_ADR 112 +#define Ki_YAW_ADR 116 +#define GEOG_CORRECTION_FACTOR_ADR 120 +#define MAGNETOMETER_ADR 124 +#define XMITFACTOR_ADR 128 +#define KP_RATEROLL_ADR 132 +#define KI_RATEROLL_ADR 136 +#define KD_RATEROLL_ADR 140 +#define KP_RATEPITCH_ADR 144 +#define KI_RATEPITCH_ADR 148 +#define KD_RATEPITCH_ADR 152 +#define KP_RATEYAW_ADR 156 +#define KI_RATEYAW_ADR 160 +#define KD_RATEYAW_ADR 164 +#define CHROLL_MID 168 +#define CHPITCH_MID 172 +#define CHYAW_MID 176 +#define ch_roll_slope_ADR 180 +#define ch_pitch_slope_ADR 184 +#define ch_throttle_slope_ADR 188 +#define ch_yaw_slope_ADR 192 +#define ch_aux_slope_ADR 196 +#define ch_aux2_slope_ADR 200 +#define ch_roll_offset_ADR 204 +#define ch_pitch_offset_ADR 208 +#define ch_throttle_offset_ADR 212 +#define ch_yaw_offset_ADR 216 +#define ch_aux_offset_ADR 220 +#define ch_aux2_offset_ADR 224 + +// Utilities for writing and reading from the EEPROM +float readEEPROM(int address) { + union floatStore { + byte floatByte[4]; + float floatVal; + } floatOut; + + for (int i = 0; i < 4; i++) + floatOut.floatByte[i] = EEPROM.read(address + i); + return floatOut.floatVal; +} + +void writeEEPROM(float value, int address) { + union floatStore { + byte floatByte[4]; + float floatVal; + } floatIn; + + floatIn.floatVal = value; + for (int i = 0; i < 4; i++) + EEPROM.write(address + i, floatIn.floatByte[i]); +} + +void readUserConfig() { + KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR); + KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR); + KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR); + KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR); + KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR); + KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR); + KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR); + KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR); + KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR); + STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR); + KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR); + KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR); + KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR); + KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR); + KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR); + KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR); + GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR); + KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR); + KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR); + KI_ALTITUDE = readEEPROM(KI_ALTITUDE_ADR); + acc_offset_x = readEEPROM(acc_offset_x_ADR); + acc_offset_y = readEEPROM(acc_offset_y_ADR); + acc_offset_z = readEEPROM(acc_offset_z_ADR); + gyro_offset_roll = readEEPROM(gyro_offset_roll_ADR); + gyro_offset_pitch = readEEPROM(gyro_offset_pitch_ADR); + gyro_offset_yaw = readEEPROM(gyro_offset_yaw_ADR); + Kp_ROLLPITCH = readEEPROM(Kp_ROLLPITCH_ADR); + Ki_ROLLPITCH = readEEPROM(Ki_ROLLPITCH_ADR); + Kp_YAW = readEEPROM(Kp_YAW_ADR); + Ki_YAW = readEEPROM(Ki_YAW_ADR); + GEOG_CORRECTION_FACTOR = readEEPROM(GEOG_CORRECTION_FACTOR_ADR); + MAGNETOMETER = readEEPROM(MAGNETOMETER_ADR); + Kp_RateRoll = readEEPROM(KP_RATEROLL_ADR); + Ki_RateRoll = readEEPROM(KI_RATEROLL_ADR); + Kd_RateRoll = readEEPROM(KD_RATEROLL_ADR); + Kp_RatePitch = readEEPROM(KP_RATEPITCH_ADR); + Ki_RatePitch = readEEPROM(KI_RATEPITCH_ADR); + Kd_RatePitch = readEEPROM(KD_RATEPITCH_ADR); + Kp_RateYaw = readEEPROM(KP_RATEYAW_ADR); + Ki_RateYaw = readEEPROM(KI_RATEYAW_ADR); + Kd_RateYaw = readEEPROM(KD_RATEYAW_ADR); + xmitFactor = readEEPROM(XMITFACTOR_ADR); + roll_mid = readEEPROM(CHROLL_MID); + pitch_mid = readEEPROM(CHPITCH_MID); + yaw_mid = readEEPROM(CHYAW_MID); + /* uncommented for now until flight tested + ch_roll_slope = readEEPROM(ch_roll_slope_ADR); + ch_pitch_slope = readEEPROM(ch_pitch_slope_ADR); + ch_throttle_slope readEEPROM(ch_throttle_slope_ADR); + ch_yaw_slope = readEEPROM(ch_yaw_slope_ADR); + ch_aux_slope = readEEPROM(ch_aux_slope_ADR); + ch_aux2_slope = readEEPROM(ch_aux2_slope_ADR); + ch_roll_offset = readEEPROM(ch_roll_offset_ADR); + ch_pitch_offset = readEEPROM(ch_pitch_offset_ADR); + ch_throttle_offset = readEEPROM(ch_throttle_offset_ADR); + ch_yaw_offset = readEEPROM(ch_yaw_offset_ADR); + ch_aux_offset = readEEPROM(ch_aux_offset_ADR); + ch_aux2_offset = readEEPROM(ch_aux2_offset_ADR); */ +} +