/* ================================================= FUNCTION: getIMU CREATED: 16-05-2014 DESCRIPTION: This main loop function for the IMU sensor suite PARAMETERS: vec GLOBAL VARIABLES: None. RETURNS: vec. AUTHOR: P. Kantue ================================================== */ void getIMU(int vec[], unsigned int time_el) { getMagnetometerData(magn); getAccelerometerData(acc); getGyroscopeData(gyro); convertTemp(gyro); if (bias_flag == true){ bias_flag = computeBias(acc,acc_bias,ACC_VAR,&acc_bias_count); bias_flag = computeBias(gyro,gyro_bias,GYRO_VAR,&gyro_bias_count); } else{ removeBias(acc,acc_bias,ACC_VAR); removeBias(gyro,gyro_bias,GYRO_VAR); setGravity(acc); // Eliminate Hard-Iron offsets magn[0] = magn[0] - MX_OFFSET; magn[1] = magn[1] - MY_OFFSET; // Compute Euler angles from acceleration raw values PitchandRoll(acc,temp_euler); // Apply LPF to both pitch and roll angles LPF_euler(Euler,temp_euler,EULER_LPF); // Compute tilt compensation for magnetic heading - NOT WORKING heading = tiltCompensation(magn,Euler); // Store IMU data to be passed to other subsystems storeIMU(acc,gyro,magn,Euler,heading,vec, time_el); //----ADD EXTRA CODE HERE ------------// } }
int main(void) { int16 acc[3]; int16 gyro[3]; int16 mag[3]; int16 temperature = 0; int32 pressure = 0; int32 centimeters = 0; i2c_master_enable(I2C1, I2C_FAST_MODE); delay(200); initAcc(); delay(200); initGyro(); delay(200); zeroCalibrateGyroscope(128,5); compassInit(false); compassCalibrate(1); compassSetMode(0); bmp085Calibration(); while(1) { getAccelerometerData(acc); //Read acceleration SerialUSB.print("Accel: "); SerialUSB.print(acc[0]); SerialUSB.print(" "); SerialUSB.print(acc[1]); SerialUSB.print(" "); SerialUSB.print(acc[2]); getGyroscopeData(gyro); //Read acceleration SerialUSB.print(" Gyro: "); SerialUSB.print(gyro[0]); SerialUSB.print(" "); SerialUSB.print(gyro[1]); SerialUSB.print(" "); SerialUSB.print(gyro[2]); getMagnetometerData(mag); //Read acceleration SerialUSB.print(" Mag: "); SerialUSB.print(mag[0]); SerialUSB.print(" "); SerialUSB.print(mag[1]); SerialUSB.print(" "); SerialUSB.print(mag[2]); temperature = bmp085GetTemperature(bmp085ReadUT()); pressure = bmp085GetPressure(bmp085ReadUP()); centimeters = bmp085GetAltitude(); SerialUSB.print(" Temp: "); SerialUSB.print(temperature, DEC); SerialUSB.print(" *0.1 deg C "); SerialUSB.print("Pressure: "); SerialUSB.print(pressure, DEC); SerialUSB.print(" Pa "); SerialUSB.print("Altitude: "); SerialUSB.print(centimeters, DEC); SerialUSB.print(" cm "); SerialUSB.println(" "); delay(100); } return 0; }
void sendSerialTelemetry() { switch (queryType) { case '=': // Reserved debug command to view any variable from Serial Monitor break; case 'a': // Send roll and pitch rate mode PID values PrintPID(RATE_XAXIS_PID_IDX); PrintPID(RATE_YAXIS_PID_IDX); PrintValueComma(rotationSpeedFactor); SERIAL_PRINTLN(); queryType = 'X'; break; case 'b': // Send roll and pitch attitude mode PID values PrintPID(ATTITUDE_XAXIS_PID_IDX); PrintPID(ATTITUDE_YAXIS_PID_IDX); PrintPID(ATTITUDE_GYRO_XAXIS_PID_IDX); PrintPID(ATTITUDE_GYRO_YAXIS_PID_IDX); SERIAL_PRINTLN(windupGuard); queryType = 'X'; break; case 'c': // Send yaw PID values PrintPID(ZAXIS_PID_IDX); PrintPID(HEADING_HOLD_PID_IDX); SERIAL_PRINTLN((int)headingHoldConfig); queryType = 'X'; break; case 'd': // Altitude Hold #if defined AltitudeHoldBaro || defined AltitudeHoldRangeFinder PrintPID(BARO_ALTITUDE_HOLD_PID_IDX); PrintValueComma(PID[BARO_ALTITUDE_HOLD_PID_IDX].windupGuard); PrintValueComma(altitudeHoldBump); PrintValueComma(altitudeHoldPanicStickMovement); PrintValueComma(minThrottleAdjust); PrintValueComma(maxThrottleAdjust); #if defined AltitudeHoldBaro PrintValueComma(baroSmoothFactor); #else PrintValueComma(0); #endif PrintPID(ZDAMPENING_PID_IDX); #else PrintDummyValues(10); #endif SERIAL_PRINTLN(); queryType = 'X'; break; case 'e': // miscellaneous config values PrintValueComma(aref); SERIAL_PRINTLN(minArmedThrottle); queryType = 'X'; break; case 'f': // Send transmitter smoothing values PrintValueComma(receiverXmitFactor); for (byte axis = XAXIS; axis < LASTCHANNEL; axis++) { PrintValueComma(receiverSmoothFactor[axis]); } PrintDummyValues(10 - LASTCHANNEL); SERIAL_PRINTLN(); queryType = 'X'; break; case 'g': // Send transmitter calibration data for (byte axis = XAXIS; axis < LASTCHANNEL; axis++) { Serial.print(receiverSlope[axis], 6); Serial.print(','); } SERIAL_PRINTLN(); queryType = 'X'; break; case 'h': // Send transmitter calibration data for (byte axis = XAXIS; axis < LASTCHANNEL; axis++) { Serial.print(receiverOffset[axis], 6); Serial.print(','); } SERIAL_PRINTLN(); queryType = 'X'; break; case 'i': // Send sensor data for (byte axis = XAXIS; axis <= ZAXIS; axis++) { PrintValueComma(gyroRate[axis]); } for (byte axis = XAXIS; axis <= ZAXIS; axis++) { PrintValueComma(filteredAccel[axis]); } for (byte axis = XAXIS; axis <= ZAXIS; axis++) { #if defined(HeadingMagHold) PrintValueComma(getMagnetometerData(axis)); #else PrintValueComma(0); #endif } SERIAL_PRINTLN(); break; case 'j': // Send raw mag values #ifdef HeadingMagHold PrintValueComma(getMagnetometerRawData(XAXIS)); PrintValueComma(getMagnetometerRawData(YAXIS)); SERIAL_PRINTLN(getMagnetometerRawData(ZAXIS)); #endif break; case 'k': // Send accelerometer cal values SERIAL_PRINT(accelScaleFactor[XAXIS], 6); comma(); SERIAL_PRINT(runTimeAccelBias[XAXIS], 6); comma(); SERIAL_PRINT(accelScaleFactor[YAXIS], 6); comma(); SERIAL_PRINT(runTimeAccelBias[YAXIS], 6); comma(); SERIAL_PRINT(accelScaleFactor[ZAXIS], 6); comma(); SERIAL_PRINTLN(runTimeAccelBias[ZAXIS], 6); queryType = 'X'; break; case 'l': // Send raw accel values measureAccelSum(); PrintValueComma((int)(accelSample[XAXIS]/accelSampleCount)); accelSample[XAXIS] = 0; PrintValueComma((int)(accelSample[YAXIS]/accelSampleCount)); accelSample[YAXIS] = 0; SERIAL_PRINTLN ((int)(accelSample[ZAXIS]/accelSampleCount)); accelSample[ZAXIS] = 0; accelSampleCount = 0; break; case 'm': // Send magnetometer cal values #ifdef HeadingMagHold SERIAL_PRINT(magBias[XAXIS], 6); comma(); SERIAL_PRINT(magBias[YAXIS], 6); comma(); SERIAL_PRINTLN(magBias[ZAXIS], 6); #endif queryType = 'X'; break; case 'n': // battery monitor #ifdef BattMonitor PrintValueComma(batteryMonitorAlarmVoltage); PrintValueComma(batteryMonitorThrottleTarget); PrintValueComma(batteryMonitorGoingDownTime); #else PrintDummyValues(3); #endif SERIAL_PRINTLN(); queryType = 'X'; break; case 'o': // send waypoints #ifdef UseGPSNavigator for (byte index = 0; index < MAX_WAYPOINTS; index++) { PrintValueComma(index); PrintValueComma(waypoint[index].latitude); PrintValueComma(waypoint[index].longitude); PrintValueComma(waypoint[index].altitude); } #else PrintDummyValues(4); #endif SERIAL_PRINTLN(); queryType = 'X'; break; case 'p': // Send Camera values #ifdef CameraControl PrintValueComma(cameraMode); PrintValueComma(servoCenterPitch); PrintValueComma(servoCenterRoll); PrintValueComma(servoCenterYaw); PrintValueComma(mCameraPitch); PrintValueComma(mCameraRoll); PrintValueComma(mCameraYaw); PrintValueComma(servoMinPitch); PrintValueComma(servoMinRoll); PrintValueComma(servoMinYaw); PrintValueComma(servoMaxPitch); PrintValueComma(servoMaxRoll); PrintValueComma(servoMaxYaw); #ifdef CameraTXControl PrintValueComma(servoTXChannels); #endif #else #ifdef CameraTXControl PrintDummyValues(14); #else PrintDummyValues(13); #endif #endif SERIAL_PRINTLN(); queryType = 'X'; break; case 'q': // Send Vehicle State Value SERIAL_PRINTLN(vehicleState); queryType = 'X'; break; case 'r': // Vehicle attitude PrintValueComma(kinematicsAngle[XAXIS]); PrintValueComma(kinematicsAngle[YAXIS]); SERIAL_PRINTLN(getHeading()); break; case 's': // Send all flight data PrintValueComma(motorArmed); PrintValueComma(kinematicsAngle[XAXIS]); PrintValueComma(kinematicsAngle[YAXIS]); PrintValueComma(getHeading()); #if defined AltitudeHoldBaro || defined AltitudeHoldRangeFinder #if defined AltitudeHoldBaro PrintValueComma(getBaroAltitude()); #elif defined AltitudeHoldRangeFinder PrintValueComma(rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX] != INVALID_RANGE ? rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX] : 0.0); #endif PrintValueComma((int)altitudeHoldState); #else PrintValueComma(0); PrintValueComma(0); #endif for (byte channel = 0; channel < 8; channel++) { // Configurator expects 8 values PrintValueComma((channel < LASTCHANNEL) ? receiverCommand[channel] : 0); } for (byte motor = 0; motor < LASTMOTOR; motor++) { PrintValueComma(motorCommand[motor]); } PrintDummyValues(8 - LASTMOTOR); // max of 8 motor outputs supported #ifdef BattMonitor PrintValueComma((float)batteryData[0].voltage/100.0); // voltage internally stored at 10mV:s #else PrintValueComma(0); #endif PrintValueComma(flightMode); SERIAL_PRINTLN(); break; case 't': // Send processed transmitter values for (byte axis = 0; axis < LASTCHANNEL; axis++) { PrintValueComma(receiverCommand[axis]); } SERIAL_PRINTLN(); break; case 'u': // Send range finder values #if defined (AltitudeHoldRangeFinder) PrintValueComma(maxRangeFinderRange); SERIAL_PRINTLN(minRangeFinderRange); #else PrintValueComma(0); SERIAL_PRINTLN(0); #endif queryType = 'X'; break; case 'v': // Send GPS PIDs #if defined (UseGPSNavigator) PrintPID(GPSROLL_PID_IDX); PrintPID(GPSPITCH_PID_IDX); PrintPID(GPSYAW_PID_IDX); queryType = 'X'; #else PrintDummyValues(9); #endif SERIAL_PRINTLN(); queryType = 'X'; break; case 'y': // send GPS info #if defined (UseGPS) PrintValueComma(gpsData.state); PrintValueComma(gpsData.lat); PrintValueComma(gpsData.lon); PrintValueComma(gpsData.height); PrintValueComma(gpsData.course); PrintValueComma(gpsData.speed); PrintValueComma(gpsData.accuracy); PrintValueComma(gpsData.sats); PrintValueComma(gpsData.fixtime); PrintValueComma(gpsData.sentences); PrintValueComma(gpsData.idlecount); #else PrintDummyValues(11); #endif SERIAL_PRINTLN(); break; case 'z': // Send all Altitude data #if defined (AltitudeHoldBaro) PrintValueComma(getBaroAltitude()); #else PrintValueComma(0); #endif #if defined (AltitudeHoldRangeFinder) SERIAL_PRINTLN(rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX]); #else SERIAL_PRINTLN(0); #endif break; case '$': // send BatteryMonitor voltage/current readings #if defined (BattMonitor) PrintValueComma((float)batteryData[0].voltage/100.0); // voltage internally stored at 10mV:s #if defined (BM_EXTENDED) PrintValueComma((float)batteryData[0].current/100.0); PrintValueComma((float)batteryData[0].usedCapacity/1000.0); #else PrintDummyValues(2); #endif #else PrintDummyValues(3); #endif SERIAL_PRINTLN(); break; case '%': // send RSSI #if defined (UseAnalogRSSIReader) || defined (UseEzUHFRSSIReader) || defined (UseSBUSRSSIReader) SERIAL_PRINTLN(rssiRawValue); #else SERIAL_PRINTLN(0); #endif break; case 'x': // Stop sending messages break; case '!': // Send flight software version SERIAL_PRINTLN(SOFTWARE_VERSION, 1); queryType = 'X'; break; case '#': // Send configuration reportVehicleState(); queryType = 'X'; break; case '6': // Report remote commands for (byte motor = 0; motor < LASTMOTOR; motor++) { PrintValueComma(motorCommand[motor]); } SERIAL_PRINTLN(); queryType = 'X'; break; #if defined(OSD) && defined(OSD_LOADFONT) case '&': // fontload if (OFF == motorArmed) { max7456LoadFont(); } queryType = 'X'; break; #endif } }