/* =================================================
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
}
}
