I am building a student satellite package to be used in rockets and high altitude balloons. It integrates a bunch of sensors (IMU, barometer, GPS), stores the data locally and reports telemetry via either cellular (Electron - when planned altitude up to about 5k’), WiFi (Photon, when altitude up to about 30k’ and a more specialized radio for HAB and sounding rockets.
I originally built the package for Electron, then moved it (for OTA debug cost reasons) to Photon to finish some debugging. 98% of the package worked on Electron (all but the GPS) and it now works on the Photon just fine (other than some compile time differences (battery status, antenna choice, signal quality). When I move back to the Electron - it loads OTA and then gives a red SOS pattern from the system LED - reboots (system LED white?) - cycles thru blinking green, a short period of breathing cyan and back to red SOS.
Program runs on Photon. When I recycle the Electron thru safe mode, I can run small other programs on the Electron (GPS test program for example) but when I reload this program it ends up in SOS mode again.
Any thoughts? Program below.
Ken
/*****************************************************************
This is the core code of the 2016 S4 (Small Satelleites for Seconddary
Students) Particle (either Photon or Electron) based payload.
Ken Biba
9DOF sensor I2C
Baro Sensor I2C
Serial 0 (USB) - diagnostic
Serial 1 - GPS
Serial 2 - Storage
On Photon telemetry is via WiFi, on Electron cellular
Optional for LoRa radio
Save to storage happens at 10 Hz
Telemetry is currently 1 Hz based on Particle Cloud limitations of 1/event/second
*****************************************************************/
#include "SparkFunLSM9DS1/SparkFunLSM9DS1.h"
#include "math.h"
#include "SparkFun_MPL3115A2/SparkFun_MPL3115A2.h"
#include "Serial4/Serial4.h"
#include "SparkFunMAX17043/SparkFunMAX17043.h" // Include the SparkFun MAX17043 library
#include "TinyGPS++/TinyGPS++.h"
// Mode
#define DIAG TRUE // Print to serial port diagnostic stream
#define ELECTRON
// Communication Configuration
#define CLOUD TRUE // Tranmit to Particle cloud
#define WIFI FALSE // WiFi enable?
#define CELL TRUE // Cellular enable?
#define LORA FALSE // LoRa enable? LoRa can be enabled in parallel with either WiFi or Cellular
#define SAVE FALSE // Local storage enable?
// Sensors configuration
#define IMU TRUE // Photon IMU sensors enabled
#define BARO TRUE // Baro sensor enabled
#define GPS TRUE // GPS sensor enabled
#define POWER TRUE // Battery sensors enabled
#define STORAGE FALSE // Local storage enabled
#define BAROMODE TRUE // Barometer mode. False is altimeter mode
#define BAROSAMPLE 3 // Barometer oversampling rate. Major overhead if much higher
#define STORAGESPEED 38400 // Speed of serial interface to storage for microSD card
#define STORAGETIME 100 // Time between state storage
#define GPSSPEED 9600 // Serial speed to GPS
#define TELEMETRYTIME 1000 // Milliseconds between telemetry messages. For Electron at 1/second it is 1000.
#define FLIGHTTIME 500 // Milliseconds between flashes. Blue flash indicates 3D GPS lock and ready for flight.
#define FLASHTIME 1000 // LED flash interval in milliseconds
#define LED 7
#ifdef PHOTON
#define SSID "S4" // SSID of WiFi access point when used with Photon
#endif
//////////////////////////
// LSM9DS1 Library Init //
//////////////////////////
// Use the LSM9DS1 class to create an object. [imu] can be
// named anything, we'll refer to that throught the sketch.
LSM9DS1 imu;
///////////////////////
// Example I2C Setup //
///////////////////////
// SDO_XM and SDO_G are both pulled high, so our addresses are:
#define LSM9DS1_M 0x1E // Would be 0x1C if SDO_M is LOW
#define LSM9DS1_AG 0x6B // Would be 0x6A if SDO_AG is LOW
////////////////////////////
// 9DOF Output Settings //
////////////////////////////
#define PRINT_CALCULATED
// Earth's magnetic field varies by location. Add or subtract
// a declination to get a more accurate heading. Calculate
// your's here:
// http://www.ngdc.noaa.gov/geomag-web/#declination
#define DECLINATION 13.67 // Declination (degrees) in San Francisco, CA
// Create instance of baro sensor
MPL3115A2 baro = MPL3115A2(); //create instance of MPL3115A2 barrometric sensor
#ifdef ELECTRON
// Create instance of fuel gauage for the Electron
FuelGauge fuel;
#endif
#ifdef PHOTON
// Create instance of fuel gauge for the Photon with Sparkfun Battery Shield
#endif
// State vector of the pocketQube
struct pocketQube {
bool storeEnable = true;
bool telemetryEnable = true;
int year;
byte month;
byte day;
byte hour;
byte minute;
byte second;
long milli;
CellularSignal sig;
float voltage;
float soc;
float pressure;
float temp;
float palt;
float lat = -999.99;
float lon = -999.99;
float galt;
int sats = 0;
bool gpsLock = false;
float accelX;
float accelY;
float accelZ;
float gyroX;
float gyroY;
float gyroZ;
float magX;
float magY;
float magZ;
float roll;
float pitch;
float heading;
String buffer = "";
long lastTime = 0;
int lostPackets = 0;
bool ledState = false;
// Other sensors
} pq;
void diagnostic (String message) {
String debug;
if (DIAG) {
debug = millis();
debug.concat(": ");
debug.concat(message);
Serial.println(debug);
}
}
// GPS Sensor
// The TinyGPS++ object
TinyGPSPlus gps;
bool initGPS() {
Serial1.begin(GPSSPEED);
pq.year = Time.year();
pq.month = Time.month();
pq.day = Time.day();
pq.hour = Time.hour();
pq.minute = Time.minute();
pq.second = Time.second();
return(TRUE);
}
void getGPS() {
// Process GPS strings, decode and store in state vector
pq.gpsLock = false;
while (Serial1.available()) gps.encode(Serial1.read());
if (gps.location.isValid()) {
pq.sats = gps.satellites.value();
pq.lat = gps.location.lat();
pq.lon = gps.location.lng();
}
if (gps.altitude.isValid()) {
pq.galt = gps.altitude.meters();
}
if (gps.date.isValid()) {
pq.year = gps.date.year();
pq.month = gps.date.month();
pq.day = gps.date.day();
}
if (gps.time.isValid()) {
pq.hour = gps.time.hour();
pq.minute = gps.time.minute();
pq.second = gps.time.second();
}
pq.milli = millis();
if (pq.sats >= 4) pq.gpsLock = true;
}
// This function formats the pocketQube state vector as a string.
String formatCellular() {
// convert PocketQube state into a string
pq.buffer ="";
pq.buffer.concat(String(pq.milli)); pq.buffer.concat(":");
// Telemetry signal data
pq.buffer.concat(String(pq.sig.rssi)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.sig.qual)); pq.buffer.concat(",");
// Power data
pq.buffer.concat(String(pq.voltage,1)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.soc,0)); pq.buffer.concat(",");
// Location data - lat,lon, alt
pq.buffer.concat(String(pq.lat,5)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.lon,5)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.galt,1)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.sats)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gpsLock)); pq.buffer.concat(",");
// Baro data - pressure, palt, temp
pq.buffer.concat(String(pq.pressure,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.palt,1)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.temp,1)); pq.buffer.concat(",");
// IMU Data
pq.buffer.concat(String(pq.magX,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.magY,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.magZ,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.accelX,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.accelY,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.accelZ,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gyroX,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gyroY,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gyroZ,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.roll,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.pitch,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.heading,2)); pq.buffer.concat(",");
return (pq.buffer);
}
String formatWiFi() {
// convert PocketQube state into a string
pq.buffer ="";
pq.buffer.concat(String(pq.milli)); pq.buffer.concat(":");
// Telemetry signal data
pq.buffer.concat(String(pq.sig.rssi)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.sig.qual)); pq.buffer.concat(",");
// Power data
pq.buffer.concat(String(pq.voltage,1)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.soc,0)); pq.buffer.concat(",");
// Location data - lat,lon, alt
pq.buffer.concat(String(pq.lat,5)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.lon,5)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.galt,1)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.sats)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gpsLock)); pq.buffer.concat(",");
// Baro data - pressure, palt, temp
pq.buffer.concat(String(pq.pressure,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.palt,1)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.temp,1)); pq.buffer.concat(",");
// IMU Data
pq.buffer.concat(String(pq.magX,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.magY,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.magZ,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.accelX,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.accelY,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.accelZ,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gyroX,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gyroY,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gyroZ,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.roll,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.pitch,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.heading,2)); pq.buffer.concat(",");
return (pq.buffer);
}
String formatLoRa() {
pq.buffer ="";
pq.buffer.concat(String(pq.milli)); pq.buffer.concat(":");
// Telemetry signal data
pq.buffer.concat(String(pq.sig.rssi)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.sig.qual)); pq.buffer.concat(",");
// Power data
pq.buffer.concat(String(pq.voltage,1)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.soc,0)); pq.buffer.concat(",");
// Location data - lat,lon, alt
pq.buffer.concat(String(pq.lat,5)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.lon,5)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.galt,1)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.sats)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gpsLock)); pq.buffer.concat(",");
// Baro data - pressure, palt, temp
pq.buffer.concat(String(pq.pressure,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.palt,1)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.temp,1)); pq.buffer.concat(",");
// IMU Data
pq.buffer.concat(String(pq.magX,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.magY,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.magZ,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.accelX,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.accelY,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.accelZ,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gyroX,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gyroY,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gyroZ,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.roll,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.pitch,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.heading,2)); pq.buffer.concat(",");
return (pq.buffer);
}
String formatStorage() {
// convert PocketQube state into a string
pq.buffer ="";
pq.buffer.concat(String(pq.milli)); pq.buffer.concat(":");
pq.buffer.concat(String(pq.lostPackets)); pq.buffer.concat(",");
// Telemetry signal data
pq.buffer.concat(String(pq.sig.rssi)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.sig.qual)); pq.buffer.concat(",");
// Power data
pq.buffer.concat(String(pq.voltage,1)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.soc,0)); pq.buffer.concat(",");
// Location data - lat,lon, alt
pq.buffer.concat(String(pq.lat,3)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.lon,3)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.galt,1)); pq.buffer.concat(",");
// Baro data - pressure, palt, temp
pq.buffer.concat(String(pq.pressure,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.palt,1)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.temp,1)); pq.buffer.concat(",");
// IMU Data
pq.buffer.concat(String(pq.magX,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.magY,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.magZ,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.accelX,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.accelY,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.accelZ,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gyroX,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gyroY,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.gyroZ,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.roll,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.pitch,2)); pq.buffer.concat(",");
pq.buffer.concat(String(pq.heading,2)); pq.buffer.concat(",");
return (pq.buffer);
}
// Local storage
void saveStorage () {
// Save PocketQube state in local storage
// Use the local Photon storage initially
// Use external I2C storage as possible
// Use either external solid state storage or microSD as file system on Serial4
if (SAVE && pq.storeEnable) {
pq.buffer = formatStorage();
Serial2.println(pq.buffer);
}
diagnostic("Save executed");
}
bool initStorage () {
// Initialize the local storage
if (SAVE) Serial2.begin(STORAGESPEED);
return (TRUE);
}
void sendTelemetry () {
// Send current PocketQube state via either/both the Cloud or the LoRa modem
// Almost all telemetry channels (except WiFi) have limits on how fast the telemetry channel is
// We throttle the speed of the telemetry channel via the TELEMETRYRATE parameter
flashLED();
getGPS();
#ifdef ELECTRON
if (pq.telemetryEnable) {
// Format and send a cellular frame
pq.buffer = formatCellular();
Particle.publish("telemetry", pq.buffer);
diagnostic(pq.buffer);
pq.lastTime = millis();
};
#endif
if (LORA && pq.telemetryEnable) {
// Format and sent a LoRa frame
pq.buffer = formatLoRa();
sendLoRa(pq.buffer);
diagnostic(pq.buffer);
pq.lastTime = millis();
};
#ifdef PHOTON
if (pq.telemetryEnable) {
// Format and send a WiFi frame
pq.buffer = formatWiFi();
Particle.publish("telemetry", pq.buffer);
diagnostic(pq.buffer);
pq.lastTime = millis();
}
#endif
flashLED();
}
bool initTelemetry() {
pq.lastTime = 0;
if (LORA) initLoRa();
if (CLOUD) initCloud();
return (TRUE);
}
// Initialize Clourd (WiFi or Cellular) Particle connections
bool initCloud () {
// Initialize WiFi and/or cell Cloud connections
#ifdef PHOTON
WiFi.selectAntenna(ANT_AUTO);
#endif
Particle.variable("pqstate", pq.buffer);
Particle.function("storeEnable", storeToggle);
Particle.function("telmtyEnable", telemetryToggle);
return(TRUE);
}
// Initialize LoRa modem
// Find a LoRa mode that gives a couple kb/s of performance and send 1 message/second.
bool initLoRa () {
// Initialize LoRa modem
return (TRUE);
}
bool sendLoRa (String message) {
return (TRUE);
}
// Baro sensor functions
// Initialize baro/temperature sensor
bool initBaro() {
//Initialize
if (BARO) {
if (!baro.begin()) return (FALSE);
//MPL3115A2 Settings
if (BAROMODE) baro.setModeBarometer(); //Set to Barometer Mode
else baro.setModeAltimeter();
baro.setOversampleRate(BAROSAMPLE); // Set Oversample to the recommended 128
baro.enableEventFlags(); //Necessary register calls to enble temp, baro ansd alt
return(TRUE);
}
return (TRUE);
}
// Get pressure/temperature data from baro sensor into PQ state vector
void getBaro () {
if (BARO) {
pq.pressure = baro.readPressure();
pq.palt = baro.readAltitude();
pq.temp = baro.readTemp();
}
}
// Fuel Gauge functions
// Get power levels from battery system into PQ state vector
void getPower() {
#ifdef ELECTRON
pq.voltage = fuel.getVCell();
pq.soc = fuel.getSoC();
#endif
#ifdef PHOTON
// lipo.getVoltage() returns a voltage value (e.g. 3.93)
pq.voltage = lipo.getVoltage();
// lipo.getSOC() returns the estimated state of charge (e.g. 79%)
pq.soc = lipo.getSOC();
#endif
}
void initPower() {
#ifdef ELECTRON
#endif
#ifdef PHOTON
// Set up the MAX17043 LiPo fuel gauge:
lipo.begin(); // Initialize the MAX17043 LiPo fuel gauge
// Quick start restarts the MAX17043 in hopes of getting a more accurate
// guess for the SOC.
lipo.quickStart();
#endif
}
// Get RF signal levels into PQ state vector
void getSignalQuality() {
#ifdef ELECTRON
pq.sig = Cellular.RSSI();
#endif
#ifdef PHOTON
pq.sig.rssi = WiFi.RSSI();
#endif
if (LORA) ;
}
// Flash system LED to show this program in control
void flashLED () {
if (!pq.ledState) {
pq.ledState = true;
if (pq.gpsLock) digitalWrite(LED, HIGH);
} else {
pq.ledState = false;
digitalWrite(LED, LOW);
}
return;
}
// Timer ledFlash(FLASHTIME, flashLED);
Timer telemetryTimer(TELEMETRYTIME, sendTelemetry);
Timer flightReady(FLIGHTTIME, flashLED);
Timer saveTimer(STORAGETIME, saveStorage);
void setup()
{
// Initialize sensors
if (!initIMU()) diagnostic("IMU sensor failure");
if (!initBaro()) diagnostic("Baro sensor failure");
// Initialize local storage (if any)
if (!initStorage()) diagnostic("Storage failure");
// Initialize GPS
if (!initGPS()) diagnostic("GPS failure");
// Initialize telemetry
if (!initTelemetry()) diagnostic("Telemetry failure");
Serial.begin(115200);
pinMode(LED, OUTPUT);
telemetryTimer.start(); // Start telemetry timer to send telemetry periodically
flightReady.start(); // Start LED with flight status based on 3D GPS lock
saveTimer.start(); // Start storage save timer
}
void loop()
{
// Get sensor values into pocketQube state vector
pq.milli = millis();
getGPS();
displayGPSInfo();
// Get IMU sensor values
getGyro(); // Get gyro values
getAccel(); // Get acclerometer values
getMag(); // Get magnetometer values
// Print the heading and orientation for fun!
// Call print attitude. The LSM9DS1's magnetometer x and y
// axes are opposite to the accelerometer, so my and mx are
// substituted for each other
getAttitude(imu.ax, imu.ay, imu.az, -imu.my, -imu.mx, imu.mz);
// Get barometric sensor valuues
getBaro();
// Get telemetry signal quality
getSignalQuality();
// Get the battery state
getPower();
// Send telemtry via timer
// Save state via timer
}
bool initIMU() {
// Before initializing the IMU, there are a few settings
// we may need to adjust. Use the settings struct to set
// the device's communication mode and addresses:
imu.settings.device.commInterface = IMU_MODE_I2C;
imu.settings.device.mAddress = LSM9DS1_M;
imu.settings.device.agAddress = LSM9DS1_AG;
// The above lines will only take effect AFTER calling
// imu.begin(), which verifies communication with the IMU
// and turns it on.
if (!imu.begin()) return (FALSE);
return(TRUE);
}
void getGyro()
{
// To read from the gyroscope, you must first call the
// readGyro() function. When this exits, it'll update the
// gx, gy, and gz variables with the most current data.
imu.readGyro();
// Now we can use the gx, gy, and gz variables as we please.
// Either print them as raw ADC values, or calculated in DPS.
#ifdef PRINT_CALCULATED
// If you want to print calculated values, you can use the
// calcGyro helper function to convert a raw ADC value to
// DPS. Give the function the value that you want to convert.
pq.gyroX = imu.calcGyro(imu.gx);
pq.gyroY = imu.calcGyro(imu.gy);
pq.gyroZ = imu.calcGyro(imu.gz);
#elif defined PRINT_RAW
pq.gyroX = imu.gx;
pq.gyroY = imu.gy;
pq.gyroZ = imu.gz;
#endif
}
void getAccel()
{
// To read from the accelerometer, you must first call the
// readAccel() function. When this exits, it'll update the
// ax, ay, and az variables with the most current data.
imu.readAccel();
// Now we can use the ax, ay, and az variables as we please.
// Either print them as raw ADC values, or calculated in g's.
#ifdef PRINT_CALCULATED
// If you want to print calculated values, you can use the
// calcAccel helper function to convert a raw ADC value to
// g's. Give the function the value that you want to convert.
pq.accelX = imu.calcAccel(imu.ax);
pq.accelY = imu.calcAccel(imu.ay);
pq.accelZ = imu.calcAccel(imu.az);
#elif defined PRINT_RAW
pq.accelX = imu.ax;
pq.accelY = imu.ay;
pq.accelZ = imu.az;
#endif
}
void getMag()
{
// To read from the magnetometer, you must first call the
// readMag() function. When this exits, it'll update the
// mx, my, and mz variables with the most current data.
imu.readMag();
// Now we can use the mx, my, and mz variables as we please.
// Either print them as raw ADC values, or calculated in Gauss.
#ifdef PRINT_CALCULATED
// If you want to print calculated values, you can use the
// calcMag helper function to convert a raw ADC value to
// Gauss. Give the function the value that you want to convert.
pq.magX = imu.calcMag(imu.mx);
pq.magY = imu.calcMag(imu.my);
pq.magZ = imu.calcMag(imu.mz);
#elif defined PRINT_RAW
pq.magX = imu.mx;
pq.magY = imu.my;
pq.magZ = imu.mz;
#endif
}
// Calculate pitch, roll, and heading.
// Pitch/roll calculations take from this app note:
// http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
// Heading calculations taken from this app note:
// http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
void getAttitude(float ax, float ay, float az, float mx, float my, float mz) {
float roll = atan2(ay, az);
float pitch = atan2(-ax, sqrt(ay * ay + az * az));
float heading;
if (my == 0)
heading = (mx < 0) ? 180.0 : 0;
else
heading = atan2(mx, my);
heading -= DECLINATION * M_PI / 180;
if (heading > M_PI) heading -= (2 * M_PI);
else if (heading < -M_PI) heading += (2 * M_PI);
else if (heading < 0) heading += 2 * M_PI;
// Convert everything from radians to degrees:
heading *= 180.0 / M_PI;
pitch *= 180.0 / M_PI;
roll *= 180.0 / M_PI;
pq.heading = heading;
pq.pitch = pitch;
pq.roll = roll;
}
// Web control of local storage
bool storeToggle(String command) {
if (command=="on") {
pq.storeEnable = TRUE;
diagnostic("Local storage started");
return (TRUE);
} else if (command=="off") {
pq.storeEnable = FALSE;
diagnostic("Local storage stopped");
return (FALSE);
} else {
return (FALSE);
}
}
// Web control of telemetry
bool telemetryToggle(String command) {
if (command=="on") {
pq.telemetryEnable = TRUE;
diagnostic("Telemetry started");
return (TRUE);
} else if (command=="off") {
pq.telemetryEnable = FALSE;
diagnostic("Telemetry stopped");
return (FALSE);
} else {
return (FALSE);
}
}
/* Possible I2C EEPROM storage code
#include <Wire.h> // Para a EEPROM 24LC256, assim chamamos as bibliotecas i2c
#define eeprom 0x50 // endereço da eeprom já shiftado
// Think about organizing as an array of fixed length string records. With functions to store to the next record and then serially
// retrieve all the records
void initEEPROM {
Wire.begin(); // Iniciar ligações i2c
unsigned int address = 0; //endereçamento a 2 bytes
Serial.println("A escrever o melhor numero de sempre nos 10 primeiros bytes!");
for(address = 0; address<10; address++) writeEEPROM(eeprom, address, '3'); // Va pub a mim! Encher os 10 primeiros bytes com o número 3
Serial.println("Vamos ler se esta tudo ok, deves ver 33, 33, 33, 33... Vá 10x 33");
for(address = 0; address<10; address++) {
Serial.print(readEEPROM(eeprom, address), HEX);
Serial.print(", ");
}
}
//-------Rotinas para EEPROMS i2c por Daniel Gonçalves a.k.a. Tr3s------
// Podem usar estas rotinas à vontade para projectos particulares.
// Para fins comerciais entrar em contacto com we_real_cool@hotmail.com
// Partilhem com apenas com o meu concentimento.
// Se virem este código noutro sitio sem ser [url=http://www.lusorobotica.com]www.lusorobotica.com[/url] avisem de imediato para we_real_cool@hotmail.com!
void storeEEPROM (String buffer) {
}
void writeEEPROM(int deviceaddress, unsigned int eeaddress, byte data ) {
Wire.beginTransmission(deviceaddress);
Wire.send((int)(eeaddress >> 8)); // MSB
Wire.send((int)(eeaddress & 0xFF)); // LSB
Wire.send(data);
Wire.endTransmission();
}
byte readEEPROM(int deviceaddress, unsigned int eeaddress ) {
byte rdata = 0xFF;
Wire.beginTransmission(deviceaddress);
Wire.send((int)(eeaddress >> 8)); // MSB
Wire.send((int)(eeaddress & 0xFF)); // LSB
Wire.endTransmission();
Wire.requestFrom(deviceaddress,1);
if (Wire.available()) rdata = Wire.receive();
return rdata;
}
*/
void displayGPSInfo() {
if (!DIAG) return;
Serial.print(F("Location: "));
Serial.print(gps.satellites.value());
Serial.print(F(","));
Serial.print(gps.location.lat(), 6);
Serial.print(F(","));
Serial.print(gps.location.lng(), 6);
Serial.print(F(","));
Serial.print(gps.altitude.meters(), 1);
Serial.print(F(" Date/Time: "));
Serial.print(gps.date.month());
Serial.print(F("/"));
Serial.print(gps.date.day());
Serial.print(F("/"));
Serial.print(gps.date.year());
Serial.print(F(" "));
Serial.print(gps.time.hour());
Serial.print(F(":"));
Serial.print(gps.time.minute());
Serial.print(F(":"));
Serial.print(gps.time.second());
Serial.print(F("."));
Serial.print(gps.time.centisecond());
Serial.print(F(" "));
Serial.print(gps.charsProcessed()); Serial.print(F(","));
Serial.print(gps.sentencesWithFix()); Serial.print(F(","));
Serial.print(gps.failedChecksum()); Serial.print(F(","));
Serial.print(gps.passedChecksum());
Serial.println();
}