Project

General

Profile

Statistics
| Branch: | Revision:

root / quad1 / AeroQuad / FlightCommand.pde @ 9240aaa3

History | View | Annotate | Download (4.59 KB)

1
/*
2
  AeroQuad v2.1 - October 2010
3
  www.AeroQuad.com
4
  Copyright (c) 2010 Ted Carancho.  All rights reserved.
5
  An Open Source Arduino based multicopter.
6
 
7
  This program is free software: you can redistribute it and/or modify 
8
  it under the terms of the GNU General Public License as published by 
9
  the Free Software Foundation, either version 3 of the License, or 
10
  (at your option) any later version. 
11

    
12
  This program is distributed in the hope that it will be useful, 
13
  but WITHOUT ANY WARRANTY; without even the implied warranty of 
14
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
15
  GNU General Public License for more details. 
16

    
17
  You should have received a copy of the GNU General Public License 
18
  along with this program. If not, see <http://www.gnu.org/licenses/>. 
19
*/
20

    
21
// FlightCommand.pde is responsible for decoding transmitter stick combinations
22
// for setting up AeroQuad modes such as motor arming and disarming
23

    
24
void readPilotCommands() {
25
  receiver.read();
26
  // Read quad configuration commands from transmitter when throttle down
27
  if (receiver.getRaw(THROTTLE) < MINCHECK) {
28
    zeroIntegralError();
29
    throttleAdjust = 0;
30
    //receiver.adjustThrottle(throttleAdjust);
31
    // Disarm motors (left stick lower left corner)
32
    if (receiver.getRaw(YAW) < MINCHECK && armed == ON) {
33
      armed = OFF;
34
      motors.commandAllMotors(MINCOMMAND);
35
    }    
36
    // Zero Gyro and Accel sensors (left stick lower left, right stick lower right corner)
37
    if ((receiver.getRaw(YAW) < MINCHECK) && (receiver.getRaw(ROLL) > MAXCHECK) && (receiver.getRaw(PITCH) < MINCHECK)) {
38
      gyro.calibrate(); // defined in Gyro.h
39
      accel.calibrate(); // defined in Accel.h
40
      zeroIntegralError();
41
      motors.pulseMotors(3);
42
      #ifdef ArduCopter
43
        zero_ArduCopter_ADC();
44
      #endif
45
    }   
46
    // Multipilot Zero Gyro sensors (left stick no throttle, right stick upper right corner)
47
    if ((receiver.getRaw(ROLL) > MAXCHECK) && (receiver.getRaw(PITCH) > MAXCHECK)) {
48
      accel.calibrate(); // defined in Accel.h
49
      zeroIntegralError();
50
      motors.pulseMotors(3);
51
      #ifdef ArduCopter
52
        zero_ArduCopter_ADC();
53
      #endif
54
    }   
55
    // Multipilot Zero Gyros (left stick no throttle, right stick upper left corner)
56
    if ((receiver.getRaw(ROLL) < MINCHECK) && (receiver.getRaw(PITCH) > MAXCHECK)) {
57
      gyro.calibrate();
58
      zeroIntegralError();
59
      motors.pulseMotors(4);
60
      #ifdef ArduCopter
61
        zero_ArduCopter_ADC();
62
      #endif
63
    }
64
    // Arm motors (left stick lower right corner)
65
    if (receiver.getRaw(YAW) > MAXCHECK && armed == OFF && safetyCheck == ON) {
66
      zeroIntegralError();
67
      armed = ON;
68
      for (motor=FRONT; motor < LASTMOTOR; motor++)
69
        motors.setMinCommand(motor, MINTHROTTLE);
70
      delay(100);
71
    }
72
    // Prevents accidental arming of motor output if no transmitter command received
73
    if (receiver.getRaw(YAW) > MINCHECK) safetyCheck = ON; 
74
  }
75
  
76
  // Get center value of roll/pitch/yaw channels when enough throttle to lift off
77
  if (receiver.getRaw(THROTTLE) < 1300) {
78
    receiver.setTransmitterTrim(ROLL, receiver.getRaw(ROLL));
79
    receiver.setTransmitterTrim(PITCH, receiver.getRaw(PITCH));
80
    receiver.setTransmitterTrim(YAW, receiver.getRaw(YAW));
81
  }
82
  
83
  // Check Mode switch for Acro or Stable
84
  if (receiver.getRaw(MODE) > 1500) {
85
    #if defined(AeroQuad_v18) || defined(AeroQuadMega_v2)
86
      if (flightMode == ACRO)
87
        digitalWrite(LED2PIN, HIGH);
88
    #endif
89
    flightMode = STABLE;
90
 }
91
  else {
92
    #if defined(AeroQuad_v18) || defined(AeroQuadMega_v2)
93
      if (flightMode == STABLE)
94
        digitalWrite(LED2PIN, LOW);
95
    #endif
96
    flightMode = ACRO;
97
  }
98
  
99
  #ifdef AltitudeHold
100
    // Check if altitude hold is enabled
101
    if (receiver.getRaw(AUX) < 1400) {
102
      // return to preset altitude or land?
103
    }
104
    else if (receiver.getRaw(AUX) < 1700) {
105
      if (storeAltitude == ON) {
106
        holdAltitude = altitude.getData();
107
        PID[ALTITUDE].integratedError = 0;
108
        accel.setOneG(accel.getFlightData(ZAXIS));
109
        storeAltitude = OFF;
110
      }
111
      altitudeHold = ON;
112
    }
113
    else {
114
      storeAltitude = ON;
115
      altitudeHold = OFF;
116
    }
117
  #endif
118
  
119
    #ifdef SonarHold
120
    // Check if sonar hold is enabled
121
    if (receiver.getRaw(AUX2) > 1500) {
122
      if (sonarStoreAltitude == ON) {
123
        sonarHoldAltitude = 50;//sonar.range;
124
        PID[SONAR].integratedError = 0;
125
        sonarStoreAltitude = OFF;
126
      }
127
      sonarHold = ON;
128
    }
129
    else {
130
      sonarStoreAltitude = ON;
131
      sonarHold = OFF;
132
    }
133
  #endif
134
  
135
  // Use for correcting gyro drift with v2.0 Shield
136
  gyro.setReceiverYaw(receiver.getData(YAW));
137
}
138

    
139