void processFlightCommands(void)
{
uint8_t channel;
if ( rcActive == true )
{
// Read receiver commands
for (channel = 0; channel < 8; channel++)
rxCommand[channel] = (float)pwmRead(systemConfig.rcMap[channel]);
rxCommand[ROLL] -= systemConfig.midCommand; // Roll Range -1000:1000
rxCommand[PITCH] -= systemConfig.midCommand; // Pitch Range -1000:1000
rxCommand[YAW] -= systemConfig.midCommand; // Yaw Range -1000:1000
rxCommand[THROTTLE] -= systemConfig.midCommand - MIDCOMMAND; // Throttle Range 2000:4000
rxCommand[AUX1] -= systemConfig.midCommand - MIDCOMMAND; // Aux1 Range 2000:4000
rxCommand[AUX2] -= systemConfig.midCommand - MIDCOMMAND; // Aux2 Range 2000:4000
rxCommand[AUX3] -= systemConfig.midCommand - MIDCOMMAND; // Aux3 Range 2000:4000
rxCommand[AUX4] -= systemConfig.midCommand - MIDCOMMAND; // Aux4 Range 2000:4000
}
// Set past command in detent values
for (channel = 0; channel < 3; channel++)
previousCommandInDetent[channel] = commandInDetent[channel];
// Apply deadbands and set detent discretes'
for (channel = 0; channel < 3; channel++)
{
if ((rxCommand[channel] <= DEADBAND) && (rxCommand[channel] >= -DEADBAND))
{
rxCommand[channel] = 0;
commandInDetent[channel] = true;
}
else
{
commandInDetent[channel] = false;
if (rxCommand[channel] > 0)
{
rxCommand[channel] = (rxCommand[channel] - DEADBAND) * DEADBAND_SLOPE;
}
else
{
rxCommand[channel] = (rxCommand[channel] + DEADBAND) * DEADBAND_SLOPE;
}
}
}
///////////////////////////////////
// Check for low throttle
if ( rxCommand[THROTTLE] < systemConfig.minCheck )
{
// Check for disarm command ( low throttle, left yaw ), will disarm immediately
if ( (rxCommand[YAW] < (systemConfig.minCheck - MIDCOMMAND)) && (armed == true) )
{
armed = false;
// Zero PID integrators when disarmed
zeroIntegralError();
zeroLastError();
}
// Check for gyro bias command ( low throttle, left yaw, aft pitch, right roll )
if ( (rxCommand[YAW ] < (systemConfig.minCheck - MIDCOMMAND)) &&
(rxCommand[ROLL ] > (systemConfig.maxCheck - MIDCOMMAND)) &&
(rxCommand[PITCH] < (systemConfig.minCheck - MIDCOMMAND)) )
{
computeGyroRTBias();
pulseMotors(3);
}
// Check for arm command ( low throttle, right yaw), must be present for 1 sec before arming
if ( (rxCommand[YAW] > (systemConfig.maxCheck - MIDCOMMAND) ) && (armed == false) )
{
armingTimer++;
if ( armingTimer > 50 )
{
zeroIntegralError();
armed = true;
armingTimer = 0;
}
}
else
{
armingTimer = 0;
}
}
// Check for armed true and throttle command > minThrottle
if ( (armed == true) && (rxCommand[THROTTLE] > systemConfig.minThrottle) )
holdIntegrators = false;
else
holdIntegrators = true;
// Check AUX1 for rate or attitude mode
if ( rxCommand[AUX1] > MIDCOMMAND )
{
flightMode = ATTITUDE;
}
else
{
flightMode = RATE;
}
}
void readSerialCommand() {
// Check for serial message
if (SERIAL_AVAILABLE()) {
queryType = SERIAL_READ();
switch (queryType) {
case 'A': // Receive roll and pitch rate mode PID
readSerialPID(RATE_XAXIS_PID_IDX);
readSerialPID(RATE_YAXIS_PID_IDX);
rotationSpeedFactor = readFloatSerial();
break;
case 'B': // Receive roll/pitch attitude mode PID
readSerialPID(ATTITUDE_XAXIS_PID_IDX);
readSerialPID(ATTITUDE_YAXIS_PID_IDX);
readSerialPID(ATTITUDE_GYRO_XAXIS_PID_IDX);
readSerialPID(ATTITUDE_GYRO_YAXIS_PID_IDX);
windupGuard = readFloatSerial(); // defaults found in setup() of AeroQuad.pde
break;
case 'C': // Receive yaw PID
readSerialPID(ZAXIS_PID_IDX);
readSerialPID(HEADING_HOLD_PID_IDX);
headingHoldConfig = readFloatSerial();
break;
case 'D': // Altitude hold PID
#if defined AltitudeHoldBaro || defined AltitudeHoldRangeFinder
readSerialPID(BARO_ALTITUDE_HOLD_PID_IDX);
PID[BARO_ALTITUDE_HOLD_PID_IDX].windupGuard = readFloatSerial();
altitudeHoldBump = readFloatSerial();
altitudeHoldPanicStickMovement = readFloatSerial();
minThrottleAdjust = readFloatSerial();
maxThrottleAdjust = readFloatSerial();
#if defined AltitudeHoldBaro
baroSmoothFactor = readFloatSerial();
#else
readFloatSerial();
#endif
readSerialPID(ZDAMPENING_PID_IDX);
#endif
break;
case 'E': // Receive sensor filtering values
aref = readFloatSerial();
minArmedThrottle = readFloatSerial();
break;
case 'F': // Receive transmitter smoothing values
receiverXmitFactor = readFloatSerial();
for(byte channel = XAXIS; channel<LASTCHANNEL; channel++) {
receiverSmoothFactor[channel] = readFloatSerial();
}
break;
case 'G': // Receive transmitter calibration values
channelCal = (int)readFloatSerial();
receiverSlope[channelCal] = readFloatSerial();
break;
case 'H': // Receive transmitter calibration values
channelCal = (int)readFloatSerial();
receiverOffset[channelCal] = readFloatSerial();
break;
case 'I': // Initialize EEPROM with default values
initializeEEPROM(); // defined in DataStorage.h
writeEEPROM();
storeSensorsZeroToEEPROM();
calibrateGyro();
zeroIntegralError();
#ifdef HeadingMagHold
initializeMagnetometer();
#endif
#ifdef AltitudeHoldBaro
initializeBaro();
#endif
break;
case 'J': // calibrate gyros
calibrateGyro();
break;
case 'K': // Write accel calibration values
accelScaleFactor[XAXIS] = readFloatSerial();
readFloatSerial();
accelScaleFactor[YAXIS] = readFloatSerial();
readFloatSerial();
accelScaleFactor[ZAXIS] = readFloatSerial();
readFloatSerial();
computeAccelBias();
storeSensorsZeroToEEPROM();
break;
case 'L': // generate accel bias
computeAccelBias();
#if defined(AeroQuadMega_CHR6DM) || defined(APM_OP_CHR6DM)
calibrateKinematics();
accelOneG = meterPerSecSec[ZAXIS];
#endif
storeSensorsZeroToEEPROM();
break;
case 'M': // calibrate magnetometer
#ifdef HeadingMagHold
magBias[XAXIS] = readFloatSerial();
magBias[YAXIS] = readFloatSerial();
magBias[ZAXIS] = readFloatSerial();
writeEEPROM();
#else
skipSerialValues(3);
#endif
break;
case 'N': // battery monitor
#ifdef BattMonitor
batteryMonitorAlarmVoltage = readFloatSerial();
batteryMonitorThrottleTarget = readFloatSerial();
batteryMonitorGoingDownTime = readFloatSerial();
setBatteryCellVoltageThreshold(batteryMonitorAlarmVoltage);
#else
skipSerialValues(3);
#endif
break;
case 'O': // define waypoints
#ifdef UseGPSNavigator
missionNbPoint = readIntegerSerial();
waypoint[missionNbPoint].latitude = readIntegerSerial();
waypoint[missionNbPoint].longitude = readIntegerSerial();
waypoint[missionNbPoint].altitude = readIntegerSerial();
#else
for(byte i = 0; i < 4; i++) {
readFloatSerial();
}
#endif
break;
case 'P': // read Camera values
#ifdef CameraControl
cameraMode = readFloatSerial();
servoCenterPitch = readFloatSerial();
servoCenterRoll = readFloatSerial();
servoCenterYaw = readFloatSerial();
mCameraPitch = readFloatSerial();
mCameraRoll = readFloatSerial();
mCameraYaw = readFloatSerial();
servoMinPitch = readFloatSerial();
servoMinRoll = readFloatSerial();
servoMinYaw = readFloatSerial();
servoMaxPitch = readFloatSerial();
servoMaxRoll = readFloatSerial();
servoMaxYaw = readFloatSerial();
#ifdef CameraTXControl
servoTXChannels = readFloatSerial();
#endif
#else
#ifdef CameraTXControl
skipSerialValues(14)
#else
skipSerialValues(13);
#endif
#endif
break;
case 'R': //Rotate
RotateDrone( readFloatSerial());
break;
case 'S':
StopMotors();
break;
case 'T':
ThrottleDrone(readFloatSerial());
break;
case 'U': // Range Finder
#if defined (AltitudeHoldRangeFinder)
maxRangeFinderRange = readFloatSerial();
minRangeFinderRange = readFloatSerial();
#else
skipSerialValues(2);
#endif
break;
case 'V': // GPS
#if defined (UseGPSNavigator)
readSerialPID(GPSROLL_PID_IDX);
readSerialPID(GPSPITCH_PID_IDX);
readSerialPID(GPSYAW_PID_IDX);
writeEEPROM();
#else
skipSerialValues(9);
#endif
break;
case 'W': // Write all user configurable values to EEPROM
writeEEPROM(); // defined in DataStorage.h
zeroIntegralError();
break;
case 'X': // Stop sending messages
break;
case '1': // Calibrate ESCS's by setting Throttle high on all channels
validateCalibrateCommand(1);
break;
case '2': // Calibrate ESC's by setting Throttle low on all channels
validateCalibrateCommand(2);
break;
case '3': // Test ESC calibration
if (validateCalibrateCommand(3)) {
testCommand = readFloatSerial();
}
break;
case '4': // Turn off ESC calibration
if (validateCalibrateCommand(4)) {
calibrateESC = 0;
testCommand = 1000;
}
break;
case '5': // Send individual motor commands (motor, command)
if (validateCalibrateCommand(5)) {
for (byte motor = 0; motor < LASTMOTOR; motor++) {
motorConfiguratorCommand[motor] = (int)readFloatSerial();
}
}
break;
case 'Z': // fast telemetry transfer <--- get rid if this?
if (readFloatSerial() == 1.0)
fastTransfer = ON;
else
fastTransfer = OFF;
break;
}
}
}
