File content as of revision 70:c6b61c5cadf4:

/**
 ******************************************************************************
 * @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 <math.h>

#include <Ticker.h>


/*** Constants ---------------------------------------------------------------- ***/
namespace {
	const int MS_INTERVALS = 1000;
	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)


/*** 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) ------------------------------------------------------------ ***/
/* print floats & doubles */
static char *printDouble(char* str, double v, int decimalDigits=2)
{
  int i = 1;
  int intPart, fractPart;
  int len;
  char *ptr;

  /* prepare decimal digits multiplicator */
  for (;decimalDigits!=0; i*=10, decimalDigits--);

  /* calculate integer & fractinal parts */
  intPart = (int)v;
  fractPart = (int)((v-(double)(int)v)*i);

  /* fill in integer part */
  sprintf(str, "%i.", intPart);

  /* prepare fill in of fractional part */
  len = strlen(str);
  ptr = &str[len];

  /* fill in leading fractional zeros */
  for (i/=10;i>1; i/=10, ptr++) {
	  if(fractPart >= i) break;
	  *ptr = '0';
  }

  /* fill in (rest of) fractional part */
  sprintf(ptr, "%i", fractPart);

  return str;
}

/* Initialization function */
static void init(void) {
	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();
	}
}

/* Main cycle function */
static void main_cycle(void) {
	AxesRaw_TypeDef MAG_Value;
	AxesRaw_TypeDef ACC_Value;
	AxesRaw_TypeDef GYR_Value;
	char buffer1[32];
	char buffer2[32];
	char buffer3[32];
	char buffer4[32];
	unsigned int ret = 0;
	
	/* Declaration of sensors variables */
	double accX,accY,accZ;
	double gyroX,gyroY,gyroZ;

	
	/* 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("MAG [mgauss]: %9ld %9ld %9ld\n", 
	       MAG_Value.AXIS_X, MAG_Value.AXIS_Y, MAG_Value.AXIS_Z);
        printf("ACC [mg]:     %9ld %9ld %9ld\n", 
	       ACC_Value.AXIS_X, ACC_Value.AXIS_Y, ACC_Value.AXIS_Z);
        printf("GYR [mdps]:   %9ld %9ld %9ld\n", 
	       GYR_Value.AXIS_X, GYR_Value.AXIS_Y, GYR_Value.AXIS_Z);
	       
	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;
	
	/* Print Serially *//*
	ser.printf("%lf",pitch);
	ser.printf(":");
	ser.printf("%lf",roll);
	ser.printf(":");
	ser.printf("%lf\n",yaw);
	*/
	ser.printf("1");
	ser.printf(":");
	ser.printf("2");
	ser.printf(":");
	ser.printf("3\n");
	
	
	/* 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 */
		}
	}
}