Drivers for UBX, MS5611, AK09916 and minor fixes

.gitignore

@@ -1 +1,3 @@
 **/*.pyc
+*.o.*
+*.DS_Store

examples/driver_ak09916/Makefile

@@ -0,0 +1,32 @@
+CSRC = $(shell find src -name "*.c")
+INCDIR = ./include
+USE_OPT = -O1 -g
+USE_LTO = no
+MODULES_ENABLED = \
+chibios_sys_init \
+chibios_hal_init \
+app_descriptor \
+boot_msg \
+timing \
+system \
+pubsub \
+worker_thread \
+can_driver_stm32 \
+can \
+can_autobaud \
+uavcan \
+uavcan_nodestatus_publisher \
+uavcan_getnodeinfo_server \
+uavcan_beginfirmwareupdate_server \
+uavcan_allocatee \
+uavcan_restart \
+freemem_check \
+spi_device \
+uavcan_debug \
+driver_ak09916 \
+driver_icm20x48
+
+MESSAGES_ENABLED = \
+uavcan.protocol.debug.LogMessage
+
+include ../../include.mk

examples/driver_ak09916/board/com.hex.here+_2.0/board.c

@@ -0,0 +1,81 @@
+#include <hal.h>
+#define TOSHIBALED_I2C_ADDRESS 0x55
+#define AK09916_I2C_ADDRESS 0x0C
+
+/**
+ * @name    TIMINGR register definitions
+ * @{
+ */
+#define STM32_TIMINGR_PRESC_MASK        (15U << 28)
+#define STM32_TIMINGR_PRESC(n)          ((n) << 28)
+#define STM32_TIMINGR_SCLDEL_MASK       (15U << 20)
+#define STM32_TIMINGR_SCLDEL(n)         ((n) << 20)
+#define STM32_TIMINGR_SDADEL_MASK       (15U << 16)
+#define STM32_TIMINGR_SDADEL(n)         ((n) << 16)
+#define STM32_TIMINGR_SCLH_MASK         (255U << 8)
+#define STM32_TIMINGR_SCLH(n)           ((n) << 8)
+#define STM32_TIMINGR_SCLL_MASK         (255U << 0)
+#define STM32_TIMINGR_SCLL(n)           ((n) << 0)
+
+void boardInit(void) {
+    palSetLineMode(BOARD_PAL_LINE_SPI3_SCK, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUPDR_PULLDOWN); // SPI3 SCK
+    palSetLineMode(BOARD_PAL_LINE_SPI3_MISO, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST); // SPI3 MISO
+    palSetLineMode(BOARD_PAL_LINE_SPI3_MOSI, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST); // SPI3 MOSI
+    palSetLineMode(BOARD_PAL_LINE_SPI3_ICM_CS, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); // SPI CS
+    palSetLineMode(BOARD_PAL_LINE_SPI3_MS5611_CS, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); // SPI CS
+    palSetLineMode(BOARD_PAL_LINE_CAN_RX, PAL_MODE_ALTERNATE(9) | PAL_STM32_OSPEED_HIGHEST);
+    palSetLineMode(BOARD_PAL_LINE_CAN_TX, PAL_MODE_ALTERNATE(9) | PAL_STM32_OSPEED_HIGHEST);
+    palSetLineMode(BOARD_PAL_LINE_GPS_RX, PAL_MODE_ALTERNATE(7) | PAL_STM32_OSPEED_HIGHEST);    //GPS Rx USART2_Rx
+    palSetLineMode(BOARD_PAL_LINE_GPS_TX, PAL_MODE_ALTERNATE(7) | PAL_STM32_OSPEED_HIGHEST);    //GPS Tx USART2_Tx
+    palSetLineMode(BOARD_PAL_LINE_I2C_SLAVE_SCL, PAL_MODE_ALTERNATE(4));
+    palSetLineMode(BOARD_PAL_LINE_I2C_SLAVE_SDA, PAL_MODE_ALTERNATE(4));
+
+
+    //Setup I2C slave
+
+    rccEnableI2C2(FALSE);
+    rccResetI2C2();
+
+    //Disable I2C
+    I2C2->CR1 &= ~I2C_CR1_PE;
+
+    //Enable Analog Filter
+	I2C2->CR1 &= ~I2C_CR1_ANFOFF;
+
+    //Disable Digital Filter
+    I2C2->CR1 &=  ~(I2C_CR1_DNF);
+
+    //Set Prescaler
+    I2C2->TIMINGR = (I2C2->TIMINGR & ~STM32_TIMINGR_PRESC_MASK) |
+			   (STM32_TIMINGR_PRESC(8));
+
+    //Set Data Setup Time
+    I2C2->TIMINGR = (I2C2->TIMINGR & ~STM32_TIMINGR_SCLDEL_MASK) |
+			   (STM32_TIMINGR_SCLDEL(9));
+
+
+
+    //Set Data Hold Time
+    I2C2->TIMINGR = (I2C2->TIMINGR & ~STM32_TIMINGR_SDADEL_MASK) |
+			   (STM32_TIMINGR_SDADEL(11));
+
+    //Enable Stretching
+	I2C2->CR1 &= ~I2C_CR1_NOSTRETCH;
+
+    //7Bit Address Mode
+    I2C2->CR2 &= ~I2C_CR2_ADD10;
+
+
+    I2C2->OAR1 = (TOSHIBALED_I2C_ADDRESS & 0xFF) << 1; //Emulate Toshiba LED I2C Slave
+    I2C2->OAR1 |= (1<<15);
+    I2C2->OAR2 = (AK09916_I2C_ADDRESS & 0xFF) << 1; //Emulate AK09916 I2C Slave
+    I2C2->OAR2 |= (1<<15);
+    //Enable I2C interrupt
+    nvicEnableVector(I2C2_EV_IRQn, 3);
+
+    I2C2->CR1 |= (1<<1); // TXIE
+    I2C2->CR1 |= (1<<2); // RXIE
+    I2C2->CR1 |= (1<<3); // ADDRIE
+	I2C2->CR1 |= I2C_CR1_PE;
+}
+

examples/driver_ak09916/board/com.hex.here+_2.0/board.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <stdint.h>
+#include <modules/platform_stm32f302x8/platform_stm32f302x8.h>
+
+#define BOARD_CONFIG_HW_NAME "org.hex.here+"
+#define BOARD_CONFIG_HW_MAJOR_VER 2
+#define BOARD_CONFIG_HW_MINOR_VER 0
+
+#define BOARD_CONFIG_HW_INFO_STRUCTURE { \
+    .hw_name = BOARD_CONFIG_HW_NAME, \
+    .hw_major_version = BOARD_CONFIG_HW_MAJOR_VER, \
+    .hw_minor_version = BOARD_CONFIG_HW_MINOR_VER, \
+    .board_desc_fmt = SHARED_HW_INFO_BOARD_DESC_FMT_NONE, \
+    .board_desc = 0, \
+}
+
+#define BOARD_PAL_LINE_SPI3_SCK PAL_LINE(GPIOB,3)
+#define BOARD_PAL_LINE_SPI3_MISO PAL_LINE(GPIOB,4)
+#define BOARD_PAL_LINE_SPI3_MOSI PAL_LINE(GPIOB,5)
+#define BOARD_PAL_LINE_SPI3_ICM_CS PAL_LINE(GPIOB,0) // NOTE: never drive high by external source
+#define BOARD_PAL_LINE_SPI3_MS5611_CS PAL_LINE(GPIOA,5)
+#define BOARD_PAL_LINE_CAN_RX PAL_LINE(GPIOA,11)
+#define BOARD_PAL_LINE_CAN_TX PAL_LINE(GPIOA,12)
+#define BOARD_PAL_LINE_GPS_RX PAL_LINE(GPIOA,2)
+#define BOARD_PAL_LINE_GPS_TX PAL_LINE(GPIOA,3)
+#define BOARD_PAL_LINE_I2C_SLAVE_SCL PAL_LINE(GPIOA,9)
+#define BOARD_PAL_LINE_I2C_SLAVE_SDA PAL_LINE(GPIOA,10)
+
+
+#define GPS_SERIAL  SD2
+
+#define HAL_USE_SERIAL TRUE
+#define SERIAL_BUFFERS_SIZE 32

examples/driver_ak09916/board/com.hex.here+_2.0/board.mk

@@ -0,0 +1,4 @@
+BOARD_DIR := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST))))
+BOARD_SRC = $(BOARD_DIR)/board.c
+BOARD_INC = $(BOARD_DIR)
+MODULES_ENABLED += platform_stm32f302x8

examples/driver_ak09916/board/com.hex.here+_2.0/mcuconf.h

@@ -0,0 +1,83 @@
+#pragma once
+
+#define STM32F3xx_MCUCONF
+#define STM32_HSECLK 24000000
+
+#define STM32_NO_INIT                       FALSE
+#define STM32_PVD_ENABLE                    FALSE
+#define STM32_PLS                           STM32_PLS_LEV0
+#define STM32_HSI_ENABLED                   TRUE
+#define STM32_LSI_ENABLED                   TRUE
+#define STM32_HSE_ENABLED                   TRUE
+#define STM32_LSE_ENABLED                   FALSE
+#define STM32_SW                            STM32_SW_PLL
+#define STM32_PLLSRC                        STM32_PLLSRC_HSE
+#define STM32_PREDIV_VALUE                  3
+#define STM32_PLLMUL_VALUE                  9
+#define STM32_HPRE                          STM32_HPRE_DIV1
+#define STM32_PPRE1                         STM32_PPRE1_DIV2
+#define STM32_PPRE2                         STM32_PPRE2_DIV2
+#define STM32_MCOSEL                        STM32_MCOSEL_NOCLOCK
+#define STM32_ADC12PRES                     STM32_ADC12PRES_DIV1
+#define STM32_ADC34PRES                     STM32_ADC34PRES_DIV1
+#define STM32_USART1SW                      STM32_USART1SW_PCLK
+#define STM32_USART2SW                      STM32_USART2SW_PCLK
+#define STM32_USART3SW                      STM32_USART3SW_PCLK
+#define STM32_UART4SW                       STM32_UART4SW_PCLK
+#define STM32_UART5SW                       STM32_UART5SW_PCLK
+#define STM32_I2C1SW                        STM32_I2C1SW_SYSCLK
+#define STM32_I2C2SW                        STM32_I2C2SW_SYSCLK
+#define STM32_TIM1SW                        STM32_TIM1SW_PCLK2
+#define STM32_TIM8SW                        STM32_TIM8SW_PCLK2
+#define STM32_RTCSEL                        STM32_RTCSEL_LSI
+#define STM32_USB_CLOCK_REQUIRED            FALSE
+#define STM32_USBPRE                        STM32_USBPRE_DIV1P5
+
+#define STM32_SPI_USE_SPI1                  FALSE
+#define STM32_SPI_USE_SPI2                  FALSE
+#define STM32_SPI_USE_SPI3                  TRUE
+#define STM32_SPI_SPI1_DMA_PRIORITY         1
+#define STM32_SPI_SPI2_DMA_PRIORITY         1
+#define STM32_SPI_SPI3_DMA_PRIORITY         1
+#define STM32_SPI_SPI1_IRQ_PRIORITY         10
+#define STM32_SPI_SPI2_IRQ_PRIORITY         10
+#define STM32_SPI_SPI3_IRQ_PRIORITY         10
+#define STM32_SPI_DMA_ERROR_HOOK(spip)      osalSysHalt("DMA failure")
+
+/*
+ * SERIAL driver system settings.
+ */
+#define STM32_SERIAL_USE_USART1             FALSE
+#define STM32_SERIAL_USE_USART2             TRUE
+#define STM32_SERIAL_USE_USART3             FALSE
+#define STM32_SERIAL_USART1_PRIORITY        12
+#define STM32_SERIAL_USART2_PRIORITY        12
+#define STM32_SERIAL_USART3_PRIORITY        12
+
+/*
+ * CAN driver system settings.
+ */
+#define STM32_CAN_USE_CAN1                  TRUE
+#define STM32_CAN_CAN1_IRQ_PRIORITY         11
+
+#define STM32_ST_IRQ_PRIORITY               8
+#define STM32_ST_USE_TIMER                  2
+
+/*
+ * EXT driver system settings.
+ */
+#define STM32_EXT_EXTI0_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI1_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI2_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI3_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI4_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI5_9_IRQ_PRIORITY      6
+#define STM32_EXT_EXTI10_15_IRQ_PRIORITY    6
+#define STM32_EXT_EXTI16_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI17_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI18_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI19_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI20_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI21_22_29_IRQ_PRIORITY 6
+#define STM32_EXT_EXTI30_32_IRQ_PRIORITY    6
+#define STM32_EXT_EXTI33_IRQ_PRIORITY       6

examples/driver_ak09916/include/app_config.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#define APP_CONFIG_BOOT_DELAY_SEC 0
+#define APP_CONFIG_CAN_DEFAULT_BAUDRATE 1000000
+#define APP_CONFIG_CAN_LOCAL_NODE_ID 0
+#define APP_CONFIG_CAN_AUTO_BAUD_ENABLE 0
+
+#define SHARED_APP_DESCRIPTOR_MAJOR_VERSION 1
+#define SHARED_APP_DESCRIPTOR_MINOR_VERSION 0
+

examples/driver_ak09916/include/framework_conf.h

@@ -0,0 +1,40 @@
+#pragma once
+
+//
+// Configure worker threads
+//
+
+#define TIMING_WORKER_THREAD                            lpwork_thread
+#define UAVCAN_NODESTATUS_PUBLISHER_WORKER_THREAD       lpwork_thread
+#define CAN_AUTOBAUD_WORKER_THREAD                      lpwork_thread
+#define UAVCAN_PARAM_INTERFACE_WORKER_THREAD            lpwork_thread
+#define UAVCAN_GETNODEINFO_SERVER_WORKER_THREAD         lpwork_thread
+#define UAVCAN_RESTART_WORKER_THREAD                    lpwork_thread
+#define UAVCAN_BEGINFIRMWAREUPDATE_SERVER_WORKER_THREAD lpwork_thread
+#define UAVCAN_ALLOCATEE_WORKER_THREAD                  lpwork_thread
+#define PIN_CHANGE_PUBLISHER_WORKER_THREAD              lpwork_thread
+
+#define CAN_TRX_WORKER_THREAD                           can_thread
+#define CAN_EXPIRE_WORKER_THREAD                        can_thread
+#define UAVCAN_RX_WORKER_THREAD                         can_thread
+
+//
+// Configure topic groups
+//
+
+#define PUBSUB_DEFAULT_TOPIC_GROUP default_topic_group
+
+//
+// Misc configs
+//
+
+#define REQUIRED_RAM_MARGIN_AFTER_INIT      512
+
+//
+// Configure debug checks
+//
+
+#define CH_DBG_SYSTEM_STATE_CHECK           TRUE
+#define CH_DBG_ENABLE_CHECKS                TRUE
+#define CH_DBG_ENABLE_ASSERTS               TRUE
+#define CH_DBG_ENABLE_STACK_CHECK           TRUE

examples/driver_ak09916/openocd.cfg

@@ -0,0 +1,5 @@
+source [find interface/stlink-v2.cfg]
+source [find target/stm32f3x.cfg]
+init
+reset run
+#$_TARGETNAME configure -rtos ChibiOS

examples/driver_ak09916/src/ak09916_test.c

@@ -0,0 +1,92 @@
+#include <hal.h>
+#include <modules/driver_ak09916/driver_ak09916.h>
+#include <modules/worker_thread/worker_thread.h>
+#include <modules/uavcan_debug/uavcan_debug.h>
+#include <modules/timing/timing.h>
+
+#define WT hpwork_thread
+WORKER_THREAD_DECLARE_EXTERN(WT)
+
+#define AK09916_I2C_ADDR 0x0C
+
+
+static struct ak09916_instance_s ak09916;
+static struct icm20x48_instance_s icm20x48;
+static struct worker_thread_timer_task_s ak09916_test_task;
+static void ak09916_test_task_func(struct worker_thread_timer_task_s* task);
+bool ak09916_initialised;
+RUN_AFTER(INIT_END) {
+    for (uint8_t i = 0; i < 5; i++) {
+        if (icm20x48_init(&icm20x48, 3, BOARD_PAL_LINE_SPI3_ICM_CS, ICM20x48_IMU_TYPE_ICM20948)) {
+            if (ak09916_init(&ak09916, &icm20x48)) {
+                ak09916_initialised = true;
+                break;
+            }
+        }
+        usleep(10000);
+    }
+    worker_thread_add_timer_task(&WT, &ak09916_test_task, ak09916_test_task_func, NULL, MS2ST(1), true);
+}
+
+static void ak09916_test_task_func(struct worker_thread_timer_task_s* task) {
+    (void)task;
+    if (!ak09916_initialised) {
+        if (icm20x48_init(&icm20x48, 3, BOARD_PAL_LINE_SPI3_ICM_CS, ICM20x48_IMU_TYPE_ICM20948)) {
+            if (ak09916_init(&ak09916, &icm20x48)) {
+                ak09916_initialised = true;
+            }
+        }
+        usleep(10000);
+    } else if (ak09916_update(&ak09916)) {
+        uavcan_send_debug_keyvalue("magX", ak09916.meas.x);
+        uavcan_send_debug_keyvalue("magY", ak09916.meas.y);
+        uavcan_send_debug_keyvalue("magZ", ak09916.meas.z);
+    }
+}
+
+void i2c_serve_interrupt(uint32_t isr) {
+    static uint8_t i2c2_transfer_byte_idx;
+    static uint8_t i2c2_transfer_address;
+    static uint8_t i2c2_transfer_direction;
+    if (isr & (1<<3)) { // ADDR
+        i2c2_transfer_address = (isr >> 17) & 0x7FU; // ADDCODE
+        i2c2_transfer_direction = (isr >> 16) & 1; // direction
+        i2c2_transfer_byte_idx = 0;
+        if (i2c2_transfer_direction) {
+            I2C2->ISR |= (1<<0); // TXE
+        }
+        I2C2->ICR |= (1<<3); // ADDRCF
+    }
+
+    if (isr & I2C_ISR_RXNE) {
+        uint8_t recv_byte = I2C2->RXDR & 0xff;; // reading clears our interrupt flag
+        switch(i2c2_transfer_address) {
+            case AK09916_I2C_ADDR:
+                ak09916_recv_byte(i2c2_transfer_byte_idx, recv_byte);
+                break;
+        }
+        i2c2_transfer_byte_idx++;
+    }
+
+    if (isr & I2C_ISR_TXIS) {
+        uint8_t send_byte = 0;
+        switch(i2c2_transfer_address) {
+            case AK09916_I2C_ADDR:
+                send_byte = ak09916_send_byte();
+                break;
+        }
+    	I2C2->TXDR = send_byte;
+
+        i2c2_transfer_byte_idx++;
+    }
+}
+
+OSAL_IRQ_HANDLER(STM32_I2C2_EVENT_HANDLER) {
+    uint32_t isr = I2C2->ISR;
+
+    OSAL_IRQ_PROLOGUE();
+
+    i2c_serve_interrupt(isr);
+
+    OSAL_IRQ_EPILOGUE();
+}