Skip to content

Commit 6577b44

Browse files
AeroQuadAeroQuad
authored andcommitted
experimenting with auto cal for zero stick input, added template examples
1 parent 5ac1648 commit 6577b44

File tree

8 files changed

+135
-15
lines changed

8 files changed

+135
-15
lines changed

AeroQuad.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -214,7 +214,7 @@ unsigned long receiverTime = 0;
214214
unsigned long telemetryTime = 50; // make telemetry output 50ms offset from receiver check
215215
unsigned long sensorTime = 0;
216216
unsigned long controlLoopTime = 1; // offset control loop from analog input loop by 1ms
217-
unsigned long cameraTime = 0;
217+
unsigned long cameraTime = 10;
218218
unsigned long fastTelemetryTime = 0;
219219
unsigned long autoZeroGyroTime = 0;
220220

DataStorage.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -69,8 +69,8 @@ void initializeEEPROM(void) {
6969
PID[LEVELGYROPITCH].D = -15;
7070
windupGuard = 2000.0;
7171
receiver.setXmitFactor(0.60);
72-
levelLimit = 90.0;
73-
levelOff = 0.0;
72+
levelLimit = 500.0;
73+
levelOff = 50.0;
7474
gyro.setSmoothFactor(1.0);
7575
accel.setSmoothFactor(1.0);
7676
timeConstant = 7.0;

FlightCommand.pde

Lines changed: 22 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -55,22 +55,40 @@ void readPilotCommands() {
5555
#ifdef TELEMETRY_DEBUG
5656
Serial.println("ZeroG Gyro");
5757
#endif
58-
}
58+
}
5959
// Arm motors (left stick lower right corner)
6060
if (receiver.getRaw(YAW) > MAXCHECK && armed == 0 && safetyCheck == 1) {
61-
armed = 1;
6261
zeroIntegralError();
62+
armed = 1;
6363
for (motor=FRONT; motor < LASTMOTOR; motor++)
6464
motors.setMinCommand(motor, MINTHROTTLE);
6565
}
6666
// Prevents accidental arming of motor output if no transmitter command received
6767
if (receiver.getRaw(YAW) > MINCHECK) safetyCheck = 1;
6868
}
6969

70-
if (receiver.getRaw(MODE) > 1500)
70+
// Get center value of roll/pitch/yaw channels when enough throttle to lift off
71+
/*if (receiver.getRaw(THROTTLE) < 1300) {
72+
receiver.setZero(ROLL, receiver.getRaw(ROLL));
73+
receiver.setZero(PITCH, receiver.getRaw(PITCH));
74+
//receiver.setZero(YAW, receiver.getRaw(YAW));
75+
}*/
76+
77+
// Check Mode switch for Acro or Stable
78+
if (receiver.getRaw(MODE) > 1500) {
79+
#if defined(AeroQuad_v18) || defined(AeroQuadMega_v2)
80+
if (flightMode == ACRO)
81+
digitalWrite(LED2PIN, HIGH);
82+
#endif
7183
flightMode = STABLE;
72-
else
84+
}
85+
else {
86+
#if defined(AeroQuad_v18) || defined(AeroQuadMega_v2)
87+
if (flightMode == STABLE)
88+
digitalWrite(LED2PIN, LOW);
89+
#endif
7390
flightMode = ACRO;
91+
}
7492
}
7593

7694

FlightControl.pde

Lines changed: 16 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -30,15 +30,28 @@ void flightControl(void) {
3030
// updatePID() is defined in PID.h
3131
motors.setMotorAxisCommand(ROLL, updatePID(receiver.getData(ROLL), gyro.getFlightData(ROLL) + 1500, &PID[ROLL]));
3232
motors.setMotorAxisCommand(PITCH, updatePID(receiver.getData(PITCH), gyro.getFlightData(PITCH) + 1500, &PID[PITCH]));
33-
}
33+
zeroIntegralError();
34+
}
3435
if (flightMode == STABLE) {
3536
// Stable Mode
3637
//levelAdjust[ROLL] = updatePID(receiver.getAngle(ROLL), flightAngle.getData(ROLL), &PID[LEVELROLL]);
3738
//levelAdjust[PITCH] = updatePID(receiver.getAngle(PITCH), -flightAngle.getData(PITCH), &PID[LEVELPITCH]);
3839
levelAdjust[ROLL] = (receiver.getAngle(ROLL) - flightAngle.getData(ROLL)) * PID[LEVELROLL].P;
3940
levelAdjust[PITCH] = (receiver.getAngle(PITCH) + flightAngle.getData(PITCH)) * PID[LEVELPITCH].P;
40-
PID[LEVELROLL].integratedError += (receiver.getAngle(ROLL) - flightAngle.getData(ROLL)) * G_Dt; // next try removing dT
41-
PID[LEVELPITCH].integratedError += (receiver.getAngle(PITCH) + flightAngle.getData(PITCH)) * G_Dt;
41+
// Check if pilot commands are not in hover, don't auto trim
42+
if ((abs(receiver.getRaw(ROLL) - receiver.getZero(ROLL)) > levelOff) || (abs(receiver.getRaw(PITCH) - receiver.getZero(PITCH)) > levelOff)) {
43+
zeroIntegralError();
44+
#if defined(AeroQuad_v18) || defined(AeroQuadMega_v2)
45+
digitalWrite(LED2PIN, LOW);
46+
#endif
47+
}
48+
else {
49+
PID[LEVELROLL].integratedError = constrain(PID[LEVELROLL].integratedError + ((receiver.getAngle(ROLL) - flightAngle.getData(ROLL)) * G_Dt), -levelLimit, levelLimit);
50+
PID[LEVELPITCH].integratedError = constrain(PID[LEVELPITCH].integratedError + ((receiver.getAngle(PITCH) + flightAngle.getData(PITCH)) * G_Dt), -levelLimit, levelLimit);
51+
#if defined(AeroQuad_v18) || defined(AeroQuadMega_v2)
52+
digitalWrite(LED2PIN, HIGH);
53+
#endif
54+
}
4255
motors.setMotorAxisCommand(ROLL, updatePID(receiver.getData(ROLL) + levelAdjust[ROLL], gyro.getFlightData(ROLL) + 1500, &PID[LEVELGYROROLL]) + PID[LEVELROLL].integratedError);
4356
motors.setMotorAxisCommand(PITCH, updatePID(receiver.getData(PITCH) + levelAdjust[PITCH], gyro.getFlightData(PITCH) + 1500, &PID[LEVELGYROPITCH]) + PID[LEVELPITCH].integratedError);
4457
}

PID.h

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -43,4 +43,5 @@ float updatePID(float targetPosition, float currentPosition, struct PIDdata *PID
4343
void zeroIntegralError() {
4444
for (axis = ROLL; axis < LASTLEVELAXIS; axis++)
4545
PID[axis].integratedError = 0;
46-
}
46+
}
47+

Receiver.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -86,7 +86,7 @@ class Receiver {
8686
// reduce sensitivity of transmitter input by xmitFactor
8787
return transmitterCommand[channel];
8888
}
89-
89+
9090
const int getZero(byte channel) {
9191
return transmitterZero[channel];
9292
}

SerialCom.pde

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -178,14 +178,18 @@ void sendSerialTelemetry() {
178178
update = 0;
179179
switch (queryType) {
180180
case '=': // Reserved debug command to view any variable from Serial Monitor
181-
/*Serial.print(receiver.getAngle(ROLL));
182-
comma();
181+
Serial.print(receiver.getRaw(ROLL)); Serial.print(" - "); Serial.print(receiver.getZero(ROLL)); Serial.print(" = ");
182+
Serial.print(receiver.getRaw(ROLL) - receiver.getZero(ROLL));
183+
Serial.println();
184+
Serial.print(receiver.getRaw(PITCH)); Serial.print(" - "); Serial.print(receiver.getZero(PITCH)); Serial.print(" = ");
185+
Serial.print(receiver.getRaw(ROLL) - receiver.getZero(PITCH));
186+
Serial.println();
187+
/*comma();
183188
Serial.print(flightAngle.getData(ROLL));
184189
comma();
185190
Serial.print(flightAngle.getGyroAngle(ROLL));
186191
comma();
187192
Serial.print(receiver.getRaw(ROLL));
188-
Serial.println();
189193
//queryType = 'X';*/
190194
break;
191195
case 'B': // Send roll and pitch gyro PID values

Template.h

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
/*
2+
AeroQuad v2.0.1 - September 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+
// User this as a template for new classes or subclasses
22+
23+
// ***********************************************************************
24+
// ************************** Example Class ******************************
25+
// ***********************************************************************
26+
class exampleClass {
27+
public:
28+
int exampleVariable;
29+
float exampleData[3];
30+
exampleClass(void) {
31+
// this is the constructor of the object and must have the same name
32+
// can be used to initialize any of the variables declared above
33+
}
34+
35+
// **********************************************************************
36+
// The following function calls must be defined inside any new subclasses
37+
// **********************************************************************
38+
virtual void initialize(void);
39+
virtual void exampleFunction(int);
40+
virtual const int getExampleData(byte);
41+
42+
// *********************************************************
43+
// The following functions are common between all subclasses
44+
// *********************************************************
45+
void examplePublicFunction(byte axis, int value) {
46+
// insert common code here
47+
}
48+
const int getPublicData(byte axis) {
49+
return exampleData[axis];
50+
}
51+
};
52+
53+
// ***********************************************************************
54+
// ************************ Example Subclass *****************************
55+
// ***********************************************************************
56+
class exampleSubClass : public exampleClass {
57+
private:
58+
int exampleArray[3]; // only for use inside this subclass
59+
int examplePrivateData; // only for use inside this subclass
60+
void examplePrivateFunction(int functionVariable) {
61+
// it’s possible to declare functions just for this subclass
62+
}
63+
64+
public:
65+
exampleSubClass() : exampleClass(){
66+
// this is the constructor of the object and must have the same name
67+
// can be used to initialize any of the variables declared above
68+
}
69+
70+
// ***********************************************************
71+
// Define all the virtual functions declared in the main class
72+
// ***********************************************************
73+
void initialize(void) {
74+
// insert code here
75+
}
76+
void exampleFunction(int someVariable) {
77+
// insert code here
78+
examplePrivateFunction(someVariable);
79+
}
80+
const int getExampleData(byte axis) {
81+
// insert code here
82+
return exampleArray[axis];
83+
}
84+
};

0 commit comments

Comments
 (0)