You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
218 lines
7.3 KiB
218 lines
7.3 KiB
/* |
|
www.ArduCopter.com - www.DIYDrones.com |
|
Copyright (c) 2010. All rights reserved. |
|
An Open Source Arduino based multicopter. |
|
|
|
File : System.pde |
|
Version : v1.0, Aug 27, 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 <http://www.gnu.org/licenses/>. |
|
|
|
* ************************************************************** * |
|
ChangeLog: |
|
|
|
|
|
* ************************************************************** * |
|
TODO: |
|
|
|
|
|
* ************************************************************** */ |
|
|
|
// General Initialization for all APM electronics |
|
void APM_Init() { |
|
|
|
// Setup proper PIN modes for our switched, LEDs, Relays etc on IMU Board |
|
pinMode(LED_Yellow,OUTPUT); // Yellow LED A (PC1) |
|
pinMode(LED_Red,OUTPUT); // Red LED B (PC2) |
|
pinMode(LED_Green,OUTPUT); // Green LED C (PC0) |
|
pinMode(RELAY,OUTPUT); // Relay output (PL2) |
|
pinMode(SW1,INPUT); // Switch SW1 (PG0) |
|
pinMode(SW2,INPUT); // Switch SW2 (PG1) |
|
|
|
// Because DDRE and DDRL Ports are not included to normal Arduino Libraries, we need to |
|
// initialize them with a special command |
|
APMPinMode(DDRE,7,INPUT); // DIP1, (PE7), Closest DIP to sliding SW2 switch |
|
APMPinMode(DDRE,6,INPUT); // DIP2, (PE6) |
|
APMPinMode(DDRL,6,INPUT); // DIP3, (PL6) |
|
APMPinMode(DDRL,7,INPUT); // DIP4, (PL7), Furthest DIP from sliding SW2 switch |
|
|
|
|
|
/* ********************************************************* */ |
|
///////////////////////////////////////////////////////// |
|
// Normal Initialization sequence starts from here. |
|
readUserConfig(); // Load user configurable items from EEPROM |
|
|
|
APM_RC.Init(); // APM Radio initialization |
|
|
|
#if AIRFRAME == QUAD |
|
// RC channels Initialization (Quad motors) |
|
APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped |
|
APM_RC.OutputCh(1,MIN_THROTTLE); |
|
APM_RC.OutputCh(2,MIN_THROTTLE); |
|
APM_RC.OutputCh(3,MIN_THROTTLE); |
|
#endif |
|
|
|
#if AIRFRAME == HELI |
|
// RC channels Initialization (heli servos) |
|
APM_RC.OutputCh(0,CHANN_CENTER); // mid position |
|
APM_RC.OutputCh(1,CHANN_CENTER); |
|
APM_RC.OutputCh(2,CHANN_CENTER); |
|
APM_RC.OutputCh(3,CHANN_CENTER); |
|
#endif |
|
// Make sure that Relay is switched off. |
|
digitalWrite(RELAY,LOW); |
|
|
|
// Wiggle LEDs while ESCs are rebooting |
|
FullBlink(50,20); |
|
|
|
adc.Init(); // APM ADC library initialization |
|
DataFlash.Init(); // DataFlash log initialization |
|
|
|
#ifdef IsGPS |
|
GPS.Init(); // GPS Initialization |
|
|
|
#ifdef IsNEWMTEK |
|
delay(250); |
|
|
|
// DIY Drones MTEK GPS needs binary sentences activated if you upgraded to latest firmware. |
|
// If your GPS shows solid blue but LED C (Red) does not go on, your GPS is on NMEA mode |
|
Serial1.print("$PMTK220,200*2C\r\n"); // 5Hz update rate |
|
delay(200); |
|
Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n"); |
|
|
|
#endif |
|
#endif |
|
|
|
// Read DIP Switches and other important values. DIP switches needs special functions to |
|
// read due they are not defined as normal pins like other GPIO's are. |
|
SW_DIP1 = APMPinRead(PINE, 7); |
|
SW_DIP2 = APMPinRead(PINE, 6); |
|
SW_DIP3 = APMPinRead(PINL, 6); |
|
SW_DIP4 = APMPinRead(PINL, 7); |
|
|
|
// Is CLI mode active or not, if it is fire it up and never return. |
|
if(!digitalRead(SW2)) { |
|
RunCLI(); |
|
// Btw.. We never return from this.... |
|
} |
|
|
|
|
|
flightOrientation = SW_DIP1; // DIP1 off = we are in + mode, DIP1 on = we are in x mode |
|
|
|
// readUserConfig moved to up to ensure min throttle is read from eeprom |
|
//readUserConfig(); // Load user configurable items from EEPROM |
|
|
|
// Safety measure for Channel mids |
|
if(roll_mid < 1400 || roll_mid > 1600) roll_mid = 1500; |
|
if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500; |
|
if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500; |
|
|
|
#if AIRFRAME == QUAD |
|
// RC channels Initialization (Quad motors) |
|
APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped |
|
APM_RC.OutputCh(1,MIN_THROTTLE); |
|
APM_RC.OutputCh(2,MIN_THROTTLE); |
|
APM_RC.OutputCh(3,MIN_THROTTLE); |
|
#endif |
|
|
|
#if AIRFRAME == HELI |
|
// RC channels Initialization (heli servos) |
|
APM_RC.OutputCh(0,CHANN_CENTER); // mid position |
|
APM_RC.OutputCh(1,CHANN_CENTER); |
|
APM_RC.OutputCh(2,CHANN_CENTER); |
|
APM_RC.OutputCh(3,CHANN_CENTER); |
|
#endif |
|
|
|
// Initialise Wire library used by Magnetometer and Barometer |
|
Wire.begin(); |
|
|
|
#ifdef IsMAG |
|
if (MAGNETOMETER == 1) { |
|
AP_Compass.init(FALSE); // I2C initialization |
|
AP_Compass.set_orientation(MAGORIENTATION); |
|
AP_Compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); |
|
AP_Compass.set_declination(ToRad(DECLINATION)); |
|
} |
|
#endif |
|
|
|
DataFlash.StartWrite(1); // Start a write session on page 1 |
|
|
|
// Proper Serial port/baud are defined on main .pde and then Arducopter.h with |
|
// Choises of Xbee or normal serial port |
|
SerBeg(SerBau); |
|
|
|
// Check if we enable the DataFlash log Read Mode (switch) |
|
// If we press switch 1 at startup we read the Dataflash eeprom |
|
while (digitalRead(SW1)==0) // LEGACY remove soon by jp, 30-10-10 |
|
{ |
|
Serial.println("Entering Log Read Mode..."); // This will be obsole soon due moving to CLI system |
|
Log_Read(1,1000); |
|
delay(30000); |
|
} |
|
|
|
calibrateSensors(); // Calibrate neutral values of gyros (in Sensors.pde) |
|
|
|
// Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value |
|
#ifndef CONFIGURATOR |
|
for(i=0;i<6;i++) |
|
{ |
|
SerPri("AN[]:"); |
|
SerPrln(AN_OFFSET[i]); |
|
} |
|
SerPri("Yaw neutral value:"); |
|
SerPri(yaw_mid); |
|
#endif |
|
|
|
#ifdef UseBMP |
|
APM_BMP085.Init(FALSE); |
|
#endif |
|
|
|
// Sonar for Altitude hold |
|
#ifdef IsSONAR |
|
AP_RangeFinder_down.init(AP_RANGEFINDER_PITOT_TUBE, &adc); AP_RangeFinder_down.set_orientation(AP_RANGEFINDER_ORIENTATION_DOWN); |
|
//AP_RangeFinder_down.init(AN5); AP_RangeFinder_down.set_orientation(AP_RANGEFINDER_ORIENTATION_DOWN); |
|
sonar_threshold = AP_RangeFinder_down.max_distance * 0.8; |
|
sonar_status = SONAR_STATUS_OK; // assume sonar is ok to start with |
|
#endif |
|
|
|
// RangeFinders for obstacle avoidance |
|
#ifdef IsRANGEFINDER |
|
AP_RangeFinder_frontRight.init(AN5); AP_RangeFinder_frontRight.set_orientation(AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT); |
|
AP_RangeFinder_backRight.init(AN4); AP_RangeFinder_backRight.set_orientation(AP_RANGEFINDER_ORIENTATION_BACK_RIGHT); |
|
AP_RangeFinder_backLeft.init(AN3); AP_RangeFinder_backLeft.set_orientation(AP_RANGEFINDER_ORIENTATION_BACK_LEFT); |
|
AP_RangeFinder_frontLeft.init(AN2); AP_RangeFinder_frontLeft.set_orientation(AP_RANGEFINDER_ORIENTATION_FRONT_LEFT); |
|
#endif |
|
|
|
delay(1000); |
|
|
|
DataFlash.StartWrite(1); // Start a write session on page 1 |
|
|
|
// initialise helicopter |
|
#if AIRFRAME == HELI |
|
heli_setup(); |
|
#endif |
|
|
|
#ifdef IsAM |
|
// Switch Left & Right lights on |
|
digitalWrite(RI_LED, HIGH); |
|
digitalWrite(LE_LED, HIGH); |
|
#endif |
|
|
|
} |
|
|
|
|
|
|