State Estimations & Telemetry Updates

_test/driver/gps/Makefile

@@ -17,7 +17,7 @@ TARGET = test_gps.out
 ################################################################
 
 # Debug build?
-DEBUG = 1
+DEBUG ?= 0
 # Optimization
 OPT   = -Og
 
@@ -78,6 +78,7 @@ C_INCLUDES =                                           \
 -I$(ROOT_DIR)/driver/gps/								   \
 -I$(ROOT_DIR)/mod/sensor/							   \
 -I$(ROOT_DIR)/driver/imu/								   \
+-I$(ROOT_DIR)/mod/math_sdr/								   \
 -I$(LIB_DIR)                                           \
 -I$(LIB_DIR)/Drivers/STM32H7xx_HAL_Driver/Inc          \
 -I$(LIB_DIR)/Drivers/STM32H7xx_HAL_Driver/Inc/Legacy   \
@@ -106,6 +107,13 @@ CFLAGS += -Wno-unused-variable
 # Get coverage report
 CFLAGS += -ftest-coverage
 CFLAGS += -fprofile-arcs
+CFLAGS += -ffile-prefix-map=$(shell pwd)/=
+
+ifeq ($(DEBUG), 1)
+CFLAGS += -DDEBUG
+else
+CFLAGS += -DRELBLD
+endif
 
 ################################################################
 # Build all
@@ -137,7 +145,8 @@ test:
 	mkdir -p coverage
 	gcovr $(BUILD_DIR) \
 		--filter $(abspath $(COV_C_SOURCES)) \
-		--html-details coverage/coverage.html
+		--html-details coverage/coverage.html  \
+		--json coverage/coverage.json
 
 ################################################################
 # clean up 

_test/driver/servo/Makefile

@@ -16,7 +16,7 @@ TARGET = servo.out
 ################################################################
 
 # Debug build?
-DEBUG = 1
+DEBUG ?= 0
 # Optimization
 OPT   = -Og
 
@@ -111,6 +111,13 @@ CFLAGS += -Wno-unused-variable
 # Get coverage report
 CFLAGS += -ftest-coverage
 CFLAGS += -fprofile-arcs
+CFLAGS += -ffile-prefix-map=$(shell pwd)/=
+
+ifeq ($(DEBUG), 1)
+CFLAGS += -DDEBUG
+else
+CFLAGS += -DRELBLD
+endif
 
 ################################################################
 # Build all
@@ -144,7 +151,8 @@ test:
 	mkdir -p coverage
 	gcovr $(BUILD_DIR) \
 		--filter $(abspath $(COV_C_SOURCES)) \
-		--html-details coverage/coverage.html
+		--html-details coverage/coverage.html \
+		--json coverage/coverage.json
 
 ################################################################
 # clean up 

imu/imu.c

@@ -504,8 +504,8 @@ return IMU_OK;
 * 		imu_get_mag_xyz                                                        *
 *                                                                              *
 * DESCRIPTION:                                                                 * 
-* 		Return the pointer to structure that updates the x,y,z magnetometer    *
-*       values from the IMU                                                    *
+*       Return the pointer to structure that updates the x, y, z, and RHALL    *
+*       magnetometer values from the BMM150.                                   *
 *                                                                              *
 *******************************************************************************/
 IMU_STATUS imu_get_mag_xyz
@@ -516,52 +516,63 @@ IMU_STATUS imu_get_mag_xyz
 /*------------------------------------------------------------------------------
  Local variables 
 ------------------------------------------------------------------------------*/
-uint8_t     regMag[6];    /* Magnetometer register bytes      */
-uint16_t    mag_x_raw;    /* Raw magnetometer sensor readouts */ 
-uint16_t    mag_y_raw; 
-uint16_t    mag_z_raw; 
-IMU_STATUS  imu_status;   /* IMU status return codes          */
+uint8_t    regMag[8];      /* Magnetometer register bytes      */
+int16_t    mag_x_raw;      /* Raw magnetometer sensor readouts */
+int16_t    mag_y_raw;
+int16_t    mag_z_raw;
+uint16_t   mag_hall_raw;
+IMU_STATUS imu_status;     /* IMU status return codes           */
 
 
 /*------------------------------------------------------------------------------
  API function implementation 
 ------------------------------------------------------------------------------*/
 
-/* Read MAG_X, MAG_Y, MAG_Z high byte and low byte registers */
-#if   defined( A0002_REV1 )
-    imu_status = read_mag_regs( IMU_REG_MAG_XOUT_H, 
-                                &regMag[0]        , 
-                                sizeof( regMag ) );
-#elif defined( A0002_REV2 )
-    imu_status = read_mag_regs( MAG_REG_DATAX_L, 
-                                &regMag[0]     , 
-                                sizeof( regMag ) );
-#endif
+/* Read BMM150 X, Y, Z, and RHALL registers */
+imu_status = read_mag_regs( MAG_REG_DATAX_L,
+                            &regMag[0],
+                            sizeof( regMag ) );
 
-/* Check for HAL IMU error */
-if ( imu_status == IMU_TIMEOUT )
-	{
-	return IMU_TIMEOUT;
-	}
+/* Check for magnetometer read error */
+if ( imu_status != IMU_OK )
+    {
+    return imu_status;
+    }
 
-/* Combine high byte and low byte to 16 bit data */
-#if   defined( A0002_REV1 )
-    mag_x_raw  = ( (uint16_t) regMag[1] ) << 8 | regMag[0];
-    mag_y_raw  = ( (uint16_t) regMag[3] ) << 8 | regMag[2];
-    mag_z_raw  = ( (uint16_t) regMag[5] ) << 8 | regMag[4];
-#elif defined( A0002_REV2 )
-    mag_x_raw  = (   (uint16_t) regMag[1]                         << MAG_XY_MSB_BITSHIFT ) | 
-                 ( ( (uint16_t) regMag[0] && MAG_XY_LSB_BITMASK ) >> MAG_XY_LSB_BITSHIFT );
-    mag_y_raw  = (   (uint16_t) regMag[3]                         << MAG_XY_MSB_BITSHIFT ) | 
-                 ( ( (uint16_t) regMag[2] && MAG_XY_LSB_BITMASK ) >> MAG_XY_LSB_BITSHIFT );
-    mag_z_raw  = (   (uint16_t) regMag[5]                         << MAG_Z_MSB_BITSHIFT  ) | 
-                 ( ( (uint16_t) regMag[4] && MAG_Z_LSB_BITMASK )  >> MAG_Z_LSB_BITSHIFT  );
-#endif
+/* Combine and scale the packed BMM150 register values */
+mag_x_raw = (int16_t)(   (uint16_t) regMag[1]                        << MAG_XY_MSB_BITSHIFT ) |
+                     ( ( (uint16_t) regMag[0] & MAG_XY_LSB_BITMASK ) >> MAG_XY_LSB_BITSHIFT );
+
+mag_y_raw = (int16_t)(   (uint16_t) regMag[3]                        << MAG_XY_MSB_BITSHIFT ) |
+                     ( ( (uint16_t) regMag[2] & MAG_XY_LSB_BITMASK ) >> MAG_XY_LSB_BITSHIFT );
+
+mag_z_raw = (int16_t)(   (uint16_t) regMag[5]                       << MAG_Z_MSB_BITSHIFT ) |
+                     ( ( (uint16_t) regMag[4] & MAG_Z_LSB_BITMASK ) >> MAG_Z_LSB_BITSHIFT );
+
+mag_hall_raw = (uint16_t)(   (uint16_t) regMag[7]                            << MAG_RHALL_MSB_BITSHIFT ) |
+                          ( ( (uint16_t) regMag[6] & MAG_RHALL_LSB_BITMASK ) >> MAG_RHALL_LSB_BITSHIFT );
+
+/* Sign-extend the packed 13-bit X/Y and 15-bit Z measurements */
+if ( mag_x_raw & ( 1 << 12 ) )
+    {
+    mag_x_raw |= (int16_t) ~( ( 1 << 13 ) - 1 );
+    }
+
+if ( mag_y_raw & ( 1 << 12 ) )
+    {
+    mag_y_raw |= (int16_t) ~( ( 1 << 13 ) - 1 );
+    }
+
+if ( mag_z_raw & ( 1 << 14 ) )
+    {
+    mag_z_raw |= (int16_t) ~( ( 1 << 15 ) - 1 );
+    }
 
 /* Export sensor data */
-pIMU->mag_x = mag_x_raw;
-pIMU->mag_y = mag_y_raw;
-pIMU->mag_z = mag_z_raw;
+pIMU->mag_x    = mag_x_raw;
+pIMU->mag_y    = mag_y_raw;
+pIMU->mag_z    = mag_z_raw;
+pIMU->mag_hall = mag_hall_raw;
 
 return IMU_OK;
 } /* imu_get_mag_xyz */

imu/imu.h

@@ -26,6 +26,7 @@
 
 #include <stdbool.h>
 #include "stm32h7xx_hal.h"
+#include "math_sdr.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -246,21 +247,6 @@ typedef struct _IMU_RAW {
     uint16_t    mag_hall;
 } IMU_RAW;
 
-/* Processed IMU aata */
-typedef struct _STATE_ESTIMATION {
-	float roll_angle;
-	float pitch_angle;
-    float yaw_angle;
-	float roll_rate;
-	float pitch_rate;
-    float yaw_rate;
-    float velocity;
-    float velo_x;
-    float velo_y;
-    float velo_z;     
-	float position;
-} STATE_ESTIMATION;
-
 typedef struct _IMU_CONVERTED {
     float accel_x;
     float accel_y;
@@ -273,13 +259,6 @@ typedef struct _IMU_CONVERTED {
     float mag_z ;
 } IMU_CONVERTED;
 
-/* Structure for imu containing all accel, gyro, and mag data */
-typedef struct _IMU_DATA 
-	{
-    IMU_CONVERTED imu_converted;
-    STATE_ESTIMATION state_estimate;
-	} IMU_DATA;
-
 /* Struct containing imu offset */
 typedef struct _IMU_OFFSET {
     float accel_x;

lora/lora.c

@@ -51,7 +51,7 @@
  Global Variables
 ------------------------------------------------------------------------------*/
 static LORA_STATUS lora_rx_done = LORA_WAITING;
-
+static bool is_lora_configured = false;
 
 /*------------------------------------------------------------------------------
     Internal function prototypes
@@ -274,6 +274,25 @@ return LORA_OK;
 }
 
 
+/*******************************************************************************
+*                                                                              *
+* PROCEDURE:                                                                   *
+* 		lora_is_lora_initialized                                               *
+*                                                                              *
+* DESCRIPTION:                                                                 *
+*       Determine initialization state of LoRa modem.                          *
+*                                                                              *
+*******************************************************************************/
+bool lora_is_lora_initialized
+    (
+    void
+    )
+{
+return is_lora_configured;
+
+} /* lora_is_lora_initialized */
+
+
 /*******************************************************************************
 *                                                                              *
 * PROCEDURE:                                                                   *
@@ -410,6 +429,7 @@ lora_reset();
 HAL_Delay(10);
 if (lora_init( &lora_config ) == LORA_OK)
     {
+    is_lora_configured = true;
     return lora_status;
     }
 else

lora/lora.h

@@ -118,7 +118,8 @@ typedef enum _LORA_STATUS {
    LORA_BUFFER_UNDERSIZED,
    LORA_READY,
    LORA_WAITING,
-   LORA_USING_DEFAULTS
+   LORA_USING_DEFAULTS,
+   LORA_INVALID_CMD
 } LORA_STATUS;
 
 /* Datasheet page 107 */
@@ -166,6 +167,12 @@ typedef enum _LORA_PA_SELECT {
    LORA_PA_BOOST = 0x01
 } LORA_PA_SELECT;
 
+typedef enum _LORA_ASYNC_OP_MODE {
+    LORA_ASYNC_OFF = 0x00,
+    LORA_ASYNC_TX  = 0x10,
+    LORA_ASYNC_RX  = 0x20,
+} LORA_ASYNC_OP_MODE;
+
 /* LORA CONFIG SETTINGS -- arg for lora_init() */
 typedef struct _LORA_CONFIG {
    LORA_CHIPMODE lora_mode; // Current LORA Chipmode
@@ -194,6 +201,26 @@ typedef enum LORA_SUBCMD_CODES {
     LORA_PRESET_DOWNLOAD = 0x02
 } LORA_SUBCMD_CODES;
 
+typedef enum LORA_TX_FSM_STATE {
+    LORA_TX_STATE_BLOCKING = 0,
+    LORA_TX_STATE_STATUS_CHECK,
+    LORA_TX_STATE_GETTING_BUF,
+    LORA_TX_STATE_SETTING_TX_BASE,
+    LORA_TX_STATE_WRITING_MSG_LEN,
+    LORA_TX_STATE_WRITING_MSG,
+    LORA_TX_STATE_PRE_TX_STATUS_CHECK,
+    LORA_TX_STATE_STARTING_TRANSMISSION,
+    LORA_TX_STATE_TRANSMITTING
+} LORA_TX_FSM_STATE;
+
+typedef enum LORA_FSM_EVENT {
+    LORA_FSM_EVENT_CANCEL = 0,
+    LORA_FSM_EVENT_SYNCHRONOUS_UPDATE,
+    LORA_FSM_EVENT_REG_READ_CPLT,
+    LORA_FSM_EVENT_WRITE_CPLT,
+    LORA_FSM_EVENT_EXTI_RAISED
+} LORA_FSM_EVENT;
+
 /*------------------------------------------------------------------------------
  Function Prototypes
 ------------------------------------------------------------------------------*/
@@ -233,6 +260,11 @@ void lora_reset
     void
     );
 
+bool lora_is_lora_initialized
+    (
+    void
+    );
+
 LORA_STATUS lora_cmd_execute
     ( 
     uint8_t subcommand_code,
@@ -265,6 +297,17 @@ LORA_STATUS lora_receive
     uint8_t* num_bytes_received
     );
 
+/* lora_async.c */
+LORA_STATUS lora_fsm_set_mode
+    (
+    LORA_ASYNC_OP_MODE new_mode
+    );
+
+void lora_fsm_update
+    (
+    LORA_FSM_EVENT update_cause
+    );
+
 #ifdef __cplusplus
 }
 #endif