Browse Source
git-svn-id: https://arducopter.googlecode.com/svn/trunk@38 f9c3cf11-9bcb-44bc-f272-b75c42450872master
jjulio1234
15 years ago
11 changed files with 840 additions and 137 deletions
@ -0,0 +1,410 @@
@@ -0,0 +1,410 @@
|
||||
/* |
||||
ArduCopter v1.2 - June 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 <http://www.gnu.org/licenses/>. |
||||
*/ |
||||
|
||||
void readSerialCommand() { |
||||
// Check for serial message |
||||
if (Serial.available()) { |
||||
queryType = Serial.read(); |
||||
switch (queryType) { |
||||
case 'A': // Stable PID |
||||
KP_QUAD_ROLL = readFloatSerial(); |
||||
KI_QUAD_ROLL = readFloatSerial(); |
||||
KD_QUAD_ROLL = readFloatSerial(); |
||||
KP_QUAD_PITCH = readFloatSerial(); |
||||
KI_QUAD_PITCH = readFloatSerial(); |
||||
KD_QUAD_PITCH = readFloatSerial(); |
||||
KP_QUAD_YAW = readFloatSerial(); |
||||
KI_QUAD_YAW = readFloatSerial(); |
||||
KD_QUAD_YAW = readFloatSerial(); |
||||
KD_QUAD_COMMAND_PART = readFloatSerial(); |
||||
MAGNETOMETER = readFloatSerial(); |
||||
break; |
||||
case 'C': // Receive GPS PID |
||||
KP_GPS_ROLL = readFloatSerial(); |
||||
KI_GPS_ROLL = readFloatSerial(); |
||||
KD_GPS_ROLL = readFloatSerial(); |
||||
KP_GPS_PITCH = readFloatSerial(); |
||||
KI_GPS_PITCH = readFloatSerial(); |
||||
KD_GPS_PITCH = readFloatSerial(); |
||||
GPS_MAX_ANGLE = readFloatSerial(); |
||||
GEOG_CORRECTION_FACTOR = readFloatSerial(); |
||||
break; |
||||
case 'E': // Receive altitude PID |
||||
KP_ALTITUDE = readFloatSerial(); |
||||
KD_ALTITUDE = readFloatSerial(); |
||||
KI_ALTITUDE = readFloatSerial(); |
||||
break; |
||||
case 'G': // Receive drift correction PID |
||||
Kp_ROLLPITCH = readFloatSerial(); |
||||
Ki_ROLLPITCH = readFloatSerial(); |
||||
Kp_YAW = readFloatSerial(); |
||||
Ki_YAW = readFloatSerial(); |
||||
break; |
||||
case 'I': // Receive sensor offset |
||||
gyro_offset_roll = readFloatSerial(); |
||||
gyro_offset_pitch = readFloatSerial(); |
||||
gyro_offset_yaw = readFloatSerial(); |
||||
acc_offset_x = readFloatSerial(); |
||||
acc_offset_y = readFloatSerial(); |
||||
acc_offset_z = readFloatSerial(); |
||||
break; |
||||
case 'K': // Spare |
||||
break; |
||||
case 'M': // Receive debug motor commands |
||||
frontMotor = readFloatSerial(); |
||||
backMotor = readFloatSerial(); |
||||
rightMotor = readFloatSerial(); |
||||
leftMotor = readFloatSerial(); |
||||
motorArmed = readFloatSerial(); |
||||
break; |
||||
case 'O': // Rate Control PID |
||||
Kp_RateRoll = readFloatSerial(); |
||||
Ki_RateRoll = readFloatSerial(); |
||||
Kd_RateRoll = readFloatSerial(); |
||||
Kp_RatePitch = readFloatSerial(); |
||||
Ki_RatePitch = readFloatSerial(); |
||||
Kd_RatePitch = readFloatSerial(); |
||||
Kp_RateYaw = readFloatSerial(); |
||||
Ki_RateYaw = readFloatSerial(); |
||||
Kd_RateYaw = readFloatSerial(); |
||||
xmitFactor = readFloatSerial(); |
||||
break; |
||||
case 'W': // Write all user configurable values to EEPROM |
||||
writeEEPROM(KP_QUAD_ROLL, KP_QUAD_ROLL_ADR); |
||||
writeEEPROM(KD_QUAD_ROLL, KD_QUAD_ROLL_ADR); |
||||
writeEEPROM(KI_QUAD_ROLL, KI_QUAD_ROLL_ADR); |
||||
writeEEPROM(KP_QUAD_PITCH, KP_QUAD_PITCH_ADR); |
||||
writeEEPROM(KD_QUAD_PITCH, KD_QUAD_PITCH_ADR); |
||||
writeEEPROM(KI_QUAD_PITCH, KI_QUAD_PITCH_ADR); |
||||
writeEEPROM(KP_QUAD_YAW, KP_QUAD_YAW_ADR); |
||||
writeEEPROM(KD_QUAD_YAW, KD_QUAD_YAW_ADR); |
||||
writeEEPROM(KI_QUAD_YAW, KI_QUAD_YAW_ADR); |
||||
writeEEPROM(KD_QUAD_COMMAND_PART, KD_QUAD_COMMAND_PART_ADR); |
||||
writeEEPROM(KP_GPS_ROLL, KP_GPS_ROLL_ADR); |
||||
writeEEPROM(KD_GPS_ROLL, KD_GPS_ROLL_ADR); |
||||
writeEEPROM(KI_GPS_ROLL, KI_GPS_ROLL_ADR); |
||||
writeEEPROM(KP_GPS_PITCH, KP_GPS_PITCH_ADR); |
||||
writeEEPROM(KD_GPS_PITCH, KD_GPS_PITCH_ADR); |
||||
writeEEPROM(KI_GPS_PITCH, KI_GPS_PITCH_ADR); |
||||
writeEEPROM(GPS_MAX_ANGLE, GPS_MAX_ANGLE_ADR); |
||||
writeEEPROM(KP_ALTITUDE, KP_ALTITUDE_ADR); |
||||
writeEEPROM(KD_ALTITUDE, KD_ALTITUDE_ADR); |
||||
writeEEPROM(KI_ALTITUDE, KI_ALTITUDE_ADR); |
||||
writeEEPROM(acc_offset_x, acc_offset_x_ADR); |
||||
writeEEPROM(acc_offset_y, acc_offset_y_ADR); |
||||
writeEEPROM(acc_offset_z, acc_offset_z_ADR); |
||||
writeEEPROM(gyro_offset_roll, gyro_offset_roll_ADR); |
||||
writeEEPROM(gyro_offset_pitch, gyro_offset_pitch_ADR); |
||||
writeEEPROM(gyro_offset_yaw, gyro_offset_yaw_ADR); |
||||
writeEEPROM(Kp_ROLLPITCH, Kp_ROLLPITCH_ADR); |
||||
writeEEPROM(Ki_ROLLPITCH, Ki_ROLLPITCH_ADR); |
||||
writeEEPROM(Kp_YAW, Kp_YAW_ADR); |
||||
writeEEPROM(Ki_YAW, Ki_YAW_ADR); |
||||
writeEEPROM(GEOG_CORRECTION_FACTOR, GEOG_CORRECTION_FACTOR_ADR); |
||||
writeEEPROM(MAGNETOMETER, MAGNETOMETER_ADR); |
||||
writeEEPROM(Kp_RateRoll, KP_RATEROLL_ADR); |
||||
writeEEPROM(Ki_RateRoll, KI_RATEROLL_ADR); |
||||
writeEEPROM(Kd_RateRoll, KD_RATEROLL_ADR); |
||||
writeEEPROM(Kp_RatePitch, KP_RATEPITCH_ADR); |
||||
writeEEPROM(Ki_RatePitch, KI_RATEPITCH_ADR); |
||||
writeEEPROM(Kd_RatePitch, KD_RATEPITCH_ADR); |
||||
writeEEPROM(Kp_RateYaw, KP_RATEYAW_ADR); |
||||
writeEEPROM(Ki_RateYaw, KI_RATEYAW_ADR); |
||||
writeEEPROM(Kd_RateYaw, KD_RATEYAW_ADR); |
||||
writeEEPROM(xmitFactor, XMITFACTOR_ADR); |
||||
break; |
||||
case 'Y': // Initialize EEPROM with default values |
||||
KP_QUAD_ROLL = 1.8; |
||||
KD_QUAD_ROLL = 0.48; |
||||
KI_QUAD_ROLL = 0.40; |
||||
KP_QUAD_PITCH = 1.8; |
||||
KD_QUAD_PITCH = 0.48; |
||||
KI_QUAD_PITCH = 0.40; |
||||
KP_QUAD_YAW = 3.6; |
||||
KD_QUAD_YAW = 1.2; |
||||
KI_QUAD_YAW = 0.15; |
||||
KD_QUAD_COMMAND_PART = 4.0; |
||||
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.002; |
||||
Ki_ROLLPITCH = 0.0000005; |
||||
Kp_YAW = 1.5; |
||||
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; |
||||
break; |
||||
} |
||||
} |
||||
} |
||||
|
||||
void sendSerialTelemetry() { |
||||
float aux_float[3]; // used for sensor calibration |
||||
switch (queryType) { |
||||
case '=': // Reserved debug command to view any variable from Serial Monitor |
||||
Serial.print("throttle =");Serial.println(ch_throttle); |
||||
Serial.print("control roll =");Serial.println(control_roll-CHANN_CENTER); |
||||
Serial.print("control pitch =");Serial.println(control_pitch-CHANN_CENTER); |
||||
Serial.print("control yaw =");Serial.println(control_yaw-CHANN_CENTER); |
||||
Serial.print("front left yaw =");Serial.println(frontMotor); |
||||
Serial.print("front right yaw =");Serial.println(rightMotor); |
||||
Serial.print("rear left yaw =");Serial.println(leftMotor); |
||||
Serial.print("rear right motor =");Serial.println(backMotor);Serial.println(); |
||||
|
||||
Serial.print("current roll rate =");Serial.println(read_adc(0)); |
||||
Serial.print("current pitch rate =");Serial.println(read_adc(1)); |
||||
Serial.print("current yaw rate =");Serial.println(read_adc(2)); |
||||
Serial.print("command rx yaw =");Serial.println(command_rx_yaw); |
||||
Serial.println(); |
||||
queryType = 'X'; |
||||
break; |
||||
case 'B': // Send roll, pitch and yaw PID values |
||||
Serial.print(KP_QUAD_ROLL, 3); |
||||
comma(); |
||||
Serial.print(KI_QUAD_ROLL, 3); |
||||
comma(); |
||||
Serial.print(KD_QUAD_ROLL, 3); |
||||
comma(); |
||||
Serial.print(KP_QUAD_PITCH, 3); |
||||
comma(); |
||||
Serial.print(KI_QUAD_PITCH, 3); |
||||
comma(); |
||||
Serial.print(KD_QUAD_PITCH, 3); |
||||
comma(); |
||||
Serial.print(KP_QUAD_YAW, 3); |
||||
comma(); |
||||
Serial.print(KI_QUAD_YAW, 3); |
||||
comma(); |
||||
Serial.print(KD_QUAD_YAW, 3); |
||||
comma(); |
||||
Serial.print(KD_QUAD_COMMAND_PART, 3); |
||||
comma(); |
||||
Serial.println(MAGNETOMETER, 3); |
||||
queryType = 'X'; |
||||
break; |
||||
case 'D': // Send GPS PID |
||||
Serial.print(KP_GPS_ROLL, 3); |
||||
comma(); |
||||
Serial.print(KI_GPS_ROLL, 3); |
||||
comma(); |
||||
Serial.print(KD_GPS_ROLL, 3); |
||||
comma(); |
||||
Serial.print(KP_GPS_PITCH, 3); |
||||
comma(); |
||||
Serial.print(KI_GPS_PITCH, 3); |
||||
comma(); |
||||
Serial.print(KD_GPS_PITCH, 3); |
||||
comma(); |
||||
Serial.print(GPS_MAX_ANGLE, 3); |
||||
comma(); |
||||
Serial.println(GEOG_CORRECTION_FACTOR, 3); |
||||
queryType = 'X'; |
||||
break; |
||||
case 'F': // Send altitude PID |
||||
Serial.print(KP_ALTITUDE, 3); |
||||
comma(); |
||||
Serial.print(KI_ALTITUDE, 3); |
||||
comma(); |
||||
Serial.println(KD_ALTITUDE, 3); |
||||
queryType = 'X'; |
||||
break; |
||||
case 'H': // Send drift correction PID |
||||
Serial.print(Kp_ROLLPITCH, 4); |
||||
comma(); |
||||
Serial.print(Ki_ROLLPITCH, 7); |
||||
comma(); |
||||
Serial.print(Kp_YAW, 4); |
||||
comma(); |
||||
Serial.println(Ki_YAW, 6); |
||||
queryType = 'X'; |
||||
break; |
||||
case 'J': // Send sensor offset |
||||
Serial.print(gyro_offset_roll); |
||||
comma(); |
||||
Serial.print(gyro_offset_pitch); |
||||
comma(); |
||||
Serial.print(gyro_offset_yaw); |
||||
comma(); |
||||
Serial.print(acc_offset_x); |
||||
comma(); |
||||
Serial.print(acc_offset_y); |
||||
comma(); |
||||
Serial.println(acc_offset_z); |
||||
AN_OFFSET[3] = acc_offset_x; |
||||
AN_OFFSET[4] = acc_offset_y; |
||||
AN_OFFSET[5] = acc_offset_z; |
||||
queryType = 'X'; |
||||
break; |
||||
case 'L': // Spare |
||||
queryType = 'X'; |
||||
break; |
||||
case 'N': // Send magnetometer config |
||||
queryType = 'X'; |
||||
break; |
||||
case 'P': // Send rate control PID |
||||
Serial.print(Kp_RateRoll, 3); |
||||
comma(); |
||||
Serial.print(Ki_RateRoll, 3); |
||||
comma(); |
||||
Serial.print(Kd_RateRoll, 3); |
||||
comma(); |
||||
Serial.print(Kp_RatePitch, 3); |
||||
comma(); |
||||
Serial.print(Ki_RatePitch, 3); |
||||
comma(); |
||||
Serial.print(Kd_RatePitch, 3); |
||||
comma(); |
||||
Serial.print(Kp_RateYaw, 3); |
||||
comma(); |
||||
Serial.print(Ki_RateYaw, 3); |
||||
comma(); |
||||
Serial.print(Kd_RateYaw, 3); |
||||
comma(); |
||||
Serial.println(xmitFactor, 3); |
||||
queryType = 'X'; |
||||
break; |
||||
case 'Q': // Send sensor data |
||||
Serial.print(read_adc(0)); |
||||
comma(); |
||||
Serial.print(read_adc(1)); |
||||
comma(); |
||||
Serial.print(read_adc(2)); |
||||
comma(); |
||||
Serial.print(read_adc(4)); |
||||
comma(); |
||||
Serial.print(read_adc(3)); |
||||
comma(); |
||||
Serial.print(read_adc(5)); |
||||
comma(); |
||||
Serial.print(err_roll); |
||||
comma(); |
||||
Serial.print(err_pitch); |
||||
comma(); |
||||
Serial.print(degrees(-roll)); |
||||
comma(); |
||||
Serial.print(degrees(-pitch)); |
||||
comma(); |
||||
Serial.println(degrees(yaw)); |
||||
break; |
||||
case 'R': // Send raw sensor data |
||||
break; |
||||
case 'S': // Send all flight data |
||||
Serial.print(timer-timer_old); |
||||
comma(); |
||||
Serial.print(read_adc(0)); |
||||
comma(); |
||||
Serial.print(read_adc(1)); |
||||
comma(); |
||||
Serial.print(read_adc(2)); |
||||
comma(); |
||||
Serial.print(ch_throttle); |
||||
comma(); |
||||
Serial.print(control_roll); |
||||
comma(); |
||||
Serial.print(control_pitch); |
||||
comma(); |
||||
Serial.print(control_yaw); |
||||
comma(); |
||||
Serial.print(frontMotor); // Front Motor |
||||
comma(); |
||||
Serial.print(backMotor); // Back Motor |
||||
comma(); |
||||
Serial.print(rightMotor); // Right Motor |
||||
comma(); |
||||
Serial.print(leftMotor); // Left Motor |
||||
comma(); |
||||
Serial.print(read_adc(4)); |
||||
comma(); |
||||
Serial.print(read_adc(3)); |
||||
comma(); |
||||
Serial.println(read_adc(5)); |
||||
break; |
||||
case 'T': // Spare |
||||
break; |
||||
case 'U': // Send receiver values |
||||
Serial.print(ch_roll); // Aileron |
||||
comma(); |
||||
Serial.print(ch_pitch); // Elevator |
||||
comma(); |
||||
Serial.print(ch_yaw); // Yaw |
||||
comma(); |
||||
Serial.print(ch_throttle); // Throttle |
||||
comma(); |
||||
Serial.print(ch_aux); // AUX1 (Mode) |
||||
comma(); |
||||
Serial.println(ch_aux2); // AUX2 |
||||
break; |
||||
case 'V': // Spare |
||||
break; |
||||
case 'X': // Stop sending messages |
||||
break; |
||||
case '!': // Send flight software version |
||||
Serial.println("1.2"); |
||||
queryType = 'X'; |
||||
break; |
||||
} |
||||
} |
||||
|
||||
// Used to read floating point values from the serial port |
||||
float readFloatSerial() { |
||||
byte index = 0; |
||||
byte timeout = 0; |
||||
char data[128] = ""; |
||||
|
||||
do { |
||||
if (Serial.available() == 0) { |
||||
delay(10); |
||||
timeout++; |
||||
} |
||||
else { |
||||
data[index] = Serial.read(); |
||||
timeout = 0; |
||||
index++; |
||||
} |
||||
} while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128)); |
||||
return atof(data); |
||||
} |
||||
|
||||
void comma() { |
||||
Serial.print(','); |
||||
} |
||||
@ -0,0 +1,177 @@
@@ -0,0 +1,177 @@
|
||||
/*
|
||||
ArduCopter v1.2 |
||||
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 <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
|
||||
#include "WProgram.h" |
||||
|
||||
// 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 KD_QUAD_COMMAND_PART; |
||||
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 KD_QUAD_COMMAND_PART_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 |
||||
|
||||
// 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); |
||||
KD_QUAD_COMMAND_PART = readEEPROM(KD_QUAD_COMMAND_PART_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); |
||||
} |
||||
Loading…
Reference in new issue