void readSerialPID(unsigned char PIDid) {
struct PIDdata* pid = &PID[PIDid];
pid->P = readFloatSerial();
pid->I = readFloatSerial();
pid->D = readFloatSerial();
pid->lastError = 0;
pid->integratedError = 0;
}
//***************************************************************************************************
//********************************** Serial Commands ************************************************
//***************************************************************************************************
bool validateCalibrateCommand(byte command)
{
if (readFloatSerial() == 123.45) {// use a specific float value to validate full throttle call is being sent
motorArmed = OFF;
calibrateESC = command;
return true;
}
else {
calibrateESC = 0;
testCommand = 1000;
return false;
}
}
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 skipSerialValues(byte number) {
for(byte i=0; i<number; i++) {
readFloatSerial();
}
}
/*
* Process incoming serial commands.
*/
void Telemetry::processSerialCommand(char cmd, PID controller[], AR6210 receiver) {
float gain;
float smoothFactor;
switch(cmd) {
case 'P': // fetch PID P value
Serial.print("Pgain=");
Serial.print(controller[ROLL_AXIS].getPGain());
Serial.print("/n");
break;
case 'p': // update PID P value
gain = readFloatSerial();
controller[ROLL_AXIS].setPGain(gain);
controller[PITCH_AXIS].setPGain(gain);
Serial.println("P gain changed");
break;
case 'I': // fetch PID I value
Serial.print("Igain=");
Serial.print(controller[ROLL_AXIS].getIGain());
Serial.print("/n");
break;
case 'i': // update PID I value
gain = readFloatSerial();
controller[ROLL_AXIS].setIGain(gain);
controller[PITCH_AXIS].setIGain(gain);
Serial.println("I gain changed");
break;
case 'D': // fetch PID D value
Serial.print("Dgain=");
Serial.print(controller[ROLL_AXIS].getDGain());
Serial.print("/n");
break;
case 'd': // update PID D values
gain = readFloatSerial();
controller[ROLL_AXIS].setDGain(gain);
controller[PITCH_AXIS].setDGain(gain);
Serial.println("D gain changed");
break;
case 's':
smoothFactor = readFloatSerial();
receiver.setSmoothFactor(smoothFactor);
Serial.println("Smoothfactor changed");
break;
case 'e':
break;
default:
break;
}
}
