1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
|
/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
* Copyright 2012, Ted Carancho
*
* Ported from AeroQuad
*
* 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/>.
*/
public class FlightControl {
FlightMode mode;
PID[] pid;
int throttle;
const double ATTITUDE_SCALING = 1.0;
public FlightControl() throws Error {
pid = new PID[PIDEntry.length];
for(int i=0; i<PIDEntry.length; i++)
pid[i] = new PID();
}
/**
* calculateFlightError
*
* Calculate roll/pitch axis error with gyro/accel data to
* compute motor command thrust so used command are executed
*/
void calculateFlightError(out int roll, out int pitch) throws Error {
roll = 0; pitch = 0;
if (mode == FlightMode.ATTITUDE_FLIGHT_MODE) {
double rollAttitudeCmd = pid[PIDEntry.ATTITUDE_XAXIS].update(receiver.get_value(AXIS.X) * ATTITUDE_SCALING, kinematics.kinematicsAngle[AXIS.X]);
double pitchAttitudeCmd = pid[PIDEntry.ATTITUDE_YAXIS].update(receiver.get_value(AXIS.Y) * ATTITUDE_SCALING, kinematics.kinematicsAngle[AXIS.Y]);
roll = (int) pid[PIDEntry.ATTITUDE_GYRO_XAXIS].update(rollAttitudeCmd, gyroscope.rate[AXIS.X]*1.2);
pitch = (int) pid[PIDEntry.ATTITUDE_GYRO_YAXIS].update(pitchAttitudeCmd, -gyroscope.rate[AXIS.Y]*1.2);
} else {
roll = (int) pid[PIDEntry.RATE_XAXIS].update(receiver.get_value(AXIS.X), gyroscope.rate[AXIS.X]*0.8);
pitch = (int) pid[PIDEntry.RATE_YAXIS].update(receiver.get_value(AXIS.Y), -gyroscope.rate[AXIS.Y]*0.8);
}
}
void processThrottleCorrection() {
int throttleAdjust = (int) (throttle / (Math.cos(radians(kinematics.kinematicsAngle[AXIS.X])) * Math.cos(radians(kinematics.kinematicsAngle[AXIS.Y]))));
throttleAdjust = (int) constrain ((throttleAdjust - throttle), 0, 160); //compensate max +/- 25 deg XAXIS or YAXIS or +/- 18 ( 18(XAXIS) + 18(YAXIS))
throttle = throttle + throttleAdjust; // + (int)batteyMonitorThrottleCorrection;
/* limit throttle to leave some space for motor correction in max throttle manuever */
throttle = constrain_int(throttle,motorCtrl.min, motorCtrl.max - 10);
}
private void processHeadingHold() {
// TODO
}
private void processAltitudeHold() {
// TODO
}
/**
* processFlightControl
*
* Main flight control processos function
*/
public void process() throws Error {
int throttle = 0, yaw = 0, roll, pitch;
int[] motor_data;
calculateFlightError(out roll, out pitch);
processHeadingHold();
/* TODO: 50Hz tasks */
processAltitudeHold();
processThrottleCorrection();
/* TODO: if running */
motor_data = model.get_motor_data(throttle, yaw, roll, pitch);
//TODO: processMinMaxCommand();
#if 0
/* If throttle in minimum position, don't apply yaw */
if (receiver.get_value(THROTTLE) < MINCHECK) {
for (uint8 motor = 0; motor < LASTMOTOR; motor++) {
motorMaxCommand[motor] = minArmedThrottle;
}
}
/* Apply limits to motor commands */
for (uint8 motor = 0; motor < LASTMOTOR; motor++) {
motor_data[motor] = constraint_int(motor_data[motor], motorMinCommand[motor], motorMaxCommand[motor]);
}
/* ESC Calibration */
if (motorArmed == OFF) {
processCalibrateESC();
}
#endif
//if (motorArmed == ON && safetyCheck == ON) {
for(uint8 i=0; i<motor_data.length; i++)
motorCtrl.set_single(i, (uint8) motor_data[i]);
//}
}
}
|
