Subversion Repositories FlightCtrl

Rev

Rev 1968 | Rev 1971 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1612 dongfang 1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) 04.2007 Holger Buss
3
// + Nur für den privaten Gebrauch
4
// + www.MikroKopter.com
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
1963 - 7
// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zulässig ist.
1612 dongfang 8
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
9
// + bzgl. der Nutzungsbedingungen aufzunehmen.
10
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
11
// + Verkauf von Luftbildaufnahmen, usw.
12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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
15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
1963 - 17
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18
// + eindeutig als Ursprung verlinkt und genannt werden
1612 dongfang 19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
21
// + Benutzung auf eigene Gefahr
22
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
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
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
34
// +     for non-commercial use (directly or indirectly)
1868 - 35
// +     Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
1612 dongfang 36
// +     with our written permission
37
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
38
// +     clearly linked as origin
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"
41
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
1963 - 47
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
48
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
1612 dongfang 49
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
50
// +  POSSIBILITY OF SUCH DAMAGE.
51
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
52
#include <util/delay.h>
1775 - 53
#include <stddef.h>
1960 - 54
#include <string.h>
1612 dongfang 55
#include "configuration.h"
1960 - 56
#include "sensors.h"
57
#include "rc.h"
1961 - 58
int16_t variables[VARIABLE_COUNT];
1960 - 59
paramset_t staticParams;
60
channelMap_t channelMap;
61
mixerMatrix_t mixerMatrix;
1969 - 62
volatile dynamicParam_t dynamicParams;
63
 
1612 dongfang 64
uint8_t CPUType = ATMEGA644;
1960 - 65
uint8_t boardRelease = 13;
66
uint8_t requiredMotors;
1963 - 67
 
68
// MK flags. TODO: Replace by enum. State machine.
69
uint16_t isFlying = 0;
70
volatile uint8_t MKFlags = 0;
71
 
1612 dongfang 72
/************************************************************************
73
 * Map the parameter to pot values                                    
1775 - 74
 * Replacing this code by the code below saved almost 1 kbyte.
1612 dongfang 75
 ************************************************************************/
1775 - 76
 
1612 dongfang 77
void configuration_applyVariablesToParams(void) {
78
  uint8_t i;
1961 - 79
#define SET_POT_MM(b,a,min,max) {if (a<255) {if (a>=255-VARIABLE_COUNT) b=variables[a+VARIABLE_COUNT-255]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;}
80
#define SET_POT(b,a) { if (a<255) {if (a>=255-VARIABLE_COUNT) b=variables[a+VARIABLE_COUNT-255]; else b=a;}}
1960 - 81
  SET_POT_MM(dynamicParams.gyroP, staticParams.gyroP, 5, 200);
82
  SET_POT(dynamicParams.gyroI, staticParams.gyroI);
83
  SET_POT(dynamicParams.gyroD, staticParams.gyroD);
84
  SET_POT(dynamicParams.IFactor, staticParams.IFactor);
85
  SET_POT(dynamicParams.yawIFactor, staticParams.yawIFactor);
86
  SET_POT(dynamicParams.compassYawEffect,staticParams.compassYawEffect);
87
 
88
  SET_POT(dynamicParams.externalControl, staticParams.externalControl);
89
  SET_POT(dynamicParams.axisCoupling1, staticParams.axisCoupling1);
90
  SET_POT(dynamicParams.axisCoupling2, staticParams.axisCoupling2);
91
  SET_POT(dynamicParams.axisCouplingYawCorrection, staticParams.axisCouplingYawCorrection);
92
  SET_POT(dynamicParams.dynamicStability,staticParams.dynamicStability);
93
  SET_POT(dynamicParams.maxControlActivityForAcc,staticParams.maxControlActivityForAcc);
94
 
95
  SET_POT_MM(dynamicParams.heightP, staticParams.heightP,0,100);
96
  SET_POT_MM(dynamicParams.heightD, staticParams.heightD,0,100);
97
  SET_POT(dynamicParams.heightSetting,staticParams.heightSetting);
98
  SET_POT(dynamicParams.heightACCEffect,staticParams.heightACCEffect);
99
  SET_POT(dynamicParams.attitudeControl,staticParams.attitudeControl);
100
 
101
  for (i=0; i<sizeof(staticParams.userParams); i++) {
102
    SET_POT(dynamicParams.userParams[i],staticParams.userParams[i]);
1612 dongfang 103
  }
1805 - 104
 
1960 - 105
  SET_POT(dynamicParams.servoManualControl[0], staticParams.servoConfigurations[0].manualControl);
106
  SET_POT(dynamicParams.servoManualControl[1], staticParams.servoConfigurations[1].manualControl);
107
 
108
  SET_POT_MM(dynamicParams.output0Timing, staticParams.outputFlash[0].timing,1,255);
109
  SET_POT_MM(dynamicParams.output1Timing, staticParams.outputFlash[1].timing,1,255);
1612 dongfang 110
}
111
 
1775 - 112
const XLATION XLATIONS[] = {
1961 - 113
  {offsetof(paramset_t, heightSetting), offsetof(dynamicParam_t, heightSetting)},
1775 - 114
};
115
 
116
const MMXLATION MMXLATIONS[] = {
1961 - 117
  {offsetof(paramset_t, heightD), offsetof(dynamicParam_t, heightD),0,100},
1775 - 118
};
119
 
120
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) {
121
  uint8_t result;
122
  if (src>=251) result = variables[src-251];
123
  else result = src;
124
  if (result < min) result = min;
125
  else if (result > max) result = max;
126
  return result;
127
}
128
 
129
void configuration_applyVariablesToParams_dead(void) {
130
  uint8_t i, src;
131
  uint8_t* pointerToTgt;
132
  for(i=0; i<sizeof(XLATIONS)/sizeof(XLATION); i++) {
133
    src = *((uint8_t*)(&staticParams + XLATIONS[i].sourceIdx));
134
    pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx);
135
    if (src < 255) {
136
      *pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
137
    }
138
  }
139
 
140
  for(i=0; i<sizeof(MMXLATIONS)/sizeof(MMXLATION); i++) {
141
    src = *((uint8_t*)(&staticParams + MMXLATIONS[i].sourceIdx));
142
    pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx);
143
    if (src < 255) {
144
      *pointerToTgt = configuration_applyVariableToParam(src, MMXLATIONS[i].min, MMXLATIONS[i].max);
145
    }
146
  }
147
 
1960 - 148
  for (i=0; i<sizeof(staticParams.userParams); i++) {
149
    src = *((uint8_t*)(&staticParams + offsetof(paramset_t, userParams) + i));
150
    pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, userParams) + i);
1775 - 151
    if (src < 255) {
152
      *pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
153
    }    
154
  }
155
}
156
 
1612 dongfang 157
uint8_t getCPUType(void) {   // works only after reset or power on when the registers have default values
158
  uint8_t CPUType = ATMEGA644;
1961 - 159
  //if((UCSR1A == 0x20) && (UCSR1C == 0x06)) CPUType = ATMEGA644P;  // initial Values for 644P after reset
1612 dongfang 160
  return CPUType;
161
}
162
 
163
/*
164
 * Automatic detection of hardware components is not supported in this development-oriented
165
 * FC firmware. It would go against the point of it: To enable alternative hardware
166
 * configurations with otherwise unsupported components. Instead, one should write
167
 * custom code + adjust constants for the new hardware, and include the relevant code
168
 * from the makefile.
169
 * However - we still do detect the board release. Reason: Otherwise it would be too
170
 * tedious to have to modify the code for how to turn on and off LEDs when deploying
171
 * on different HW version....
172
 */
173
 
174
uint8_t getBoardRelease(void) {
1960 - 175
  uint8_t boardRelease = 13;
1612 dongfang 176
  // the board release is coded via the pull up or down the 2 status LED
177
 
178
  PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate
179
  DDRB  &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input
180
 
181
  _delay_loop_2(1000); // make some delay
182
 
183
  switch( PINB & ((1<<PINB1)|(1<<PINB0))) {
184
    case 0x00:
1960 - 185
      boardRelease = 10; // 1.0
1612 dongfang 186
      break;
187
    case 0x01:
1960 - 188
      boardRelease = 11; // 1.1 or 1.2
1612 dongfang 189
      break;
190
    case 0x02:
1960 - 191
      boardRelease = 20; // 2.0
1612 dongfang 192
      break;
193
    case 0x03:
1960 - 194
      boardRelease = 13; // 1.3
1612 dongfang 195
      break;
196
    default:
197
      break;
198
    }
199
  // set LED ports as output
200
  DDRB |= (1<<DDB1)|(1<<DDB0);
1964 - 201
  RED_OFF;
1612 dongfang 202
  GRN_OFF;
1960 - 203
  return boardRelease;
1612 dongfang 204
}
1960 - 205
 
206
void setOtherDefaults(void) {
207
  // Height Control
208
  staticParams.heightP = 10;
209
  staticParams.heightD = 30;
210
  staticParams.heightSetting = 251;
1961 - 211
  staticParams.heightControlMaxThrottleChange = 10;
1960 - 212
  staticParams.heightSlewRate = 4;
213
 
214
  // Control
215
  staticParams.stickP = 8;
216
  staticParams.stickD = 12;
217
  staticParams.stickYawP = 12;
218
  staticParams.stickThrottleD = 12;
219
  staticParams.minThrottle = 8;
220
  staticParams.maxThrottle = 230;
221
  staticParams.externalControl = 0;
222
 
223
  // IMU
224
  staticParams.gyroPIDFilterConstant = 1;
225
  staticParams.gyroATTFilterConstant = 1;
226
  staticParams.gyroDFilterConstant = 1;
227
  staticParams.accFilterConstant = 4;
228
 
229
  staticParams.gyroP = 60;
230
  staticParams.gyroI = 80;
231
  staticParams.gyroD = 4;
232
 
1961 - 233
  // set by gyro-specific code: gyro_setDefaults().
1960 - 234
  // staticParams.zerothOrderCorrection = 
235
  // staticParams.driftCompDivider = 
236
  // staticParams.driftCompLimit = 
237
 
238
  staticParams.axisCoupling1 = 90;
239
  staticParams.axisCoupling2 = 67;
240
  staticParams.axisCouplingYawCorrection = 0;
241
  staticParams.dynamicStability = 50;
242
  staticParams.IFactor = 32;  
243
  staticParams.compassYawEffect = 128;
244
 
245
  // Servos
246
  for (uint8_t i=0; i<2; i++) {
247
    staticParams.servoConfigurations[i].manualControl = 128;
248
    staticParams.servoConfigurations[i].compensationFactor = 0;
249
    staticParams.servoConfigurations[i].minValue = 64;
250
    staticParams.servoConfigurations[i].maxValue = 192;
251
    staticParams.servoConfigurations[i].flags = 0;
252
  }
253
  staticParams.servoCount = 4;
254
 
255
  // Battery warning and emergency flight
256
  staticParams.batteryVoltageWarning = 101;  // 3.7 each for 3S
257
  staticParams.emergencyThrottle = 35;
258
  staticParams.emergencyFlightDuration = 30;
259
 
260
  // Outputs
1961 - 261
  staticParams.outputFlash[0].bitmask = 1; //0b01011111;
1960 - 262
  staticParams.outputFlash[0].timing = 15;
1961 - 263
  staticParams.outputFlash[1].bitmask = 3; //0b11110011;
1960 - 264
  staticParams.outputFlash[1].timing = 15;
265
 
1964 - 266
  staticParams.outputDebugMask = 16;
267
  staticParams.outputOptions = 8;
1960 - 268
}
269
 
270
/***************************************************/
271
/*    Default Values for parameter set 1           */
272
/***************************************************/
273
void paramSet_default(uint8_t setnumber) {
274
  setOtherDefaults();
275
  gyro_setDefaults();
276
 
277
  for (uint8_t i=0; i<8; i++) {
278
    staticParams.userParams[i] = 0;
279
  }
280
 
281
  staticParams.bitConfig =
1961 - 282
    CFG_GYRO_SATURATION_PREVENTION | CFG_HEADING_HOLD;
1960 - 283
 
284
  memcpy(staticParams.name, "Default\0", 6);
285
}
286
 
287
/***************************************************/
288
/*    Default Values for Mixer Table               */
289
/***************************************************/
290
void mixerMatrix_default(void) { // Quadro 
291
  uint8_t i;
292
  // mixerMatric.revision = EEMIXER_REVISION;
293
  // clear mixer table (but preset throttle)
294
  for (i = 0; i < 16; i++) {
295
    mixerMatrix.motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0;
296
    mixerMatrix.motor[i][MIX_PITCH] = 0;
297
    mixerMatrix.motor[i][MIX_ROLL] = 0;
298
    mixerMatrix.motor[i][MIX_YAW] = 0;
299
  }
300
  // default = Quadro
301
  mixerMatrix.motor[0][MIX_PITCH] = +64;
302
  mixerMatrix.motor[0][MIX_YAW] = +64;
303
  mixerMatrix.motor[1][MIX_PITCH] = -64;
304
  mixerMatrix.motor[1][MIX_YAW] = +64;
305
  mixerMatrix.motor[2][MIX_ROLL] = -64;
306
  mixerMatrix.motor[2][MIX_YAW] = -64;
307
  mixerMatrix.motor[3][MIX_ROLL] = +64;
308
  mixerMatrix.motor[3][MIX_YAW] = -64;
309
  memcpy(mixerMatrix.name, "Quadro\0", 7);
310
}
311
 
1968 - 312
/***************************************************/
313
/*    Default Values for R/C Channels              */
314
/***************************************************/
1960 - 315
void channelMap_default(void) {
1968 - 316
  channelMap.channels[CH_PITCH]    = 1;
317
  channelMap.channels[CH_ROLL]     = 0;
318
  channelMap.channels[CH_THROTTLE] = 2;
319
  channelMap.channels[CH_YAW]      = 3;
320
  channelMap.channels[CH_POTS + 0] = 4;
321
  channelMap.channels[CH_POTS + 1] = 5;
322
  channelMap.channels[CH_POTS + 2] = 6;
323
  channelMap.channels[CH_POTS + 3] = 7;
1960 - 324
}
325