diff --git a/ArducopterNG/ArduUser.h b/ArducopterNG/ArduUser.h index 7f06d082bf..eccebc288d 100644 --- a/ArducopterNG/ArduUser.h +++ b/ArducopterNG/ArduUser.h @@ -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; diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h index 284a6b0019..095e100b8d 100644 --- a/ArducopterNG/Arducopter.h +++ b/ArducopterNG/Arducopter.h @@ -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 @@ -388,4 +325,64 @@ int mainLoop_count = 0; 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 + diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index 626dbfdce8..657549f057 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -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; } } } diff --git a/ArducopterNG/Radio.pde b/ArducopterNG/Radio.pde index 58910f481c..7f00abd192 100644 --- a/ArducopterNG/Radio.pde +++ b/ArducopterNG/Radio.pde @@ -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 } diff --git a/ArducopterNG/Sensors.pde b/ArducopterNG/Sensors.pde index 16242a0996..5a824ccf47 100644 --- a/ArducopterNG/Sensors.pde +++ b/ArducopterNG/Sensors.pde @@ -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 } diff --git a/ArducopterNG/System.pde b/ArducopterNG/System.pde index a3978687ca..bf4a0c41e6 100644 --- a/ArducopterNG/System.pde +++ b/ArducopterNG/System.pde @@ -35,9 +35,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) @@ -94,44 +91,8 @@ void APM_Init() { Log_Read(1,1000); 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