Subversion Repositories FlightCtrl

Rev

Rev 2132 | 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 
 
2142 - 11 
// The -1024 is an arbrtrary, very low value that will shut off the motor. This is default until the control mixer runs for the first time.
2132 - 12 
int16_t controls[4] = { 0, 0, 0, -1024 };
2108 - 13 
 
14 
// Internal variables for reading commands made with an R/C stick.
15 
uint8_t lastCommand = COMMAND_NONE;
16 
uint8_t lastArgument;
17 
 
18 
uint8_t isCommandRepeated = 0;
19 
uint8_t controlMixer_didReceiveSignal = 0;
20 
 
21 
/*
22 
 * This could be expanded to take arguments from ohter sources than the RC
23 
 * (read: Custom MK RC project)
24 
 */
25 
uint8_t controlMixer_getArgument(void) {
26 
  return lastArgument;
27 
}
28 
 
29 
/*
30 
 * This could be expanded to take calibrate / start / stop commands from ohter sources
31 
 * than the R/C (read: Custom MK R/C project)
32 
 */
33 
uint8_t controlMixer_getCommand(void) {
34 
  return lastCommand;
35 
}
36 
 
37 
uint8_t controlMixer_isCommandRepeated(void) {
38 
  return isCommandRepeated;
39 
}
40 
 
41 
void controlMixer_setNeutral() {
42 
  for (uint8_t i=0; i<VARIABLE_COUNT; i++) {
43 
    variables[i] = RC_getVariable(i);
44 
  }
45 
  EC_setNeutral();
46 
  FC_setNeutral();  // FC is FailsafeControl, not FlightCtrl.
47 
}
48 
 
49 
/*
50 
 * Update potentiometer values with limited slew rate. Could be made faster if desired.
51 
 * TODO: It assumes R/C as source. Not necessarily true.
52 
 */
53 
void controlMixer_updateVariables(void) {
54 
  uint8_t i;
55 
  int16_t targetvalue;
56 
  for (i=0; i < VARIABLE_COUNT; i++) {
57 
    targetvalue = RC_getVariable(i);
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) {
2132 - 111 
  int16_t tempPRYT[4] = { 0, 0, 0, RC_getZeroThrottle()};
2108 - 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 
 }