Subversion Repositories FlightCtrl

Rev

Rev 2089 | 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"
1612 dongfang 5
#include "externalControl.h"
2048 - 6
#include "compassControl.h"
2055 - 7
#include "failsafeControl.h"
2039 - 8
#include "naviControl.h"
1612 dongfang 9
#include "configuration.h"
2189 - 10
//#include "attitude.h"
1775 - 11
#include "commands.h"
2189 - 12
#include "debug.h"
13
#include "definitions.h"
1612 dongfang 14
 
1908 - 15
// uint16_t maxControl[2] = { 0, 0 };
2089 - 16
// uint16_t controlActivity = 0;
2189 - 17
int16_t controls[4];
1612 dongfang 18
 
19
// Internal variables for reading commands made with an R/C stick.
2189 - 20
uint8_t lastCommand;
1775 - 21
uint8_t lastArgument;
1612 dongfang 22
 
2189 - 23
uint8_t isCommandRepeated;
24
uint8_t controlMixer_didReceiveSignal;
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
 
2189 - 46
/*
47
 * TODO: This assumes R/C as source. Not necessarily true.
48
 */
49
void controlMixer_updateVariables(void) {
50
  uint8_t i;
51
  for (i=0; i < VARIABLE_COUNT; i++) {
52
    int16_t targetvalue = RC_getVariable(i);
53
    if (targetvalue < 0)
54
      variables[i] = 0;
55
    else if (targetvalue > 255)
56
      variables[i] = 255;
57
    else variables[i] = targetvalue;
58
  }
59
}
60
 
1775 - 61
void controlMixer_setNeutral() {
2189 - 62
  controlMixer_updateVariables();
1961 - 63
  EC_setNeutral();
64
  HC_setGround();
2055 - 65
  FC_setNeutral();  // FC is FailsafeControl, not FlightCtrl.
2058 - 66
 
67
  // This is to set the home pos in navi.
68
  // MKFlags |= MKFLAG_CALIBRATE;
1612 dongfang 69
}
70
 
71
uint8_t controlMixer_getSignalQuality(void) {
1961 - 72
  uint8_t rcQ = RC_getSignalQuality();
73
  uint8_t ecQ = EC_getSignalQuality();
1964 - 74
 
1961 - 75
  // This needs not be the only correct solution...
76
  return rcQ > ecQ ? rcQ : ecQ;
1612 dongfang 77
}
78
 
2089 - 79
/*
1908 - 80
void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
1961 - 81
  int16_t tmp = controls[index];
82
  controls[index] = newValue;
2017 - 83
 
1961 - 84
  tmp -= newValue;
85
  tmp /= 2;
86
  tmp = tmp * tmp;
87
  // tmp += (newValue >= 0) ? newValue : -newValue;
2089 - 88
 
89
  / *
1961 - 90
  if (controlActivity + (uint16_t)tmp >= controlActivity)
91
    controlActivity += tmp;
92
  else controlActivity = 0xffff;
2089 - 93
  * /
2055 - 94
  if (controlActivity + (uint16_t)tmp < 0x8000)
1986 - 95
    controlActivity += tmp;
1908 - 96
}
97
 
98
#define CADAMPING 10
99
void dampenControlActivity(void) {
1961 - 100
  uint32_t tmp = controlActivity;
101
  tmp *= ((1<<CADAMPING)-1);
102
  tmp >>= CADAMPING;
103
  controlActivity = tmp;
1908 - 104
}
2089 - 105
*/
1908 - 106
 
1612 dongfang 107
/*
2048 - 108
 * Update the variables indicating stick position from the sum of R/C, GPS and external control
109
 * and whatever other controls we invented in the meantime...
110
 * Update variables.
111
 * Decode commands but do not execute them.
1612 dongfang 112
 */
2048 - 113
 
2039 - 114
void controlMixer_periodicTask(void) {
2189 - 115
  int16_t tempRPTY[4] = { 0, 0, 0, 0 };
2053 - 116
 
2049 - 117
  // Decode commands.
118
  uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
119
    : COMMAND_NONE;
2073 - 120
 
2049 - 121
  uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
122
    : COMMAND_NONE;
123
 
124
  // Update variables ("potis").
125
  if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
126
    controlMixer_updateVariables();
127
    controlMixer_didReceiveSignal = 1;
128
  } else { // Signal is not OK
129
    // Could handle switch to emergency flight here.
130
    // throttle is handled elsewhere.
131
  }
132
 
133
  if (rcCommand != COMMAND_NONE) {
134
    isCommandRepeated = (lastCommand == rcCommand);
135
    lastCommand = rcCommand;
136
    lastArgument = RC_getArgument();
137
  } else if (ecCommand != COMMAND_NONE) {
138
    isCommandRepeated = (lastCommand == ecCommand);
139
    lastCommand = ecCommand;
140
    lastArgument = EC_getArgument();
2189 - 141
    if (ecCommand == COMMAND_GYROCAL) debugOut.digital[0] |= DEBUG_COMMAND;
142
    if (ecCommand == COMMAND_ACCCAL) debugOut.digital[1] |= DEBUG_COMMAND;
2049 - 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).
2189 - 151
  RC_periodicTaskAndRPTY(tempRPTY);
2045 - 152
 
2048 - 153
  // Add external control to RC
2189 - 154
  EC_periodicTaskAndRPTY(tempRPTY);
2039 - 155
 
2052 - 156
#ifdef USE_DIRECT_GPS
2088 - 157
  if (staticParams.bitConfig & (CFG_NAVI_ENABLED))
2189 - 158
    navigation_periodicTaskAndRPTY(tempRPTY);
2052 - 159
#endif
2039 - 160
 
2048 - 161
  // Add compass control (could also have been before navi, they are independent)
2189 - 162
  // CC_periodicTaskAndRPTY(tempRPTY);
2048 - 163
 
2189 - 164
  FC_periodicTaskAndRPTY(tempRPTY);
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)
2189 - 169
    HC_periodicTaskAndRPTY(tempRPTY);
2048 - 170
 
2055 - 171
    // Add attitude control (could also have been before navi and/or compass, they are independent)
2189 - 172
    // AC_getRPTY(tempRPTY);
2055 - 173
  }
174
 
2048 - 175
  // Commit results to global variable and also measure control activity.
2189 - 176
  controls[CONTROL_ROLL]     = tempRPTY[CONTROL_ROLL];
177
  controls[CONTROL_PITCH]    = tempRPTY[CONTROL_PITCH];
178
  controls[CONTROL_THROTTLE] = tempRPTY[CONTROL_THROTTLE];
179
  controls[CONTROL_YAW]      = tempRPTY[CONTROL_YAW];
180
 
181
  debugOut.analog[14] =controls[CONTROL_ROLL];
182
  debugOut.analog[15] =controls[CONTROL_PITCH];
1961 - 183
 
1980 - 184
  // We can safely do this even with a bad signal - the variables will not have been updated then.
185
  configuration_applyVariablesToParams();
1964 - 186
 }