mirror of https://github.com/ArduPilot/ardupilot
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
parent
085f0741a8
commit
12d54e7e72
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
*/
|
||||
|
||||
|
||||
/* ******* ADC functions ********************* */
|
||||
|
|
@ -244,4 +244,4 @@ void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
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() {
|
|||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
*/
|
||||
|
||||
/* ************************************************************ */
|
||||
/* GPS based Position control */
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
*/
|
||||
|
||||
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(',');
|
||||
}
|
||||
*/
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
*/
|
||||
|
||||
/* *************************************************************
|
||||
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); */
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue