mirror of https://github.com/ArduPilot/ardupilot
CalibrateSensors update. Some cleanups...
git-svn-id: https://arducopter.googlecode.com/svn/trunk@655 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234
parent
7667214987
commit
6baf85d880
|
|
@ -56,10 +56,8 @@ TODO:
|
|||
|
||||
/* AM PIN Definitions - END */
|
||||
|
||||
|
||||
/*************************************************************/
|
||||
// Radio related definitions
|
||||
|
||||
#define CH_ROLL 0
|
||||
#define CH_PITCH 1
|
||||
#define CH_THROTTLE 2
|
||||
|
|
@ -82,6 +80,28 @@ TODO:
|
|||
#define CHANN_CENTER 1500 // Channel center, legacy
|
||||
#define MIN_THROTTLE 1040 // Throttle pulse width at minimun...
|
||||
|
||||
/*************************************************************/
|
||||
// General definitions
|
||||
//Modes
|
||||
#define STABLE_MODE 0
|
||||
#define ACRO_MODE 1
|
||||
|
||||
//Axis
|
||||
#define ROLL 0
|
||||
#define PITCH 1
|
||||
#define YAW 2
|
||||
#define XAXIS 0
|
||||
#define YAXIS 1
|
||||
#define ZAXIS 2
|
||||
|
||||
#define GYROZ 0
|
||||
#define GYROX 1
|
||||
#define GYROY 2
|
||||
#define ACCELX 3
|
||||
#define ACCELY 4
|
||||
#define ACCELZ 5
|
||||
#define LASTSENSOR 6
|
||||
|
||||
// Following variables stored in EEPROM
|
||||
float KP_QUAD_ROLL;
|
||||
float KI_QUAD_ROLL;
|
||||
|
|
|
|||
|
|
@ -56,11 +56,10 @@ TODO:
|
|||
#define SLIDE_SWITCH_PIN 40
|
||||
#define PUSHBUTTON_PIN 41
|
||||
|
||||
#define A_LED_PIN 37 //36 = B, 37 = A, 35 = C
|
||||
#define A_LED_PIN 37
|
||||
#define B_LED_PIN 36
|
||||
#define C_LED_PIN 35
|
||||
|
||||
|
||||
#define EE_LAST_LOG_PAGE 0xE00
|
||||
#define EE_LAST_LOG_NUM 0xE02
|
||||
#define EE_LOG_1_START 0xE04
|
||||
|
|
@ -90,68 +89,7 @@ TODO:
|
|||
#define SerAv Serial.available
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
/****************************************************/
|
||||
/*Logging Stuff - These should be 1 (on) or 0 (off)*/
|
||||
/****************************************************/
|
||||
#define LOG_ATTITUDE 1 // Logs basic attitude info
|
||||
#define LOG_GPS 1 // Logs GPS info
|
||||
#define LOG_PM 1 // Logs IMU performance monitoring info£
|
||||
#define LOG_CTUN 0 // Logs control loop tuning info
|
||||
#define LOG_NTUN 0 // Logs navigation loop tuning info
|
||||
#define LOG_MODE 1 // Logs mode changes
|
||||
#define LOG_RAW 0 // Logs raw accel/gyro data
|
||||
#define LOG_SEN 1 // Logs sensor data
|
||||
|
||||
// GCS Message ID's
|
||||
#define MSG_ACKNOWLEDGE 0x00
|
||||
#define MSG_HEARTBEAT 0x01
|
||||
#define MSG_ATTITUDE 0x02
|
||||
#define MSG_LOCATION 0x03
|
||||
#define MSG_PRESSURE 0x04
|
||||
#define MSG_STATUS_TEXT 0x05
|
||||
#define MSG_PERF_REPORT 0x06
|
||||
#define MSG_COMMAND 0x22
|
||||
#define MSG_VALUE 0x32
|
||||
#define MSG_PID 0x42
|
||||
#define MSG_TRIMS 0x50
|
||||
#define MSG_MINS 0x51
|
||||
#define MSG_MAXS 0x52
|
||||
#define MSG_IMU_OUT 0x53
|
||||
|
||||
#define SEVERITY_LOW 1
|
||||
#define SEVERITY_MEDIUM 2
|
||||
#define SEVERITY_HIGH 3
|
||||
#define SEVERITY_CRITICAL 4
|
||||
|
||||
// Debug options - set only one of these options to 1 at a time, set the others to 0
|
||||
#define DEBUG_SUBSYSTEM 0 // 0 = no debug
|
||||
// 1 = Debug the Radio input
|
||||
// 2 = Debug the Servo output
|
||||
// 3 = Debug the Sensor input
|
||||
// 4 = Debug the GPS input
|
||||
// 5 = Debug the GPS input - RAW HEX OUTPUT
|
||||
// 6 = Debug the IMU
|
||||
// 7 = Debug the Control Switch
|
||||
// 8 = Debug the Servo DIP switches
|
||||
// 9 = Debug the Relay out
|
||||
// 10 = Debug the Magnetometer
|
||||
// 11 = Debug the ABS pressure sensor
|
||||
// 12 = Debug the stored waypoints
|
||||
// 13 = Debug the Throttle
|
||||
// 14 = Debug the Radio Min Max
|
||||
// 15 = Debug the EEPROM - Hex Dump
|
||||
|
||||
|
||||
#define DEBUG_LEVEL SEVERITY_LOW
|
||||
// SEVERITY_LOW
|
||||
// SEVERITY_MEDIUM
|
||||
// SEVERITY_HIGH
|
||||
// SEVERITY_CRITICAL
|
||||
|
||||
|
||||
// IMU definitions
|
||||
// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
|
||||
uint8_t sensors[6] = {1, 2, 0, 4, 5, 6}; // For ArduPilot Mega Sensor Shield Hardware
|
||||
|
||||
|
|
@ -162,7 +100,6 @@ int SENSOR_SIGN[]={
|
|||
/* APM Hardware definitions, END */
|
||||
|
||||
/* General definitions */
|
||||
|
||||
#define TRUE 1
|
||||
#define FALSE 0
|
||||
#define ON 1
|
||||
|
|
@ -389,3 +326,63 @@ unsigned long elapsedTime = 0; // for doing custom events
|
|||
//unsigned int GPS_timer = 0;
|
||||
|
||||
|
||||
|
||||
/****************************************************/
|
||||
/*Logging Stuff - These should be 1 (on) or 0 (off)*/
|
||||
/****************************************************/
|
||||
#define LOG_ATTITUDE 1 // Logs basic attitude info
|
||||
#define LOG_GPS 1 // Logs GPS info
|
||||
#define LOG_PM 1 // Logs IMU performance monitoring info£
|
||||
#define LOG_CTUN 0 // Logs control loop tuning info
|
||||
#define LOG_NTUN 0 // Logs navigation loop tuning info
|
||||
#define LOG_MODE 1 // Logs mode changes
|
||||
#define LOG_RAW 0 // Logs raw accel/gyro data
|
||||
#define LOG_SEN 1 // Logs sensor data
|
||||
|
||||
// GCS Message ID's
|
||||
#define MSG_ACKNOWLEDGE 0x00
|
||||
#define MSG_HEARTBEAT 0x01
|
||||
#define MSG_ATTITUDE 0x02
|
||||
#define MSG_LOCATION 0x03
|
||||
#define MSG_PRESSURE 0x04
|
||||
#define MSG_STATUS_TEXT 0x05
|
||||
#define MSG_PERF_REPORT 0x06
|
||||
#define MSG_COMMAND 0x22
|
||||
#define MSG_VALUE 0x32
|
||||
#define MSG_PID 0x42
|
||||
#define MSG_TRIMS 0x50
|
||||
#define MSG_MINS 0x51
|
||||
#define MSG_MAXS 0x52
|
||||
#define MSG_IMU_OUT 0x53
|
||||
|
||||
#define SEVERITY_LOW 1
|
||||
#define SEVERITY_MEDIUM 2
|
||||
#define SEVERITY_HIGH 3
|
||||
#define SEVERITY_CRITICAL 4
|
||||
|
||||
// Debug options - set only one of these options to 1 at a time, set the others to 0
|
||||
#define DEBUG_SUBSYSTEM 0 // 0 = no debug
|
||||
// 1 = Debug the Radio input
|
||||
// 2 = Debug the Servo output
|
||||
// 3 = Debug the Sensor input
|
||||
// 4 = Debug the GPS input
|
||||
// 5 = Debug the GPS input - RAW HEX OUTPUT
|
||||
// 6 = Debug the IMU
|
||||
// 7 = Debug the Control Switch
|
||||
// 8 = Debug the Servo DIP switches
|
||||
// 9 = Debug the Relay out
|
||||
// 10 = Debug the Magnetometer
|
||||
// 11 = Debug the ABS pressure sensor
|
||||
// 12 = Debug the stored waypoints
|
||||
// 13 = Debug the Throttle
|
||||
// 14 = Debug the Radio Min Max
|
||||
// 15 = Debug the EEPROM - Hex Dump
|
||||
|
||||
|
||||
#define DEBUG_LEVEL SEVERITY_LOW
|
||||
// SEVERITY_LOW
|
||||
// SEVERITY_MEDIUM
|
||||
// SEVERITY_HIGH
|
||||
// SEVERITY_CRITICAL
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -92,28 +92,7 @@
|
|||
/* ************************************************************ */
|
||||
/* ************* MAIN PROGRAM - DECLARATIONS ****************** */
|
||||
/* ************************************************************ */
|
||||
#define ROLL 0
|
||||
#define PITCH 1
|
||||
#define YAW 2
|
||||
#define XAXIS 0
|
||||
#define YAXIS 1
|
||||
#define ZAXIS 2
|
||||
|
||||
#define GYROZ 0
|
||||
#define GYROX 1
|
||||
#define GYROY 2
|
||||
#define ACCELX 3
|
||||
#define ACCELY 4
|
||||
#define ACCELZ 5
|
||||
#define LASTSENSOR 6
|
||||
int rawADC[6];
|
||||
int zeroADC[6];
|
||||
int dataADC[6];
|
||||
byte channel;
|
||||
int sensorSign[6] = {1, 1, 1, 1, 1, 1}; // GYROZ, GYROX, GYROY, ACCELX, ACCELY, ACCELZ
|
||||
|
||||
#define STABLE 0
|
||||
#define ACRO 1
|
||||
byte flightMode;
|
||||
|
||||
unsigned long currentTime, previousTime, deltaTime;
|
||||
|
|
@ -128,7 +107,7 @@ unsigned long motorLoop = 0;
|
|||
/* ************************************************************ */
|
||||
void setup() {
|
||||
|
||||
APM_Init(); // APM Hardware initialization
|
||||
APM_Init(); // APM Hardware initialization (in System.pde)
|
||||
|
||||
previousTime = millis();
|
||||
motorArmed = 0;
|
||||
|
|
@ -190,10 +169,11 @@ void loop()
|
|||
Euler_angles();
|
||||
|
||||
// Read radio values (if new data is available)
|
||||
read_radio();
|
||||
if (APM_RC.GetState() == 1) // New radio frame?
|
||||
read_radio();
|
||||
|
||||
// Attitude control
|
||||
if(flightMode == STABLE) { // STABLE Mode
|
||||
if(flightMode == STABLE_MODE) { // STABLE Mode
|
||||
gled_speed = 1200;
|
||||
if (AP_mode == 0) // Normal mode
|
||||
Attitude_control_v3(command_rx_roll,command_rx_pitch,command_rx_yaw);
|
||||
|
|
@ -207,7 +187,7 @@ void loop()
|
|||
command_rx_yaw = ToDeg(yaw);
|
||||
}
|
||||
|
||||
// Send output commands to motors...
|
||||
// Send output commands to motor ESCs...
|
||||
motor_output();
|
||||
|
||||
// Performance optimization: Magnetometer sensor and pressure sensor are slowly to read (I2C)
|
||||
|
|
@ -224,8 +204,9 @@ void loop()
|
|||
#endif
|
||||
#ifdef UseBMP
|
||||
#endif
|
||||
|
||||
// Slow loop (10Hz)
|
||||
if((millis()-tlmTimer)>=100) {
|
||||
if((currentTime-tlmTimer)>=100) {
|
||||
//#if BATTERY_EVENT == 1
|
||||
// read_battery(); // Battery monitor
|
||||
//#endif
|
||||
|
|
@ -233,7 +214,7 @@ void loop()
|
|||
readSerialCommand();
|
||||
sendSerialTelemetry();
|
||||
#endif
|
||||
tlmTimer = millis();
|
||||
tlmTimer = currentTime;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -38,8 +38,6 @@ TODO:
|
|||
|
||||
void read_radio()
|
||||
{
|
||||
if (APM_RC.GetState() == 1) // New radio frame?
|
||||
{
|
||||
// Commands from radio Rx
|
||||
// We apply the Radio calibration parameters (from configurator) except for throttle
|
||||
ch_roll = channel_filter(APM_RC.InputCh(CH_ROLL) * ch_roll_slope + ch_roll_offset, ch_roll);
|
||||
|
|
@ -51,12 +49,12 @@ void read_radio()
|
|||
|
||||
// Flight mode
|
||||
if(ch_aux2 > 1800)
|
||||
flightMode = 1; // Force to Acro mode from radio
|
||||
flightMode = ACRO_MODE; // Force to Acro mode from radio
|
||||
else
|
||||
flightMode = 0; // Stable mode (default)
|
||||
flightMode = STABLE_MODE; // Stable mode (default)
|
||||
|
||||
// Autopilot mode (only works on Stable mode)
|
||||
if (flightMode == 0)
|
||||
if (flightMode == STABLE_MODE)
|
||||
{
|
||||
if(ch_aux > 1800)
|
||||
AP_mode = 1; // Automatic mode : GPS position hold mode + altitude hold
|
||||
|
|
@ -88,7 +86,6 @@ void read_radio()
|
|||
|
||||
// Write Radio data to DataFlash log
|
||||
Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,int(K_aux*100),(int)AP_mode);
|
||||
} // END new radio data
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -44,43 +44,40 @@ int read_adc(int select)
|
|||
return (AN[select]-AN_OFFSET[select]);
|
||||
}
|
||||
|
||||
int readADC(byte channel) {
|
||||
if (sensorSign[channel] < 0)
|
||||
return (zeroADC[channel] - APM_ADC.Ch(channel));
|
||||
else
|
||||
return (APM_ADC.Ch(channel) - zeroADC[channel]);
|
||||
}
|
||||
|
||||
void calibrateSensors(void) {
|
||||
int i;
|
||||
int j = 0;
|
||||
byte gyro;
|
||||
float aux_float[3];
|
||||
|
||||
Read_adc_raw(); // Read sensors data
|
||||
delay(5);
|
||||
|
||||
// Offset values for accels and gyros...
|
||||
AN_OFFSET[3] = acc_offset_x; // Accel offset values are taken from external calibration (In Configurator)
|
||||
AN_OFFSET[4] = acc_offset_y;
|
||||
AN_OFFSET[5] = acc_offset_z;
|
||||
aux_float[0] = gyro_offset_roll;
|
||||
aux_float[1] = gyro_offset_pitch;
|
||||
aux_float[2] = gyro_offset_yaw;
|
||||
|
||||
// Take the gyro offset values
|
||||
for(int i=0;i<300;i++) {
|
||||
for (channel = GYROZ; channel < LASTSENSOR; channel++) {
|
||||
rawADC[channel] = APM_ADC.Ch(channel);
|
||||
zeroADC[channel] = (zeroADC[channel] * 0.8) + (rawADC[channel] * 0.2);
|
||||
//Log_Write_Sensor(rawADC[GYROZ], rawADC[GYROX], rawADC[GYROY], rawADC[ACCELX], rawADC[ACCELY], rawADC[ACCELZ], receiverData[THROTTLE]);
|
||||
}
|
||||
for(i=0;i<600;i++)
|
||||
{
|
||||
Read_adc_raw(); // Read sensors
|
||||
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);
|
||||
delay(5);
|
||||
RunningLights(j); // (in Functions.pde)
|
||||
// Runnings lights effect to let user know that we are taking mesurements
|
||||
if(j == 0) {
|
||||
digitalWrite(LED_Green, HIGH);
|
||||
digitalWrite(LED_Yellow, LOW);
|
||||
digitalWrite(LED_Red, LOW);
|
||||
}
|
||||
else if (j == 1) {
|
||||
digitalWrite(LED_Green, LOW);
|
||||
digitalWrite(LED_Yellow, HIGH);
|
||||
digitalWrite(LED_Red, LOW);
|
||||
}
|
||||
else {
|
||||
digitalWrite(LED_Green, LOW);
|
||||
digitalWrite(LED_Yellow, LOW);
|
||||
digitalWrite(LED_Red, HIGH);
|
||||
}
|
||||
if((i % 5) == 0) j++;
|
||||
if(j >= 3) j = 0;
|
||||
}
|
||||
digitalWrite(LED_Green, LOW);
|
||||
digitalWrite(LED_Yellow, LOW);
|
||||
digitalWrite(LED_Red, LOW);
|
||||
|
||||
// Switch off all ABC lights
|
||||
LightsOff();
|
||||
|
||||
for(gyro=GYROZ; gyro<=GYROY; gyro++)
|
||||
AN_OFFSET[gyro]=aux_float[gyro]; // Update sensor OFFSETs from values read
|
||||
}
|
||||
|
|
|
|||
|
|
@ -36,9 +36,6 @@ TODO:
|
|||
// General Initialization for all APM electronics
|
||||
void APM_Init() {
|
||||
|
||||
int i, j; // Temporary variables used to count things
|
||||
float aux_float[3];
|
||||
|
||||
pinMode(LED_Yellow,OUTPUT); //Yellow LED A (PC1)
|
||||
pinMode(LED_Red,OUTPUT); //Red LED B (PC2)
|
||||
pinMode(LED_Green,OUTPUT); //Green LED C (PC0)
|
||||
|
|
@ -95,43 +92,7 @@ void APM_Init() {
|
|||
delay(30000);
|
||||
}
|
||||
|
||||
Read_adc_raw();
|
||||
delay(10);
|
||||
|
||||
// Offset values for accels and gyros...
|
||||
AN_OFFSET[3] = acc_offset_x;
|
||||
AN_OFFSET[4] = acc_offset_y;
|
||||
AN_OFFSET[5] = acc_offset_z;
|
||||
aux_float[0] = gyro_offset_roll;
|
||||
aux_float[1] = gyro_offset_pitch;
|
||||
aux_float[2] = gyro_offset_yaw;
|
||||
|
||||
j = 0;
|
||||
// Take the gyro offset values
|
||||
for(i=0;i<300;i++)
|
||||
{
|
||||
Read_adc_raw();
|
||||
for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset.
|
||||
{
|
||||
aux_float[y]=aux_float[y]*0.8 + AN[y]*0.2;
|
||||
//Serial.print(AN[y]);
|
||||
//Serial.print(",");
|
||||
}
|
||||
//Serial.println();
|
||||
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
|
||||
delay(10);
|
||||
|
||||
RunningLights(j);
|
||||
// Runnings lights effect to let user know that we are taking mesurements
|
||||
if((i % 5) == 0) j++;
|
||||
if(j >= 3) j = 0;
|
||||
}
|
||||
|
||||
// Switch off all ABC lights
|
||||
LightsOff();
|
||||
|
||||
for(int y=0; y<=2; y++)
|
||||
AN_OFFSET[y]=aux_float[y];
|
||||
calibrateSensors(); // Calibrate neutral values of gyros (in Sensors.pde)
|
||||
|
||||
// Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value
|
||||
#ifndef CONFIGURATOR
|
||||
|
|
|
|||
Loading…
Reference in New Issue