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];