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