Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
2108 - 1
#include <stdlib.h>
2
#include "controlMixer.h"
3
#include "rc.h"
4
#include "externalControl.h"
5
#include "failsafeControl.h"
6
#include "configuration.h"
7
#include "attitude.h"
8
#include "commands.h"
9
#include "output.h"
10
 
11
int16_t controls[4] = { 0, 0, 0, 0 };
12
 
13
// Internal variables for reading commands made with an R/C stick.
14
uint8_t lastCommand = COMMAND_NONE;
15
uint8_t lastArgument;
16
 
17
uint8_t isCommandRepeated = 0;
18
uint8_t controlMixer_didReceiveSignal = 0;
19
 
20
/*
21
 * This could be expanded to take arguments from ohter sources than the RC
22
 * (read: Custom MK RC project)
23
 */
24
uint8_t controlMixer_getArgument(void) {
25
  return lastArgument;
26
}
27
 
28
/*
29
 * This could be expanded to take calibrate / start / stop commands from ohter sources
30
 * than the R/C (read: Custom MK R/C project)
31
 */
32
uint8_t controlMixer_getCommand(void) {
33
  return lastCommand;
34
}
35
 
36
uint8_t controlMixer_isCommandRepeated(void) {
37
  return isCommandRepeated;
38
}
39
 
40
void controlMixer_setNeutral() {
41
  for (uint8_t i=0; i<VARIABLE_COUNT; i++) {
42
    variables[i] = RC_getVariable(i);
43
  }
44
  EC_setNeutral();
45
  FC_setNeutral();  // FC is FailsafeControl, not FlightCtrl.
46
}
47
 
48
/*
49
 * Update potentiometer values with limited slew rate. Could be made faster if desired.
50
 * TODO: It assumes R/C as source. Not necessarily true.
51
 */
52
void controlMixer_updateVariables(void) {
53
  uint8_t i;
54
  int16_t targetvalue;
55
  for (i=0; i < VARIABLE_COUNT; i++) {
56
    targetvalue = RC_getVariable(i);
2110 - 57
    // if (i<2) debugOut.analog[18+i] = targetvalue;
2108 - 58
    if (targetvalue < 0)
59
      targetvalue = 0;
60
    if (variables[i] < targetvalue && variables[i] < 255)
61
      variables[i]++;
62
    else if (variables[i] > 0 && variables[i] > targetvalue)
63
      variables[i]--;
64
  }
65
}
66
 
67
uint8_t controlMixer_getSignalQuality(void) {
68
  uint8_t rcQ = RC_getSignalQuality();
69
  uint8_t ecQ = EC_getSignalQuality();
70
 
71
  // This needs not be the only correct solution...
72
  return rcQ > ecQ ? rcQ : ecQ;
73
}
74
 
75
/*
76
void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
77
  int16_t tmp = controls[index];
78
  controls[index] = newValue;
79
 
80
  tmp -= newValue;
81
  tmp /= 2;
82
  tmp = tmp * tmp;
83
  // tmp += (newValue >= 0) ? newValue : -newValue;
84
 
85
  / *
86
  if (controlActivity + (uint16_t)tmp >= controlActivity)
87
    controlActivity += tmp;
88
  else controlActivity = 0xffff;
89
  * /
90
  if (controlActivity + (uint16_t)tmp < 0x8000)
91
    controlActivity += tmp;
92
}
93
 
94
#define CADAMPING 10
95
void dampenControlActivity(void) {
96
  uint32_t tmp = controlActivity;
97
  tmp *= ((1<<CADAMPING)-1);
98
  tmp >>= CADAMPING;
99
  controlActivity = tmp;
100
}
101
*/
102
 
103
/*
104
 * Update the variables indicating stick position from the sum of R/C, GPS and external control
105
 * and whatever other controls we invented in the meantime...
106
 * Update variables.
107
 * Decode commands but do not execute them.
108
 */
109
 
110
void controlMixer_periodicTask(void) {
111
  int16_t tempPRYT[4] = { 0, 0, 0, 0 };
112
 
113
  // Decode commands.
114
  uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE;
115
  uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE;
116
 
117
  // Update variables ("potis").
118
  if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
119
    controlMixer_updateVariables();
120
    controlMixer_didReceiveSignal = 1;
121
  } else { // Signal is not OK
122
    // Could handle switch to emergency flight here.
123
    // throttle is handled elsewhere.
124
  }
125
 
126
  if (rcCommand != COMMAND_NONE) {
127
    isCommandRepeated = (lastCommand == rcCommand);
128
    lastCommand = rcCommand;
129
    lastArgument = RC_getArgument();
130
  } else if (ecCommand != COMMAND_NONE) {
131
    isCommandRepeated = (lastCommand == ecCommand);
132
    lastCommand = ecCommand;
133
    lastArgument = EC_getArgument();
134
  } else {
135
    // Both sources have no command, or one or both are out.
136
    // Just set to false. There is no reason to check if the none-command was repeated anyway.
137
    isCommandRepeated = 0;
138
    lastCommand = COMMAND_NONE;
139
  }
140
 
141
  // This will init the values (not just add to them).
142
  RC_periodicTaskAndPRYT(tempPRYT);
143
 
144
  // Add external control to RC
145
  EC_periodicTaskAndPRYT(tempPRYT);
146
 
147
  FC_periodicTaskAndPRYT(tempPRYT);
148
 
149
  // Commit results to global variable and also measure control activity.
150
  controls[CONTROL_THROTTLE] = tempPRYT[CONTROL_THROTTLE];
151
  controls[CONTROL_ELEVATOR] = tempPRYT[CONTROL_ELEVATOR];
152
  controls[CONTROL_AILERONS] = tempPRYT[CONTROL_AILERONS];
153
  controls[CONTROL_RUDDER] =   tempPRYT[CONTROL_RUDDER];
154
  // dampenControlActivity();
155
 
156
  // We can safely do this even with a bad signal - the variables will not have been updated then.
157
  configuration_applyVariablesToParams();
158
 }