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;
}
}
}
void SerialComm::ProcessData() {
MessageType code;
uint32_t mask;
if (!data_input.ParseInto(code, mask))
return;
if (code != MessageType::Command)
return;
uint32_t ack_data{0};
if (mask & COM_SET_EEPROM_DATA && data_input.ParseInto(config->raw)) {
writeEEPROM(); // TODO: deal with side effect code
ack_data |= COM_SET_EEPROM_DATA;
}
if (mask & COM_REINIT_EEPROM_DATA) {
initializeEEPROM(); // TODO: deal with side effect code
writeEEPROM(); // TODO: deal with side effect code
}
if (mask & COM_REQ_EEPROM_DATA) {
SendConfiguration();
ack_data |= COM_REQ_EEPROM_DATA;
}
if (mask & COM_REQ_ENABLE_ITERATION) {
uint8_t flag;
if (data_input.ParseInto(flag)) {
if (flag == 1)
state->processMotorEnablingIteration();
else
state->disableMotors();
ack_data |= COM_REQ_ENABLE_ITERATION;
}
}
// This should pass if any motor speed is set
if (mask & COM_MOTOR_OVERRIDE_SPEED_ALL) {
if (mask & COM_MOTOR_OVERRIDE_SPEED_0 && data_input.ParseInto(state->MotorOut[0]))
ack_data |= COM_MOTOR_OVERRIDE_SPEED_0;
if (mask & COM_MOTOR_OVERRIDE_SPEED_1 && data_input.ParseInto(state->MotorOut[1]))
ack_data |= COM_MOTOR_OVERRIDE_SPEED_1;
if (mask & COM_MOTOR_OVERRIDE_SPEED_2 && data_input.ParseInto(state->MotorOut[2]))
ack_data |= COM_MOTOR_OVERRIDE_SPEED_2;
if (mask & COM_MOTOR_OVERRIDE_SPEED_3 && data_input.ParseInto(state->MotorOut[3]))
ack_data |= COM_MOTOR_OVERRIDE_SPEED_3;
if (mask & COM_MOTOR_OVERRIDE_SPEED_4 && data_input.ParseInto(state->MotorOut[4]))
ack_data |= COM_MOTOR_OVERRIDE_SPEED_4;
if (mask & COM_MOTOR_OVERRIDE_SPEED_5 && data_input.ParseInto(state->MotorOut[5]))
ack_data |= COM_MOTOR_OVERRIDE_SPEED_5;
if (mask & COM_MOTOR_OVERRIDE_SPEED_6 && data_input.ParseInto(state->MotorOut[6]))
ack_data |= COM_MOTOR_OVERRIDE_SPEED_6;
if (mask & COM_MOTOR_OVERRIDE_SPEED_7 && data_input.ParseInto(state->MotorOut[7]))
ack_data |= COM_MOTOR_OVERRIDE_SPEED_7;
}
if (mask & COM_SET_COMMAND_OVERRIDE) {
uint8_t flag;
if (data_input.ParseInto(flag)) {
if (flag == 1)
state->set(STATUS_OVERRIDE);
else
state->clear(STATUS_OVERRIDE);
ack_data |= COM_SET_COMMAND_OVERRIDE;
}
}
if (mask & COM_SET_STATE_MASK) {
uint32_t new_state_mask;
if (data_input.ParseInto(new_state_mask)) {
SetStateMsg(new_state_mask);
ack_data |= COM_SET_STATE_MASK;
}
}
if (mask & COM_SET_STATE_DELAY) {
uint16_t new_state_delay;
if (data_input.ParseInto(new_state_delay)) {
send_state_delay = new_state_delay;
ack_data |= COM_SET_STATE_DELAY;
}
}
if (mask & COM_REQ_HISTORY) {
String eeprom_data;
for (size_t i = EEPROM_LOG_START; i < EEPROM_LOG_END; ++i)
eeprom_data += char(EEPROM[i]);
SendDebugString(eeprom_data, MessageType::HistoryData);
ack_data |= COM_REQ_HISTORY;
}
if (mask & COM_SET_LED) {
uint8_t mode, r1, g1, b1, r2, g2, b2, ind_r, ind_g;
if (data_input.ParseInto(mode, r1, g1, b1, r2, g2, b2, ind_r, ind_g)) {
led->set(LED::Pattern(mode), r1, g1, b1, r2, g2, b2, ind_r, ind_g);
ack_data |= COM_SET_LED;
}
}
if (mask & COM_REQ_RESPONSE) {
SendResponse(mask, ack_data);
}
}
void readEEPROM() {
initializeEEPROM();
}
