Subversion Repositories FlightCtrl

Rev

Rev 2089 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2089 Rev 2189
Line 1... Line 1...
1 
#include <stdlib.h>
 1 
#include <stdlib.h>
2 
#include "controlMixer.h"
 2 
#include "controlMixer.h"
3 
#include "rc.h"
 3 
#include "rc.h"
4 
#include "heightControl.h"
 4 
#include "heightControl.h"
5 
#include "attitudeControl.h"
 - 
 
6 
#include "externalControl.h"
 5 
#include "externalControl.h"
7 
#include "compassControl.h"
 6 
#include "compassControl.h"
8 
#include "failsafeControl.h"
 7 
#include "failsafeControl.h"
9 
#include "naviControl.h"
 8 
#include "naviControl.h"
10 
#include "configuration.h"
 9 
#include "configuration.h"
11 
#include "attitude.h"
 10 
//#include "attitude.h"
12 
#include "commands.h"
 11 
#include "commands.h"
13 
#include "output.h"
 12 
#include "debug.h"
- 
 
 13 
#include "definitions.h"
Line 14... Line 14...
14 
 
 14 
 
15 
// uint16_t maxControl[2] = { 0, 0 };
 15 
// uint16_t maxControl[2] = { 0, 0 };
16 
// uint16_t controlActivity = 0;
 16 
// uint16_t controlActivity = 0;
Line 17... Line 17...
17 
int16_t controls[4] = { 0, 0, 0, 0 };
 17 
int16_t controls[4];
18 
 
 18 
 
19 
// Internal variables for reading commands made with an R/C stick.
 19 
// Internal variables for reading commands made with an R/C stick.
Line 20... Line 20...
20 
uint8_t lastCommand = COMMAND_NONE;
 20 
uint8_t lastCommand;
21 
uint8_t lastArgument;
 21 
uint8_t lastArgument;
Line 22... Line 22...
22 
 
 22 
 
23 
uint8_t isCommandRepeated = 0;
 23 
uint8_t isCommandRepeated;
24 
uint8_t controlMixer_didReceiveSignal = 0;
 24 
uint8_t controlMixer_didReceiveSignal;
25 
 
 25 
 
Line 41... Line 41...
41 
 
 41 
 
42 
uint8_t controlMixer_isCommandRepeated(void) {
 42 
uint8_t controlMixer_isCommandRepeated(void) {
43 
  return isCommandRepeated;
 43 
  return isCommandRepeated;
Line 44... Line -...
44 
}
 - 
 
45 
 
 - 
 
46 
void controlMixer_setNeutral() {
 - 
 
47 
  for (uint8_t i=0; i<VARIABLE_COUNT; i++) {
 - 
 
48 
    variables[i] = RC_getVariable(i);
 - 
 
49 
  }
 - 
 
50 
  EC_setNeutral();
 - 
 
51 
  HC_setGround();
 - 
 
52 
  FC_setNeutral();  // FC is FailsafeControl, not FlightCtrl.
 - 
 
53 
 
 - 
 
54 
  // This is to set the home pos in navi.
 - 
 
55 
  // MKFlags |= MKFLAG_CALIBRATE;
 - 
 
56 
}
 44 
}
57 
 
 - 
 
58 
/*
 45 
 
59 
 * Update potentiometer values with limited slew rate. Could be made faster if desired.
 46 
/*
60 
 * TODO: It assumes R/C as source. Not necessarily true.
 47 
 * TODO: This assumes R/C as source. Not necessarily true.
61 
 */
 48 
 */
62 
void controlMixer_updateVariables(void) {
 - 
 
63 
  uint8_t i;
 49 
void controlMixer_updateVariables(void) {
64 
  int16_t targetvalue;
 50 
  uint8_t i;
65 
  for (i=0; i < VARIABLE_COUNT; i++) {
 51 
  for (i=0; i < VARIABLE_COUNT; i++) {
66 
    targetvalue = RC_getVariable(i);
 52 
    int16_t targetvalue = RC_getVariable(i);
67 
    if (targetvalue < 0)
 53 
    if (targetvalue < 0)
68 
      targetvalue = 0;
 54 
      variables[i] = 0;
69 
    if (variables[i] < targetvalue && variables[i] < 255)
 55 
    else if (targetvalue > 255)
70 
      variables[i]++;
 - 
 
71 
    else if (variables[i] > 0 && variables[i] > targetvalue)
 56 
      variables[i] = 255;
72 
      variables[i]--;
 57 
    else variables[i] = targetvalue;
Line -... Line 58...
- 
 
 58 
  }
- 
 
 59 
}
- 
 
 60 
 
- 
 
 61 
void controlMixer_setNeutral() {
- 
 
 62 
  controlMixer_updateVariables();
- 
 
 63 
  EC_setNeutral();
- 
 
 64 
  HC_setGround();
- 
 
 65 
  FC_setNeutral();  // FC is FailsafeControl, not FlightCtrl.
- 
 
 66 
 
- 
 
 67 
  // This is to set the home pos in navi.
73 
  }
 68 
  // MKFlags |= MKFLAG_CALIBRATE;
74 
}
 69 
}
75 
 
 70 
 
Line 76... Line 71...
76 
uint8_t controlMixer_getSignalQuality(void) {
 71 
uint8_t controlMixer_getSignalQuality(void) {
Line 115... Line 110...
115 
 * Update variables.
 110 
 * Update variables.
116 
 * Decode commands but do not execute them.
 111 
 * Decode commands but do not execute them.
117 
 */
 112 
 */
Line 118... Line 113...
118 
 
 113 
 
119 
void controlMixer_periodicTask(void) {
 114 
void controlMixer_periodicTask(void) {
Line 120... Line 115...
120 
  int16_t tempPRTY[4] = { 0, 0, 0, 0 };
 115 
  int16_t tempRPTY[4] = { 0, 0, 0, 0 };
121 
 
 116 
 
122 
  // Decode commands.
 117 
  // Decode commands.
Line 141... Line 136...
141 
    lastArgument = RC_getArgument();
 136 
    lastArgument = RC_getArgument();
142 
  } else if (ecCommand != COMMAND_NONE) {
 137 
  } else if (ecCommand != COMMAND_NONE) {
143 
    isCommandRepeated = (lastCommand == ecCommand);
 138 
    isCommandRepeated = (lastCommand == ecCommand);
144 
    lastCommand = ecCommand;
 139 
    lastCommand = ecCommand;
145 
    lastArgument = EC_getArgument();
 140 
    lastArgument = EC_getArgument();
- 
 
 141 
    if (ecCommand == COMMAND_GYROCAL) debugOut.digital[0] |= DEBUG_COMMAND;
- 
 
 142 
    if (ecCommand == COMMAND_ACCCAL) debugOut.digital[1] |= DEBUG_COMMAND;
146 
  } else {
 143 
  } else {
147 
    // Both sources have no command, or one or both are out.
 144 
    // Both sources have no command, or one or both are out.
148 
    // Just set to false. There is no reason to check if the none-command was repeated anyway.
 145 
    // Just set to false. There is no reason to check if the none-command was repeated anyway.
149 
    isCommandRepeated = 0;
 146 
    isCommandRepeated = 0;
150 
    lastCommand = COMMAND_NONE;
 147 
    lastCommand = COMMAND_NONE;
151 
  }
 148 
  }
Line 152... Line 149...
152 
 
 149 
 
153 
  // This will init the values (not just add to them).
 150 
  // This will init the values (not just add to them).
Line 154... Line 151...
154 
  RC_periodicTaskAndPRTY(tempPRTY);
 151 
  RC_periodicTaskAndRPTY(tempRPTY);
155 
 
 152 
 
Line 156... Line 153...
156 
  // Add external control to RC
 153 
  // Add external control to RC
157 
  EC_periodicTaskAndPRTY(tempPRTY);
 154 
  EC_periodicTaskAndRPTY(tempRPTY);
158 
 
 155 
 
159 
#ifdef USE_DIRECT_GPS
 156 
#ifdef USE_DIRECT_GPS
Line 160... Line 157...
160 
  if (staticParams.bitConfig & (CFG_NAVI_ENABLED))
 157 
  if (staticParams.bitConfig & (CFG_NAVI_ENABLED))
161 
    navigation_periodicTaskAndPRTY(tempPRTY);
 158 
    navigation_periodicTaskAndRPTY(tempRPTY);
162 
#endif
 - 
 
163 
 
 - 
 
Line -... Line 159...
- 
 
 159 
#endif
- 
 
 160 
 
164 
  // Add compass control (could also have been before navi, they are independent)
 161 
  // Add compass control (could also have been before navi, they are independent)
165 
  CC_periodicTaskAndPRTY(tempPRTY);
 162 
  // CC_periodicTaskAndRPTY(tempRPTY);
166 
 
 163 
 
167 
  FC_periodicTaskAndPRTY(tempPRTY);
 164 
  FC_periodicTaskAndRPTY(tempRPTY);
Line 168... Line 165...
168 
 
 165 
 
169 
  // This is temporary. There might be some emergency height control also.
 166 
  // This is temporary. There might be some emergency height control also.
170 
  if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
 167 
  if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
Line 171... Line 168...
171 
    // Add height control (could also have been before navi and/or compass, they are independent)
 168 
    // Add height control (could also have been before navi and/or compass, they are independent)
172 
    HC_periodicTaskAndPRTY(tempPRTY);
 169 
    HC_periodicTaskAndRPTY(tempRPTY);
173 
 
 170 
 
174 
    // Add attitude control (could also have been before navi and/or compass, they are independent)
 171 
    // Add attitude control (could also have been before navi and/or compass, they are independent)
175 
    AC_getPRTY(tempPRTY);
 172 
    // AC_getRPTY(tempRPTY);
- 
 
 173 
  }
176 
  }
 174 
 
- 
 
 175 
  // Commit results to global variable and also measure control activity.
Line 177... Line 176...
177 
 
 176 
  controls[CONTROL_ROLL]     = tempRPTY[CONTROL_ROLL];
178 
  // Commit results to global variable and also measure control activity.
 177 
  controls[CONTROL_PITCH]    = tempRPTY[CONTROL_PITCH];
179 
  controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];
 178 
  controls[CONTROL_THROTTLE] = tempRPTY[CONTROL_THROTTLE];