Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1910 - 1
#include <util/delay.h>
2
#include <stddef.h>
2099 - 3
#include <string.h>
1910 - 4
#include "configuration.h"
2142 - 5
#include "controlMixer.h"
2099 - 6
#include "rc.h"
7
#include "output.h"
2119 - 8
#include "sensors.h"
2099 - 9
#include "flight.h"
1910 - 10
 
2099 - 11
int16_t variables[VARIABLE_COUNT];
12
ParamSet_t staticParams;
2116 - 13
ChannelMap_t channelMap;
2122 - 14
RCTrim_t rcTrim;
2099 - 15
IMUConfig_t IMUConfig;
16
volatile DynamicParams_t dynamicParams;
2025 - 17
 
2099 - 18
uint8_t CPUType;
19
uint8_t boardRelease;
1910 - 20
 
2099 - 21
VersionInfo_t versionInfo;
1910 - 22
 
2102 - 23
FlightMode_t currentFlightMode = FLIGHT_MODE_MANUAL;
2129 - 24
// updated by considering airspeed..
2132 - 25
volatile uint16_t isFlying = 0;
1910 - 26
 
2099 - 27
const MMXLATION XLATIONS[] = {
28
{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255},
2104 - 29
 
30
{offsetof(ParamSet_t, gyroPID[0].P), offsetof(DynamicParams_t, gyroPID[0].P),0,255},
31
{offsetof(ParamSet_t, gyroPID[0].I), offsetof(DynamicParams_t, gyroPID[0].I),0,255},
32
{offsetof(ParamSet_t, gyroPID[0].D), offsetof(DynamicParams_t, gyroPID[0].D),0,255},
33
 
34
{offsetof(ParamSet_t, gyroPID[1].P), offsetof(DynamicParams_t, gyroPID[1].P),0,255},
35
{offsetof(ParamSet_t, gyroPID[1].I), offsetof(DynamicParams_t, gyroPID[1].I),0,255},
36
{offsetof(ParamSet_t, gyroPID[1].D), offsetof(DynamicParams_t, gyroPID[1].D),0,255},
37
 
38
{offsetof(ParamSet_t, gyroPID[2].P), offsetof(DynamicParams_t, gyroPID[2].P),0,255},
39
{offsetof(ParamSet_t, gyroPID[2].I), offsetof(DynamicParams_t, gyroPID[2].I),0,255},
40
{offsetof(ParamSet_t, gyroPID[2].D), offsetof(DynamicParams_t, gyroPID[2].D),0,255},
41
 
2099 - 42
{offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255},
43
{offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255},
44
};
1910 - 45
 
2099 - 46
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) {
47
  uint8_t result;
48
  if (src>=(256-VARIABLE_COUNT)) result = variables[src-(256-VARIABLE_COUNT)];
49
  else result = src;
50
  if (result < min) result = min;
51
  else if (result > max) result = max;
52
  return result;
53
}
1910 - 54
 
2099 - 55
void configuration_applyVariablesToParams(void) {
56
  uint8_t i, src;
57
  uint8_t* pointerToTgt;
58
 
59
  for(i=0; i<sizeof(XLATIONS)/sizeof(MMXLATION); i++) {
60
    src = *((uint8_t*)(&staticParams) + XLATIONS[i].sourceIdx);
61
    pointerToTgt = (uint8_t*)(&dynamicParams) + XLATIONS[i].targetIdx;
62
    *pointerToTgt = configuration_applyVariableToParam(src, XLATIONS[i].min, XLATIONS[i].max);
1910 - 63
  }
2025 - 64
 
2099 - 65
  // User parameters are always variable.
66
  for (i=0; i<sizeof(staticParams.userParams); i++) {
67
    src = *((uint8_t*)(&staticParams) + offsetof(ParamSet_t, userParams) + i);
68
    pointerToTgt = (uint8_t*)(&dynamicParams) + offsetof(DynamicParams_t, userParams) + i;
69
    *pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
70
  }
1910 - 71
}
72
 
2099 - 73
void setCPUType(void) {   // works only after reset or power on when the registers have default values
74
  if((UCSR1A == 0x20) && (UCSR1C == 0x06)) CPUType = ATMEGA644P;  // initial Values for 644P after reset
75
  else CPUType = ATMEGA644;
1910 - 76
}
77
 
78
/*
79
 * Automatic detection of hardware components is not supported in this development-oriented
80
 * FC firmware. It would go against the point of it: To enable alternative hardware
81
 * configurations with otherwise unsupported components. Instead, one should write
82
 * custom code + adjust constants for the new hardware, and include the relevant code
83
 * from the makefile.
84
 * However - we still do detect the board release. Reason: Otherwise it would be too
85
 * tedious to have to modify the code for how to turn on and off LEDs when deploying
86
 * on different HW version....
87
 */
2099 - 88
void setBoardRelease(void) {
1910 - 89
  // the board release is coded via the pull up or down the 2 status LED
90
 
91
  PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate
92
  DDRB  &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input
93
 
94
  _delay_loop_2(1000); // make some delay
95
 
96
  switch( PINB & ((1<<PINB1)|(1<<PINB0))) {
97
    case 0x00:
2099 - 98
      boardRelease = 10; // 1.0
1910 - 99
      break;
100
    case 0x01:
2099 - 101
      boardRelease = 11; // 1.1 or 1.2
1910 - 102
      break;
103
    case 0x02:
2099 - 104
      boardRelease = 20; // 2.0
1910 - 105
      break;
106
    case 0x03:
2099 - 107
      boardRelease = 13; // 1.3
1910 - 108
      break;
109
    default:
110
      break;
111
    }
112
  // set LED ports as output
113
  DDRB |= (1<<DDB1)|(1<<DDB0);
2099 - 114
  RED_OFF;
1910 - 115
  GRN_OFF;
116
}
2099 - 117
 
2102 - 118
void configuration_setFlightParameters(uint8_t newFlightMode) {
2104 - 119
        currentFlightMode = newFlightMode;
120
        flight_updateFlightParametersToFlightMode();
2099 - 121
}
122
 
123
// Called after a change in configuration parameters, as a hook for modules to take over changes.
124
void configuration_paramSetDidChange(void) {
125
  // This should be OK to do here as long as we don't change parameters during emergency flight. We don't.
2102 - 126
  configuration_setFlightParameters(currentFlightMode);
2099 - 127
  // Immediately load changes to output, and also signal the paramset change.
128
  output_init();
129
}
130
 
131
void setOtherDefaults(void) {
132
  // Control
133
  staticParams.externalControl = 0;
134
  staticParams.IFactor = 52;
135
 
2142 - 136
  staticParams.airspeedCorrection = 1;
137
  staticParams.isFlyingThreshold = 10;
2109 - 138
 
2099 - 139
  // Servos
140
  staticParams.servoCount = 7;
2142 - 141
  staticParams.servos[CONTROL_ELEVATOR].reverse = 1;
142
  staticParams.servos[CONTROL_AILERONS].reverse = 0;
143
  staticParams.servos[CONTROL_RUDDER].reverse = 1;
2119 - 144
 
2099 - 145
  // Battery warning and emergency flight
2119 - 146
  staticParams.batteryWarningVoltage = 101;  // 3.7 each for 3S
2099 - 147
 
2104 - 148
  for (uint8_t i=0; i<3; i++) {
2129 - 149
          staticParams.gyroPID[i].P = 80;
2126 - 150
          staticParams.gyroPID[i].I = 40;
2104 - 151
          staticParams.gyroPID[i].D = 40;
2126 - 152
          staticParams.gyroPID[i].iMax = 45;
2104 - 153
  }
154
 
2126 - 155
  staticParams.stickIElevator = 40;
156
  staticParams.stickIAilerons = 60;
157
  staticParams.stickIRudder = 20;
2103 - 158
 
2099 - 159
  // Outputs
160
  staticParams.outputFlash[0].bitmask = 1; //0b01011111;
161
  staticParams.outputFlash[0].timing = 15;
162
  staticParams.outputFlash[1].bitmask = 3; //0b11110011;
163
  staticParams.outputFlash[1].timing = 15;
164
 
2126 - 165
  staticParams.outputDebugMask = 0;
2099 - 166
  staticParams.outputFlags   = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS;
167
}
168
 
169
/***************************************************/
170
/*    Default Values for parameter set 1           */
171
/***************************************************/
172
void paramSet_default(uint8_t setnumber) {
173
  setOtherDefaults();
174
 
175
  for (uint8_t i=0; i<8; i++) {
176
    staticParams.userParams[i] = i;
177
  }
178
 
179
  staticParams.bitConfig =
2104 - 180
    CFG_GYRO_SATURATION_PREVENTION;
2099 - 181
 
182
  memcpy(staticParams.name, "Default\0", 6);
183
}
184
 
185
void IMUConfig_default(void) {
2137 - 186
  IMUConfig.gyroPIDFilterConstant = 8;
2099 - 187
  IMUConfig.gyroDFilterConstant = 1;
188
  IMUConfig.rateTolerance = 120;
2142 - 189
  IMUConfig.gyroDWindowLength = 8;
2119 - 190
  IMUConfig.gyroQuadrant = 2;
2103 - 191
  IMUConfig.imuReversedFlags = 0;
2137 - 192
 
2099 - 193
  gyro_setDefaultParameters();
194
}
195
 
196
/***************************************************/
197
/*    Default Values for R/C Channels              */
198
/***************************************************/
199
void channelMap_default(void) {
2137 - 200
  channelMap.HWTrim = 178;
201
  channelMap.variableOffset = 128;
202
  channelMap.channels[CH_ELEVATOR] = 1;
2099 - 203
  channelMap.channels[CH_AILERONS] = 0;
204
  channelMap.channels[CH_THROTTLE] = 2;
205
  channelMap.channels[CH_RUDDER]   = 3;
206
  channelMap.channels[CH_POTS + 0] = 4;
207
  channelMap.channels[CH_POTS + 1] = 5;
208
  channelMap.channels[CH_POTS + 2] = 6;
209
  channelMap.channels[CH_POTS + 3] = 7;
210
}