Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1612 dongfang 1
#include <stdlib.h>
2
#include "controlMixer.h"
3
#include "rc.h"
1775 - 4
#include "heightControl.h"
5
#include "attitudeControl.h"
1612 dongfang 6
#include "externalControl.h"
2048 - 7
#include "compassControl.h"
2055 - 8
#include "failsafeControl.h"
2039 - 9
#include "naviControl.h"
1612 dongfang 10
#include "configuration.h"
11
#include "attitude.h"
1775 - 12
#include "commands.h"
13
#include "output.h"
1612 dongfang 14
 
1908 - 15
// uint16_t maxControl[2] = { 0, 0 };
16
uint16_t controlActivity = 0;
17
int16_t controls[4] = { 0, 0, 0, 0 };
1612 dongfang 18
 
19
// Internal variables for reading commands made with an R/C stick.
1821 - 20
uint8_t lastCommand = COMMAND_NONE;
1775 - 21
uint8_t lastArgument;
1612 dongfang 22
 
1775 - 23
uint8_t isCommandRepeated = 0;
1963 - 24
uint8_t controlMixer_didReceiveSignal = 0;
1775 - 25
 
1612 dongfang 26
/*
27
 * This could be expanded to take arguments from ohter sources than the RC
28
 * (read: Custom MK RC project)
29
 */
30
uint8_t controlMixer_getArgument(void) {
1961 - 31
  return lastArgument;
1612 dongfang 32
}
33
 
34
/*
35
 * This could be expanded to take calibrate / start / stop commands from ohter sources
36
 * than the R/C (read: Custom MK R/C project)
37
 */
38
uint8_t controlMixer_getCommand(void) {
1961 - 39
  return lastCommand;
1612 dongfang 40
}
41
 
42
uint8_t controlMixer_isCommandRepeated(void) {
1961 - 43
  return isCommandRepeated;
1612 dongfang 44
}
45
 
1775 - 46
void controlMixer_setNeutral() {
1961 - 47
  for (uint8_t i=0; i<VARIABLE_COUNT; i++) {
2055 - 48
    variables[i] = RC_getVariable(i);
1961 - 49
  }
50
  EC_setNeutral();
51
  HC_setGround();
2055 - 52
  FC_setNeutral();  // FC is FailsafeControl, not FlightCtrl.
2058 - 53
 
54
  // This is to set the home pos in navi.
55
  // MKFlags |= MKFLAG_CALIBRATE;
1612 dongfang 56
}
57
 
58
/*
59
 * Update potentiometer values with limited slew rate. Could be made faster if desired.
1775 - 60
 * TODO: It assumes R/C as source. Not necessarily true.
1612 dongfang 61
 */
62
void controlMixer_updateVariables(void) {
1961 - 63
  uint8_t i;
64
  int16_t targetvalue;
65
  for (i=0; i < VARIABLE_COUNT; i++) {
66
    targetvalue = RC_getVariable(i);
67
    if (targetvalue < 0)
68
      targetvalue = 0;
69
    if (variables[i] < targetvalue && variables[i] < 255)
70
      variables[i]++;
71
    else if (variables[i] > 0 && variables[i] > targetvalue)
72
      variables[i]--;
73
  }
1612 dongfang 74
}
75
 
76
uint8_t controlMixer_getSignalQuality(void) {
1961 - 77
  uint8_t rcQ = RC_getSignalQuality();
78
  uint8_t ecQ = EC_getSignalQuality();
1964 - 79
 
1961 - 80
  // This needs not be the only correct solution...
81
  return rcQ > ecQ ? rcQ : ecQ;
1612 dongfang 82
}
83
 
1908 - 84
void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
1961 - 85
  int16_t tmp = controls[index];
86
  controls[index] = newValue;
2017 - 87
 
1961 - 88
  tmp -= newValue;
89
  tmp /= 2;
90
  tmp = tmp * tmp;
91
  // tmp += (newValue >= 0) ? newValue : -newValue;
1986 - 92
  /*
1961 - 93
  if (controlActivity + (uint16_t)tmp >= controlActivity)
94
    controlActivity += tmp;
95
  else controlActivity = 0xffff;
1986 - 96
  */
2055 - 97
  if (controlActivity + (uint16_t)tmp < 0x8000)
1986 - 98
    controlActivity += tmp;
1908 - 99
}
100
 
101
#define CADAMPING 10
102
void dampenControlActivity(void) {
1961 - 103
  uint32_t tmp = controlActivity;
104
  tmp *= ((1<<CADAMPING)-1);
105
  tmp >>= CADAMPING;
106
  controlActivity = tmp;
1908 - 107
}
108
 
1612 dongfang 109
/*
2048 - 110
 * Update the variables indicating stick position from the sum of R/C, GPS and external control
111
 * and whatever other controls we invented in the meantime...
112
 * Update variables.
113
 * Decode commands but do not execute them.
1612 dongfang 114
 */
2048 - 115
 
2039 - 116
void controlMixer_periodicTask(void) {
2053 - 117
  int16_t tempPRTY[4] = { 0, 0, 0, 0 };
118
 
2049 - 119
  // Decode commands.
120
  uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
121
    : COMMAND_NONE;
2073 - 122
 
2049 - 123
  uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
124
    : COMMAND_NONE;
125
 
126
  // Update variables ("potis").
127
  if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
128
    controlMixer_updateVariables();
129
    controlMixer_didReceiveSignal = 1;
130
  } else { // Signal is not OK
131
    // Could handle switch to emergency flight here.
132
    // throttle is handled elsewhere.
133
  }
134
 
135
  if (rcCommand != COMMAND_NONE) {
136
    isCommandRepeated = (lastCommand == rcCommand);
137
    lastCommand = rcCommand;
138
    lastArgument = RC_getArgument();
139
  } else if (ecCommand != COMMAND_NONE) {
140
    isCommandRepeated = (lastCommand == ecCommand);
141
    lastCommand = ecCommand;
142
    lastArgument = EC_getArgument();
143
  } else {
144
    // Both sources have no command, or one or both are out.
145
    // Just set to false. There is no reason to check if the none-command was repeated anyway.
146
    isCommandRepeated = 0;
147
    lastCommand = COMMAND_NONE;
148
  }
1961 - 149
 
2048 - 150
  // This will init the values (not just add to them).
151
  RC_periodicTaskAndPRTY(tempPRTY);
2045 - 152
 
2048 - 153
  // Add external control to RC
154
  EC_periodicTaskAndPRTY(tempPRTY);
2039 - 155
 
2052 - 156
#ifdef USE_DIRECT_GPS
2058 - 157
  if (staticParams.bitConfig & (CFG_COMPASS_ENABLED))
158
    navigation_periodicTaskAndPRTY(tempPRTY);
2052 - 159
#endif
2039 - 160
 
2048 - 161
  // Add compass control (could also have been before navi, they are independent)
162
  CC_periodicTaskAndPRTY(tempPRTY);
163
 
2055 - 164
  FC_periodicTaskAndPRTY(tempPRTY);
2048 - 165
 
2058 - 166
  // This is temporary. There might be some emergency height control also.
167
  if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
2055 - 168
    // Add height control (could also have been before navi and/or compass, they are independent)
169
    HC_periodicTaskAndPRTY(tempPRTY);
2048 - 170
 
2055 - 171
    // Add attitude control (could also have been before navi and/or compass, they are independent)
172
    AC_getPRTY(tempPRTY);
173
  }
174
 
2048 - 175
  // Commit results to global variable and also measure control activity.
2051 - 176
  controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];
2048 - 177
  updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]);
178
  updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]);
179
  updateControlAndMeasureControlActivity(CONTROL_YAW, tempPRTY[CONTROL_YAW]);
1961 - 180
  dampenControlActivity();
181
 
1980 - 182
  // We can safely do this even with a bad signal - the variables will not have been updated then.
183
  configuration_applyVariablesToParams();
1961 - 184
 
185
  // part1a end.
186
 
187
  /* This is not really necessary with the dead-band feature on all sticks (see rc.c)
188
     if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
189
     if (controlYaw > 2) controlYaw-= 2;
190
     else if (controlYaw< -2) controlYaw += 2;
191
     else controlYaw = 0;
192
     }
193
  */
194
 
195
  /*
2048 - 196
   * Record maxima. Predecessor of the control activity stuff.
1961 - 197
   for (axis = PITCH; axis <= ROLL; axis++) {
198
   if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
199
   maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
200
   if (maxControl[axis] > 100)
201
   maxControl[axis] = 100;
202
   } else if (maxControl[axis])
203
   maxControl[axis]--;
204
   }
205
  */
1964 - 206
 }