Subversion Repositories FlightCtrl

Rev

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

Rev 1955 Rev 1960
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 example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
35
// +     Commercial use (for example: 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
#include <util/delay.h>
51
#include <util/delay.h>
52
#include <avr/eeprom.h>
-
 
53
#include <stddef.h>
52
#include <stddef.h>
-
 
53
#include <string.h>
54
#include "configuration.h"
54
#include "configuration.h"
55
#include "eeprom.h"
55
#include "sensors.h"
-
 
56
#include "rc.h"
56
 
57
 
-
 
58
int16_t variables[8] = {0,0,0,0,0,0,0,0};
-
 
59
paramset_t staticParams;
-
 
60
channelMap_t channelMap;
57
int16_t variables[8] = {0,0,0,0,0,0,0,0};
61
mixerMatrix_t mixerMatrix;
58
dynamicParam_t dynamicParams = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0};
62
dynamicParam_t dynamicParams; // = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0};
59
uint8_t CPUType = ATMEGA644;
63
uint8_t CPUType = ATMEGA644;
60
uint8_t BoardRelease = 13;
-
 
-
 
64
uint8_t boardRelease = 13;
61
 
65
uint8_t requiredMotors;
62
/************************************************************************
66
/************************************************************************
63
 * Map the parameter to pot values                                    
67
 * Map the parameter to pot values                                    
64
 * Replacing this code by the code below saved almost 1 kbyte.
68
 * Replacing this code by the code below saved almost 1 kbyte.
65
 ************************************************************************/
69
 ************************************************************************/
66
 
70
 
67
void configuration_applyVariablesToParams(void) {
71
void configuration_applyVariablesToParams(void) {
68
  uint8_t i;
72
  uint8_t i;
69
#define SET_POT_MM(b,a,min,max) {if (a<255) {if (a>=251) b=variables[a-251]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;}
73
#define SET_POT_MM(b,a,min,max) {if (a<255) {if (a>=251) b=variables[a-251]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;}
70
#define SET_POT(b,a) { if (a<255) {if (a>=251) b=variables[a-251]; else b=a;}}
74
#define SET_POT(b,a) { if (a<255) {if (a>=251) b=variables[a-251]; else b=a;}}
71
  SET_POT(dynamicParams.MaxHeight,staticParams.MaxHeight);
-
 
72
  SET_POT_MM(dynamicParams.HeightD,staticParams.HeightD,0,100);
-
 
73
  SET_POT_MM(dynamicParams.HeightP,staticParams.HeightP,0,100);
-
 
74
  SET_POT(dynamicParams.Height_ACC_Effect,staticParams.Height_ACC_Effect);
-
 
75
  SET_POT(dynamicParams.CompassYawEffect,staticParams.CompassYawEffect);
-
 
76
  SET_POT_MM(dynamicParams.GyroP,staticParams.GyroP,0,255);
75
  SET_POT_MM(dynamicParams.gyroP, staticParams.gyroP, 5, 200);
77
  SET_POT(dynamicParams.GyroI,staticParams.GyroI);
76
  SET_POT(dynamicParams.gyroI, staticParams.gyroI);
78
  SET_POT(dynamicParams.GyroD,staticParams.GyroD);
77
  SET_POT(dynamicParams.gyroD, staticParams.gyroD);
79
  SET_POT(dynamicParams.IFactor,staticParams.IFactor);
78
  SET_POT(dynamicParams.IFactor, staticParams.IFactor);
80
  for (i=0; i<sizeof(staticParams.UserParams1); i++) {
-
 
81
    SET_POT(dynamicParams.UserParams[i],staticParams.UserParams1[i]);
79
  SET_POT(dynamicParams.yawIFactor, staticParams.yawIFactor);
82
  }
-
 
83
  for (i=0; i<sizeof(staticParams.UserParams2); i++) {
-
 
84
    SET_POT(dynamicParams.UserParams[i + sizeof(staticParams.UserParams1)], staticParams.UserParams2[i]);
80
  SET_POT(dynamicParams.compassYawEffect,staticParams.compassYawEffect);
85
  }
81
 
86
  SET_POT(dynamicParams.ServoPitchControl,staticParams.ServoPitchControl);
82
  SET_POT(dynamicParams.externalControl, staticParams.externalControl);
87
  SET_POT(dynamicParams.LoopGasLimit,staticParams.LoopGasLimit);
-
 
88
  SET_POT(dynamicParams.AxisCoupling1,staticParams.AxisCoupling1);
83
  SET_POT(dynamicParams.axisCoupling1, staticParams.axisCoupling1);
89
  SET_POT(dynamicParams.AxisCoupling2,staticParams.AxisCoupling2);
84
  SET_POT(dynamicParams.axisCoupling2, staticParams.axisCoupling2);
90
  SET_POT(dynamicParams.AxisCouplingYawCorrection,staticParams.AxisCouplingYawCorrection);
85
  SET_POT(dynamicParams.axisCouplingYawCorrection, staticParams.axisCouplingYawCorrection);
91
  SET_POT(dynamicParams.DynamicStability,staticParams.DynamicStability);
86
  SET_POT(dynamicParams.dynamicStability,staticParams.dynamicStability);
92
  SET_POT_MM(dynamicParams.J16Timing,staticParams.J16Timing,1,255);
-
 
93
  SET_POT_MM(dynamicParams.J17Timing,staticParams.J17Timing,1,255);
87
  SET_POT(dynamicParams.maxControlActivityForAcc,staticParams.maxControlActivityForAcc);
94
 
88
 
95
#if defined (USE_NAVICTRL)
89
  SET_POT_MM(dynamicParams.heightP, staticParams.heightP,0,100);
96
  SET_POT(dynamicParams.NaviGpsModeControl,staticParams.NaviGpsModeControl);
90
  SET_POT_MM(dynamicParams.heightD, staticParams.heightD,0,100);
97
  SET_POT(dynamicParams.NaviGpsGain,staticParams.NaviGpsGain);
91
  SET_POT(dynamicParams.heightSetting,staticParams.heightSetting);
98
  SET_POT(dynamicParams.NaviGpsP,staticParams.NaviGpsP);
92
  SET_POT(dynamicParams.heightACCEffect,staticParams.heightACCEffect);
99
  SET_POT(dynamicParams.NaviGpsI,staticParams.NaviGpsI);
93
  SET_POT(dynamicParams.attitudeControl,staticParams.attitudeControl);
-
 
94
 
100
  SET_POT(dynamicParams.NaviGpsD,staticParams.NaviGpsD);
95
  for (i=0; i<sizeof(staticParams.userParams); i++) {
101
  SET_POT(dynamicParams.NaviGpsACC,staticParams.NaviGpsACC);
96
    SET_POT(dynamicParams.userParams[i],staticParams.userParams[i]);
-
 
97
  }
-
 
98
 
102
  SET_POT_MM(dynamicParams.NaviOperatingRadius,staticParams.NaviOperatingRadius,10, 255);
99
  SET_POT(dynamicParams.servoManualControl[0], staticParams.servoConfigurations[0].manualControl);
103
  SET_POT(dynamicParams.NaviWindCorrection,staticParams.NaviWindCorrection);
100
  SET_POT(dynamicParams.servoManualControl[1], staticParams.servoConfigurations[1].manualControl);
-
 
101
 
104
  SET_POT(dynamicParams.NaviSpeedCompensation,staticParams.NaviSpeedCompensation);
102
  SET_POT_MM(dynamicParams.output0Timing, staticParams.outputFlash[0].timing,1,255);
105
#endif
-
 
106
  SET_POT(dynamicParams.ExternalControl,staticParams.ExternalControl);
103
  SET_POT_MM(dynamicParams.output1Timing, staticParams.outputFlash[1].timing,1,255);
107
}
104
}
108
 
105
 
109
const XLATION XLATIONS[] = {
106
const XLATION XLATIONS[] = {
110
  {offsetof(paramset_t, MaxHeight), offsetof(dynamicParam_t, MaxHeight)},
107
  //  {offsetof(paramset_t, MaxHeight), offsetof(dynamicParam_t, MaxHeight)},
111
  {offsetof(paramset_t, Height_ACC_Effect), offsetof(dynamicParam_t, Height_ACC_Effect)},
-
 
112
  {offsetof(paramset_t, CompassYawEffect), offsetof(dynamicParam_t, CompassYawEffect)},
-
 
113
  {offsetof(paramset_t, GyroI), offsetof(dynamicParam_t, GyroI)},
-
 
114
  {offsetof(paramset_t, GyroD), offsetof(dynamicParam_t, GyroD)},
-
 
115
  {offsetof(paramset_t, IFactor), offsetof(dynamicParam_t, IFactor)},
-
 
116
  {offsetof(paramset_t, ServoPitchControl), offsetof(dynamicParam_t, ServoPitchControl)},
-
 
117
  {offsetof(paramset_t, LoopGasLimit), offsetof(dynamicParam_t, LoopGasLimit)},
-
 
118
  {offsetof(paramset_t, AxisCoupling1), offsetof(dynamicParam_t, AxisCoupling1)},
-
 
119
  {offsetof(paramset_t, AxisCoupling2), offsetof(dynamicParam_t, AxisCoupling2)},
-
 
120
  {offsetof(paramset_t, AxisCouplingYawCorrection), offsetof(dynamicParam_t, AxisCouplingYawCorrection)},
-
 
121
  {offsetof(paramset_t, DynamicStability), offsetof(dynamicParam_t, DynamicStability)},
-
 
122
  {offsetof(paramset_t, NaviGpsModeControl),
-
 
123
   offsetof(dynamicParam_t, NaviGpsModeControl)},
-
 
124
  {offsetof(paramset_t, NaviGpsGain), offsetof(dynamicParam_t, NaviGpsGain)},
-
 
125
  {offsetof(paramset_t, NaviGpsP), offsetof(dynamicParam_t, NaviGpsP)},
-
 
126
  {offsetof(paramset_t, NaviGpsI), offsetof(dynamicParam_t, NaviGpsI)},
-
 
127
  {offsetof(paramset_t, NaviGpsD), offsetof(dynamicParam_t, NaviGpsD)},
-
 
128
  {offsetof(paramset_t, NaviGpsACC), offsetof(dynamicParam_t, NaviGpsACC)},
-
 
129
  {offsetof(paramset_t, NaviWindCorrection), offsetof(dynamicParam_t, NaviWindCorrection)},
-
 
130
  {offsetof(paramset_t, NaviSpeedCompensation), offsetof(dynamicParam_t, NaviSpeedCompensation)},
-
 
131
  {offsetof(paramset_t, ExternalControl), offsetof(dynamicParam_t, ExternalControl)}
-
 
132
};
108
};
133
 
109
 
134
const MMXLATION MMXLATIONS[] = {
110
const MMXLATION MMXLATIONS[] = {
135
  {offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100},
111
  //  {offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100},
136
  {offsetof(paramset_t, HeightP), offsetof(dynamicParam_t, HeightP),0,150},
-
 
137
  {offsetof(paramset_t, GyroP), offsetof(dynamicParam_t, GyroP),0,255},
-
 
138
  {offsetof(paramset_t, J16Timing), offsetof(dynamicParam_t, J16Timing),1,255},
-
 
139
  {offsetof(paramset_t, J17Timing), offsetof(dynamicParam_t, J17Timing),1,255},
-
 
140
  {offsetof(paramset_t, NaviOperatingRadius), offsetof(dynamicParam_t, NaviOperatingRadius),10,255}
-
 
141
};
112
};
142
 
113
 
143
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) {
114
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) {
144
  uint8_t result;
115
  uint8_t result;
145
  if (src>=251) result = variables[src-251];
116
  if (src>=251) result = variables[src-251];
146
  else result = src;
117
  else result = src;
147
  if (result < min) result = min;
118
  if (result < min) result = min;
148
  else if (result > max) result = max;
119
  else if (result > max) result = max;
149
  return result;
120
  return result;
150
}
121
}
151
 
122
 
152
void configuration_applyVariablesToParams_dead(void) {
123
void configuration_applyVariablesToParams_dead(void) {
153
  uint8_t i, src;
124
  uint8_t i, src;
154
  uint8_t* pointerToTgt;
125
  uint8_t* pointerToTgt;
155
  for(i=0; i<sizeof(XLATIONS)/sizeof(XLATION); i++) {
126
  for(i=0; i<sizeof(XLATIONS)/sizeof(XLATION); i++) {
156
    src = *((uint8_t*)(&staticParams + XLATIONS[i].sourceIdx));
127
    src = *((uint8_t*)(&staticParams + XLATIONS[i].sourceIdx));
157
    pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx);
128
    pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx);
158
    if (src < 255) {
129
    if (src < 255) {
159
      *pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
130
      *pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
160
    }
131
    }
161
  }
132
  }
162
 
133
 
163
  for(i=0; i<sizeof(MMXLATIONS)/sizeof(MMXLATION); i++) {
134
  for(i=0; i<sizeof(MMXLATIONS)/sizeof(MMXLATION); i++) {
164
    src = *((uint8_t*)(&staticParams + MMXLATIONS[i].sourceIdx));
135
    src = *((uint8_t*)(&staticParams + MMXLATIONS[i].sourceIdx));
165
    pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx);
136
    pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx);
166
    if (src < 255) {
137
    if (src < 255) {
167
      *pointerToTgt = configuration_applyVariableToParam(src, MMXLATIONS[i].min, MMXLATIONS[i].max);
138
      *pointerToTgt = configuration_applyVariableToParam(src, MMXLATIONS[i].min, MMXLATIONS[i].max);
168
    }
139
    }
169
  }
140
  }
170
 
141
 
171
  for (i=0; i<sizeof(staticParams.UserParams1); i++) {
142
  for (i=0; i<sizeof(staticParams.userParams); i++) {
172
    src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams1) + i));
143
    src = *((uint8_t*)(&staticParams + offsetof(paramset_t, userParams) + i));
173
    pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + i);
-
 
174
    if (src < 255) {
-
 
175
      *pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
-
 
176
    }    
-
 
177
  }
-
 
178
 
-
 
179
  for (i=0; i<sizeof(staticParams.UserParams2); i++) {
-
 
180
    src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams2) + i));
-
 
181
    pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + sizeof(staticParams.UserParams1) + i);
144
    pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, userParams) + i);
182
    if (src < 255) {
145
    if (src < 255) {
183
      *pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
146
      *pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
184
    }    
147
    }    
185
  }
148
  }
186
}
149
}
187
 
150
 
188
uint8_t getCPUType(void) {   // works only after reset or power on when the registers have default values
151
uint8_t getCPUType(void) {   // works only after reset or power on when the registers have default values
189
  uint8_t CPUType = ATMEGA644;
152
  uint8_t CPUType = ATMEGA644;
190
  if( (UCSR1A == 0x20) && (UCSR1C == 0x06) ) CPUType = ATMEGA644P;  // initial Values for 644P after reset
153
  if( (UCSR1A == 0x20) && (UCSR1C == 0x06) ) CPUType = ATMEGA644P;  // initial Values for 644P after reset
191
  return CPUType;
154
  return CPUType;
192
}
155
}
193
 
156
 
194
/*
157
/*
195
 * Automatic detection of hardware components is not supported in this development-oriented
158
 * Automatic detection of hardware components is not supported in this development-oriented
196
 * FC firmware. It would go against the point of it: To enable alternative hardware
159
 * FC firmware. It would go against the point of it: To enable alternative hardware
197
 * configurations with otherwise unsupported components. Instead, one should write
160
 * configurations with otherwise unsupported components. Instead, one should write
198
 * custom code + adjust constants for the new hardware, and include the relevant code
161
 * custom code + adjust constants for the new hardware, and include the relevant code
199
 * from the makefile.
162
 * from the makefile.
200
 * However - we still do detect the board release. Reason: Otherwise it would be too
163
 * However - we still do detect the board release. Reason: Otherwise it would be too
201
 * tedious to have to modify the code for how to turn on and off LEDs when deploying
164
 * tedious to have to modify the code for how to turn on and off LEDs when deploying
202
 * on different HW version....
165
 * on different HW version....
203
 */
166
 */
204
 
167
 
205
uint8_t getBoardRelease(void) {
168
uint8_t getBoardRelease(void) {
206
  uint8_t BoardRelease = 13;
169
  uint8_t boardRelease = 13;
207
  // the board release is coded via the pull up or down the 2 status LED
170
  // the board release is coded via the pull up or down the 2 status LED
208
 
171
 
209
  PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate
172
  PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate
210
  DDRB  &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input
173
  DDRB  &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input
211
 
174
 
212
  _delay_loop_2(1000); // make some delay
175
  _delay_loop_2(1000); // make some delay
213
 
176
 
214
  switch( PINB & ((1<<PINB1)|(1<<PINB0))) {
177
  switch( PINB & ((1<<PINB1)|(1<<PINB0))) {
215
    case 0x00:
178
    case 0x00:
216
      BoardRelease = 10; // 1.0
179
      boardRelease = 10; // 1.0
217
      break;
180
      break;
218
    case 0x01:
181
    case 0x01:
219
      BoardRelease = 11; // 1.1 or 1.2
182
      boardRelease = 11; // 1.1 or 1.2
220
      break;
183
      break;
221
    case 0x02:
184
    case 0x02:
222
      BoardRelease = 20; // 2.0
185
      boardRelease = 20; // 2.0
223
      break;
186
      break;
224
    case 0x03:
187
    case 0x03:
225
      BoardRelease = 13; // 1.3
188
      boardRelease = 13; // 1.3
226
      break;
189
      break;
227
    default:
190
    default:
228
      break;
191
      break;
229
    }
192
    }
230
  // set LED ports as output
193
  // set LED ports as output
231
  DDRB |= (1<<DDB1)|(1<<DDB0);
194
  DDRB |= (1<<DDB1)|(1<<DDB0);
232
  RED_ON;
195
  RED_ON;
233
  GRN_OFF;
196
  GRN_OFF;
234
  return BoardRelease;
197
  return boardRelease;
235
}
198
}
-
 
199
 
-
 
200
void setOtherDefaults(void) {
-
 
201
  // Height Control
-
 
202
  staticParams.heightP = 10;
-
 
203
  staticParams.heightD = 30;
-
 
204
  staticParams.heightSetting = 251;
-
 
205
  staticParams.heightMaxThrottleChange = 10;
-
 
206
  staticParams.heightSlewRate = 4;
-
 
207
  staticParams.heightACCEffect = 30;
-
 
208
 
-
 
209
  // Control
-
 
210
  staticParams.stickP = 8;
-
 
211
  staticParams.stickD = 12;
-
 
212
  staticParams.stickYawP = 12;
-
 
213
  staticParams.stickThrottleD = 12;
-
 
214
  staticParams.minThrottle = 8;
-
 
215
  staticParams.maxThrottle = 230;
-
 
216
  staticParams.externalControl = 0;
-
 
217
 
-
 
218
  // IMU
-
 
219
  staticParams.gyroPIDFilterConstant = 1;
-
 
220
  staticParams.gyroATTFilterConstant = 1;
-
 
221
  staticParams.gyroDFilterConstant = 1;
-
 
222
  staticParams.accFilterConstant = 4;
-
 
223
 
-
 
224
  staticParams.gyroP = 60;
-
 
225
  staticParams.gyroI = 80;
-
 
226
  staticParams.gyroD = 4;
-
 
227
 
-
 
228
  // set by gyro code.
-
 
229
  // staticParams.zerothOrderCorrection = 
-
 
230
  // staticParams.driftCompDivider = 
-
 
231
  // staticParams.driftCompLimit = 
-
 
232
 
-
 
233
  staticParams.axisCoupling1 = 90;
-
 
234
  staticParams.axisCoupling2 = 67;
-
 
235
  staticParams.axisCouplingYawCorrection = 0;
-
 
236
  staticParams.dynamicStability = 50;
-
 
237
  staticParams.IFactor = 32;  
-
 
238
  staticParams.compassYawEffect = 128;
-
 
239
 
-
 
240
  // Servos
-
 
241
  for (uint8_t i=0; i<2; i++) {
-
 
242
    staticParams.servoConfigurations[i].manualControl = 128;
-
 
243
    staticParams.servoConfigurations[i].compensationFactor = 0;
-
 
244
    staticParams.servoConfigurations[i].minValue = 64;
-
 
245
    staticParams.servoConfigurations[i].maxValue = 192;
-
 
246
    staticParams.servoConfigurations[i].flags = 0;
-
 
247
  }
-
 
248
  staticParams.servoCount = 4;
-
 
249
 
-
 
250
  // Battery warning and emergency flight
-
 
251
  staticParams.batteryVoltageWarning = 101;  // 3.7 each for 3S
-
 
252
  staticParams.emergencyThrottle = 35;
-
 
253
  staticParams.emergencyFlightDuration = 30;
-
 
254
 
-
 
255
  // Outputs
-
 
256
  staticParams.outputFlash[0].bitmask = 0b01011111;
-
 
257
  staticParams.outputFlash[0].timing = 15;
-
 
258
  staticParams.outputFlash[1].bitmask = 0b11110011;
-
 
259
  staticParams.outputFlash[1].timing = 15;
-
 
260
 
-
 
261
  staticParams.outputFlags = 0;
-
 
262
}
-
 
263
 
-
 
264
/***************************************************/
-
 
265
/*    Default Values for parameter set 1           */
-
 
266
/***************************************************/
-
 
267
void paramSet_default(uint8_t setnumber) {
-
 
268
  setOtherDefaults();
-
 
269
  gyro_setDefaults();
-
 
270
 
-
 
271
  for (uint8_t i=0; i<8; i++) {
-
 
272
    staticParams.userParams[i] = 0;
-
 
273
  }
-
 
274
 
-
 
275
  staticParams.bitConfig =
-
 
276
    CFG_GYRO_SATURATION_PREVENTION |
-
 
277
    CFG_HEADING_HOLD;
-
 
278
  staticParams.bitConfig2 = 0;
-
 
279
 
-
 
280
  memcpy(staticParams.name, "Default\0", 6);
-
 
281
}
-
 
282
 
-
 
283
/***************************************************/
-
 
284
/*    Default Values for Mixer Table               */
-
 
285
/***************************************************/
-
 
286
void mixerMatrix_default(void) { // Quadro 
-
 
287
  uint8_t i;
-
 
288
  // mixerMatric.revision = EEMIXER_REVISION;
-
 
289
  // clear mixer table (but preset throttle)
-
 
290
  for (i = 0; i < 16; i++) {
-
 
291
    mixerMatrix.motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0;
-
 
292
    mixerMatrix.motor[i][MIX_PITCH] = 0;
-
 
293
    mixerMatrix.motor[i][MIX_ROLL] = 0;
-
 
294
    mixerMatrix.motor[i][MIX_YAW] = 0;
-
 
295
  }
-
 
296
  // default = Quadro
-
 
297
  mixerMatrix.motor[0][MIX_PITCH] = +64;
-
 
298
  mixerMatrix.motor[0][MIX_YAW] = +64;
-
 
299
  mixerMatrix.motor[1][MIX_PITCH] = -64;
-
 
300
  mixerMatrix.motor[1][MIX_YAW] = +64;
-
 
301
  mixerMatrix.motor[2][MIX_ROLL] = -64;
-
 
302
  mixerMatrix.motor[2][MIX_YAW] = -64;
-
 
303
  mixerMatrix.motor[3][MIX_ROLL] = +64;
-
 
304
  mixerMatrix.motor[3][MIX_YAW] = -64;
-
 
305
  memcpy(mixerMatrix.name, "Quadro\0", 7);
-
 
306
}
-
 
307
 
-
 
308
void channelMap_default(void) {
-
 
309
  channelMap.channels[CH_PITCH] = 2;
-
 
310
  channelMap.channels[CH_ROLL] = 1;
-
 
311
  channelMap.channels[CH_THROTTLE] = 3;
-
 
312
  channelMap.channels[CH_YAW] = 4;
-
 
313
  channelMap.channels[CH_POTS + 0] = 5;
-
 
314
  channelMap.channels[CH_POTS + 1] = 6;
-
 
315
  channelMap.channels[CH_POTS + 2] = 7;
-
 
316
  channelMap.channels[CH_POTS + 3] = 8;
-
 
317
}
-
 
318
 
236
 
319