#include <stdlib.h>
#include "controlMixer.h"
#include "rc.h"
#include "attitude.h"
#include "externalControl.h"
#include "configuration.h"
#include "attitude.h"
#include "commands.h"
#include "output.h"
uint16_t maxControl
[2] = {0, 0};
uint16_t controlActivity
= 0;
int16_t control
[4] = {0, 0, 0, 0};
// int32_t controlIntegrals[4] = {0, 0, 0, 0};
// Internal variables for reading commands made with an R/C stick.
uint8_t lastCommand
= COMMAND_NONE
;
uint8_t lastArgument
;
uint8_t isCommandRepeated
= 0;
// MK flags. TODO: Replace by enum. State machine.
uint16_t isFlying
= 0;
volatile uint8_t MKFlags
= 0;
/*
* This could be expanded to take arguments from ohter sources than the RC
* (read: Custom MK RC project)
*/
uint8_t controlMixer_getArgument
(void) {
return lastArgument
;
}
/*
* This could be expanded to take calibrate / start / stop commands from ohter sources
* than the R/C (read: Custom MK R/C project)
*/
uint8_t controlMixer_getCommand
(void) {
return lastCommand
;
}
uint8_t controlMixer_isCommandRepeated
(void) {
return isCommandRepeated
;
}
void controlMixer_setNeutral
() {
//EC_setNeutral();
//HC_setGround();
}
/*
* Set the potientiometer values to the momentary values of the respective R/C channels.
* No slew rate limitation.
*/
void controlMixer_initVariables
(void) {
uint8_t i
;
for (i
= 0; i
< 8; i
++) {
variables
[i
] = RC_getVariable
(i
);
}
}
/*
* Update potentiometer values with limited slew rate. Could be made faster if desired.
* TODO: It assumes R/C as source. Not necessarily true.
*/
void controlMixer_updateVariables
(void) {
uint8_t i
;
int16_t targetvalue
;
for (i
= 0; i
< 8; i
++) {
targetvalue
= RC_getVariable
(i
);
if (targetvalue
< 0)
targetvalue
= 0;
if (variables
[i
] < targetvalue
&& variables
[i
] < 255)
variables
[i
]++;
else if (variables
[i
] > 0 && variables
[i
] > targetvalue
)
variables
[i
]--;
}
}
uint8_t controlMixer_getSignalQuality
(void) {
uint8_t rcQ
= RC_getSignalQuality
();
uint8_t ecQ
= EC_getSignalQuality
();
// This needs not be the only correct solution...
return rcQ
> ecQ
? rcQ
: ecQ
;
}
void updateControlAndMeasureControlActivity
(uint8_t index
, int16_t newValue
) {
int16_t tmp
= control
[index
];
// TODO: Scale by some factor. To be determined.
// controlIntegrals[index] += tmp * 4;
/*
if (controlIntegrals[index] > PITCHROLLOVER180) {
controlIntegrals[index] -= PITCHROLLOVER360;
} else if (controlIntegrals[index] <= -PITCHROLLOVER180) {
controlIntegrals[index] += PITCHROLLOVER360;
}
*/
control
[index
] = newValue
;
tmp
-= newValue
;
tmp
= tmp
* tmp
;
// tmp += (newValue >= 0) ? newValue : -newValue;
controlActivity
+= tmp
;
}
#define CADAMPING 10
void dampenControlActivity
(void) {
int32_t tmp
= controlActivity
;
tmp
*= ((1<<CADAMPING
)-1);
tmp
>>= CADAMPING
;
controlActivity
= tmp
;
}
/*
* Update the variables indicating stick position from the sum of R/C, GPS and external control.
*/
void controlMixer_update
(void) {
// calculate Stick inputs by rc channels (P) and changing of rc channels (D)
// TODO: If no signal --> zero.
uint8_t axis
;
RC_update
();
// EC_update();
// HC_update();
int16_t* RC_EATR
= RC_getEATR
();
// int16_t* EC_PRTY = EC_getPRTY();
updateControlAndMeasureControlActivity
(CONTROL_ELEVATOR
, RC_EATR
[CONTROL_ELEVATOR
] /* + EC_PRTY[CONTROL_PITCH] */);
updateControlAndMeasureControlActivity
(CONTROL_AILERONS
, RC_EATR
[CONTROL_AILERONS
] /* + EC_PRTY[CONTROL_ROLL] */);
updateControlAndMeasureControlActivity
(CONTROL_RUDDER
, RC_EATR
[CONTROL_RUDDER
] /* + EC_PRTY[CONTROL_YAW] */);
dampenControlActivity
();
// Do we also want to have activity measurement on throttle?
control
[CONTROL_THROTTLE
] = RC_EATR
[CONTROL_THROTTLE
]; // + EC_PRTY[CONTROL_THROTTLE]);
if (controlMixer_getSignalQuality
() >= SIGNAL_GOOD
) {
controlMixer_updateVariables
();
configuration_staticToDynamic
();
} else { // Signal is not OK
// Could handle switch to emergency flight here.
// throttle is handled elsewhere.
}
// part1a end.
/* This is not really necessary with the dead-band feature on all sticks (see rc.c)
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
if (controlYaw > 2) controlYaw-= 2;
else if (controlYaw< -2) controlYaw += 2;
else controlYaw = 0;
}
*/
/*
* Record maxima
*/
for (axis
= PITCH
; axis
<= ROLL
; axis
++) {
if (abs(control
[axis
]) > maxControl
[axis
]) {
maxControl
[axis
] = abs(control
[axis
]);
if (maxControl
[axis
] > 100)
maxControl
[axis
] = 100;
} else if (maxControl
[axis
])
maxControl
[axis
]--;
}
uint8_t rcCommand
= (RC_getSignalQuality
() >= SIGNAL_OK
) ? RC_getCommand
() : COMMAND_NONE
;
// uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE;
if (rcCommand
!= COMMAND_NONE
) {
isCommandRepeated
= (lastCommand
== rcCommand
);
lastCommand
= rcCommand
;
lastArgument
= RC_getArgument
();
} /*else if (ecCommand != COMMAND_NONE) {
isCommandRepeated = (lastCommand == ecCommand);
lastCommand = ecCommand;
lastArgument = EC_getArgument();
} */
else {
// Both sources have no command, or one or both are out.
// Just set to false. There is no reason to check if the none-command was repeated anyway.
isCommandRepeated
= 0;
lastCommand
= COMMAND_NONE
;
}
}
// TODO: Integrate into command system.
uint8_t controlMixer_testCompassCalState
(void) {
return RC_testCompassCalState
();
}