File content as of revision 73:2e4b834c065d:

/**
 ******************************************************************************
 * @file    main.cpp
 * @author  AST / EST
 * @version V0.0.1
 * @date    14-April-2015
 * @brief   Example application for using the X_NUCLEO_IKS01A1 
 *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *   1. Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *   2. Redistributions in binary form must reproduce the above copyright notice,
 *      this list of conditions and the following disclaimer in the documentation
 *      and/or other materials provided with the distribution.
 *   3. Neither the name of STMicroelectronics nor the names of its contributors
 *      may be used to endorse or promote products derived from this software
 *      without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 ******************************************************************************
*/ 

/**
 * @mainpage X_NUCLEO_IKS01A1 MEMS Inertial & Environmental Sensor Nucleo Expansion Board Firmware Package
 *
 * <b>Introduction</b>
 *
 * This firmware package includes Components Device Drivers, Board Support Package
 * and example application for STMicroelectronics X_NUCLEO_IKS01A1 MEMS Inertial & Environmental Nucleo
 * Expansion Board
 * 
 * <b>Example Application</b>
 *
 */


/*** Includes ----------------------------------------------------------------- ***/
#include "mbed.h"
#include "assert.h"
#include "x_nucleo_iks01a1.h"
#include "Kalman.h"
#include <math.h>

#include <Ticker.h>


/*** Constants ---------------------------------------------------------------- ***/
namespace {
	const int MS_INTERVALS = 5;
	const double RAD_TO_DEG = 57.2957786;
	const double PI = 3.14159265;
}


/*** Macros ------------------------------------------------------------------- ***/
#define APP_LOOP_PERIOD 3000 // in ms

#if defined(TARGET_STM)
#define LED_OFF (0)
#else
#define LED_OFF (1)
#endif
#define LED_ON  (!LED_OFF)

#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead
#define DECLINATION 2.23 

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
Kalman kalmanZ;

double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY, kalAngleZ; // Calculated angle using a Kalman filter

Timer t;
uint32_t timer;

/*** Typedefs ----------------------------------------------------------------- ***/
typedef struct {
    int32_t AXIS_X;
    int32_t AXIS_Y;
    int32_t AXIS_Z;
} AxesRaw_TypeDef;

/*** Serial declaration --------------------------------------------------------- ***/
Serial ser(USBTX,USBRX,115200);

/*** Static variables --------------------------------------------------------- ***/
#ifdef DBG_MCU
/* betzw: enable debugging while using sleep modes */
#include "DbgMCU.h"
static DbgMCU enable_dbg;
#endif // DBG_MCU

static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance();
static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;

static Ticker ticker;
static DigitalOut myled(LED1, LED_OFF);

static volatile bool timer_irq_triggered = false;
static volatile bool ff_irq_triggered = false;


/*** Helper Functions (1/2) ------------------------------------------------------------ ***/


/*** Interrupt Handler Top-Halves ------------------------------------------------------ ***/
/* Called in interrupt context, therefore just set a trigger variable */
static void timer_irq(void) {
	timer_irq_triggered = true;
}

/* Called in interrupt context, therefore just set a trigger variable */
static void ff_irq(void) {
	ff_irq_triggered = true;

	/* Disable IRQ until handled */
	mems_expansion_board->gyro_lsm6ds3->Disable_Free_Fall_Detection_IRQ();
}


/*** Interrupt Handler Bottom-Halves ------------------------------------------------- ***/
/* Handle Free Fall Interrupt
   (here we are in "normal" context, i.e. not in IRQ context)
*/
static void handle_ff_irq(void) {
    printf("\nFree Fall Detected!\n\n");

    /* Re-enable IRQ */
    mems_expansion_board->gyro_lsm6ds3->Enable_Free_Fall_Detection_IRQ();
}


/*** Helper Functions (2/2) ------------------------------------------------------------ ***/
/* Initialization function */
static void init(void) {
	t.start();
	uint8_t id1, id2;
	
	/* Determine ID of Gyro & Motion Sensor */
	assert((mems_expansion_board->gyro_lsm6ds0 == NULL) ||
	       (mems_expansion_board->gyro_lsm6ds3 == NULL));
	CALL_METH(gyroscope, read_id, &id1, 0x0);
	CALL_METH(accelerometer, read_id, &id2, 0x0);
    	printf("Gyroscope | Motion Sensor ID      = %s (0x%x | 0x%x)\n", 
	       ((id1 == I_AM_LSM6DS3_XG) ? "LSM6DS3" : 
		((id1 == I_AM_LSM6DS0_XG) ? "LSM6DS0" : "UNKNOWN")),
	       id1, id2
	       );
	assert(id1 == id2);

	/* Register Free Fall Detection IRQ Handler & Enable Detection */
	if(mems_expansion_board->gyro_lsm6ds3 != NULL) {
		mems_expansion_board->gyro_lsm6ds3->Attach_Free_Fall_Detection_IRQ(ff_irq);
		mems_expansion_board->gyro_lsm6ds3->Enable_Free_Fall_Detection();
	}
	
	AxesRaw_TypeDef MAG_Value;
	AxesRaw_TypeDef ACC_Value;
	AxesRaw_TypeDef GYR_Value;
	unsigned int ret = 0;
	
	/* Switch LED On */
	myled = LED_ON;
	//printf("===\n");

	/* Determine Environmental Values */
	ret |= (!CALL_METH(magnetometer, get_m_axes, (int32_t *)&MAG_Value, 0) ? 0x0 : 0x10);;
	ret |= (!CALL_METH(accelerometer, get_x_axes, (int32_t *)&ACC_Value, 0) ? 0x0 : 0x20);;
	ret |= (!CALL_METH(gyroscope, get_g_axes, (int32_t *)&GYR_Value, 0) ? 0x0 : 0x40);
	
	/* IMU Data */
	double accX, accY, accZ;
	double gyroX, gyroY, gyroZ;

	accX = ACC_Value.AXIS_X;
  	accY = ACC_Value.AXIS_Y;
  	accZ = ACC_Value.AXIS_Z;
  	/*
  	**
  	gyroX = GYR_Value.AXIS_X;
  	gyroY = GYR_Value.AXIS_Y;
  	gyroZ = GYR_Value.AXIS_Z;
  	**
  	*/
  	
  	#ifdef RESTRICT_PITCH // Eq. 25 and 26
  	double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  	double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
	#else // Eq. 28 and 29
	double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  	double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
	#endif
	//double yaw = atan2(-accZ, sqrt(accY * accY + accZ * accZ)) * 180.0/PI;
	kalmanX.setAngle(roll); // Set starting angle
	kalmanY.setAngle(pitch);
	gyroXangle = roll;
	gyroYangle = pitch;
	compAngleX = roll;
	compAngleY = pitch;
	
	timer = t.read_us();
}

/* Main cycle function */
static void main_cycle(void) {
	AxesRaw_TypeDef MAG_Value;
	AxesRaw_TypeDef ACC_Value;
	AxesRaw_TypeDef GYR_Value;
	unsigned int ret = 0;
	
	/* Switch LED On */
	myled = LED_ON;
	//printf("===\n");

	/* Determine Environmental Values */
	ret |= (!CALL_METH(magnetometer, get_m_axes, (int32_t *)&MAG_Value, 0) ? 0x0 : 0x10);;
	ret |= (!CALL_METH(accelerometer, get_x_axes, (int32_t *)&ACC_Value, 0) ? 0x0 : 0x20);;
	ret |= (!CALL_METH(gyroscope, get_g_axes, (int32_t *)&GYR_Value, 0) ? 0x0 : 0x40);

	/* Print Values Out */
        //printf("I2C [errors]: 0x%.2x    X         Y         Z\n", ret); 
        /*
        **
        printf("%9ld:%9ld:%9ld:", ACC_Value.AXIS_X, ACC_Value.AXIS_Y, ACC_Value.AXIS_Z);
	    printf("%9ld:%9ld:%9ld:", GYR_Value.AXIS_X, GYR_Value.AXIS_Y, GYR_Value.AXIS_Z);
        printf("%9ld:%9ld:%9ld\n", MAG_Value.AXIS_X, MAG_Value.AXIS_Y, MAG_Value.AXIS_Z);
        **
        */
        
	/* IMU Data */
	double accX, accY, accZ;
	double gyroX, gyroY, gyroZ;
	double magX, magY, magZ;
    
	accX = ACC_Value.AXIS_X;
  	accY = ACC_Value.AXIS_Y;
  	accZ = ACC_Value.AXIS_Z;
  	gyroX = GYR_Value.AXIS_X;
  	gyroY = GYR_Value.AXIS_Y;
  	gyroZ = GYR_Value.AXIS_Z;
  	
  	double dt = (double)(t.read_us() - timer) / 1000000; // Calculate delta time
  	timer = t.read_us();
  	
  	#ifdef RESTRICT_PITCH // Eq. 25 and 26
  	double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  	double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
  	#else // Eq. 28 and 29
  	double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  	double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
  	#endif
  	
  	double gyroXrate = gyroX / 131.0; // Convert to deg/s
  	double gyroYrate = gyroY / 131.0; // Convert to deg/s
  	
  	#ifdef RESTRICT_PITCH
  	// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  	if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
  		kalmanX.setAngle(roll);
  		compAngleX = roll;
  		kalAngleX = roll;
  		gyroXangle = roll;
  	}
  	else
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
    
    if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
    #else
    // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
    if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
    	kalmanY.setAngle(pitch);
    	compAngleY = pitch;
    	kalAngleY = pitch;
    	gyroYangle = pitch;
    }
    else
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
    
    if (abs(kalAngleY) > 90)
    gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
    #endif
    
    gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
    gyroYangle += gyroYrate * dt;
    //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
    //gyroYangle += kalmanY.getRate() * dt;
    
    compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
    compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
    
    // Reset the gyro angle when it has drifted too much
    if (gyroXangle < -180 || gyroXangle > 180)
    gyroXangle = kalAngleX;
    if (gyroYangle < -180 || gyroYangle > 180)
    gyroYangle = kalAngleY;
    
    // Compute the Heading
    
    magX = MAG_Value.AXIS_X;
  	magY = MAG_Value.AXIS_Y;
  	magZ = MAG_Value.AXIS_Z;
	
	float heading;
	if (magY== 0)
    heading = (magX < 0) ? 180.0 : 0;
    else
    heading = atan2(magX , magY);
    //arctan(imu.mx / sqrt(imu.mz*imu.mz + imu.my*imu.my)):
    heading -= DECLINATION * PI / 180;
    
    if (heading > PI)
    heading -= (2 * PI);
    else if (heading < -PI || heading < 0)
    heading += (2 * PI);
    heading *= 180.0 / PI;
  
	printf("%lf:%1f:%1f\n", kalAngleY, kalAngleX, heading);
	
	
	/* Switch LED Off */
	myled = LED_OFF;
}


/*** Main function ------------------------------------------------------------- ***/
/* Generic main function/loop for enabling WFE in case of 
   interrupt based cyclic execution
*/
int main()
{
	/* Start & initialize */
	//printf("\n--- Starting new run ---\n");
	init();

	/* Start timer irq */
	ticker.attach_us(timer_irq, MS_INTERVALS * APP_LOOP_PERIOD);

	while (true) {
		if(timer_irq_triggered) {
			timer_irq_triggered = false;
			main_cycle();
		} else if(ff_irq_triggered) {
			ff_irq_triggered = false;
			handle_ff_irq();
		} else {
			__WFE(); /* it is recommended that SEVONPEND in the 
				    System Control Register is NOT set */
		}
	}
	t.stop();
}