Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
CaranchoEngineering 2010-08-20 06:12:44 +00:00
parent 085f0741a8
commit 12d54e7e72
7 changed files with 314 additions and 264 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

173
Arducopter/ArduCopter.h
View File

@ -1,5 +1,5 @@
/* /*
ArduCopter 1.3 - Aug 2010 ArduCopter 1.3 - August 2010
www.ArduCopter.com www.ArduCopter.com
Copyright (c) 2010. All rights reserved. Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter. An Open Source Arduino based multicopter.
@ -19,7 +19,6 @@
*/ */
#include "WProgram.h" #include "WProgram.h"
#include "UserConfig.h"
/*******************************************************************/ /*******************************************************************/
// ArduPilot Mega specific hardware and software settings // ArduPilot Mega specific hardware and software settings
@ -244,172 +243,4 @@ long tlmTimer = 0;
// Arming/Disarming // Arming/Disarming
uint8_t Arming_counter=0; uint8_t Arming_counter=0;
uint8_t Disarming_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);
}

15
Arducopter/Arducopter.pde
View File

@ -37,8 +37,7 @@
Red LED Off = No GPS Fix Red LED Off = No GPS Fix
Green LED blink slow = Motors armed, Stable mode 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 */ /* User definable modules */
@ -444,12 +443,12 @@ void loop(){
{ {
// Commands from radio Rx... // Commands from radio Rx...
// Stick position defines the desired angle in roll, pitch and yaw // Stick position defines the desired angle in roll, pitch and yaw
ch_roll = channel_filter(APM_RC.InputCh(0), ch_roll); 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 = 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 = 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 = channel_filter(APM_RC.InputCh(3), ch_yaw) * ch_yaw_slope + ch_yaw_offset;
ch_aux = APM_RC.InputCh(4); ch_aux = APM_RC.InputCh(4) * ch_aux_slope + ch_aux_offset;
ch_aux2 = APM_RC.InputCh(5); ch_aux2 = APM_RC.InputCh(5) * ch_aux2_slope + ch_aux2_offset;
command_rx_roll_old = command_rx_roll; command_rx_roll_old = command_rx_roll;
command_rx_roll = (ch_roll-CHANN_CENTER) / 12.0; command_rx_roll = (ch_roll-CHANN_CENTER) / 12.0;
command_rx_roll_diff = command_rx_roll - command_rx_roll_old; command_rx_roll_diff = command_rx_roll - command_rx_roll_old;

6
Arducopter/DCM.pde
View File

@ -1,5 +1,5 @@
/* /*
ArduCopter v1.3 - Aug 2010 ArduCopter v1.3 - August 2010
www.ArduCopter.com www.ArduCopter.com
Copyright (c) 2010. All rights reserved. Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter. An Open Source Arduino based multicopter.
@ -16,7 +16,7 @@
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
/* ******* ADC functions ********************* */ /* ******* ADC functions ********************* */
@ -244,4 +244,4 @@ void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
} }
} }

39
Arducopter/Functions.pde
View File

@ -1,22 +1,22 @@
/* /*
ArduCopter 1.3 - Aug 2010 ArduCopter 1.3 - August 2010
www.ArduCopter.com www.ArduCopter.com
Copyright (c) 2010. All rights reserved. Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter. An Open Source Arduino based multicopter.
This program is free software: you can redistribute it and/or modify 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 it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or the Free Software Foundation, either version 3 of the License, or
(at your option) any later version. (at your option) any later version.
This program is distributed in the hope that it will be useful, This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU General Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
void RadioCalibration() { void RadioCalibration() {
long command_timer; long command_timer;
@ -131,7 +131,6 @@ void RadioCalibration() {
else Serial.println("Command timeout, returning normal mode...."); else Serial.println("Command timeout, returning normal mode....");
} }
void comma() { void comma() {
Serial.print(','); Serial.print(',');
} }
@ -143,4 +142,4 @@ void comma() {

4
Arducopter/Navigation.pde
View File

@ -1,5 +1,5 @@
/* /*
ArduCopter v1.3 - Aug 2010 ArduCopter v1.3 - August 2010
www.ArduCopter.com www.ArduCopter.com
Copyright (c) 2010. All rights reserved. Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter. An Open Source Arduino based multicopter.
@ -16,7 +16,7 @@
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
/* ************************************************************ */ /* ************************************************************ */
/* GPS based Position control */ /* GPS based Position control */

74
Arducopter/SerialCom.pde
View File

@ -1,5 +1,5 @@
/* /*
ArduCopter v1.2 - June 2010 ArduCopter v1.3 - August 2010
www.ArduCopter.com www.ArduCopter.com
Copyright (c) 2010. All rights reserved. Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter. An Open Source Arduino based multicopter.
@ -16,7 +16,7 @@
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
void readSerialCommand() { void readSerialCommand() {
// Check for serial message // Check for serial message
@ -134,52 +134,22 @@ void readSerialCommand() {
writeEEPROM(yaw_mid, CHYAW_MID); writeEEPROM(yaw_mid, CHYAW_MID);
break; break;
case 'Y': // Initialize EEPROM with default values case 'Y': // Initialize EEPROM with default values
KP_QUAD_ROLL = 1.8; setUserConfig();
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;
break; 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 Serial.print(pitch_mid); // Pitch MID value
comma(); comma();
Serial.println(yaw_mid); // Yaw MID Value Serial.println(yaw_mid); // Yaw MID Value
break; break;
case 'V': // Spare case 'V': // Spare
break; break;
@ -434,11 +403,4 @@ float readFloatSerial() {
} }
while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128)); while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128));
return atof(data); return atof(data);
} }
/*
void comma() {
Serial.print(',');
}
*/

267
Arducopter/UserConfig.h
View File

@ -1,5 +1,5 @@
/* /*
ArduCopter v1.3 - Aug 2010 ArduCopter v1.3 - August 2010
www.ArduCopter.com www.ArduCopter.com
Copyright (c) 2010. All rights reserved. Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter. An Open Source Arduino based multicopter.
@ -16,15 +16,13 @@
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
/* ************************************************************* /* *************************************************************
TODO: TODO:
- move all user definable variables from main pde to here
- comment variables properly - comment variables properly
************************************************************* */ ************************************************************* */
@ -68,3 +66,264 @@ TODO:
#define CHANN_CENTER 1500 // Channel center, legacy #define CHANN_CENTER 1500 // Channel center, legacy
#define MIN_THROTTLE 1040 // Throttle pulse width at minimun... #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); */
}