2010-08-28 13:07:33 -03:00
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
2010-10-11 09:14:04 -03:00
File : ArducopterNG.pde
Version : v1.0, 11 October 2010
2010-08-28 13:07:33 -03:00
Author(s): ArduCopter Team
2010-10-17 14:34:17 -03:00
Ted Carancho (AeroQuad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
2010-08-28 13:07:33 -03:00
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/>.
2010-10-17 14:34:17 -03:00
2010-08-28 13:07:33 -03:00
/* ********************************************************************** */
/* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */
/* Mounting position : RC connectors pointing backwards */
/* This code use this libraries : */
/* APM_RC : Radio library (with InstantPWM) */
2010-11-27 00:56:31 -04:00
/* AP_ADC : External ADC library */
2010-08-28 13:07:33 -03:00
/* DataFlash : DataFlash log library */
/* APM_BMP085 : BMP085 barometer library */
2010-12-12 10:05:31 -04:00
/* AP_Compass : HMC5843 compass library [optional] */
2010-10-11 09:14:04 -03:00
/* GPS_MTK or GPS_UBLOX or GPS_NMEA : GPS library [optional] */
2010-08-28 13:07:33 -03:00
/* ********************************************************************** */
/* ************************************************************ */
/* **************** MAIN PROGRAM - MODULES ******************** */
/* ************************************************************ */
2010-10-19 12:22:58 -03:00
/* ************************************************************ */
2010-11-03 09:39:11 -03:00
// 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
//
2010-10-19 12:22:58 -03:00
// Comment out with // modules that you are not using
2010-11-03 09:39:11 -03:00
//
// Do check ArduUser.h settings file too !!
//
///////////////////////////////////////
// Modules Config
// --------------------------
2010-08-28 13:07:33 -03:00
2010-11-09 17:38:54 -04:00
#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
2010-10-19 12:22:58 -03:00
//#define IsAM // Do we have motormount LED's. AM = Atraction Mode
2010-10-30 11:38:52 -03:00
//#define IsCAM // Do we have camera stabilization in use, If you activate, check OUTPUT pins from ArduUser.h
2010-10-17 14:34:17 -03:00
2010-10-19 12:22:58 -03:00
//#define UseAirspeed // Quads don't use AirSpeed... Legacy, jp 19-10-10
2011-01-03 00:37:23 -04:00
#define UseBMP // Use pressure sensor
2010-10-17 14:34:17 -03:00
//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it _wired_ up!)
2011-01-03 00:37:23 -04:00
//#define IsSONAR // are we using a Sonar for altitude hold? use this or "UseBMP" not both!
//#define IsRANGEFINDER // are we using range finders for obstacle avoidance?
2010-08-28 13:07:33 -03:00
2010-10-11 17:43:25 -03:00
#define CONFIGURATOR
2010-11-03 09:39:11 -03:00
///////////////////////////////////////
// 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
2010-10-17 14:34:17 -03:00
2010-11-28 13:20:16 -04:00
////////////////////////////////////////
// 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
2010-10-17 14:34:17 -03:00
////////////////////
2010-10-19 12:22:58 -03:00
// Serial ports & speeds
2010-10-17 14:34:17 -03:00
2010-09-20 06:38:11 -03:00
// Serial data, do we have FTDI cable or Xbee on Telemetry port as our primary command link
2010-10-19 12:22:58 -03:00
// If we are using normal FTDI/USB port as our telemetry/configuration, keep next line disabled
2010-10-17 13:18:14 -03:00
//#define SerXbee
2010-10-17 14:34:17 -03:00
// Telemetry port speed, default is 115200
2010-10-19 12:22:58 -03:00
//#define SerBau 19200
//#define SerBau 38400
//#define SerBau 57600
2010-10-17 14:34:17 -03:00
#define SerBau 115200
2010-10-19 12:22:58 -03:00
// 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
2010-11-03 13:52:04 -03:00
/* ************************************************* */
// 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
2010-10-17 14:34:17 -03:00
2010-10-19 12:22:58 -03:00
/* ************************************************* */
// Flight & Electronics orientation
2010-08-28 13:07:33 -03:00
// Frame build condiguration
2010-10-19 13:07:51 -03:00
//#define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration
2010-08-28 13:07:33 -03:00
//#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms
2010-10-19 13:07:51 -03:00
// 19-10-10 by JP
2010-11-03 09:39:11 -03:00
2010-10-19 13:07:51 -03:00
// 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 = +
2010-10-17 14:34:17 -03:00
2010-10-19 12:22:58 -03:00
// Magneto orientation and corrections.
2010-11-03 09:39:11 -03:00
// If you don't have magneto activated, It is safe to ignore these
//#ifdef IsMAG
2010-12-12 10:05:31 -04:00
#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
2010-10-19 12:22:58 -03:00
// To get Magneto offsets, switch to CLI mode and run offset calibration. During calibration
2010-10-30 11:38:52 -03:00
// you need to roll/bank/tilt/yaw/shake etc your ArduCopter. Don't kick like Jani always does :)
2010-12-10 01:22:52 -04:00
//#define MAGOFFSET 0,0,0
2010-12-06 10:56:55 -04:00
//#define MAGOFFSET -27.50,23.00,81.00
2010-10-19 12:22:58 -03:00
2010-12-10 01:22:52 -04:00
// Obsolete, Magnetometer offset are moved to CLI
2010-10-19 12:22:58 -03:00
// 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/
2010-12-10 15:06:14 -04:00
#define DECLINATION 0.0
2010-10-19 12:22:58 -03:00
2010-12-06 10:56:55 -04:00
//#define DECLINATION 0.61
2010-10-19 12:22:58 -03:00
// 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
2010-11-03 09:39:11 -03:00
//#endif
2010-10-17 14:34:17 -03:00
2010-11-03 13:52:04 -03:00
2010-08-28 13:07:33 -03:00
/* ************************************************************ */
/* **************** MAIN PROGRAM - INCLUDES ******************* */
/* ************************************************************ */
2010-11-03 09:39:11 -03:00
//#include <AP_GPS.h>
2010-08-28 13:07:33 -03:00
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <math.h>
#include <APM_RC.h> // ArduPilot Mega RC Library
2010-11-27 00:56:31 -04:00
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
2010-08-28 13:07:33 -03:00
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
2010-12-12 10:05:31 -04:00
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
2010-08-28 13:07:33 -03:00
#include <Wire.h> // I2C Communication library
2010-09-20 06:38:11 -03:00
#include <EEPROM.h> // EEPROM
2010-12-22 09:38:54 -04:00
#include <AP_RangeFinder.h> // RangeFinders (Sonars, IR Sensors)
2010-11-03 09:39:11 -03:00
//#include <AP_GPS.h>
2010-09-20 06:38:11 -03:00
#include "Arducopter.h"
#include "ArduUser.h"
2010-08-28 13:07:33 -03:00
2010-11-03 09:39:11 -03:00
#ifdef IsGPS
2010-10-13 13:48:09 -03:00
// GPS library (Include only one library)
2010-11-03 09:39:11 -03:00
#include <GPS_MTK.h> // ArduPilot MTK GPS Library
//#include <GPS_IMU.h> // ArduPilot IMU/SIM GPS Library
//#include <GPS_UBLOX.h> // ArduPilot Ublox GPS Library
//#include <GPS_NMEA.h> // ArduPilot NMEA GPS library
#endif
2010-11-23 21:30:41 -04:00
#if AIRFRAME == HELI
#include "Heli.h"
#endif
2010-08-28 13:07:33 -03:00
/* Software version */
2010-12-10 01:22:52 -04:00
#define VER 1.54 // Current software version (only numeric values)
2010-08-28 13:07:33 -03:00
2010-11-22 09:53:13 -04:00
// Sensors - declare one global instance
2010-11-27 00:56:31 -04:00
AP_ADC_ADS7844 adc;
2010-11-22 09:53:13 -04:00
APM_BMP085_Class APM_BMP085;
2010-12-12 10:05:31 -04:00
AP_Compass_HMC5843 AP_Compass;
2011-01-03 00:37:23 -04:00
#ifdef IsSONAR
AP_RangeFinder_MaxsonarXL AP_RangeFinder_down; // Default sonar for altitude hold
//AP_RangeFinder_MaxsonarLV AP_RangeFinder_down; // Alternative sonar is AP_RangeFinder_MaxsonarLV
#endif
#ifdef IsRANGEFINDER
AP_RangeFinder_MaxsonarLV AP_RangeFinder_frontRight;
AP_RangeFinder_MaxsonarLV AP_RangeFinder_backRight;
AP_RangeFinder_MaxsonarLV AP_RangeFinder_backLeft;
AP_RangeFinder_MaxsonarLV AP_RangeFinder_frontLeft;
#endif
2010-10-17 14:34:17 -03:00
2010-08-28 13:07:33 -03:00
/* ************************************************************ */
/* ************* MAIN PROGRAM - DECLARATIONS ****************** */
/* ************************************************************ */
2010-10-11 18:54:42 -03:00
2010-09-20 06:38:11 -03:00
byte flightMode;
2010-11-23 21:30:41 -04:00
unsigned long currentTime; // current time in milliseconds
unsigned long currentTimeMicros = 0, previousTimeMicros = 0; // current and previous loop time in microseconds
2010-10-11 11:12:44 -03:00
unsigned long mainLoop = 0;
2010-10-13 13:48:09 -03:00
unsigned long mediumLoop = 0;
unsigned long slowLoop = 0;
2010-08-28 13:07:33 -03:00
/* ************************************************************ */
/* **************** MAIN PROGRAM - SETUP ********************** */
/* ************************************************************ */
void setup() {
2010-10-17 14:34:17 -03:00
2010-10-18 15:24:46 -03:00
APM_Init(); // APM Hardware initialization (in System.pde)
2010-10-17 14:34:17 -03:00
2010-10-18 15:24:46 -03:00
mainLoop = millis(); // Initialize timers
2010-10-13 13:48:09 -03:00
mediumLoop = mainLoop;
GPS_timer = mainLoop;
2010-09-20 06:38:11 -03:00
motorArmed = 0;
2010-11-10 17:38:56 -04:00
GEOG_CORRECTION_FACTOR = 0; // Geographic correction factor will be automatically calculated
2010-10-18 15:24:46 -03:00
Read_adc_raw(); // Initialize ADC readings...
2010-11-28 06:56:27 -04:00
#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
2010-10-18 15:24:46 -03:00
2010-10-13 13:48:09 -03:00
delay(10);
2010-09-20 06:38:11 -03:00
digitalWrite(LED_Green,HIGH); // Ready to go...
2010-08-28 13:07:33 -03:00
}
/* ************************************************************ */
/* ************** MAIN PROGRAM - MAIN LOOP ******************** */
/* ************************************************************ */
2010-09-20 06:38:11 -03:00
2010-11-27 00:56:31 -04:00
// Sensor reading loop is inside AP_ADC and runs at 400Hz (based on Timer2 interrupt)
2010-10-13 13:48:09 -03:00
// * fast rate loop => Main loop => 200Hz
2010-09-20 06:38:11 -03:00
// read sensors
2010-10-11 11:12:44 -03:00
// IMU : update attitude
2010-09-20 06:38:11 -03:00
// motor control
2010-10-13 13:48:09 -03:00
// Asyncronous task : read transmitter
// * medium rate loop (60Hz)
// Asyncronous task : read GPS
// * slow rate loop (10Hz)
2010-09-20 06:38:11 -03:00
// magnetometer
2010-10-13 13:48:09 -03:00
// barometer (20Hz)
2011-01-03 00:37:23 -04:00
// sonar (20Hz)
// obstacle avoidance (20Hz)
2010-09-20 06:38:11 -03:00
// external command/telemetry
2010-10-13 13:48:09 -03:00
// Battery monitor
2010-09-20 06:38:11 -03:00
2010-10-19 13:07:51 -03:00
/* ***************************************************** */
// Main loop
2010-10-11 11:12:44 -03:00
void loop()
{
2010-12-26 00:30:55 -04:00
2010-11-23 21:30:41 -04:00
currentTimeMicros = micros();
currentTime = currentTimeMicros / 1000;
2010-10-17 14:34:17 -03:00
2010-10-11 11:12:44 -03:00
// Main loop at 200Hz (IMU + control)
2010-11-09 18:03:09 -04:00
if ((currentTime-mainLoop) > 5) // about 200Hz (every 5ms)
2010-10-17 14:34:17 -03:00
{
2010-11-23 21:30:41 -04:00
G_Dt = (currentTimeMicros-previousTimeMicros) * 0.000001; // Microseconds!!!
2010-10-11 11:12:44 -03:00
mainLoop = currentTime;
2010-11-23 21:30:41 -04:00
previousTimeMicros = currentTimeMicros;
2010-10-11 11:12:44 -03:00
//IMU DCM Algorithm
Read_adc_raw(); // Read sensors raw data
Matrix_update();
2010-10-13 13:48:09 -03:00
Normalize();
Drift_correction();
2010-10-11 11:12:44 -03:00
Euler_angles();
// Read radio values (if new data is available)
2010-11-23 21:30:41 -04:00
if (APM_RC.GetState() == 1) { // New radio frame?
#if AIRFRAME == QUAD
2010-10-11 18:54:42 -03:00
read_radio();
2010-11-23 21:30:41 -04:00
#endif
#if AIRFRAME == HELI
heli_read_radio();
#endif
}
2010-10-11 11:12:44 -03:00
// Attitude control
2010-10-11 18:54:42 -03:00
if(flightMode == STABLE_MODE) { // STABLE Mode
2010-09-13 06:07:31 -03:00
gled_speed = 1200;
2010-11-23 21:30:41 -04:00
if (AP_mode == AP_NORMAL_MODE) { // Normal mode
#if AIRFRAME == QUAD
2010-10-11 11:12:44 -03:00
Attitude_control_v3(command_rx_roll,command_rx_pitch,command_rx_yaw);
2010-11-23 21:30:41 -04:00
#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
2010-12-22 09:38:54 -04:00
Attitude_control_v3(command_rx_roll+command_gps_roll+command_RF_roll,command_rx_pitch+command_gps_pitch+command_RF_pitch,command_rx_yaw);
2011-01-03 00:37:23 -04:00
#endif
2010-11-23 21:30:41 -04:00
#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
}
2010-10-17 14:34:17 -03:00
}
2010-10-13 13:48:09 -03:00
else { // ACRO Mode
2010-09-13 06:07:31 -03:00
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);
2010-10-17 14:34:17 -03:00
}
2010-08-28 13:07:33 -03:00
2010-10-11 18:54:42 -03:00
// Send output commands to motor ESCs...
2010-11-23 21:30:41 -04:00
#if AIRFRAME == QUAD // we update the heli swashplate at about 60hz
2010-10-11 11:12:44 -03:00
motor_output();
2010-11-23 21:30:41 -04:00
#endif
2010-10-17 14:34:17 -03:00
2010-10-30 11:38:52 -03:00
#ifdef IsCAM
2010-10-30 05:30:46 -03:00
// Do we have cameras stabilization connected and in use?
if(!SW_DIP2) camera_output();
2010-10-30 11:38:52 -03:00
#endif
2010-10-29 02:47:54 -03:00
2010-12-22 09:38:54 -04:00
// Autopilot mode functions - GPS Hold, Altitude Hold + object avoidance
2010-10-13 13:48:09 -03:00
if (AP_mode == AP_AUTOMATIC_MODE)
2010-10-17 14:34:17 -03:00
{
2010-11-09 18:03:09 -04:00
digitalWrite(LED_Yellow,HIGH); // Yellow LED ON : GPS Position Hold MODE
2011-01-03 00:37:23 -04:00
// Do GPS Position hold (lattitude & longitude)
2010-11-09 18:03:09 -04:00
if (target_position)
2010-10-17 14:34:17 -03:00
{
2010-11-03 09:39:11 -03:00
#ifdef IsGPS
2010-10-13 13:48:09 -03:00
if (GPS.NewData) // New GPS info?
2010-10-17 14:34:17 -03:00
{
2010-11-09 18:03:09 -04:00
if (GPS.Fix)
2011-01-03 00:37:23 -04:00
{
2010-11-09 18:03:09 -04:00
read_GPS_data(); // In Navigation.pde
2011-01-03 00:37:23 -04:00
//Position_control(target_lattitude,target_longitude); // Call GPS position hold routine
Position_control_v2(target_lattitude,target_longitude); // V2 of GPS Position holdCall GPS position hold routine
}else{
2010-11-09 18:03:09 -04:00
command_gps_roll=0;
command_gps_pitch=0;
2011-01-03 00:37:23 -04:00
}
2010-10-13 13:48:09 -03:00
}
2010-11-03 09:39:11 -03:00
#endif
2010-12-26 00:30:55 -04:00
} else { // First time we enter in GPS position hold we capture the target position as the actual position
2010-11-03 09:39:11 -03:00
#ifdef IsGPS
2010-10-13 13:48:09 -03:00
if (GPS.Fix){ // We need a GPS Fix to capture the actual position...
target_lattitude = GPS.Lattitude;
target_longitude = GPS.Longitude;
target_position=1;
2010-10-17 14:34:17 -03:00
}
2010-11-03 09:39:11 -03:00
#endif
2010-10-13 13:48:09 -03:00
command_gps_roll=0;
command_gps_pitch=0;
2010-11-21 08:56:25 -04:00
Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde)
2010-10-11 11:12:44 -03:00
}
2010-12-26 00:30:55 -04:00
2011-01-03 00:37:23 -04:00
// Switch on altitude control if we have a barometer or Sonar
#if defined(UseBMP) || defined(IsSONAR)
if( altitude_control_method == ALTITUDE_CONTROL_NONE )
{
// by default turn on altitude hold using barometer
#ifdef UseBMP
altitude_control_method = ALTITUDE_CONTROL_BARO;
target_baro_altitude = press_baro_altitude;
baro_altitude_I = 0; // don't carry over any I values from previous times user may have switched on altitude control
#endif
// use sonar if it's available
#ifdef IsSONAR
if( sonar_status == SONAR_STATUS_OK )
{
altitude_control_method = ALTITUDE_CONTROL_SONAR;
target_sonar_altitude = press_sonar_altitude;
}
sonar_altitude_I = 0; // don't carry over any I values from previous times user may have switched on altitude control
#endif
// capture current throttle to use as base for altitude control
initial_throttle = ch_throttle;
ch_throttle_altitude_hold = ch_throttle;
}
// Sonar Altitude Control
#ifdef IsSONAR
if( sonar_new_data ) // if new sonar data has arrived
2010-12-26 00:30:55 -04:00
{
2011-01-03 00:37:23 -04:00
// Allow switching between sonar and barometer
#if defined(UseBMP)
// if SONAR become invalid switch to barometer
if( altitude_control_method == ALTITUDE_CONTROL_SONAR && sonar_status == SONAR_STATUS_BAD )
{
// next target barometer altitude to current barometer altitude + user's desired change over last sonar altitude (i.e. keeps up the momentum)
altitude_control_method = ALTITUDE_CONTROL_BARO;
target_baro_altitude = press_baro_altitude + constrain((target_sonar_altitude - press_sonar_altitude),-50,50);
}
// if SONAR becomes valid switch to sonar control
if( altitude_control_method == ALTITUDE_CONTROL_BARO && sonar_status == SONAR_STATUS_OK )
{
altitude_control_method = ALTITUDE_CONTROL_SONAR;
if( target_sonar_altitude == 0 ) { // if target sonar altitude hasn't been intialised before..
target_sonar_altitude = press_sonar_altitude + constrain((target_baro_altitude - press_baro_altitude),-50,50); // maybe this should just use the user's last valid target sonar altitude
}
// ensure target altitude is reasonable
target_sonar_altitude = constrain(target_sonar_altitude,AP_RangeFinder_down.min_distance*3,sonar_threshold);
}
#endif // defined(UseBMP)
// main Sonar control
if( altitude_control_method == ALTITUDE_CONTROL_SONAR )
{
if( sonar_status == SONAR_STATUS_OK ) {
ch_throttle_altitude_hold = Altitude_control_Sonar(press_sonar_altitude,target_sonar_altitude); // calculate throttle to maintain altitude
} else {
// if sonar_altitude becomes invalid we return control to user temporarily
2010-12-26 00:30:55 -04:00
ch_throttle_altitude_hold = ch_throttle;
2011-01-03 00:37:23 -04:00
}
// modify the target altitude if user moves stick more than 100 up or down
if( abs(ch_throttle-initial_throttle)>100 )
{
target_sonar_altitude += (ch_throttle-initial_throttle)/25;
if( target_sonar_altitude < AP_RangeFinder_down.min_distance*3 )
target_sonar_altitude = AP_RangeFinder_down.min_distance*3;
#if !defined(UseBMP) // limit the upper altitude if no barometer used
if( target_sonar_altitude > sonar_threshold)
target_sonar_altitude = sonar_threshold;
#endif
}
2010-12-26 00:30:55 -04:00
}
2011-01-03 00:37:23 -04:00
sonar_new_data = 0; // record that we've consumed the sonar data
} // new sonar data
#endif // #ifdef IsSONAR
// Barometer Altitude control
#ifdef UseBMP
if( baro_new_data && altitude_control_method == ALTITUDE_CONTROL_BARO ) // New altitude data?
{
ch_throttle_altitude_hold = Altitude_control_baro(press_baro_altitude,target_baro_altitude); // calculate throttle to maintain altitude
baro_new_data=0; // record that we have consumed the new data
2010-12-26 00:30:55 -04:00
// modify the target altitude if user moves stick more than 100 up or down
2011-01-03 00:37:23 -04:00
if (abs(ch_throttle-initial_throttle)>100)
{
target_baro_altitude += (ch_throttle-initial_throttle)/25; // Change in stick position => altitude ascend/descend rate control
}
2010-12-26 00:30:55 -04:00
}
#endif
2011-01-03 00:37:23 -04:00
#endif // defined(UseBMP) || defined(IsSONAR)
2010-12-26 00:30:55 -04:00
2011-01-03 00:37:23 -04:00
// Object avoidance
#ifdef IsRANGEFINDER
if( RF_new_data )
{
2010-12-26 00:30:55 -04:00
Obstacle_avoidance(RF_SAFETY_ZONE); // main obstacle avoidance function
RF_new_data = 0; // record that we have consumed the rangefinder data
2010-12-22 09:38:54 -04:00
}
#endif
}else{
2010-11-09 18:03:09 -04:00
digitalWrite(LED_Yellow,LOW);
2010-10-13 13:48:09 -03:00
target_position=0;
2011-01-03 00:37:23 -04:00
if( altitude_control_method != ALTITUDE_CONTROL_NONE )
{
altitude_control_method = ALTITUDE_CONTROL_NONE; // turn off altitude control
}
} // if (AP_mode == AP_AUTOMATIC_MODE)
}
2010-10-17 14:34:17 -03:00
// Medium loop (about 60Hz)
if ((currentTime-mediumLoop)>=17){
mediumLoop = currentTime;
2010-11-03 09:39:11 -03:00
#ifdef IsGPS
2010-10-17 14:34:17 -03:00
GPS.Read(); // Read GPS data
2010-11-03 09:39:11 -03:00
#endif
2011-01-03 00:37:23 -04:00
2010-11-23 21:30:41 -04:00
#if AIRFRAME == HELI
// Send output commands to heli swashplate...
heli_moveSwashPlate();
#endif
2011-01-03 00:37:23 -04:00
#ifdef IsSONAR
// read altitude from Sonar at 60Hz but only process every 3rd value (=20hz)
read_Sonar();
#endif
2010-10-17 14:34:17 -03:00
// Each of the six cases executes at 10Hz
switch (medium_loopCounter){
case 0: // Magnetometer reading (10Hz)
medium_loopCounter++;
slowLoop++;
#ifdef IsMAG
if (MAGNETOMETER == 1) {
2010-12-12 10:05:31 -04:00
AP_Compass.read(); // Read magnetometer
AP_Compass.calculate(roll,pitch); // Calculate heading
2010-10-17 14:34:17 -03:00
}
#endif
break;
2010-12-22 09:38:54 -04:00
case 1: // Barometer + RangeFinder reading (2x10Hz = 20Hz)
2010-10-17 14:34:17 -03:00
medium_loopCounter++;
#ifdef UseBMP
if (APM_BMP085.Read()){
read_baro();
2011-01-03 00:37:23 -04:00
baro_new_data = 1;
2010-10-17 14:34:17 -03:00
}
2010-12-22 09:38:54 -04:00
#endif
2011-01-03 00:37:23 -04:00
#ifdef IsSONAR
sonar_new_data = 1; // process sonar values at 20Hz
#endif
2010-12-22 09:38:54 -04:00
#ifdef IsRANGEFINDER
read_RF_Sensors();
RF_new_data = 1;
2010-10-17 14:34:17 -03:00
#endif
break;
case 2: // Send serial telemetry (10Hz)
medium_loopCounter++;
#ifdef CONFIGURATOR
sendSerialTelemetry();
#endif
break;
case 3: // Read serial telemetry (10Hz)
2011-01-03 00:37:23 -04:00
medium_loopCounter++;
2010-10-17 14:34:17 -03:00
#ifdef CONFIGURATOR
readSerialCommand();
#endif
break;
2010-12-22 09:38:54 -04:00
case 4: // second Barometer + RangeFinder reading (2x10Hz = 20Hz)
2010-10-17 14:34:17 -03:00
medium_loopCounter++;
#ifdef UseBMP
if (APM_BMP085.Read()){
read_baro();
2011-01-03 00:37:23 -04:00
baro_new_data = 1;
2010-10-13 13:48:09 -03:00
}
2010-12-22 09:38:54 -04:00
#endif
2011-01-03 00:37:23 -04:00
#ifdef IsSONAR
sonar_new_data = 1; // process sonar values at 20Hz
#endif
2010-12-22 09:38:54 -04:00
#ifdef IsRANGEFINDER
read_RF_Sensors();
RF_new_data = 1;
2010-10-17 14:34:17 -03:00
#endif
break;
case 5: // Battery monitor (10Hz)
2011-01-03 00:37:23 -04:00
medium_loopCounter=0;
2010-10-17 14:34:17 -03:00
#if BATTERY_EVENT == 1
read_battery(); // Battery monitor
#endif
break;
2010-10-13 13:48:09 -03:00
}
2010-10-17 14:34:17 -03:00
}
// 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;
}
}
2010-08-28 13:07:33 -03:00
}
2010-10-17 14:34:17 -03:00
2010-12-22 09:38:54 -04:00