/* www.ArduCopter.com - www.DIYDrones.com Copyright (c) 2010. All rights reserved. An Open Source Arduino based multicopter. File : ArducopterNG.pde Version : v1.0, 11 October 2010 Author(s): ArduCopter Team Ted Carancho (AeroQuad), Jose Julio, Jordi Muñoz, Jani Hirvinen, Ken McEwans, Roberto Navoni, Sandro Benigno, Chris Anderson 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 . /* ********************************************************************** */ /* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */ /* Mounting position : RC connectors pointing backwards */ /* This code use this libraries : */ /* APM_RC : Radio library (with InstantPWM) */ /* AP_ADC : External ADC library */ /* DataFlash : DataFlash log library */ /* APM_BMP085 : BMP085 barometer library */ /* AP_Compass : HMC5843 compass library [optional] */ /* GPS_MTK or GPS_UBLOX or GPS_NMEA : GPS library [optional] */ /* ********************************************************************** */ /* ************************************************************ */ /* **************** MAIN PROGRAM - MODULES ******************** */ /* ************************************************************ */ /* ************************************************************ */ // User MODULES // // Please check your modules settings for every new software downloads you have. // Also check repository / ArduCopter wiki pages for ChangeLogs and software notes // // Comment out with // modules that you are not using // // Do check ArduUser.h settings file too !! // /////////////////////////////////////// // Modules Config // -------------------------- #define IsGPS // Do we have a GPS connected #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 // 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 /////////////////////////////////////// // GPS Selection #define GPSDEVICE GPSDEV_DIYMTEK // For DIY Drones MediaTek //#define GPSDEVICE GPSDEV_DIYUBLOX // For DIY Drones uBlox GPS //#define GPSDEVICE GPSDEV_FPUBLOX // For Fah Pah Special ArduCopter GPS //#define GPSDEVICE GPSDEV_NMEA // For general NMEA compatible GPSEs //#dedine GPSDEVICE GPSDEV_IMU // For IMU Simulations only //////////////////////////////////////// // Frame / Motor / ESC definitions // Introducing new frame / Motor / ESC definitions for future expansion. Currently these are not in // use but they need to be here so implementation work can continue. // New frame model definitions. (not in use yet, 28-11-10 jp) #define FRAME_MODEL QUAD // Quad frame model //#define FRAME_MODEL HEXA // Quad frame model //#define FRAME_MODEL OCTO // Quad frame model // New motor definition for different frame type (not in use yet, 28-11-10 jp) #define MAX_MOTORS 4 // Are we using more motors than 4, possible choises are 4, 6, 8 // This has to be on main .pde to get included on all other header etc files // Not in use yet, 28-11-10 jp #define MOTORTYPE PWM // Traditional PWM ESC's controlling motors //#define MOTORTYPE I2C // I2C style ESC's controlling motors //#define MOTORTYPE UART // UART style ESC's controlling motors //////////////////// // Serial ports & speeds // Serial data, do we have FTDI cable or Xbee on Telemetry port as our primary command link // If we are using normal FTDI/USB port as our telemetry/configuration, keep next line disabled //#define SerXbee // Telemetry port speed, default is 115200 //#define SerBau 19200 //#define SerBau 38400 //#define SerBau 57600 #define SerBau 115200 // For future use, for now don't activate any! // Serial1 speed for GPS, mostly 38.4k, done from libraries //#define GpsBau 19200 //#define GpsBau 38400 //#define GpsBau 57600 //#define GpsBau 115200 /* ************************************************* */ // Radio modes #define RADIOMODE MODE2 // Most users have this eg: left stick: Throttle/Rudder, right stick: Elevator/Aileron //#define RADIOMODE MODE1 // Only if you are sure that you have Mode 1 radio. // NOTE! MODE1 is not working yet, we need to have input from users to be sure of channel orders. 03-11-10, jp /* ************************************************* */ // Flight & Electronics orientation // Frame build condiguration //#define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration //#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms // 19-10-10 by JP // This feature has been disabled for now, if you want to change between flight orientations // just use DIP switch for that. DIP1 down = X, DIP1 up = + // Magneto orientation and corrections. // If you don't have magneto activated, It is safe to ignore these //#ifdef IsMAG #define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_FORWARD // This is default solution for ArduCopter //#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK // Alternative orientation for ArduCopter //#define MAGORIENTATION AP_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 ArduCopter. Don't kick like Jani always does :) //#define MAGOFFSET 0,0,0 //#define MAGOFFSET -27.50,23.00,81.00 // Obsolete, Magnetometer offset are moved to CLI // Declination is a correction factor between North Pole and real magnetic North. This is different on every location // IF you want to use really accurate headholding and future navigation features, you should update this // You can check Declination to your location from http://www.magnetic-declination.com/ #define DECLINATION 0.0 //#define DECLINATION 0.61 // And remember result from NOAA website is in form of DEGREES°MINUTES'. Degrees you can use directly but Minutes you need to // recalculate due they one degree is 60 minutes.. For example Jani's real declination is 0.61, correct way to calculate this is // 37 / 60 = 0.61 and for Helsinki it would be 7°44' eg 7. and then 44/60 = 0.73 so declination for Helsinki/South Finland would be 7.73 // East values are positive // West values are negative // Some of devel team's Declinations and their Cities //#define DECLINATION 0.61 // Jani, Bangkok, 0°37' E (Due I live almost at Equator, my Declination is rather small) //#define DECLINATION 7.73 // Jani, Helsinki,7°44' E (My "summer" home back at Finland) //#define DECLINATION -20.68 // Sandro, Belo Horizonte, 22°08' W (Whoah... Sandro is really DECLINED) //#define DECLINATION 7.03 // Randy, Tokyo, 7°02'E //#define DECLINATION 8.91 // Doug, Denver, 8°55'E //#define DECLINATION -6.08 // Jose, Canary Islands, 6°5'W //#define DECLINATION 0.73 // Tony, Minneapolis, 0°44'E //#endif /* ************************************************************ */ /* **************** MAIN PROGRAM - INCLUDES ******************* */ /* ************************************************************ */ //#include #include #include #include #include #include // ArduPilot Mega RC Library #include // ArduPilot Mega Analog to Digital Converter Library #include // ArduPilot Mega BMP085 Library #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Magnetometer Library #include // I2C Communication library #include // EEPROM //#include #include "Arducopter.h" #include "ArduUser.h" #ifdef IsGPS // GPS library (Include only one library) #include // ArduPilot MTK GPS Library //#include // ArduPilot IMU/SIM GPS Library //#include // ArduPilot Ublox GPS Library //#include // ArduPilot NMEA GPS library #endif #if AIRFRAME == HELI #include "Heli.h" #endif /* Software version */ #define VER 1.54 // Current software version (only numeric values) // Sensors - declare one global instance AP_ADC_ADS7844 adc; APM_BMP085_Class APM_BMP085; AP_Compass_HMC5843 AP_Compass; /* ************************************************************ */ /* ************* MAIN PROGRAM - DECLARATIONS ****************** */ /* ************************************************************ */ byte flightMode; unsigned long currentTime; // current time in milliseconds unsigned long currentTimeMicros = 0, previousTimeMicros = 0; // current and previous loop time in microseconds unsigned long mainLoop = 0; unsigned long mediumLoop = 0; unsigned long slowLoop = 0; /* ************************************************************ */ /* **************** MAIN PROGRAM - SETUP ********************** */ /* ************************************************************ */ void setup() { APM_Init(); // APM Hardware initialization (in System.pde) mainLoop = millis(); // Initialize timers mediumLoop = mainLoop; GPS_timer = mainLoop; motorArmed = 0; GEOG_CORRECTION_FACTOR = 0; // Geographic correction factor will be automatically calculated Read_adc_raw(); // Initialize ADC readings... #ifdef SerXbee Serial.begin(SerBau); Serial.print("ArduCopter v"); Serial.println(VER); Serial.println("Serial data on Telemetry port"); Serial.println("No commands or output on this serial, check your Arducopter.pde if needed to change."); Serial.println(); Serial.println("General info:"); if(!SW_DIP1) Serial.println("Flight mode: + "); if(SW_DIP1) Serial.println("Flight mode: x "); #endif delay(10); digitalWrite(LED_Green,HIGH); // Ready to go... } /* ************************************************************ */ /* ************** MAIN PROGRAM - MAIN LOOP ******************** */ /* ************************************************************ */ // Sensor reading loop is inside AP_ADC and runs at 400Hz (based on Timer2 interrupt) // * fast rate loop => Main loop => 200Hz // read sensors // IMU : update attitude // motor control // Asyncronous task : read transmitter // * medium rate loop (60Hz) // Asyncronous task : read GPS // * slow rate loop (10Hz) // magnetometer // barometer (20Hz) // external command/telemetry // Battery monitor /* ***************************************************** */ // Main loop void loop() { //int aux; //int i; //float aux_float; currentTimeMicros = micros(); currentTime = currentTimeMicros / 1000; // Main loop at 200Hz (IMU + control) if ((currentTime-mainLoop) > 5) // about 200Hz (every 5ms) { //G_Dt = (currentTime-mainLoop)*0.001; // Microseconds!!! G_Dt = (currentTimeMicros-previousTimeMicros) * 0.000001; // Microseconds!!! mainLoop = currentTime; previousTimeMicros = currentTimeMicros; //IMU DCM Algorithm Read_adc_raw(); // Read sensors raw data Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); // Read radio values (if new data is available) if (APM_RC.GetState() == 1) { // New radio frame? #if AIRFRAME == QUAD read_radio(); #endif #if AIRFRAME == HELI heli_read_radio(); #endif } // Attitude control if(flightMode == STABLE_MODE) { // STABLE Mode gled_speed = 1200; if (AP_mode == AP_NORMAL_MODE) { // Normal mode #if AIRFRAME == QUAD Attitude_control_v3(command_rx_roll,command_rx_pitch,command_rx_yaw); #endif #if AIRFRAME == HELI heli_attitude_control(command_rx_roll,command_rx_pitch,command_rx_collective,command_rx_yaw); #endif }else{ // Automatic mode : GPS position hold mode #if AIRFRAME == QUAD Attitude_control_v3(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_yaw); #endif #if AIRFRAME == HELI heli_attitude_control(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_collective,command_rx_yaw); #endif } } else { // ACRO Mode gled_speed = 400; Rate_control_v2(); // Reset yaw, so if we change to stable mode we continue with the actual yaw direction command_rx_yaw = ToDeg(yaw); } // Send output commands to motor ESCs... #if AIRFRAME == QUAD // we update the heli swashplate at about 60hz motor_output(); #endif #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) { digitalWrite(LED_Yellow,HIGH); // Yellow LED ON : GPS Position Hold MODE if (target_position) { #ifdef IsGPS if (GPS.NewData) // New GPS info? { if (GPS.Fix) { read_GPS_data(); // In Navigation.pde Position_control(target_lattitude,target_longitude); // Call GPS position hold routine } else { command_gps_roll=0; command_gps_pitch=0; } } #endif #ifdef UseBMP if (Baro_new_data) // New altitude data? { ch_throttle_altitude_hold = Altitude_control_baro(press_alt,target_baro_altitude); // Altitude control Baro_new_data=0; if (abs(ch_throttle-Initial_Throttle)>100) // Change in stick position => altitude ascend/descend rate control target_baro_altitude += (ch_throttle-Initial_Throttle)/25; //Serial.print(Initial_Throttle); //Serial.print(" "); //Serial.print(ch_throttle); //Serial.print(" "); //Serial.println(target_baro_altitude); } #endif } else // First time we enter in GPS position hold we capture the target position as the actual position { #ifdef IsGPS if (GPS.Fix){ // We need a GPS Fix to capture the actual position... target_lattitude = GPS.Lattitude; target_longitude = GPS.Longitude; target_position=1; } #endif command_gps_roll=0; command_gps_pitch=0; target_baro_altitude = press_alt; Initial_Throttle = ch_throttle; ch_throttle_altitude_hold = ch_throttle; Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde) } } else { digitalWrite(LED_Yellow,LOW); target_position=0; } } // Medium loop (about 60Hz) if ((currentTime-mediumLoop)>=17){ mediumLoop = currentTime; #ifdef IsGPS GPS.Read(); // Read GPS data #endif #if AIRFRAME == HELI // Send output commands to heli swashplate... heli_moveSwashPlate(); #endif // Each of the six cases executes at 10Hz switch (medium_loopCounter){ case 0: // Magnetometer reading (10Hz) medium_loopCounter++; slowLoop++; #ifdef IsMAG if (MAGNETOMETER == 1) { AP_Compass.read(); // Read magnetometer AP_Compass.calculate(roll,pitch); // Calculate heading } #endif break; case 1: // Barometer reading (2x10Hz = 20Hz) medium_loopCounter++; #ifdef UseBMP if (APM_BMP085.Read()){ read_baro(); Baro_new_data = 1; } #endif break; case 2: // Send serial telemetry (10Hz) medium_loopCounter++; #ifdef CONFIGURATOR sendSerialTelemetry(); #endif break; case 3: // Read serial telemetry (10Hz) medium_loopCounter++; #ifdef CONFIGURATOR readSerialCommand(); #endif break; case 4: // second Barometer reading (2x10Hz = 20Hz) medium_loopCounter++; #ifdef UseBMP if (APM_BMP085.Read()){ read_baro(); Baro_new_data = 1; //Serial.print("B "); //Serial.println(press_alt); } #endif break; case 5: // Battery monitor (10Hz) medium_loopCounter=0; #if BATTERY_EVENT == 1 read_battery(); // Battery monitor #endif break; } } // AM and Mode status LED lights if(millis() - gled_timer > gled_speed) { gled_timer = millis(); if(gled_status == HIGH) { digitalWrite(LED_Green, LOW); #ifdef IsAM digitalWrite(RE_LED, LOW); #endif gled_status = LOW; // SerPrln("L"); } else { digitalWrite(LED_Green, HIGH); #ifdef IsAM if(motorArmed) digitalWrite(RE_LED, HIGH); #endif gled_status = HIGH; } } }