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

preparing for camera stabilization features 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@756 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc 2010-10-30 14:38:52 +00:00
parent 1ece81342b
commit 288c61fcce
7 changed files with 301 additions and 179 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArducopterNG/ArduUser.h
View File

@ -81,17 +81,27 @@ TODO:
#define CH_7 6
#define CH_8 7
#define ROLL_MID 1500 // Radio Roll channel mid value
#define PITCH_MID 1500 // Radio Pitch channel mid value
#define YAW_MID 1500 // Radio Yaw channel mid value
#define THROTTLE_MID 1505 // Radio Throttle channel mid value
#define ROLL_MID 1500 // Radio Roll channel mid value
#define PITCH_MID 1500 // Radio Pitch channel mid value
#define YAW_MID 1500 // Radio Yaw channel mid value
#define THROTTLE_MID 1505 // Radio Throttle channel mid value
#define AUX_MID 1500
#define CHANN_CENTER 1500 // Channel center, legacy
#define MIN_THROTTLE 1040 // Throttle pulse width at minimun...
/* ******************************************************** */
// Camera related settings
#define CAM_CENT 1500 // Camera center
#define CAM_SMOOTHING 1000 // Camera movement smoothing
#define CAM_SMOOTHING 1000 // Camera movement smoothing on pitch axis
#define CAM_SMOOTHING_ROLL -400 // Camera movement smoothing on roll axis
#define CAM_TILT_OUT 6 // OUTx pin for Tilt servo
#define CAM_ROLL_OUT 7 // OUTx pin for Roll servo
#define CAM_YAW_OUT 7 // OUTx pin for Yaw servo (often same as Roll)
#define CAM_TILT_CH CH_7 // Channel for radio knob to controll tilt "zerolevel"
/*************************************************************/
// General definitions

294
ArducopterNG/Arducopter.h
View File

@ -251,6 +251,9 @@ boolean SW_DIP2;
boolean SW_DIP3;
boolean SW_DIP4; // closest to header pins
boolean BATTLOW = FALSE; // We should be always FALSE, if we are TRUE.. it means destruction is close,
// shut down all secondary systems that uses our precious mAh's
// Attitude PID controls
float roll_I=0;
float roll_D;
@ -312,41 +315,41 @@ int Sonar_Counter=0;
// AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode)
byte AP_mode = 2;
byte cam_mode = 0;
//byte cam_mode = 0;
// Mode LED timers and variables, used to blink LED_Green
byte gled_status = HIGH;
long gled_timer;
int gled_speed;
long t0;
int num_iter;
long t0;
int num_iter;
float aux_debug;
// Radio definitions
int roll_mid;
int pitch_mid;
int yaw_mid;
int roll_mid;
int pitch_mid;
int yaw_mid;
int Neutro_yaw;
int ch_roll;
int ch_pitch;
int ch_throttle;
int ch_yaw;
int ch_aux;
int ch_aux2;
int Neutro_yaw;
int ch_roll;
int ch_pitch;
int ch_throttle;
int ch_yaw;
int ch_aux;
int ch_aux2;
int frontMotor;
int backMotor;
int leftMotor;
int rightMotor;
byte motorArmed = 0;
int minThrottle = 0;
int frontMotor;
int backMotor;
int leftMotor;
int rightMotor;
byte motorArmed = 0; // 0 = motors disarmed, 1 = motors armed
int minThrottle = 0;
boolean flightOrientation = 0; // 0 = +, 1 = x this is read from DIP1 switch during system bootup
// Serial communication
char queryType;
long tlmTimer = 0;
char queryType;
long tlmTimer = 0;
// Arming/Disarming
uint8_t Arming_counter=0;
@ -368,11 +371,11 @@ byte gcs_messages_sent = 0;
// --------------
unsigned long fast_loopTimer = 0; // Time in miliseconds of main control loop
unsigned long medium_loopTimer = 0; // Time in miliseconds of navigation control loop
byte medium_loopCounter = 0; // Counters for branching from main control loop to slower loops
byte slow_loopCounter = 0; //
byte medium_loopCounter = 0; // Counters for branching from main control loop to slower loops
byte slow_loopCounter = 0; //
unsigned long deltaMiliSeconds = 0; // Delta Time in miliseconds
unsigned long dTnav = 0; // Delta Time in milliseconds for navigation computations
int mainLoop_count = 0;
int mainLoop_count = 0;
unsigned long elapsedTime = 0; // for doing custom events
//unsigned int GPS_timer = 0;
@ -479,137 +482,140 @@ 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;
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;
byte cam_mode = 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 defaultUserConfig() {
KP_QUAD_ROLL = 4.0;
KI_QUAD_ROLL = 0.15;
STABLE_MODE_KP_RATE_ROLL = 1.2;
KP_QUAD_PITCH = 4.0;
KI_QUAD_PITCH = 0.15;
STABLE_MODE_KP_RATE_PITCH = 1.2;
KP_QUAD_YAW = 3.0;
KI_QUAD_YAW = 0.15;
STABLE_MODE_KP_RATE_YAW = 2.4;
STABLE_MODE_KP_RATE = 0.2; // NOT USED NOW
KP_GPS_ROLL = 0.015;
KI_GPS_ROLL = 0.005;
KD_GPS_ROLL = 0.01;
KP_GPS_PITCH = 0.015;
KI_GPS_PITCH = 0.005;
KD_GPS_PITCH = 0.01;
GPS_MAX_ANGLE = 22;
KP_ALTITUDE = 0.8;
KI_ALTITUDE = 0.2;
KD_ALTITUDE = 0.2;
acc_offset_x = 2048;
acc_offset_y = 2048;
acc_offset_z = 2048;
gyro_offset_roll = 1659;
gyro_offset_pitch = 1650;
gyro_offset_yaw = 1650;
Kp_ROLLPITCH = 0.0014;
Ki_ROLLPITCH = 0.00000015;
Kp_YAW = 1.0;
Ki_YAW = 0.00002;
GEOG_CORRECTION_FACTOR = 0.87;
MAGNETOMETER = 0;
Kp_RateRoll = 1.95;
Ki_RateRoll = 0.0;
Kd_RateRoll = 0.0;
Kp_RatePitch = 1.95;
Ki_RatePitch = 0.0;
Kd_RatePitch = 0.0;
Kp_RateYaw = 3.2;
Ki_RateYaw = 0.0;
Kd_RateYaw = 0.0;
xmitFactor = 0.32;
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;
KP_QUAD_ROLL = 4.0;
KI_QUAD_ROLL = 0.15;
STABLE_MODE_KP_RATE_ROLL = 1.2;
KP_QUAD_PITCH = 4.0;
KI_QUAD_PITCH = 0.15;
STABLE_MODE_KP_RATE_PITCH = 1.2;
KP_QUAD_YAW = 3.0;
KI_QUAD_YAW = 0.15;
STABLE_MODE_KP_RATE_YAW = 2.4;
STABLE_MODE_KP_RATE = 0.2; // NOT USED NOW
KP_GPS_ROLL = 0.015;
KI_GPS_ROLL = 0.005;
KD_GPS_ROLL = 0.01;
KP_GPS_PITCH = 0.015;
KI_GPS_PITCH = 0.005;
KD_GPS_PITCH = 0.01;
GPS_MAX_ANGLE = 22;
KP_ALTITUDE = 0.8;
KI_ALTITUDE = 0.2;
KD_ALTITUDE = 0.2;
acc_offset_x = 2048;
acc_offset_y = 2048;
acc_offset_z = 2048;
gyro_offset_roll = 1659;
gyro_offset_pitch = 1650;
gyro_offset_yaw = 1650;
Kp_ROLLPITCH = 0.0014;
Ki_ROLLPITCH = 0.00000015;
Kp_YAW = 1.0;
Ki_YAW = 0.00002;
GEOG_CORRECTION_FACTOR = 0.87;
MAGNETOMETER = 0;
Kp_RateRoll = 1.95;
Ki_RateRoll = 0.0;
Kd_RateRoll = 0.0;
Kp_RatePitch = 1.95;
Ki_RatePitch = 0.0;
Kd_RatePitch = 0.0;
Kp_RateYaw = 3.2;
Ki_RateYaw = 0.0;
Kd_RateYaw = 0.0;
xmitFactor = 0.32;
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;
cam_mode = 0;
}
// EEPROM storage addresses
#define KP_QUAD_ROLL_ADR 0
#define KI_QUAD_ROLL_ADR 8
#define KP_QUAD_ROLL_ADR 0
#define KI_QUAD_ROLL_ADR 8
#define STABLE_MODE_KP_RATE_ROLL_ADR 4
#define KP_QUAD_PITCH_ADR 12
#define KI_QUAD_PITCH_ADR 20
#define KP_QUAD_PITCH_ADR 12
#define KI_QUAD_PITCH_ADR 20
#define STABLE_MODE_KP_RATE_PITCH_ADR 16
#define KP_QUAD_YAW_ADR 24
#define KI_QUAD_YAW_ADR 32
#define KP_QUAD_YAW_ADR 24
#define KI_QUAD_YAW_ADR 32
#define STABLE_MODE_KP_RATE_YAW_ADR 28
#define STABLE_MODE_KP_RATE_ADR 36 // NOT USED NOW
#define KP_GPS_ROLL_ADR 40
#define KI_GPS_ROLL_ADR 48
#define KD_GPS_ROLL_ADR 44
#define KP_GPS_PITCH_ADR 52
#define KI_GPS_PITCH_ADR 60
#define KD_GPS_PITCH_ADR 56
#define GPS_MAX_ANGLE_ADR 64
#define KP_ALTITUDE_ADR 68
#define KI_ALTITUDE_ADR 76
#define KD_ALTITUDE_ADR 72
#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 KP_GPS_ROLL_ADR 40
#define KI_GPS_ROLL_ADR 48
#define KD_GPS_ROLL_ADR 44
#define KP_GPS_PITCH_ADR 52
#define KI_GPS_PITCH_ADR 60
#define KD_GPS_PITCH_ADR 56
#define GPS_MAX_ANGLE_ADR 64
#define KP_ALTITUDE_ADR 68
#define KI_ALTITUDE_ADR 76
#define KD_ALTITUDE_ADR 72
#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 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
#define ch_yaw_offset_ADR 216
#define ch_aux_offset_ADR 220
#define ch_aux2_offset_ADR 224
#define cam_mode_ADR 226

9
ArducopterNG/ArducopterNG.pde
View File

@ -49,9 +49,10 @@
//#define IsNEWMTEK // Do we have MTEK with new firmware
//#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator
//#define IsAM // Do we have motormount LED's. AM = Atraction Mode
//#define IsCAM // Do we have camera stabilization in use, If you activate, check OUTPUT pins from ArduUser.h
//#define UseAirspeed // Quads don't use AirSpeed... Legacy, jp 19-10-10
//#define UseBMP
//#define UseBMP // Use pressure sensor
//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it _wired_ up!)
#define CONFIGURATOR
@ -98,7 +99,7 @@
//#define MAGORIENTATION APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD // If you have soldered Magneto to IMU shield in WIki pictures shows
// To get Magneto offsets, switch to CLI mode and run offset calibration. During calibration
// you need to roll/bank/tilt/yaw/shake etc your ArduCoptet. Don't kick like Jani always does :)
// you need to roll/bank/tilt/yaw/shake etc your ArduCopter. Don't kick like Jani always does :)
#define MAGOFFSET 0,0,0
// Declination is a correction factor between North Pole and real magnetic North. This is different on every location
@ -152,7 +153,7 @@
//#include <GPS_NMEA.h> // ArduPilot NMEA GPS library
/* Software version */
#define VER 1.51 // Current software version (only numeric values)
#define VER 1.52 // Current software version (only numeric values)
/* ************************************************************ */
@ -251,8 +252,10 @@ void loop()
// Send output commands to motor ESCs...
motor_output();
#ifdef IsCAM
// Do we have cameras stabilization connected and in use?
if(!SW_DIP2) camera_output();
#endif
// Autopilot mode functions
if (AP_mode == AP_AUTOMATIC_MODE)

5
ArducopterNG/EEPROM.pde
View File

@ -114,6 +114,7 @@ void readUserConfig() {
ch_yaw_offset = readEEPROM(ch_yaw_offset_ADR);
ch_aux_offset = readEEPROM(ch_aux_offset_ADR);
ch_aux2_offset = readEEPROM(ch_aux2_offset_ADR);
cam_mode = readEEPROM(cam_mode_ADR);
}
void writeUserConfig() {
@ -174,4 +175,6 @@ void writeUserConfig() {
writeEEPROM(ch_yaw_offset, ch_yaw_offset_ADR);
writeEEPROM(ch_aux_offset, ch_aux_offset_ADR);
writeEEPROM(ch_aux2_offset, ch_aux2_offset_ADR);
}
writeEEPROM(cam_mode, cam_mode_ADR);
}

79
ArducopterNG/Functions.pde
View File

@ -25,7 +25,7 @@
* ************************************************************** *
ChangeLog:
30-10-10 added basic camera stabilization functions with jumptables
* ************************************************************** *
TODO:
@ -73,11 +73,9 @@ void RunningLights(int LightStep) {
}
void LightsOff() {
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, LOW);
}
// Funtion to normalize an angle in degrees to -180,180 degrees
@ -133,6 +131,14 @@ boolean APMPinRead(volatile unsigned char &Port, byte Pin)
return 0;
}
// Faster and smaller replacement for contrain() function
int limitRange(int data, int minLimit, int maxLimit) {
if (data < minLimit) return minLimit;
else if (data > maxLimit) return maxLimit;
else return data;
}
/* **************************************************** */
// Camera stabilization
@ -140,13 +146,76 @@ boolean APMPinRead(volatile unsigned char &Port, byte Pin)
// Stabilization for three different camera styles
// 1) Camera mounts that have tilt / pan
// 2) Camera mounts that have tilt / roll
// 3) Camera mounts that have tilt / roll / pan
// 3) Camera mounts that have tilt / roll / pan (future)
//
// Original code idea from Max Levine / DIY Drones
// You need to initialize proper camera mode by sending Serial command and then save it
// to EEPROM memory. Possible commands are K1, K2, K3, K4
// Look more about different camera type on ArduCopter Wiki
#ifdef IsCAM
void camera_output() {
// cam_mode = 1; // for debugging
// Camera stabilization jump tables
// SW_DIP1 is a multplier, settings
switch ((SW_DIP1 * 4) + cam_mode + (BATTLOW * 10)) {
// Cases 1 & 4 are stabilization for + Mode flying setup
// Cases 5 & 8 are stabilization for x Mode flying setup
// Modes 3/4 + 7/8 needs still proper scaling on yaw movement
// Scaling needs physical test flying with FPV cameras on, 30-10-10 jp
case 1:
// Camera stabilization with Roll / Tilt mounts, NO transmitter input
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((CAM_CENT + (pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_ROLL_OUT, limitRange((CAM_CENT + (roll) * CAM_SMOOTHING_ROLL), 1000, 2000)); // Roll correction
break;
case 2:
// Camera stabilization with Roll / Tilt mounts, transmitter controls basic "zerolevel"
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((APM_RC.InputCh(CAM_TILT_CH) + (pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_ROLL_OUT, limitRange((CAM_CENT + (roll) * CAM_SMOOTHING_ROLL), 1000, 2000)); // Roll correction
break;
case 3:
// Camera stabilization with Yaw / Tilt mounts, NO transmitter input
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((CAM_CENT - (roll - pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_YAW_OUT, limitRange((CAM_CENT - (gyro_offset_yaw - AN[2])), 1000, 2000)); // Roll correction
break;
case 4:
// Camera stabilization with Yaw / Tilt mounts, transmitter controls basic "zerolevel"
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((APM_RC.InputCh(CAM_TILT_CH) + (pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_YAW_OUT, limitRange((CAM_CENT - (gyro_offset_yaw - AN[2])), 1000, 2000)); // Roll correction
break;
// x Mode flying setup
case 5:
// Camera stabilization with Roll / Tilt mounts, NO transmitter input
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((CAM_CENT - (roll - pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_ROLL_OUT, limitRange((CAM_CENT + (roll + pitch) * CAM_SMOOTHING), 1000, 2000)); // Roll correction
break;
case 6:
// Camera stabilization with Roll / Tilt mounts, transmitter controls basic "zerolevel"
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((APM_RC.InputCh(CAM_TILT_CH) + (roll - pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_ROLL_OUT, limitRange((CAM_CENT + (roll + pitch) * CAM_SMOOTHING), 1000, 2000)); // Roll correction
break;
case 7:
// Camera stabilization with Yaw / Tilt mounts, NO transmitter input
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((CAM_CENT - (roll - pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_YAW_OUT, limitRange((CAM_CENT - (gyro_offset_yaw - AN[2])), 1000, 2000)); // Roll correction
break;
case 8:
// Camera stabilization with Yaw / Tilt mounts, transmitter controls basic "zerolevel"
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((APM_RC.InputCh(CAM_TILT_CH) - (roll - pitch) * CAM_SMOOTHING),1000,2000)); // Tilt correction
APM_RC.OutputCh(CAM_YAW_OUT, limitRange((CAM_CENT - (gyro_offset_yaw - AN[2])),1000,2000)); // Yaw correction
break;
// Only in case of we have case values over 10
default:
// We should not be here...
break;
}
}
#endif

71
ArducopterNG/GCS.pde
View File

@ -105,7 +105,10 @@ void readSerialCommand() {
acc_offset_y = readFloatSerial();
acc_offset_z = readFloatSerial();
break;
case 'K': // Spare
case 'K': // Camera mode
// 1 = Tilt / Roll without
cam_mode = readFloatSerial();
//BATTLOW = readFloatSerial();
break;
case 'M': // Receive debug motor commands
frontMotor = readFloatSerial();
@ -126,7 +129,7 @@ void readSerialCommand() {
Kd_RateYaw = readFloatSerial();
xmitFactor = readFloatSerial();
break;
case 'W': // Write all user configurable values to EEPROM
case 'W': // Write all user configurable values to EEPROM
writeUserConfig();
break;
case 'Y': // Initialize EEPROM with default values
@ -147,7 +150,7 @@ void readSerialCommand() {
ch_aux2_offset = readFloatSerial();
break;
case '5': // Special debug features
break;
}
@ -271,8 +274,11 @@ void sendSerialTelemetry() {
AN_OFFSET[5] = acc_offset_z;
queryType = 'X';
break;
case 'L': // Spare
// RadioCalibration();
case 'L': // Camera settings and
SerPri(cam_mode, DEC);
tab();
SerPri(BATTLOW, DEC);
tab();
queryType = 'X';
break;
case 'N': // Send magnetometer config
@ -354,21 +360,22 @@ void sendSerialTelemetry() {
comma();
SerPri(read_adc(3));
comma();
SerPrln(read_adc(5));
/* comma();
SerPri(APM_Compass.Heading, 4);
comma();
SerPri(APM_Compass.Heading_X, 4);
comma();
SerPri(APM_Compass.Heading_Y, 4);
comma();
SerPri(APM_Compass.Mag_X);
comma();
SerPri(APM_Compass.Mag_Y);
comma();
SerPri(APM_Compass.Mag_Z);
comma();
*/
SerPri(read_adc(5));
/* comma();
SerPri(APM_Compass.Heading, 4);
comma();
SerPri(APM_Compass.Heading_X, 4);
comma();
SerPri(APM_Compass.Heading_Y, 4);
comma();
SerPri(APM_Compass.Mag_X);
comma();
SerPri(APM_Compass.Mag_Y);
comma();
SerPri(APM_Compass.Mag_Z);
comma();
*/
SerPriln();
break;
case 'T': // Spare
break;
@ -425,6 +432,29 @@ void sendSerialTelemetry() {
SerPrln(ch_aux2_offset);
queryType = 'X';
break;
case '3': // Jani's debugs
SerPri(yaw);
tab();
SerPri(command_rx_yaw);
tab();
SerPri(control_yaw);
tab();
SerPri(err_yaw);
tab();
SerPri(AN[0]);
tab();
SerPri(AN[1]);
tab();
SerPri(AN[2] - gyro_offset_yaw);
tab();
SerPri(gyro_offset_yaw - AN[2]);
tab();
SerPri(gyro_offset_yaw);
tab();
SerPri(1500 - (gyro_offset_yaw - AN[2]));
tab();
SerPriln();
break;
case '.': // Modify GPS settings, print directly to GPS Port
Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
break;
@ -460,3 +490,4 @@ float readFloatSerial() {
return atof(data);
}

2
ArducopterNG/Sensors.pde
View File

@ -65,7 +65,7 @@ void calibrateSensors(void) {
for(i=0;i<600;i++)
{
Read_adc_raw(); // Read sensors
for(gyro=GYROZ; gyro<=GYROY; gyro++)
for(gyro = GYROZ; gyro <= GYROY; gyro++)
aux_float[gyro] = aux_float[gyro] * 0.8 + AN[gyro] * 0.2; // Filtering
Log_Write_Sensor(AN[0], AN[1], AN[2], AN[3], AN[4], AN[5], 0);