Subversion Repositories FlightCtrl

Rev

Rev 1634 | Rev 1775 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1634 Rev 1645
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) 04.2007 Holger Buss
2
// + Copyright (c) 04.2007 Holger Buss
3
// + Nur für den privaten Gebrauch
3
// + Nur für den privaten Gebrauch
4
// + www.MikroKopter.com
4
// + www.MikroKopter.com
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
6
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
7
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
7
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
8
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
8
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
9
// + bzgl. der Nutzungsbedingungen aufzunehmen.
9
// + bzgl. der Nutzungsbedingungen aufzunehmen.
10
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
10
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
11
// + Verkauf von Luftbildaufnahmen, usw.
11
// + Verkauf von Luftbildaufnahmen, usw.
12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
13
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
14
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
14
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
16
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
17
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
17
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18
// + eindeutig als Ursprung verlinkt werden
18
// + eindeutig als Ursprung verlinkt werden
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
20
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
21
// + Benutzung auf eigene Gefahr
21
// + Benutzung auf eigene Gefahr
22
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
22
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
24
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
25
// + mit unserer Zustimmung zulässig
25
// + mit unserer Zustimmung zulässig
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
27
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
29
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
30
// + this list of conditions and the following disclaimer.
30
// + this list of conditions and the following disclaimer.
31
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
31
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
32
// +     from this software without specific prior written permission.
32
// +     from this software without specific prior written permission.
33
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
33
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
34
// +     for non-commercial use (directly or indirectly)
34
// +     for non-commercial use (directly or indirectly)
35
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
35
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
36
// +     with our written permission
36
// +     with our written permission
37
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
37
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
38
// +     clearly linked as origin
38
// +     clearly linked as origin
39
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
39
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
40
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
40
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
41
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
41
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
42
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
43
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
44
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
45
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
46
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
47
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
47
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
48
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49
// +  POSSIBILITY OF SUCH DAMAGE.
49
// +  POSSIBILITY OF SUCH DAMAGE.
50
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
50
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51
 
51
 
52
#include <stdlib.h>
52
#include <stdlib.h>
53
#include "controlMixer.h"
53
#include "controlMixer.h"
54
#include "rc.h"
54
#include "rc.h"
55
#include "externalControl.h"
55
#include "externalControl.h"
56
#include "configuration.h"
56
#include "configuration.h"
57
#include "attitude.h"
57
#include "attitude.h"
58
#include "eeprom.h"
58
#include "eeprom.h"
59
#include "flight.h"
59
#include "flight.h"
60
 
60
 
61
/*
61
/*
62
 * Number of cycles a command must be repeated before commit. Maybe this really belongs in RC.
62
 * Number of cycles a command must be repeated before commit. Maybe this really belongs in RC.
63
 */
63
 */
64
#define COMMAND_TIMER 200
64
#define COMMAND_TIMER 200
65
 
65
 
66
uint16_t maxControlPitch = 0, maxControlRoll = 0;
66
uint16_t maxControl[2] = {0,0};
67
int16_t controlPitch = 0, controlRoll = 0, controlYaw = 0, controlThrottle = 0;
67
int16_t control[2] = {0,0}, controlYaw = 0, controlThrottle = 0;
68
uint8_t looping = 0;
68
uint8_t looping = 0;
69
 
69
 
70
// Internal variables for reading commands made with an R/C stick.
70
// Internal variables for reading commands made with an R/C stick.
71
uint8_t lastCommand  = COMMAND_NONE;
71
uint8_t lastCommand  = COMMAND_NONE;
72
uint8_t commandTimer = 0;
72
uint8_t commandTimer = 0;
73
 
73
 
74
/*
74
/*
75
 * This could be expanded to take arguments from ohter sources than the RC
75
 * This could be expanded to take arguments from ohter sources than the RC
76
 * (read: Custom MK RC project)
76
 * (read: Custom MK RC project)
77
 */
77
 */
78
uint8_t controlMixer_getArgument(void) {
78
uint8_t controlMixer_getArgument(void) {
79
  return RC_getArgument();
79
  return RC_getArgument();
80
}
80
}
81
 
81
 
82
/*
82
/*
83
 * This could be expanded to take calibrate / start / stop commands from ohter sources
83
 * This could be expanded to take calibrate / start / stop commands from ohter sources
84
 * than the R/C (read: Custom MK R/C project)
84
 * than the R/C (read: Custom MK R/C project)
85
 */
85
 */
86
uint8_t controlMixer_getCommand(void) {
86
uint8_t controlMixer_getCommand(void) {
87
  // If the same command was made COMMAND_TIMER times, it's stable.
87
  // If the same command was made COMMAND_TIMER times, it's stable.
88
  if (commandTimer >= COMMAND_TIMER) {
88
  if (commandTimer >= COMMAND_TIMER) {
89
    return lastCommand;
89
    return lastCommand;
90
  }
90
  }
91
  return COMMAND_NONE;
91
  return COMMAND_NONE;
92
}
92
}
93
 
93
 
94
uint8_t controlMixer_isCommandRepeated(void) {
94
uint8_t controlMixer_isCommandRepeated(void) {
95
  return commandTimer > COMMAND_TIMER ? 1 : 0;
95
  return commandTimer > COMMAND_TIMER ? 1 : 0;
96
}
96
}
97
 
97
 
98
void controlMixer_setNeutral(uint8_t calibrate) {
98
void controlMixer_setNeutral(uint8_t calibrate) {
99
  if (calibrate)
99
  if (calibrate)
100
    RC_calibrate();
100
    RC_calibrate();
101
  EC_setNeutral();
101
  EC_setNeutral();
102
}
102
}
103
 
103
 
104
/*
104
/*
105
 * Set the potientiometer values to the momentary values of the respective R/C channels.
105
 * Set the potientiometer values to the momentary values of the respective R/C channels.
106
 * No slew rate limitation.
106
 * No slew rate limitation.
107
 */
107
 */
108
void controlMixer_initVariables(void) {
108
void controlMixer_initVariables(void) {
109
  uint8_t i;
109
  uint8_t i;
110
  for (i=0; i<8; i++) {
110
  for (i=0; i<8; i++) {
111
    variables[i] = RC_getVariable(i);
111
    variables[i] = RC_getVariable(i);
112
  }
112
  }
113
}
113
}
114
 
114
 
115
/*
115
/*
116
 * Update potentiometer values with limited slew rate. Could be made faster if desired.
116
 * Update potentiometer values with limited slew rate. Could be made faster if desired.
117
 */
117
 */
118
void controlMixer_updateVariables(void) {
118
void controlMixer_updateVariables(void) {
119
  uint8_t i;
119
  uint8_t i;
120
  int16_t targetvalue;
120
  int16_t targetvalue;
121
  for (i=0; i<8; i++) {
121
  for (i=0; i<8; i++) {
122
    targetvalue = RC_getVariable(i);
122
    targetvalue = RC_getVariable(i);
123
    if (targetvalue < 0) targetvalue = 0;
123
    if (targetvalue < 0) targetvalue = 0;
124
    if (variables[i] < targetvalue && variables[i] < 255) variables[i]++; else if(variables[i] > 0 && variables[i] > targetvalue) variables[i]--;
124
    if (variables[i] < targetvalue && variables[i] < 255) variables[i]++; else if(variables[i] > 0 && variables[i] > targetvalue) variables[i]--;
125
  }
125
  }
126
}
126
}
127
 
127
 
128
uint8_t controlMixer_getSignalQuality(void) {
128
uint8_t controlMixer_getSignalQuality(void) {
129
  uint8_t rcQ = RC_getSignalQuality();
129
  uint8_t rcQ = RC_getSignalQuality();
130
  uint8_t ecQ = EC_getSignalQuality();
130
  uint8_t ecQ = EC_getSignalQuality();
131
  DebugOut.Analog[16] = rcQ;
-
 
132
  DebugOut.Analog[17] = ecQ;
-
 
133
  // This needs not be the only correct solution...
131
  // This needs not be the only correct solution...
134
  return rcQ > ecQ ? rcQ : ecQ;
132
  return rcQ > ecQ ? rcQ : ecQ;
135
}
133
}
136
 
134
 
137
/*
135
/*
138
 * Update the variables indicating stick position from the sum of R/C, GPS and external control.
136
 * Update the variables indicating stick position from the sum of R/C, GPS and external control.
139
 */
137
 */
140
void controlMixer_update(void) {
138
void controlMixer_update(void) {
141
  // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
139
  // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
142
  // TODO: If no signal --> zero.
140
  // TODO: If no signal --> zero.
-
 
141
  uint8_t axis;
143
  RC_update();
142
  RC_update();
144
  EC_update();
143
  EC_update();
145
 
144
 
146
  int16_t* RC_PRTY = RC_getPRTY();
145
  int16_t* RC_PRTY = RC_getPRTY();
147
  int16_t* EC_PRTY = EC_getPRTY();
146
  int16_t* EC_PRTY = EC_getPRTY();
148
 
147
 
149
  controlPitch     = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
148
  control[PITCH]   = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
150
  controlRoll      = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
149
  control[ROLL]    = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
151
  controlThrottle  = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE];
150
  controlThrottle  = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE];
152
  controlYaw       = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
151
  controlYaw       = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
153
 
-
 
154
  DebugOut.Analog[6] = controlPitch;
-
 
155
  DebugOut.Analog[7] = controlRoll;
-
 
156
  DebugOut.Analog[8] = controlYaw;
-
 
157
   
152
 
158
  if (RC_getSignalQuality() >= SIGNAL_GOOD) {
153
  if (RC_getSignalQuality() >= SIGNAL_GOOD) {
159
    controlMixer_updateVariables();
154
    controlMixer_updateVariables();
160
    configuration_applyVariablesToParams();
155
    configuration_applyVariablesToParams();
161
    looping = RC_getLooping(looping);
156
    looping = RC_getLooping(looping);
162
  } else { // Signal is not OK
157
  } else { // Signal is not OK
163
    // Could handle switch to emergency flight here.
158
    // Could handle switch to emergency flight here.
164
    // throttle is handled elsewhere.
159
    // throttle is handled elsewhere.
165
    looping = 0;
160
    looping = 0;
166
  }
161
  }
167
 
162
 
168
  DebugOut.Analog[18] = (int16_t)looping;
163
  DebugOut.Analog[18] = (int16_t)looping;
169
 
164
 
170
  // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
165
  // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
171
  // TODO: Correct, for changed range (stick gain + expo is now applied already)
166
  // TODO: Correct, for changed range (stick gain + expo is now applied already)
172
  if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
167
  if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
173
    if (controlYaw > 2) controlYaw-= 2;
168
    if (controlYaw > 2) controlYaw-= 2;
174
    else if (controlYaw< -2) controlYaw += 2;
169
    else if (controlYaw< -2) controlYaw += 2;
175
    else controlYaw = 0;
170
    else controlYaw = 0;
176
  }
171
  }
177
 
172
 
178
  /*
173
  /*
179
   * Record maxima
174
   * Record maxima
180
   */
175
   */
-
 
176
  for (axis=PITCH; axis<=ROLL; axis++) {
181
  if(abs(controlPitch / STICK_GAIN) > maxControlPitch) {
177
    if(abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
182
    maxControlPitch = abs(controlPitch) / STICK_GAIN;
178
      maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
183
    if(maxControlPitch > 100) maxControlPitch = 100;
179
      if(maxControl[axis] > 100) maxControl[axis] = 100;
184
  } else if (maxControlPitch) maxControlPitch--;
180
    } else if (maxControl[axis]) maxControl[axis]--;
185
  if(abs(controlRoll / STICK_GAIN) > maxControlRoll) {
-
 
186
    maxControlRoll = abs(controlRoll) / STICK_GAIN;
-
 
187
    if(maxControlRoll > 100) maxControlRoll = 100;
-
 
188
  }
181
  }
189
  else if (maxControlRoll) maxControlRoll--;
-
 
190
 
182
 
191
  // Here we could blend in other sources.
183
  // Here we could blend in other sources.
192
  uint8_t commandNow = RC_getCommand();
184
  uint8_t commandNow = RC_getCommand();
193
 
185
 
194
  if (commandNow != lastCommand) {
186
  if (commandNow != lastCommand) {
195
    lastCommand = commandNow;
187
    lastCommand = commandNow;
196
    commandTimer = 0;
188
    commandTimer = 0;
197
  } else if (commandTimer < COMMAND_TIMER + 1)
189
  } else if (commandTimer < COMMAND_TIMER + 1)
198
    commandTimer++;
190
    commandTimer++;
199
}
191
}
200
 
192
 
201
/*
193
/*
202
void setCompassCalState(void) {
194
void setCompassCalState(void) {
203
  static uint8_t stick = 1;
195
  static uint8_t stick = 1;
204
  // if pitch is centered or top set stick to zero
196
  // if pitch is centered or top set stick to zero
205
  if(RCChannel(CH_PITCH) > -20) stick = 0;
197
  if(RCChannel(CH_PITCH) > -20) stick = 0;
206
  // if pitch is down trigger to next cal state
198
  // if pitch is down trigger to next cal state
207
  if((RCChannel(CH_PITCH) < -70) && !stick) {
199
  if((RCChannel(CH_PITCH) < -70) && !stick) {
208
    stick = 1;
200
    stick = 1;
209
    compassCalState++;
201
    compassCalState++;
210
    if(compassCalState < 5) beepNumber(compassCalState);
202
    if(compassCalState < 5) beepNumber(compassCalState);
211
    else beep(1000);
203
    else beep(1000);
212
  }
204
  }
213
}
205
}
214
*/
206
*/
215
 
207