upratanie funkcií (nie kompletné)

inc/DMP_ICM20948.h

@@ -1,14 +1,26 @@
 #ifndef __Arduino_ICM20948_H__
 #define __Arduino_ICM20948_H__
 
-//#include <stdint.h>
 #include "main.h"
-//#include "stm32l4xx_hal.h"
+
+#include "HAL_impl.h"
 #include "HAL_STM32_config.h"
 
 /*************************************************************************
   Defines
 *************************************************************************/
+
+static const uint8_t EXPECTED_WHOAMI[] = { 0xEA }; /* WHOAMI value for ICM20948 or derivative */
+#define AK0991x_DEFAULT_I2C_ADDR  0x0C
+#define AK0991x_SECONDARY_I2C_ADDR  0x0E  /* The secondary I2C address for AK0991x Magnetometers */
+
+#define ICM_I2C_ADDR_REVA      0x68  /* I2C slave address for INV device on Rev A board */
+#define ICM_I2C_ADDR_REVB     0x69  /* I2C slave address for INV device on Rev B board */
+
+#define AD0_VAL   1     // The value of the last bit of the I2C address.
+
+#define THREE_AXES 3
+
 #ifdef __cplusplus
 
 typedef struct {

inc/HAL_STM32_config.h

@@ -8,6 +8,9 @@
 #ifndef INC_HAL_STM32_CONFIG_H_
 #define INC_HAL_STM32_CONFIG_H_
 
+#include "stm32l0xx_hal.h"
+#include "HAL_impl.h"
+
 #define USE_SPI 1
 
 #if (!USE_SPI && defined(HAL_I2C_MODULE_ENABLED))
@@ -22,8 +25,8 @@
 
 	extern SPI_HandleTypeDef hspi1;
 
-	#define CHIP_SELECT_PIN SPI1_CS_Pin
-	#define CHIP_SELECT_PORT SPI1_CS_GPIO_Port
+	#define CHIP_SELECT_PIN SPI_CS_Pin
+	#define CHIP_SELECT_PORT SPI_CS_GPIO_Port
 
 	#define SPI_INSTANCE hspi1
 

inc/Icm20948.h

@@ -203,6 +203,8 @@ extern struct inv_icm20948 * icm20948_instance;
  */
 extern void inv_icm20948_sleep_us(int us);
 
+extern void inv_icm20948_sleep(int ms);
+
 /** @brief Hook for low-level system time() function to be implemented by upper layer
  *  @return monotonic timestamp in us
  */

src/DMP_ICM20948.cpp

@@ -3,17 +3,14 @@
 *************************************************************************/
 
 #include "DMP_ICM20948.h"
-//#include <Wire.h>
-//#include <SPI.h>
-//#include <Arduino.h>
 #include <math.h>
-//#include "stm32l4xx_hal.h"
 
 // InvenSense drivers and utils
 #include "Icm20948.h"
 #include "SensorTypes.h"
 #include "Icm20948MPUFifoControl.h"
 
+
 /*************************************************************************
   Variables
 *************************************************************************/
@@ -54,108 +51,8 @@ unsigned long steps;
 bool steps_data_ready = false;
 
 
-// Funkcia na nastavenie CS pinu
-static void CS_Select(void) {
-    HAL_GPIO_WritePin(CHIP_SELECT_PORT, CHIP_SELECT_PIN, GPIO_PIN_RESET);
-}
-
-static void CS_Deselect(void) {
-    HAL_GPIO_WritePin(CHIP_SELECT_PORT, CHIP_SELECT_PIN, GPIO_PIN_SET);
-}
-
-/*************************************************************************
-  HAL Functions for Arduino
-*************************************************************************/
-int spi_master_read_register(uint8_t reg, uint8_t* rbuffer, uint32_t rlen)
-{
-    //return spi_master_transfer_rx(NULL, reg, rbuffer, rlen);
-
-	/*
-    SPI.beginTransaction(SPISettings(com_speed, MSBFIRST, SPI_MODE0));
-    SPI.transfer(0x00);
-    SPI.endTransaction();
-    */
-	uint8_t dummy = 0x00;
-	uint8_t status = HAL_SPI_Transmit(&SPI_INSTANCE, &dummy, 1, HAL_MAX_DELAY);
-	if (status != HAL_OK) return status;
-
-	/*
-    digitalWrite(chipSelectPin, LOW);
-    SPI.beginTransaction(SPISettings(com_speed, MSBFIRST, SPI_MODE0));
-    SPI.transfer(((reg & 0x7F) | 0x80));
-    for (uint32_t indi = 0; indi < rlen; indi++)
-    {
-        *(rbuffer + indi) = SPI.transfer(0x00);
-    }
-    SPI.endTransaction();
-    digitalWrite(chipSelectPin, HIGH);
-	 */
-
-#ifdef HAL_SPI_MODULE_ENABLED
-
-	CS_Select();
-
-    // 3. Adresovanie registra
-    uint8_t cmd = (reg & 0x7F) | 0x80;
-    status = HAL_SPI_Transmit(&SPI_INSTANCE, &cmd, 1, HAL_MAX_DELAY);
-
-    // 4. Čítanie dát
-    for (uint32_t indi = 0; indi < rlen; indi++) {
-        uint8_t rx = 0x00;
-        status = HAL_SPI_TransmitReceive(&SPI_INSTANCE, &rx, &rbuffer[indi], 1, HAL_MAX_DELAY);
-        if (status != HAL_OK) {
-            CS_Deselect();
-            return status;
-        }
-    }
-
-    // 5. Chip select HIGH
-    CS_Deselect();
-
-#endif // HAL_SPI_MODULE_ENABLED
-
-    return 0;
-}
-
-int spi_master_write_register(uint8_t reg, const uint8_t* wbuffer, uint32_t wlen)
-{
-
-#ifdef HAL_SPI_MODULE_ENABLED
-
-	HAL_StatusTypeDef status;
-	uint8_t dummy = 0x00;
-	uint8_t addr  = (reg & 0x7F) | 0x00;
-
-	// 1. Dummy transfer (pôvodne SPI.transfer(0x00) mimo transakcie)
-	status = HAL_SPI_Transmit(&SPI_INSTANCE, &dummy, 1, HAL_MAX_DELAY);
-	if (status != HAL_OK) return -1;
-
-	// 2. Chip Select LOW
-	CS_Select();
-
-	// 3. Odoslanie adresy registra
-	status = HAL_SPI_Transmit(&SPI_INSTANCE, &addr, 1, HAL_MAX_DELAY);
-	if (status != HAL_OK) {
-		CS_Deselect();
-		return -2;
-	}
 
-	// 4. Odoslanie všetkých dát
-	if (wlen > 0) {
-		status = HAL_SPI_Transmit(&SPI_INSTANCE, (uint8_t*)wbuffer, wlen, HAL_MAX_DELAY);
-		if (status != HAL_OK) {
-			CS_Deselect();
-			return -3;
-		}
-	}
 
-	// 5. Chip Select HIGH
-	CS_Deselect();
-
-#endif // HAL_SPI_MODULE_ENABLED
-
-	return 0;
-}
 
 int i2c_master_write_register(uint8_t address, uint8_t reg, uint32_t len, const uint8_t *data)
 {
@@ -214,18 +111,6 @@ int i2c_master_read_register(uint8_t address, uint8_t reg, uint32_t len, uint8_t
 *************************************************************************/
 
 inv_icm20948_t icm_device;
-int rc = 0;
-static const uint8_t EXPECTED_WHOAMI[] = { 0xEA }; /* WHOAMI value for ICM20948 or derivative */
-#define AK0991x_DEFAULT_I2C_ADDR  0x0C
-#define AK0991x_SECONDARY_I2C_ADDR  0x0E  /* The secondary I2C address for AK0991x Magnetometers */
-
-#define ICM_I2C_ADDR_REVA      0x68  /* I2C slave address for INV device on Rev A board */
-#define ICM_I2C_ADDR_REVB     0x69  /* I2C slave address for INV device on Rev B board */
-
-#define AD0_VAL   1     // The value of the last bit of the I2C address.
-
-
-#define THREE_AXES 3
 static int unscaled_bias[THREE_AXES * 2];
 
 static const float cfg_mounting_matrix[9] = {
@@ -263,37 +148,31 @@ int load_dmp3(void)
 void inv_icm20948_sleep_us(int us)
 {
   // delayMicroseconds(us);
-  while(us--)
+  while(us--)		/// !!! very weak implementation
 	  __ASM volatile ("NOP");
 }
 
 void inv_icm20948_sleep(int ms)
 {
-  //delay(ms);
 	HAL_Delay(ms);
 }
 
 uint64_t inv_icm20948_get_time_us(void)
 {
-  //return micros();
 	return HAL_GetTick()*1000;
 }
 
 
-void initiliaze_SPI(void)
-{
-    CS_Deselect();
-    uint8_t dummy = 0x00;
-    HAL_SPI_Transmit(&SPI_INSTANCE, &dummy, 1, HAL_MAX_DELAY);
-}
-
 
 void initiliaze_I2C(void)
 {
+#ifdef HAL_I2C_MODULE_ENABLED
 	I2C_Address = I2C_ADDRESS;
+#endif
 }
 
 bool is_interface_SPI = false;
+
 void set_comm_interface(DMP_ICM20948Settings settings)
 {
     is_interface_SPI = IS_SPI;
@@ -652,7 +531,7 @@ void DMP_ICM20948::init(DMP_ICM20948Settings settings)
   inv_icm20948_register_aux_compass(&icm_device, INV_ICM20948_COMPASS_ID_AK09916, AK0991x_DEFAULT_I2C_ADDR);
 
   // Setup the icm20948 device
-  rc = icm20948_sensor_setup();
+  int rc = icm20948_sensor_setup();
 
   if (icm_device.selftest_done && !icm_device.offset_done)
   {

src/Icm20948AuxCompassAkm.c

@@ -385,7 +385,7 @@ int inv_icm20948_check_akm_self_test(struct inv_icm20948 * s)
 	counter = DEF_ST_COMPASS_TRY_TIMES;
 	while (counter > 0) {
 //		usleep_range(DEF_ST_COMPASS_WAIT_MIN, DEF_ST_COMPASS_WAIT_MAX);
-        inv_icm20948_sleep_us(15000);
+		inv_icm20948_sleep(15);
 
 #if (MEMS_CHIP == HW_ICM20948)
 		result = inv_icm20948_execute_read_secondary(s, 0, addr, REG_AK09916_STATUS1, 1, data);

src/Icm20948AuxTransport.c

@@ -1,7 +1,7 @@
 /*
 * ________________________________________________________________________________________________________
-* Copyright © 2014-2015 InvenSense Inc. Portions Copyright © 2014-2015 Movea. All rights reserved.
-* This software, related documentation and any modifications thereto (collectively “Software”) is subject
+* Copyright � 2014-2015 InvenSense Inc. Portions Copyright � 2014-2015 Movea. All rights reserved.
+* This software, related documentation and any modifications thereto (collectively �Software�) is subject
 * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and
 * other intellectual property rights laws.
 * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
@@ -85,7 +85,7 @@ int inv_icm20948_execute_read_secondary(struct inv_icm20948 * s, int index, unsi
 	
 	result |= inv_icm20948_secondary_enable_i2c(s);
     
-	inv_icm20948_sleep_us(SECONDARY_INIT_WAIT*1000);
+	inv_icm20948_sleep(SECONDARY_INIT_WAIT);
     
 	result |= inv_icm20948_secondary_disable_i2c(s);
 
@@ -136,7 +136,7 @@ int inv_icm20948_execute_write_secondary(struct inv_icm20948 * s, int index, uns
 	
 	result |= inv_icm20948_secondary_enable_i2c(s);
     
-	inv_icm20948_sleep_us(SECONDARY_INIT_WAIT*1000);
+	inv_icm20948_sleep(SECONDARY_INIT_WAIT);
     
 	result |= inv_icm20948_secondary_disable_i2c(s);
 

src/Icm20948SelfTest.c

@@ -1,7 +1,7 @@
 /*
 * ________________________________________________________________________________________________________
-* Copyright © 2014-2015 InvenSense Inc. Portions Copyright © 2014-2015 Movea. All rights reserved.
-* This software, related documentation and any modifications thereto (collectively “Software”) is subject
+* Copyright � 2014-2015 InvenSense Inc. Portions Copyright � 2014-2015 Movea. All rights reserved.
+* This software, related documentation and any modifications thereto (collectively �Software�) is subject
 * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and
 * other intellectual property rights laws.
 * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
@@ -184,7 +184,7 @@ static int inv_recover_setting(struct inv_icm20948 * s, const struct recover_reg
 	// Reset DMP
 	result |= inv_icm20948_write_single_mems_reg(s, REG_USER_CTRL, 
 		(saved_regs->user_ctrl & (~BIT_FIFO_EN)) | BIT_DMP_RST);
-	inv_icm20948_sleep_us(DMP_RESET_TIME*1000);
+	inv_icm20948_sleep(DMP_RESET_TIME);
 
 	result |=inv_icm20948_set_dmp_address(s);
 	result |=inv_icm20948_set_secondary(s);
@@ -211,7 +211,7 @@ static int inv_check_accelgyro_self_test(enum INV_SENSORS sensorType, uint8_t *
 
 	// Calculate factory Self-Test value (ST_OTP) based on the following equation:
 	// The factory Self-Test value (ST_OTP) is calculated from the ST_Code (the SELF_TEST values read)
-	// using the following equation, where “FS” is the full scale value code:
+	// using the following equation, where �FS� is the full scale value code:
 	// st_otp = 2620/2^FS * 1.01^(st_value - 1)
 	// the result of the equation is in sSelfTestEquation array
 	for (i = 0; i < 3; i++) {
@@ -272,7 +272,7 @@ static int inv_setup_selftest(struct inv_icm20948 * s, struct recover_regs * rec
 	* This will clear any prior states in the chip
 	*/
 	result |= inv_icm20948_write_single_mems_reg(s, REG_PWR_MGMT_1, BIT_H_RESET);               
-	inv_icm20948_sleep_us(100000); //100ms delay after soft reset--yd
+	inv_icm20948_sleep(100); //100ms delay after soft reset--yd
 
 	// Wake up
 	result |= inv_icm20948_write_single_mems_reg(s, REG_PWR_MGMT_1, BIT_CLK_PLL);
@@ -314,7 +314,7 @@ static int inv_setup_selftest(struct inv_icm20948 * s, struct recover_regs * rec
 	result |= inv_icm20948_read_mems_reg(s, REG_SELF_TEST6, 1, &s->accel_st_data[2]);
 
 	// Restart sensors
-	inv_icm20948_sleep_us(GYRO_ENGINE_UP_TIME*1000);
+	inv_icm20948_sleep(GYRO_ENGINE_UP_TIME);
 
 	return result;
 }
@@ -351,7 +351,7 @@ static int inv_selftest_read_samples(struct inv_icm20948 * self, enum INV_SENSOR
 
 		(*s)++;
 
-		inv_icm20948_sleep_us(WAIT_TIME_BTW_2_SAMPLESREAD*1000);
+		inv_icm20948_sleep(WAIT_TIME_BTW_2_SAMPLESREAD);
 	}
 	return 0;
 }
@@ -371,7 +371,7 @@ static int inv_do_test_accelgyro(struct inv_icm20948 * s, enum INV_SENSORS senso
 	}
 
 	// read the accel/gyro output
-	// the output values are 16 bits wide and in 2’s complement
+	// the output values are 16 bits wide and in 2�s complement
 	// Average 200 readings and save the averaged values
 	result = inv_selftest_read_samples(s, sensorType, meanValue, &lNbSamples);
 	if (result)
@@ -393,7 +393,7 @@ static int inv_do_test_accelgyro(struct inv_icm20948 * s, enum INV_SENSORS senso
 		return result;
 
 	// Wait 20ms for oscillations to stabilize. 
-	inv_icm20948_sleep_us(DEF_ST_STABLE_TIME*1000);
+	inv_icm20948_sleep(DEF_ST_STABLE_TIME);
 
 	// Read the accel/gyro output and average 200 readings
 	// These readings are in units of LSBs

src/Icm20948Setup.c

@@ -412,7 +412,7 @@ int inv_icm20948_soft_reset(struct inv_icm20948 * s)
 	//soft reset like
 	int rc = inv_icm20948_write_single_mems_reg(s, REG_PWR_MGMT_1, BIT_H_RESET);
 	// max start-up time is 100 msec
-	inv_icm20948_sleep_us(100000);
+	inv_icm20948_sleep(100);
 	return rc;
 }