From d0035c686a922e5216c4a742ece69494b7fd21cd Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Tue, 5 Dec 2023 13:49:41 -0700 Subject: [PATCH] Reformatted with clang-format --- boards/varmint/include/Varmint.h | 235 ++-- boards/varmint/include/board/Adc.h | 32 +- boards/varmint/include/board/Adis165xx.h | 67 +- boards/varmint/include/board/Bmi088.h | 77 +- boards/varmint/include/board/Bmi088_config.h | 68 +- boards/varmint/include/board/Bmi08_defs.h | 1130 +++++++++--------- boards/varmint/include/board/BoardConfig.h | 465 ++++--- boards/varmint/include/board/ByteFifo.h | 80 +- boards/varmint/include/board/Callbacks.h | 12 +- boards/varmint/include/board/CubeMX.h | 73 +- boards/varmint/include/board/DlhrL20G.h | 53 +- boards/varmint/include/board/Dps310.h | 63 +- boards/varmint/include/board/Driver.h | 29 +- boards/varmint/include/board/Iis2mdc.h | 55 +- boards/varmint/include/board/PacketFifo.h | 168 +-- boards/varmint/include/board/Packets.h | 114 +- boards/varmint/include/board/Pwm.h | 102 +- boards/varmint/include/board/Sbus.h | 51 +- boards/varmint/include/board/Sd.h | 19 +- boards/varmint/include/board/Spi.h | 172 ++- boards/varmint/include/board/Telem.h | 83 +- boards/varmint/include/board/Time64.h | 158 +-- boards/varmint/include/board/Ubx.h | 242 ++-- boards/varmint/include/board/Vcp.h | 33 +- boards/varmint/include/board/misc.h | 16 +- boards/varmint/include/main.h | 121 +- boards/varmint/include/stm32h7xx_hal_conf.h | 429 +++---- boards/varmint/include/stm32h7xx_it.h | 155 +-- boards/varmint/src/Varmint.cpp | 980 +++++++-------- boards/varmint/src/board/Adc.cpp | 397 +++--- boards/varmint/src/board/Adis165xx.cpp | 376 +++--- boards/varmint/src/board/Bmi088.cpp | 607 +++++----- boards/varmint/src/board/Callbacks.cpp | 87 +- boards/varmint/src/board/DlhrL20G.cpp | 235 ++-- boards/varmint/src/board/Dps310.cpp | 438 +++---- boards/varmint/src/board/Iis2mdc.cpp | 431 +++---- boards/varmint/src/board/Pwm.cpp | 123 +- boards/varmint/src/board/Sbus.cpp | 333 +++--- boards/varmint/src/board/Sd.cpp | 130 +- boards/varmint/src/board/Telem.cpp | 282 ++--- boards/varmint/src/board/Ubx.cpp | 809 +++++++------ boards/varmint/src/board/Vcp.cpp | 125 +- boards/varmint/src/board/misc.cpp | 115 +- boards/varmint/src/board/sandbox.cpp | 131 +- boards/varmint/src/main.cpp | 40 +- scripts/fix-code-style.sh | 2 +- 46 files changed, 5105 insertions(+), 4838 deletions(-) diff --git a/boards/varmint/include/Varmint.h b/boards/varmint/include/Varmint.h index f1017dd5..b4388f2b 100644 --- a/boards/varmint/include/Varmint.h +++ b/boards/varmint/include/Varmint.h @@ -38,150 +38,147 @@ #ifndef VARMINT_H_ #define VARMINT_H_ +#include #include #include #include #include #include +#include #include -#include -#include +#include #include +#include #include -#include -#include - #include #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif -int varmint_main(void); - + int varmint_main(void); #ifdef __cplusplus } #endif - /* * */ class Varmint : public rosflight_firmware::Board { - /** - * \brief - * - * - */ + /** + * \brief + * + * + */ private: - uint32_t serial_device_; -public: - Varmint() {}; - - Adis165xx imu0_; - Bmi088 imu1_; - DlhrL20G pitot_; - Dps310 baro_; - Iis2mdc mag_; - Sbus rc_; - Ubx gps_; - Adc adc_; - Telem telem_; - Vcp vcp_; - Pwm pwm_[PWM_CHANNELS]; - Sd sd_; - - // Required ROSflight Board HAL functions: - - // setup - - void init_board(void) override; - void board_reset(bool bootloader) override; - - // clock - uint32_t clock_millis() override; - uint64_t clock_micros() override; - void clock_delay(uint32_t milliseconds) override; - - // serial - void serial_init(uint32_t baud_rate, uint32_t dev) override; - void serial_write(const uint8_t *src, size_t len) override; - uint16_t serial_bytes_available() override; - uint8_t serial_read() override; - void serial_flush() override; - - // sensors - void sensors_init() override; - uint16_t num_sensor_errors() override; - - uint8_t imu_has_new_data() override; - bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us) override; - void imu_not_responding_error() override; - - bool mag_present() override; - uint8_t mag_has_new_data() override; - bool mag_read(float mag[3]) override; - - bool baro_present() override; - uint8_t baro_has_new_data() override; - bool baro_read(float *pressure, float *temperature) override; - - bool diff_pressure_present() override; - uint8_t diff_pressure_has_new_data() override; - bool diff_pressure_read(float *diff_pressure, float *temperature) override; - - bool sonar_present() override; - uint8_t sonar_has_new_data() override; - bool sonar_read( float *range) override; - - //Battery - uint8_t battery_has_new_data() override; - bool battery_read(float* voltage, float* current) override; - bool battery_present() override; - void battery_voltage_set_multiplier(double multiplier) override; - void battery_current_set_multiplier(double multiplier) override; - - // GNSS - bool gnss_present() override; - uint8_t gnss_has_new_data() override; - bool gnss_read( rosflight_firmware::GNSSData *gnss, rosflight_firmware::GNSSFull *gnss_full) override; - - // RC - void rc_init(rc_type_t rc_type) override; - uint8_t rc_has_new_data() override; - bool rc_lost() override; -// float rc_read(uint8_t channel) override; - float rc_read(uint8_t chan) override; - - // PWM - void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) override; - void pwm_disable() override; - void pwm_write(uint8_t channel, float value) override; - uint32_t pwm_init_timers(uint32_t servo_pwm_period_us); - - // non-volatile memory - void memory_init() override; - bool memory_read(void *dest, size_t len) override; - bool memory_write(const void *src, size_t len) override; - - // LEDs - void led0_on() override; - void led0_off() override; - void led0_toggle() override; - - void led1_on() override; - void led1_off() override; - void led1_toggle() override; - - // Backup Data - void backup_memory_init() override; - bool backup_memory_read(void *dest, size_t len) override; - void backup_memory_write(const void *src, size_t len) override; - void backup_memory_clear(size_t len) override; + uint32_t serial_device_; +public: + Varmint(){}; + + Adis165xx imu0_; + Bmi088 imu1_; + DlhrL20G pitot_; + Dps310 baro_; + Iis2mdc mag_; + Sbus rc_; + Ubx gps_; + Adc adc_; + Telem telem_; + Vcp vcp_; + Pwm pwm_[PWM_CHANNELS]; + Sd sd_; + + // Required ROSflight Board HAL functions: + + // setup + + void init_board(void) override; + void board_reset(bool bootloader) override; + + // clock + uint32_t clock_millis() override; + uint64_t clock_micros() override; + void clock_delay(uint32_t milliseconds) override; + + // serial + void serial_init(uint32_t baud_rate, uint32_t dev) override; + void serial_write(const uint8_t *src, size_t len) override; + uint16_t serial_bytes_available() override; + uint8_t serial_read() override; + void serial_flush() override; + + // sensors + void sensors_init() override; + uint16_t num_sensor_errors() override; + + uint8_t imu_has_new_data() override; + bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us) override; + void imu_not_responding_error() override; + + bool mag_present() override; + uint8_t mag_has_new_data() override; + bool mag_read(float mag[3]) override; + + bool baro_present() override; + uint8_t baro_has_new_data() override; + bool baro_read(float *pressure, float *temperature) override; + + bool diff_pressure_present() override; + uint8_t diff_pressure_has_new_data() override; + bool diff_pressure_read(float *diff_pressure, float *temperature) override; + + bool sonar_present() override; + uint8_t sonar_has_new_data() override; + bool sonar_read(float *range) override; + + // Battery + uint8_t battery_has_new_data() override; + bool battery_read(float *voltage, float *current) override; + bool battery_present() override; + void battery_voltage_set_multiplier(double multiplier) override; + void battery_current_set_multiplier(double multiplier) override; + + // GNSS + bool gnss_present() override; + uint8_t gnss_has_new_data() override; + bool gnss_read(rosflight_firmware::GNSSData *gnss, rosflight_firmware::GNSSFull *gnss_full) override; + + // RC + void rc_init(rc_type_t rc_type) override; + uint8_t rc_has_new_data() override; + bool rc_lost() override; + // float rc_read(uint8_t channel) override; + float rc_read(uint8_t chan) override; + + // PWM + void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) override; + void pwm_disable() override; + void pwm_write(uint8_t channel, float value) override; + uint32_t pwm_init_timers(uint32_t servo_pwm_period_us); + + // non-volatile memory + void memory_init() override; + bool memory_read(void *dest, size_t len) override; + bool memory_write(const void *src, size_t len) override; + + // LEDs + void led0_on() override; + void led0_off() override; + void led0_toggle() override; + + void led1_on() override; + void led1_off() override; + void led1_toggle() override; + + // Backup Data + void backup_memory_init() override; + bool backup_memory_read(void *dest, size_t len) override; + void backup_memory_write(const void *src, size_t len) override; + void backup_memory_clear(size_t len) override; }; - #endif /* VARMINT_H_ */ diff --git a/boards/varmint/include/board/Adc.h b/boards/varmint/include/board/Adc.h index 78cc3e61..0c41a34a 100644 --- a/boards/varmint/include/board/Adc.h +++ b/boards/varmint/include/board/Adc.h @@ -38,8 +38,8 @@ #ifndef ADC_H_ #define ADC_H_ -#include #include +#include #include /* @@ -48,22 +48,22 @@ class Adc : public Driver { public: - uint32_t init - ( - uint16_t sample_rate_hz, - ADC_HandleTypeDef *hadc, ADC_TypeDef *adc_instance, - ADC_HandleTypeDef *hadc_internal, ADC_TypeDef *adc_instance_internal - ); - bool poll(void){return false;}; - bool poll(uint16_t poll_offset); - bool startDma(void) override; - void endDma(void) override; - bool display(void) override; - bool isMy(ADC_HandleTypeDef *hadc) { return hadcExt_==hadc; } - void setScaleFactor(uint16_t n, float scale_factor); + uint32_t init(uint16_t sample_rate_hz, + ADC_HandleTypeDef *hadc, + ADC_TypeDef *adc_instance, + ADC_HandleTypeDef *hadc_internal, + ADC_TypeDef *adc_instance_internal); + bool poll(void) { return false; }; + bool poll(uint16_t poll_offset); + bool startDma(void) override; + void endDma(void) override; + bool display(void) override; + bool isMy(ADC_HandleTypeDef *hadc) { return hadcExt_ == hadc; } + void setScaleFactor(uint16_t n, float scale_factor); + private: - ADC_HandleTypeDef *hadcExt_,*hadcInt_; // The shared SPI handle - double scaleFactor_[ADC_CHANNELS_EXT] = {1.1, 2.69, 4.01, 1.68, 10.00, 11.215}; + ADC_HandleTypeDef *hadcExt_, *hadcInt_; // The shared SPI handle + double scaleFactor_[ADC_CHANNELS_EXT] = {1.1, 2.69, 4.01, 1.68, 10.00, 11.215}; }; #endif /* ADC_H_ */ diff --git a/boards/varmint/include/board/Adis165xx.h b/boards/varmint/include/board/Adis165xx.h index 88283204..aed6053b 100644 --- a/boards/varmint/include/board/Adis165xx.h +++ b/boards/varmint/include/board/Adis165xx.h @@ -43,47 +43,44 @@ #include #include - class Adis165xx : public Driver { public: - uint32_t init - ( - // Driver initializers - uint16_t sample_rate_hz, - GPIO_TypeDef *drdy_port, // Reset GPIO Port - uint16_t drdy_pin, // Reset GPIO Pin - // SPI initializers - SPI_HandleTypeDef *hspi, - GPIO_TypeDef *cs_port, // Chip Select GPIO Port - uint16_t cs_pin, // Chip Select GPIO Pin - // ADIS165xx initializers - GPIO_TypeDef *reset_port, // Reset GPIO Port - uint16_t reset_pin, // Reset GPIO Pin - TIM_HandleTypeDef *htim, - TIM_TypeDef *htim_instance, - uint32_t htim_channel, - uint32_t htim_period_us - ); + uint32_t init( + // Driver initializers + uint16_t sample_rate_hz, + GPIO_TypeDef *drdy_port, // Reset GPIO Port + uint16_t drdy_pin, // Reset GPIO Pin + // SPI initializers + SPI_HandleTypeDef *hspi, + GPIO_TypeDef *cs_port, // Chip Select GPIO Port + uint16_t cs_pin, // Chip Select GPIO Pin + // ADIS165xx initializers + GPIO_TypeDef *reset_port, // Reset GPIO Port + uint16_t reset_pin, // Reset GPIO Pin + TIM_HandleTypeDef *htim, + TIM_TypeDef *htim_instance, + uint32_t htim_channel, + uint32_t htim_period_us); - void endDma(void) override; - bool startDma(void) override; - bool display(void) override; - bool isMy(uint16_t exti_pin) { return drdyPin_== exti_pin;} - bool isMy(SPI_HandleTypeDef *hspi) { return hspi==spi_.hspi(); } - SPI_HandleTypeDef *hspi(void) {return spi_.hspi();} + void endDma(void) override; + bool startDma(void) override; + bool display(void) override; + bool isMy(uint16_t exti_pin) { return drdyPin_ == exti_pin; } + bool isMy(SPI_HandleTypeDef *hspi) { return hspi == spi_.hspi(); } + SPI_HandleTypeDef *hspi(void) { return spi_.hspi(); } private: - // SPI Stuff - Spi spi_; - uint16_t timeoutMs_; - // ADIS165xx Stuff - GPIO_TypeDef* resetPort_; - uint16_t resetPin_; - TIM_HandleTypeDef *htim_; - uint32_t htimChannel_; - void writeRegister(uint8_t address, uint16_t value); - uint16_t readRegister(uint8_t address); + // SPI Stuff + Spi spi_; + uint16_t timeoutMs_; + // ADIS165xx Stuff + GPIO_TypeDef *resetPort_; + uint16_t resetPin_; + TIM_HandleTypeDef *htim_; + uint32_t htimChannel_; + void writeRegister(uint8_t address, uint16_t value); + uint16_t readRegister(uint8_t address); }; #endif /* ADIS165XX_H_ */ diff --git a/boards/varmint/include/board/Bmi088.h b/boards/varmint/include/board/Bmi088.h index 36c8e6bb..cff0ff8a 100644 --- a/boards/varmint/include/board/Bmi088.h +++ b/boards/varmint/include/board/Bmi088.h @@ -47,51 +47,50 @@ */ class Bmi088 : public Driver { - /** - * \brief - * - * - */ + /** + * \brief + * + * + */ public: - uint32_t init - ( - // Driver initializers - uint16_t sample_rate, - GPIO_TypeDef *drdy_port, // DRDY GPIO Port - uint16_t drdy_pin, // DRDY GPIO Pin - // SPI initializers - SPI_HandleTypeDef *hspi, - GPIO_TypeDef *cs_port_a, // Chip Select GPIO Port - uint16_t cs_pin_a, // Chip Select GPIO Pin - GPIO_TypeDef *cs_port_g, // Chip Select GPIO Port - uint16_t cs_pin_g, // Chip Select GPIO Pin - // Sensor Specific - uint8_t range_a, // 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s - uint8_t range_g // // 0,1,2,3 --> 3,6,12,24g - ); + uint32_t init( + // Driver initializers + uint16_t sample_rate, + GPIO_TypeDef *drdy_port, // DRDY GPIO Port + uint16_t drdy_pin, // DRDY GPIO Pin + // SPI initializers + SPI_HandleTypeDef *hspi, + GPIO_TypeDef *cs_port_a, // Chip Select GPIO Port + uint16_t cs_pin_a, // Chip Select GPIO Pin + GPIO_TypeDef *cs_port_g, // Chip Select GPIO Port + uint16_t cs_pin_g, // Chip Select GPIO Pin + // Sensor Specific + uint8_t range_a, // 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s + uint8_t range_g // // 0,1,2,3 --> 3,6,12,24g + ); - void endDma(void) override; - bool startDma(void) override; - bool display(void) override; + void endDma(void) override; + bool startDma(void) override; + bool display(void) override; - bool isMy(uint16_t exti_pin) { return drdyPin_== exti_pin; } - bool isMy(SPI_HandleTypeDef *hspi) { return hspi==spiA_.hspi(); } - SPI_HandleTypeDef *hspi(void) { return spiA_.hspi(); } + bool isMy(uint16_t exti_pin) { return drdyPin_ == exti_pin; } + bool isMy(SPI_HandleTypeDef *hspi) { return hspi == spiA_.hspi(); } + SPI_HandleTypeDef *hspi(void) { return spiA_.hspi(); } private: - // SPI Stuff - Spi spiA_; - Spi spiG_; - uint16_t timeoutMs_; - uint16_t seqCount_; - // BMI088 Stuff - uint8_t rangeA_,rangeG_; - uint16_t syncCfgMode_; + // SPI Stuff + Spi spiA_; + Spi spiG_; + uint16_t timeoutMs_; + uint16_t seqCount_; + // BMI088 Stuff + uint8_t rangeA_, rangeG_; + uint16_t syncCfgMode_; - void writeRegisterA(uint8_t address, uint8_t value); - uint8_t readRegisterA(uint8_t address); - void writeRegisterG(uint8_t address, uint8_t value); - uint8_t readRegisterG(uint8_t address); + void writeRegisterA(uint8_t address, uint8_t value); + uint8_t readRegisterA(uint8_t address); + void writeRegisterG(uint8_t address, uint8_t value); + uint8_t readRegisterG(uint8_t address); }; #endif /* BMI088_H_ */ diff --git a/boards/varmint/include/board/Bmi088_config.h b/boards/varmint/include/board/Bmi088_config.h index 56684131..83b53a54 100644 --- a/boards/varmint/include/board/Bmi088_config.h +++ b/boards/varmint/include/board/Bmi088_config.h @@ -1,36 +1,36 @@ /** -* Data in bmi_config[] is Copyright (c) 2022 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* 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 the copyright holder 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. -* -*/ + * Data in bmi_config[] is Copyright (c) 2022 Bosch Sensortec GmbH. All rights reserved. + * + * BSD-3-Clause + * + * 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 the copyright holder 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. + * + */ #ifndef BMI088_CONFIG_H_ #define BMI088_CONFIG_H_ @@ -359,8 +359,6 @@ static const uint8_t bmi_config[] = { 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, - 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00 -}; - + 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00}; #endif /* BMI088_CONFIG_H_ */ diff --git a/boards/varmint/include/board/Bmi08_defs.h b/boards/varmint/include/board/Bmi08_defs.h index ec76a269..7bd642ae 100644 --- a/boards/varmint/include/board/Bmi08_defs.h +++ b/boards/varmint/include/board/Bmi08_defs.h @@ -1,40 +1,40 @@ /** -* Copyright (c) 2022 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* 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 the copyright holder 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. -* -* @file bmi08_defs.h -* @date 2022-04-10 -* @version v1.6.0 -* -*/ + * Copyright (c) 2022 Bosch Sensortec GmbH. All rights reserved. + * + * BSD-3-Clause + * + * 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 the copyright holder 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. + * + * @file bmi08_defs.h + * @date 2022-04-10 + * @version v1.6.0 + * + */ #ifndef BMI08_DEFS_H_ #define BMI08_DEFS_H_ @@ -42,12 +42,12 @@ /*********************************************************************/ /**\ header files */ #ifdef __KERNEL__ -#include #include +#include #else -#include -#include #include +#include +#include #endif /*********************************************************************/ @@ -55,54 +55,54 @@ /*********************************************************************/ #if !defined(UINT8_C) && !defined(INT8_C) -#define INT8_C(x) S8_C(x) -#define UINT8_C(x) U8_C(x) +#define INT8_C(x) S8_C(x) +#define UINT8_C(x) U8_C(x) #endif #if !defined(UINT16_C) && !defined(INT16_C) -#define INT16_C(x) S16_C(x) -#define UINT16_C(x) U16_C(x) +#define INT16_C(x) S16_C(x) +#define UINT16_C(x) U16_C(x) #endif #if !defined(INT32_C) && !defined(UINT32_C) -#define INT32_C(x) S32_C(x) -#define UINT32_C(x) U32_C(x) +#define INT32_C(x) S32_C(x) +#define UINT32_C(x) U32_C(x) #endif #if !defined(INT64_C) && !defined(UINT64_C) -#define INT64_C(x) S64_C(x) -#define UINT64_C(x) U64_C(x) +#define INT64_C(x) S64_C(x) +#define UINT64_C(x) U64_C(x) #endif /**\name C standard macros */ #ifndef NULL #ifdef __cplusplus -#define NULL 0 +#define NULL 0 #else -#define NULL ((void *) 0) +#define NULL ((void *)0) #endif #endif #ifndef TRUE -#define TRUE UINT8_C(1) +#define TRUE UINT8_C(1) #endif #ifndef FALSE -#define FALSE UINT8_C(0) +#define FALSE UINT8_C(0) #endif /** * BMI08_INTF_RET_TYPE is the read/write interface return type which can be overwritten by the build system. */ #ifndef BMI08_INTF_RET_TYPE -#define BMI08_INTF_RET_TYPE int8_t +#define BMI08_INTF_RET_TYPE int8_t #endif /** * The last error code from read/write interface is stored in the device structure as intf_rslt. */ #ifndef BMI08_INTF_RET_SUCCESS -#define BMI08_INTF_RET_SUCCESS INT8_C(0) +#define BMI08_INTF_RET_SUCCESS INT8_C(0) #endif /*************************** BMI08 Accelerometer Macros *****************************/ @@ -111,635 +111,633 @@ /* Accel registers */ /**\name Accel Chip Id register */ -#define BMI08_REG_ACCEL_CHIP_ID UINT8_C(0x00) +#define BMI08_REG_ACCEL_CHIP_ID UINT8_C(0x00) /**\name Accel Error condition register */ -#define BMI08_REG_ACCEL_ERR UINT8_C(0x02) +#define BMI08_REG_ACCEL_ERR UINT8_C(0x02) /**\name Accel Status flag register */ -#define BMI08_REG_ACCEL_STATUS UINT8_C(0x03) +#define BMI08_REG_ACCEL_STATUS UINT8_C(0x03) /**\name Accel X LSB data register */ -#define BMI08_REG_ACCEL_X_LSB UINT8_C(0x12) +#define BMI08_REG_ACCEL_X_LSB UINT8_C(0x12) /**\name Accel X MSB data register */ -#define BMI08_REG_ACCEL_X_MSB UINT8_C(0x13) +#define BMI08_REG_ACCEL_X_MSB UINT8_C(0x13) /**\name Accel Y LSB data register */ -#define BMI08_REG_ACCEL_Y_LSB UINT8_C(0x14) +#define BMI08_REG_ACCEL_Y_LSB UINT8_C(0x14) /**\name Accel Y MSB data register */ -#define BMI08_REG_ACCEL_Y_MSB UINT8_C(0x15) +#define BMI08_REG_ACCEL_Y_MSB UINT8_C(0x15) /**\name Accel Z LSB data register */ -#define BMI08_REG_ACCEL_Z_LSB UINT8_C(0x16) +#define BMI08_REG_ACCEL_Z_LSB UINT8_C(0x16) /**\name Accel Z MSB data register */ -#define BMI08_REG_ACCEL_Z_MSB UINT8_C(0x17) +#define BMI08_REG_ACCEL_Z_MSB UINT8_C(0x17) /**\name Sensor time byte 0 register */ -#define BMI08_REG_ACCEL_SENSORTIME_0 UINT8_C(0x18) +#define BMI08_REG_ACCEL_SENSORTIME_0 UINT8_C(0x18) /**\name Sensor time byte 1 register */ -#define BMI08_REG_ACCEL_SENSORTIME_1 UINT8_C(0x19) +#define BMI08_REG_ACCEL_SENSORTIME_1 UINT8_C(0x19) /**\name Sensor time byte 2 register */ -#define BMI08_REG_ACCEL_SENSORTIME_2 UINT8_C(0x1A) +#define BMI08_REG_ACCEL_SENSORTIME_2 UINT8_C(0x1A) /**\name Accel Interrupt status0 register */ -#define BMI08_REG_ACCEL_INT_STAT_0 UINT8_C(0x1C) +#define BMI08_REG_ACCEL_INT_STAT_0 UINT8_C(0x1C) /**\name Accel Interrupt status1 register */ -#define BMI08_REG_ACCEL_INT_STAT_1 UINT8_C(0x1D) +#define BMI08_REG_ACCEL_INT_STAT_1 UINT8_C(0x1D) /**\name Accel general purpose register 0*/ -#define BMI08_REG_ACCEL_GP_0 UINT8_C(0x1E) +#define BMI08_REG_ACCEL_GP_0 UINT8_C(0x1E) /**\name Sensor temperature MSB data register */ -#define BMI08_REG_TEMP_MSB UINT8_C(0x22) +#define BMI08_REG_TEMP_MSB UINT8_C(0x22) /**\name Sensor temperature LSB data register */ -#define BMI08_REG_TEMP_LSB UINT8_C(0x23) +#define BMI08_REG_TEMP_LSB UINT8_C(0x23) /**\name Accel general purpose register 4*/ -#define BMI08_REG_ACCEL_GP_4 UINT8_C(0x27) +#define BMI08_REG_ACCEL_GP_4 UINT8_C(0x27) /**\name Accel Internal status register */ -#define BMI08_REG_ACCEL_INTERNAL_STAT UINT8_C(0x2A) +#define BMI08_REG_ACCEL_INTERNAL_STAT UINT8_C(0x2A) /**\name Accel configuration register */ -#define BMI08_REG_ACCEL_CONF UINT8_C(0x40) +#define BMI08_REG_ACCEL_CONF UINT8_C(0x40) /**\name Accel range setting register */ -#define BMI08_REG_ACCEL_RANGE UINT8_C(0x41) +#define BMI08_REG_ACCEL_RANGE UINT8_C(0x41) /**\name Accel Interrupt pin 1 configuration register */ -#define BMI08_REG_ACCEL_INT1_IO_CONF UINT8_C(0x53) +#define BMI08_REG_ACCEL_INT1_IO_CONF UINT8_C(0x53) /**\name Accel Interrupt pin 2 configuration register */ -#define BMI08_REG_ACCEL_INT2_IO_CONF UINT8_C(0x54) +#define BMI08_REG_ACCEL_INT2_IO_CONF UINT8_C(0x54) /**\name Accel Interrupt latch configuration register */ -#define BMI08_REG_ACCEL_INT_LATCH_CONF UINT8_C(0x55) +#define BMI08_REG_ACCEL_INT_LATCH_CONF UINT8_C(0x55) /**\name Accel Interrupt pin1 mapping register */ -#define BMI08_REG_ACCEL_INT1_MAP UINT8_C(0x56) +#define BMI08_REG_ACCEL_INT1_MAP UINT8_C(0x56) /**\name Accel Interrupt pin2 mapping register */ -#define BMI08_REG_ACCEL_INT2_MAP UINT8_C(0x57) +#define BMI08_REG_ACCEL_INT2_MAP UINT8_C(0x57) /**\name Accel Interrupt map register */ -#define BMI08_REG_ACCEL_INT1_INT2_MAP_DATA UINT8_C(0x58) +#define BMI08_REG_ACCEL_INT1_INT2_MAP_DATA UINT8_C(0x58) /**\name Accel Init control register */ -#define BMI08_REG_ACCEL_INIT_CTRL UINT8_C(0x59) +#define BMI08_REG_ACCEL_INIT_CTRL UINT8_C(0x59) /**\name Accel Self test register */ -#define BMI08_REG_ACCEL_SELF_TEST UINT8_C(0x6D) +#define BMI08_REG_ACCEL_SELF_TEST UINT8_C(0x6D) /**\name Accel Power mode configuration register */ -#define BMI08_REG_ACCEL_PWR_CONF UINT8_C(0x7C) +#define BMI08_REG_ACCEL_PWR_CONF UINT8_C(0x7C) /**\name Accel Power control (switch on or off ) register */ -#define BMI08_REG_ACCEL_PWR_CTRL UINT8_C(0x7D) +#define BMI08_REG_ACCEL_PWR_CTRL UINT8_C(0x7D) /**\name Accel Soft reset register */ -#define BMI08_REG_ACCEL_SOFTRESET UINT8_C(0x7E) +#define BMI08_REG_ACCEL_SOFTRESET UINT8_C(0x7E) /**\name Feature Config related Registers */ -#define BMI08_REG_ACCEL_RESERVED_5B UINT8_C(0x5B) -#define BMI08_REG_ACCEL_RESERVED_5C UINT8_C(0x5C) -#define BMI08_REG_ACCEL_FEATURE_CFG UINT8_C(0x5E) +#define BMI08_REG_ACCEL_RESERVED_5B UINT8_C(0x5B) +#define BMI08_REG_ACCEL_RESERVED_5C UINT8_C(0x5C) +#define BMI08_REG_ACCEL_FEATURE_CFG UINT8_C(0x5E) /**\name Accel I2C slave address */ -#define BMI08_ACCEL_I2C_ADDR_PRIMARY UINT8_C(0x18) -#define BMI08_ACCEL_I2C_ADDR_SECONDARY UINT8_C(0x19) +#define BMI08_ACCEL_I2C_ADDR_PRIMARY UINT8_C(0x18) +#define BMI08_ACCEL_I2C_ADDR_SECONDARY UINT8_C(0x19) /**\name Interrupt masks */ -#define BMI08_ACCEL_DATA_READY_INT UINT8_C(0x80) -#define BMI08_ACCEL_FIFO_WM_INT UINT8_C(0x02) -#define BMI08_ACCEL_FIFO_FULL_INT UINT8_C(0x01) +#define BMI08_ACCEL_DATA_READY_INT UINT8_C(0x80) +#define BMI08_ACCEL_FIFO_WM_INT UINT8_C(0x02) +#define BMI08_ACCEL_FIFO_FULL_INT UINT8_C(0x01) -#define BMI08_GYRO_DATA_READY_INT UINT8_C(0x80) -#define BMI08_GYRO_FIFO_WM_INT UINT8_C(0x10) -#define BMI08_GYRO_FIFO_FULL_INT UINT8_C(0x10) +#define BMI08_GYRO_DATA_READY_INT UINT8_C(0x80) +#define BMI08_GYRO_FIFO_WM_INT UINT8_C(0x10) +#define BMI08_GYRO_FIFO_FULL_INT UINT8_C(0x10) /**\name Initialization status */ -#define BMI08_INIT_NOT_OK UINT8_C(0x00) -#define BMI08_INIT_OK UINT8_C(0x01) +#define BMI08_INIT_NOT_OK UINT8_C(0x00) +#define BMI08_INIT_OK UINT8_C(0x01) /**\name Accel Bandwidth */ -#define BMI08_ACCEL_BW_OSR4 UINT8_C(0x08) -#define BMI08_ACCEL_BW_OSR2 UINT8_C(0x09) -#define BMI08_ACCEL_BW_NORMAL UINT8_C(0x0A) +#define BMI08_ACCEL_BW_OSR4 UINT8_C(0x08) +#define BMI08_ACCEL_BW_OSR2 UINT8_C(0x09) +#define BMI08_ACCEL_BW_NORMAL UINT8_C(0x0A) /**\name Accel Output data rate */ -#define BMI08_ACCEL_ODR_12_5_HZ UINT8_C(0x05) -#define BMI08_ACCEL_ODR_25_HZ UINT8_C(0x06) -#define BMI08_ACCEL_ODR_50_HZ UINT8_C(0x07) -#define BMI08_ACCEL_ODR_100_HZ UINT8_C(0x08) -#define BMI08_ACCEL_ODR_200_HZ UINT8_C(0x09) -#define BMI08_ACCEL_ODR_400_HZ UINT8_C(0x0A) -#define BMI08_ACCEL_ODR_800_HZ UINT8_C(0x0B) -#define BMI08_ACCEL_ODR_1600_HZ UINT8_C(0x0C) +#define BMI08_ACCEL_ODR_12_5_HZ UINT8_C(0x05) +#define BMI08_ACCEL_ODR_25_HZ UINT8_C(0x06) +#define BMI08_ACCEL_ODR_50_HZ UINT8_C(0x07) +#define BMI08_ACCEL_ODR_100_HZ UINT8_C(0x08) +#define BMI08_ACCEL_ODR_200_HZ UINT8_C(0x09) +#define BMI08_ACCEL_ODR_400_HZ UINT8_C(0x0A) +#define BMI08_ACCEL_ODR_800_HZ UINT8_C(0x0B) +#define BMI08_ACCEL_ODR_1600_HZ UINT8_C(0x0C) /**\name Accel Self test */ -#define BMI08_ACCEL_SWITCH_OFF_SELF_TEST UINT8_C(0x00) -#define BMI08_ACCEL_POSITIVE_SELF_TEST UINT8_C(0x0D) -#define BMI08_ACCEL_NEGATIVE_SELF_TEST UINT8_C(0x09) +#define BMI08_ACCEL_SWITCH_OFF_SELF_TEST UINT8_C(0x00) +#define BMI08_ACCEL_POSITIVE_SELF_TEST UINT8_C(0x0D) +#define BMI08_ACCEL_NEGATIVE_SELF_TEST UINT8_C(0x09) /**\name Accel Power mode */ -#define BMI08_ACCEL_PM_ACTIVE UINT8_C(0x00) -#define BMI08_ACCEL_PM_SUSPEND UINT8_C(0x03) +#define BMI08_ACCEL_PM_ACTIVE UINT8_C(0x00) +#define BMI08_ACCEL_PM_SUSPEND UINT8_C(0x03) /**\name Accel Power control settings */ -#define BMI08_ACCEL_POWER_DISABLE UINT8_C(0x00) -#define BMI08_ACCEL_POWER_ENABLE UINT8_C(0x04) +#define BMI08_ACCEL_POWER_DISABLE UINT8_C(0x00) +#define BMI08_ACCEL_POWER_ENABLE UINT8_C(0x04) /**\name Accel internal interrupt pin mapping */ -#define BMI08_ACCEL_DATA_SYNC_INT_DISABLE UINT8_C(0x00) -#define BMI08_ACCEL_DATA_SYNC_INT_ENABLE UINT8_C(0x01) +#define BMI08_ACCEL_DATA_SYNC_INT_DISABLE UINT8_C(0x00) +#define BMI08_ACCEL_DATA_SYNC_INT_ENABLE UINT8_C(0x01) /**\name Accel Soft reset delay */ -#define BMI08_ACCEL_SOFTRESET_DELAY_MS UINT8_C(1) +#define BMI08_ACCEL_SOFTRESET_DELAY_MS UINT8_C(1) /**\name Mask definitions for ACCEL_ERR_REG register */ -#define BMI08_FATAL_ERR_MASK UINT8_C(0x01) -#define BMI08_ERR_CODE_MASK UINT8_C(0x1C) +#define BMI08_FATAL_ERR_MASK UINT8_C(0x01) +#define BMI08_ERR_CODE_MASK UINT8_C(0x1C) /**\name Position definitions for ACCEL_ERR_REG register */ -#define BMI08_CMD_ERR_POS UINT8_C(1) -#define BMI08_ERR_CODE_POS UINT8_C(2) +#define BMI08_CMD_ERR_POS UINT8_C(1) +#define BMI08_ERR_CODE_POS UINT8_C(2) /**\name Mask definition for ACCEL_STATUS_REG register */ -#define BMI08_ACCEL_STATUS_MASK UINT8_C(0x80) +#define BMI08_ACCEL_STATUS_MASK UINT8_C(0x80) /**\name Position definitions for ACCEL_STATUS_REG */ -#define BMI08_ACCEL_STATUS_POS UINT8_C(7) +#define BMI08_ACCEL_STATUS_POS UINT8_C(7) /**\name Mask definitions for odr, bandwidth and range */ -#define BMI08_ACCEL_ODR_MASK UINT8_C(0x0F) -#define BMI08_ACCEL_BW_MASK UINT8_C(0xF0) -#define BMI08_ACCEL_RANGE_MASK UINT8_C(0x03) +#define BMI08_ACCEL_ODR_MASK UINT8_C(0x0F) +#define BMI08_ACCEL_BW_MASK UINT8_C(0xF0) +#define BMI08_ACCEL_RANGE_MASK UINT8_C(0x03) /**\name Position definitions for odr, bandwidth and range */ -#define BMI08_ACCEL_BW_POS UINT8_C(4) +#define BMI08_ACCEL_BW_POS UINT8_C(4) /**\name Mask definitions for INT1_IO_CONF register */ -#define BMI08_ACCEL_INT_EDGE_MASK UINT8_C(0x01) -#define BMI08_ACCEL_INT_LVL_MASK UINT8_C(0x02) -#define BMI08_ACCEL_INT_OD_MASK UINT8_C(0x04) -#define BMI08_ACCEL_INT_IO_MASK UINT8_C(0x08) -#define BMI08_ACCEL_INT_IN_MASK UINT8_C(0x10) +#define BMI08_ACCEL_INT_EDGE_MASK UINT8_C(0x01) +#define BMI08_ACCEL_INT_LVL_MASK UINT8_C(0x02) +#define BMI08_ACCEL_INT_OD_MASK UINT8_C(0x04) +#define BMI08_ACCEL_INT_IO_MASK UINT8_C(0x08) +#define BMI08_ACCEL_INT_IN_MASK UINT8_C(0x10) /**\name Position definitions for INT1_IO_CONF register */ -#define BMI08_ACCEL_INT_EDGE_POS UINT8_C(0) -#define BMI08_ACCEL_INT_LVL_POS UINT8_C(1) -#define BMI08_ACCEL_INT_OD_POS UINT8_C(2) -#define BMI08_ACCEL_INT_IO_POS UINT8_C(3) -#define BMI08_ACCEL_INT_IN_POS UINT8_C(4) +#define BMI08_ACCEL_INT_EDGE_POS UINT8_C(0) +#define BMI08_ACCEL_INT_LVL_POS UINT8_C(1) +#define BMI08_ACCEL_INT_OD_POS UINT8_C(2) +#define BMI08_ACCEL_INT_IO_POS UINT8_C(3) +#define BMI08_ACCEL_INT_IN_POS UINT8_C(4) /**\name Mask definitions for INT1/INT2 mapping register */ -#define BMI08_ACCEL_MAP_INTA_MASK UINT8_C(0x01) +#define BMI08_ACCEL_MAP_INTA_MASK UINT8_C(0x01) /**\name Mask definitions for INT1/INT2 mapping register */ -#define BMI08_ACCEL_MAP_INTA_POS UINT8_C(0x00) +#define BMI08_ACCEL_MAP_INTA_POS UINT8_C(0x00) /**\name Mask definitions for INT1_INT2_MAP_DATA register */ -#define BMI08_ACCEL_INT1_DRDY_MASK UINT8_C(0x04) -#define BMI08_ACCEL_INT2_DRDY_MASK UINT8_C(0x40) +#define BMI08_ACCEL_INT1_DRDY_MASK UINT8_C(0x04) +#define BMI08_ACCEL_INT2_DRDY_MASK UINT8_C(0x40) /**\name Position definitions for INT1_INT2_MAP_DATA register */ -#define BMI08_ACCEL_INT1_DRDY_POS UINT8_C(2) -#define BMI08_ACCEL_INT2_DRDY_POS UINT8_C(6) +#define BMI08_ACCEL_INT1_DRDY_POS UINT8_C(2) +#define BMI08_ACCEL_INT2_DRDY_POS UINT8_C(6) /**\name Asic Initialization value */ -#define BMI08_ASIC_INITIALIZED UINT8_C(0x01) +#define BMI08_ASIC_INITIALIZED UINT8_C(0x01) /*************************** BMI08 Gyroscope Macros *****************************/ /** Register map */ /* Gyro registers */ /**\name Gyro Chip Id register */ -#define BMI08_REG_GYRO_CHIP_ID UINT8_C(0x00) +#define BMI08_REG_GYRO_CHIP_ID UINT8_C(0x00) /**\name Gyro X LSB data register */ -#define BMI08_REG_GYRO_X_LSB UINT8_C(0x02) +#define BMI08_REG_GYRO_X_LSB UINT8_C(0x02) /**\name Gyro X MSB data register */ -#define BMI08_REG_GYRO_X_MSB UINT8_C(0x03) +#define BMI08_REG_GYRO_X_MSB UINT8_C(0x03) /**\name Gyro Y LSB data register */ -#define BMI08_REG_GYRO_Y_LSB UINT8_C(0x04) +#define BMI08_REG_GYRO_Y_LSB UINT8_C(0x04) /**\name Gyro Y MSB data register */ -#define BMI08_REG_GYRO_Y_MSB UINT8_C(0x05) +#define BMI08_REG_GYRO_Y_MSB UINT8_C(0x05) /**\name Gyro Z LSB data register */ -#define BMI08_REG_GYRO_Z_LSB UINT8_C(0x06) +#define BMI08_REG_GYRO_Z_LSB UINT8_C(0x06) /**\name Gyro Z MSB data register */ -#define BMI08_REG_GYRO_Z_MSB UINT8_C(0x07) +#define BMI08_REG_GYRO_Z_MSB UINT8_C(0x07) /**\name Gyro Interrupt status register */ -#define BMI08_REG_GYRO_INT_STAT_1 UINT8_C(0x0A) +#define BMI08_REG_GYRO_INT_STAT_1 UINT8_C(0x0A) /**\name Gyro FIFO status register */ -#define BMI08_REG_GYRO_FIFO_STATUS UINT8_C(0x0E) +#define BMI08_REG_GYRO_FIFO_STATUS UINT8_C(0x0E) /**\name Gyro Range register */ -#define BMI08_REG_GYRO_RANGE UINT8_C(0x0F) +#define BMI08_REG_GYRO_RANGE UINT8_C(0x0F) /**\name Gyro Bandwidth register */ -#define BMI08_REG_GYRO_BANDWIDTH UINT8_C(0x10) +#define BMI08_REG_GYRO_BANDWIDTH UINT8_C(0x10) /**\name Gyro Power register */ -#define BMI08_REG_GYRO_LPM1 UINT8_C(0x11) +#define BMI08_REG_GYRO_LPM1 UINT8_C(0x11) /**\name Gyro Soft reset register */ -#define BMI08_REG_GYRO_SOFTRESET UINT8_C(0x14) +#define BMI08_REG_GYRO_SOFTRESET UINT8_C(0x14) /**\name Gyro Interrupt control register */ -#define BMI08_REG_GYRO_INT_CTRL UINT8_C(0x15) +#define BMI08_REG_GYRO_INT_CTRL UINT8_C(0x15) /**\name Gyro Interrupt Pin configuration register */ -#define BMI08_REG_GYRO_INT3_INT4_IO_CONF UINT8_C(0x16) +#define BMI08_REG_GYRO_INT3_INT4_IO_CONF UINT8_C(0x16) /**\name Gyro Interrupt Map register */ -#define BMI08_REG_GYRO_INT3_INT4_IO_MAP UINT8_C(0x18) +#define BMI08_REG_GYRO_INT3_INT4_IO_MAP UINT8_C(0x18) /**\name Gyro FIFO watermark enable register */ -#define BMI08_REG_GYRO_FIFO_WM_ENABLE UINT8_C(0x1E) +#define BMI08_REG_GYRO_FIFO_WM_ENABLE UINT8_C(0x1E) /**\name Gyro Self test register */ -#define BMI08_REG_GYRO_SELF_TEST UINT8_C(0x3C) +#define BMI08_REG_GYRO_SELF_TEST UINT8_C(0x3C) /**\name Gyro Fifo Config 0 register */ -#define BMI08_REG_GYRO_FIFO_CONFIG0 UINT8_C(0x3D) +#define BMI08_REG_GYRO_FIFO_CONFIG0 UINT8_C(0x3D) /**\name Gyro Fifo Config 1 register */ -#define BMI08_REG_GYRO_FIFO_CONFIG1 UINT8_C(0x3E) +#define BMI08_REG_GYRO_FIFO_CONFIG1 UINT8_C(0x3E) /**\name Gyro Fifo Data register */ -#define BMI08_REG_GYRO_FIFO_DATA UINT8_C(0x3F) +#define BMI08_REG_GYRO_FIFO_DATA UINT8_C(0x3F) /**\name Gyro unique chip identifier */ -#define BMI08_GYRO_CHIP_ID UINT8_C(0x0F) +#define BMI08_GYRO_CHIP_ID UINT8_C(0x0F) /**\name Gyro I2C slave address */ -#define BMI08_GYRO_I2C_ADDR_PRIMARY UINT8_C(0x68) -#define BMI08_GYRO_I2C_ADDR_SECONDARY UINT8_C(0x69) +#define BMI08_GYRO_I2C_ADDR_PRIMARY UINT8_C(0x68) +#define BMI08_GYRO_I2C_ADDR_SECONDARY UINT8_C(0x69) /**\name Gyro Range */ -#define BMI08_GYRO_RANGE_2000_DPS UINT8_C(0x00) -#define BMI08_GYRO_RANGE_1000_DPS UINT8_C(0x01) -#define BMI08_GYRO_RANGE_500_DPS UINT8_C(0x02) -#define BMI08_GYRO_RANGE_250_DPS UINT8_C(0x03) -#define BMI08_GYRO_RANGE_125_DPS UINT8_C(0x04) +#define BMI08_GYRO_RANGE_2000_DPS UINT8_C(0x00) +#define BMI08_GYRO_RANGE_1000_DPS UINT8_C(0x01) +#define BMI08_GYRO_RANGE_500_DPS UINT8_C(0x02) +#define BMI08_GYRO_RANGE_250_DPS UINT8_C(0x03) +#define BMI08_GYRO_RANGE_125_DPS UINT8_C(0x04) /**\name Gyro Output data rate and bandwidth */ -#define BMI08_GYRO_BW_532_ODR_2000_HZ UINT8_C(0x00) -#define BMI08_GYRO_BW_230_ODR_2000_HZ UINT8_C(0x01) -#define BMI08_GYRO_BW_116_ODR_1000_HZ UINT8_C(0x02) -#define BMI08_GYRO_BW_47_ODR_400_HZ UINT8_C(0x03) -#define BMI08_GYRO_BW_23_ODR_200_HZ UINT8_C(0x04) -#define BMI08_GYRO_BW_12_ODR_100_HZ UINT8_C(0x05) -#define BMI08_GYRO_BW_64_ODR_200_HZ UINT8_C(0x06) -#define BMI08_GYRO_BW_32_ODR_100_HZ UINT8_C(0x07) -#define BMI08_GYRO_ODR_RESET_VAL UINT8_C(0x80) +#define BMI08_GYRO_BW_532_ODR_2000_HZ UINT8_C(0x00) +#define BMI08_GYRO_BW_230_ODR_2000_HZ UINT8_C(0x01) +#define BMI08_GYRO_BW_116_ODR_1000_HZ UINT8_C(0x02) +#define BMI08_GYRO_BW_47_ODR_400_HZ UINT8_C(0x03) +#define BMI08_GYRO_BW_23_ODR_200_HZ UINT8_C(0x04) +#define BMI08_GYRO_BW_12_ODR_100_HZ UINT8_C(0x05) +#define BMI08_GYRO_BW_64_ODR_200_HZ UINT8_C(0x06) +#define BMI08_GYRO_BW_32_ODR_100_HZ UINT8_C(0x07) +#define BMI08_GYRO_ODR_RESET_VAL UINT8_C(0x80) /**\name Gyro Power mode */ -#define BMI08_GYRO_PM_NORMAL UINT8_C(0x00) -#define BMI08_GYRO_PM_DEEP_SUSPEND UINT8_C(0x20) -#define BMI08_GYRO_PM_SUSPEND UINT8_C(0x80) +#define BMI08_GYRO_PM_NORMAL UINT8_C(0x00) +#define BMI08_GYRO_PM_DEEP_SUSPEND UINT8_C(0x20) +#define BMI08_GYRO_PM_SUSPEND UINT8_C(0x80) /**\name Gyro data ready interrupt enable value */ -#define BMI08_GYRO_DRDY_INT_DISABLE_VAL UINT8_C(0x00) -#define BMI08_GYRO_DRDY_INT_ENABLE_VAL UINT8_C(0x80) -#define BMI08_GYRO_FIFO_INT_DISABLE_VAL UINT8_C(0x00) -#define BMI08_GYRO_FIFO_INT_ENABLE_VAL UINT8_C(0x40) -#define BMI08_GYRO_FIFO_WM_ENABLE_VAL UINT8_C(0x80) -#define BMI08_GYRO_FIFO_WM_DISABLE_VAL UINT8_C(0x00) +#define BMI08_GYRO_DRDY_INT_DISABLE_VAL UINT8_C(0x00) +#define BMI08_GYRO_DRDY_INT_ENABLE_VAL UINT8_C(0x80) +#define BMI08_GYRO_FIFO_INT_DISABLE_VAL UINT8_C(0x00) +#define BMI08_GYRO_FIFO_INT_ENABLE_VAL UINT8_C(0x40) +#define BMI08_GYRO_FIFO_WM_ENABLE_VAL UINT8_C(0x80) +#define BMI08_GYRO_FIFO_WM_DISABLE_VAL UINT8_C(0x00) /**\name Gyro data ready map values */ -#define BMI08_GYRO_MAP_DRDY_TO_INT3 UINT8_C(0x01) -#define BMI08_GYRO_MAP_DRDY_TO_INT4 UINT8_C(0x80) -#define BMI08_GYRO_MAP_DRDY_TO_BOTH_INT3_INT4 UINT8_C(0x81) -#define BMI08_GYRO_MAP_FIFO_INT3 UINT8_C(0x04) -#define BMI08_GYRO_MAP_FIFO_INT4 UINT8_C(0x20) -#define BMI08_GYRO_MAP_FIFO_BOTH_INT3_INT4 UINT8_C(0x24) +#define BMI08_GYRO_MAP_DRDY_TO_INT3 UINT8_C(0x01) +#define BMI08_GYRO_MAP_DRDY_TO_INT4 UINT8_C(0x80) +#define BMI08_GYRO_MAP_DRDY_TO_BOTH_INT3_INT4 UINT8_C(0x81) +#define BMI08_GYRO_MAP_FIFO_INT3 UINT8_C(0x04) +#define BMI08_GYRO_MAP_FIFO_INT4 UINT8_C(0x20) +#define BMI08_GYRO_MAP_FIFO_BOTH_INT3_INT4 UINT8_C(0x24) /**\name Gyro Soft reset delay */ -#define BMI08_GYRO_SOFTRESET_DELAY UINT8_C(30) +#define BMI08_GYRO_SOFTRESET_DELAY UINT8_C(30) /**\name Gyro power mode config delay */ -#define BMI08_GYRO_POWER_MODE_CONFIG_DELAY UINT8_C(30) +#define BMI08_GYRO_POWER_MODE_CONFIG_DELAY UINT8_C(30) /** Mask definitions for range, bandwidth and power */ -#define BMI08_GYRO_RANGE_MASK UINT8_C(0x07) -#define BMI08_GYRO_BW_MASK UINT8_C(0x0F) -#define BMI08_GYRO_POWER_MASK UINT8_C(0xA0) +#define BMI08_GYRO_RANGE_MASK UINT8_C(0x07) +#define BMI08_GYRO_BW_MASK UINT8_C(0x0F) +#define BMI08_GYRO_POWER_MASK UINT8_C(0xA0) /** Position definitions for range, bandwidth and power */ -#define BMI08_GYRO_POWER_POS UINT8_C(5) +#define BMI08_GYRO_POWER_POS UINT8_C(5) /**\name Mask definitions for BMI08_GYRO_INT_CTRL_REG register */ -#define BMI08_GYRO_DATA_EN_MASK UINT8_C(0x80) +#define BMI08_GYRO_DATA_EN_MASK UINT8_C(0x80) /**\name Position definitions for BMI08_GYRO_INT_CTRL_REG register */ -#define BMI08_GYRO_DATA_EN_POS UINT8_C(7) +#define BMI08_GYRO_DATA_EN_POS UINT8_C(7) /**\name Mask definitions for BMI08_GYRO_INT3_INT4_IO_CONF_REG register */ -#define BMI08_GYRO_INT3_LVL_MASK UINT8_C(0x01) -#define BMI08_GYRO_INT3_OD_MASK UINT8_C(0x02) -#define BMI08_GYRO_INT4_LVL_MASK UINT8_C(0x04) -#define BMI08_GYRO_INT4_OD_MASK UINT8_C(0x08) +#define BMI08_GYRO_INT3_LVL_MASK UINT8_C(0x01) +#define BMI08_GYRO_INT3_OD_MASK UINT8_C(0x02) +#define BMI08_GYRO_INT4_LVL_MASK UINT8_C(0x04) +#define BMI08_GYRO_INT4_OD_MASK UINT8_C(0x08) /**\name Position definitions for BMI08_GYRO_INT3_INT4_IO_CONF_REG register */ -#define BMI08_GYRO_INT3_OD_POS UINT8_C(1) -#define BMI08_GYRO_INT4_LVL_POS UINT8_C(2) -#define BMI08_GYRO_INT4_OD_POS UINT8_C(3) +#define BMI08_GYRO_INT3_OD_POS UINT8_C(1) +#define BMI08_GYRO_INT4_LVL_POS UINT8_C(2) +#define BMI08_GYRO_INT4_OD_POS UINT8_C(3) /**\name Mask definitions for BMI08_GYRO_INT_EN_REG register */ -#define BMI08_GYRO_INT_EN_MASK UINT8_C(0x80) +#define BMI08_GYRO_INT_EN_MASK UINT8_C(0x80) /**\name Position definitions for BMI08_GYRO_INT_EN_REG register */ -#define BMI08_GYRO_INT_EN_POS UINT8_C(7) +#define BMI08_GYRO_INT_EN_POS UINT8_C(7) /**\name Mask definitions for BMI088_GYRO_INT_MAP_REG register */ -#define BMI08_GYRO_INT3_MAP_MASK UINT8_C(0x01) -#define BMI08_GYRO_INT4_MAP_MASK UINT8_C(0x80) +#define BMI08_GYRO_INT3_MAP_MASK UINT8_C(0x01) +#define BMI08_GYRO_INT4_MAP_MASK UINT8_C(0x80) /**\name Position definitions for BMI088_GYRO_INT_MAP_REG register */ -#define BMI08_GYRO_INT3_MAP_POS UINT8_C(0) -#define BMI08_GYRO_INT4_MAP_POS UINT8_C(7) +#define BMI08_GYRO_INT3_MAP_POS UINT8_C(0) +#define BMI08_GYRO_INT4_MAP_POS UINT8_C(7) /**\name Mask definitions for BMI088_GYRO_INT_MAP_REG register */ -#define BMI088_GYRO_INT3_MAP_MASK UINT8_C(0x01) -#define BMI088_GYRO_INT4_MAP_MASK UINT8_C(0x80) +#define BMI088_GYRO_INT3_MAP_MASK UINT8_C(0x01) +#define BMI088_GYRO_INT4_MAP_MASK UINT8_C(0x80) /**\name Position definitions for BMI088_GYRO_INT_MAP_REG register */ -#define BMI088_GYRO_INT3_MAP_POS UINT8_C(0) -#define BMI088_GYRO_INT4_MAP_POS UINT8_C(7) +#define BMI088_GYRO_INT3_MAP_POS UINT8_C(0) +#define BMI088_GYRO_INT4_MAP_POS UINT8_C(7) /**\name Mask definitions for GYRO_SELF_TEST register */ -#define BMI08_GYRO_SELF_TEST_EN_MASK UINT8_C(0x01) -#define BMI08_GYRO_SELF_TEST_RDY_MASK UINT8_C(0x02) -#define BMI08_GYRO_SELF_TEST_RESULT_MASK UINT8_C(0x04) -#define BMI08_GYRO_SELF_TEST_FUNCTION_MASK UINT8_C(0x08) +#define BMI08_GYRO_SELF_TEST_EN_MASK UINT8_C(0x01) +#define BMI08_GYRO_SELF_TEST_RDY_MASK UINT8_C(0x02) +#define BMI08_GYRO_SELF_TEST_RESULT_MASK UINT8_C(0x04) +#define BMI08_GYRO_SELF_TEST_FUNCTION_MASK UINT8_C(0x08) /**\name Position definitions for GYRO_SELF_TEST register */ -#define BMI08_GYRO_SELF_TEST_RDY_POS UINT8_C(1) -#define BMI08_GYRO_SELF_TEST_RESULT_POS UINT8_C(2) -#define BMI08_GYRO_SELF_TEST_FUNCTION_POS UINT8_C(3) +#define BMI08_GYRO_SELF_TEST_RDY_POS UINT8_C(1) +#define BMI08_GYRO_SELF_TEST_RESULT_POS UINT8_C(2) +#define BMI08_GYRO_SELF_TEST_FUNCTION_POS UINT8_C(3) /**\name Gyro Fifo configurations */ -#define BMI08_GYRO_FIFO_OVERRUN_MASK UINT8_C(0x80) -#define BMI08_GYRO_FIFO_OVERRUN_POS UINT8_C(0x07) -#define BMI08_GYRO_FIFO_MODE_MASK UINT8_C(0xC0) -#define BMI08_GYRO_FIFO_MODE_POS UINT8_C(0x06) -#define BMI08_GYRO_FIFO_TAG_MASK UINT8_C(0x80) -#define BMI08_GYRO_FIFO_TAG_POS UINT8_C(0x07) -#define BMI08_GYRO_FIFO_DATA_SELECT_MASK UINT8_C(0x03) -#define BMI08_GYRO_FIFO_FRAME_COUNT_MASK UINT8_C(0x7F) -#define BMI08_GYRO_FIFO_WM_LEVEL_MASK UINT8_C(0x7F) +#define BMI08_GYRO_FIFO_OVERRUN_MASK UINT8_C(0x80) +#define BMI08_GYRO_FIFO_OVERRUN_POS UINT8_C(0x07) +#define BMI08_GYRO_FIFO_MODE_MASK UINT8_C(0xC0) +#define BMI08_GYRO_FIFO_MODE_POS UINT8_C(0x06) +#define BMI08_GYRO_FIFO_TAG_MASK UINT8_C(0x80) +#define BMI08_GYRO_FIFO_TAG_POS UINT8_C(0x07) +#define BMI08_GYRO_FIFO_DATA_SELECT_MASK UINT8_C(0x03) +#define BMI08_GYRO_FIFO_FRAME_COUNT_MASK UINT8_C(0x7F) +#define BMI08_GYRO_FIFO_WM_LEVEL_MASK UINT8_C(0x7F) /*! @name Gyro Fifo interrupt map */ -#define BMI08_GYRO_FIFO_INT3_MASK UINT8_C(0x04) -#define BMI08_GYRO_FIFO_INT3_POS UINT8_C(0x02) -#define BMI08_GYRO_FIFO_INT4_MASK UINT8_C(0x20) -#define BMI08_GYRO_FIFO_INT4_POS UINT8_C(0x05) +#define BMI08_GYRO_FIFO_INT3_MASK UINT8_C(0x04) +#define BMI08_GYRO_FIFO_INT3_POS UINT8_C(0x02) +#define BMI08_GYRO_FIFO_INT4_MASK UINT8_C(0x20) +#define BMI08_GYRO_FIFO_INT4_POS UINT8_C(0x05) /**\name Gyro FIFO definitions */ -#define BMI08_GYRO_FIFO_TAG_ENABLED UINT8_C(0x01) -#define BMI08_GYRO_FIFO_TAG_DISABLED UINT8_C(0x00) -#define BMI08_GYRO_FIFO_MODE_BYPASS UINT8_C(0x00) -#define BMI08_GYRO_FIFO_MODE UINT8_C(0x01) -#define BMI08_GYRO_FIFO_MODE_STREAM UINT8_C(0x02) -#define BMI08_GYRO_FIFO_XYZ_AXIS_ENABLED UINT8_C(0x00) -#define BMI08_GYRO_FIFO_X_AXIS_ENABLED UINT8_C(0x01) -#define BMI08_GYRO_FIFO_Y_AXIS_ENABLED UINT8_C(0x02) -#define BMI08_GYRO_FIFO_Z_AXIS_ENABLED UINT8_C(0x03) -#define BMI08_GYRO_FIFO_XYZ_AXIS_FRAME_SIZE UINT8_C(0x06) -#define BMI08_GYRO_FIFO_SINGLE_AXIS_FRAME_SIZE UINT8_C(0x02) -#define BMI08_GYRO_FIFO_1KB_BUFFER UINT16_C(1024) +#define BMI08_GYRO_FIFO_TAG_ENABLED UINT8_C(0x01) +#define BMI08_GYRO_FIFO_TAG_DISABLED UINT8_C(0x00) +#define BMI08_GYRO_FIFO_MODE_BYPASS UINT8_C(0x00) +#define BMI08_GYRO_FIFO_MODE UINT8_C(0x01) +#define BMI08_GYRO_FIFO_MODE_STREAM UINT8_C(0x02) +#define BMI08_GYRO_FIFO_XYZ_AXIS_ENABLED UINT8_C(0x00) +#define BMI08_GYRO_FIFO_X_AXIS_ENABLED UINT8_C(0x01) +#define BMI08_GYRO_FIFO_Y_AXIS_ENABLED UINT8_C(0x02) +#define BMI08_GYRO_FIFO_Z_AXIS_ENABLED UINT8_C(0x03) +#define BMI08_GYRO_FIFO_XYZ_AXIS_FRAME_SIZE UINT8_C(0x06) +#define BMI08_GYRO_FIFO_SINGLE_AXIS_FRAME_SIZE UINT8_C(0x02) +#define BMI08_GYRO_FIFO_1KB_BUFFER UINT16_C(1024) /*************************** Common Macros for both Accel and Gyro *****************************/ /**\name SPI read/write mask to configure address */ -#define BMI08_SPI_RD_MASK UINT8_C(0x80) -#define BMI08_SPI_WR_MASK UINT8_C(0x7F) +#define BMI08_SPI_RD_MASK UINT8_C(0x80) +#define BMI08_SPI_WR_MASK UINT8_C(0x7F) /**\name API success code */ -#define BMI08_OK INT8_C(0) +#define BMI08_OK INT8_C(0) /**\name API error codes */ -#define BMI08_E_NULL_PTR INT8_C(-1) -#define BMI08_E_COM_FAIL INT8_C(-2) -#define BMI08_E_DEV_NOT_FOUND INT8_C(-3) -#define BMI08_E_OUT_OF_RANGE INT8_C(-4) -#define BMI08_E_INVALID_INPUT INT8_C(-5) -#define BMI08_E_CONFIG_STREAM_ERROR INT8_C(-6) -#define BMI08_E_RD_WR_LENGTH_INVALID INT8_C(-7) -#define BMI08_E_INVALID_CONFIG INT8_C(-8) -#define BMI08_E_FEATURE_NOT_SUPPORTED INT8_C(-9) -#define BMI08_E_SELF_TEST_FAIL INT8_C(-10) +#define BMI08_E_NULL_PTR INT8_C(-1) +#define BMI08_E_COM_FAIL INT8_C(-2) +#define BMI08_E_DEV_NOT_FOUND INT8_C(-3) +#define BMI08_E_OUT_OF_RANGE INT8_C(-4) +#define BMI08_E_INVALID_INPUT INT8_C(-5) +#define BMI08_E_CONFIG_STREAM_ERROR INT8_C(-6) +#define BMI08_E_RD_WR_LENGTH_INVALID INT8_C(-7) +#define BMI08_E_INVALID_CONFIG INT8_C(-8) +#define BMI08_E_FEATURE_NOT_SUPPORTED INT8_C(-9) +#define BMI08_E_SELF_TEST_FAIL INT8_C(-10) /***\name Soft reset Value */ -#define BMI08_SOFT_RESET_CMD UINT8_C(0xB6) +#define BMI08_SOFT_RESET_CMD UINT8_C(0xB6) /**\name Enable/disable macros */ -#define BMI08_DISABLE UINT8_C(0) -#define BMI08_ENABLE UINT8_C(1) +#define BMI08_DISABLE UINT8_C(0) +#define BMI08_ENABLE UINT8_C(1) /**\name To define warnings for FIFO activity */ -#define BMI08_W_FIFO_EMPTY INT8_C(1) -#define BMI08_W_PARTIAL_READ INT8_C(2) +#define BMI08_W_FIFO_EMPTY INT8_C(1) +#define BMI08_W_PARTIAL_READ INT8_C(2) /**\name Maximum length to read */ -#define BMI08_MAX_LEN UINT8_C(128) +#define BMI08_MAX_LEN UINT8_C(128) /**\name Sensortime resolution in seconds */ -#define BMI08_SENSORTIME_RESOLUTION 0.0000390625f +#define BMI08_SENSORTIME_RESOLUTION 0.0000390625f /**\name Constant values macros */ -#define BMI08_SENSOR_DATA_SYNC_TIME_MS UINT8_C(1) -#define BMI08_DELAY_BETWEEN_WRITES_MS UINT8_C(1) -#define BMI08_SELF_TEST_DELAY_MS UINT8_C(3) -#define BMI08_POWER_CONFIG_DELAY UINT8_C(5) -#define BMI08_GYRO_SET_CONFIG_DELAY UINT8_C(10) -#define BMI08_SENSOR_SETTLE_TIME_MS UINT8_C(30) -#define BMI08_SET_ACCEL_CONF_DELAY UINT8_C(40) -#define BMI08_SELF_TEST_DATA_READ_MS UINT8_C(50) -#define BMI08_ASIC_INIT_TIME_MS UINT8_C(150) - -#define BMI08_CONFIG_STREAM_SIZE UINT16_C(6144) +#define BMI08_SENSOR_DATA_SYNC_TIME_MS UINT8_C(1) +#define BMI08_DELAY_BETWEEN_WRITES_MS UINT8_C(1) +#define BMI08_SELF_TEST_DELAY_MS UINT8_C(3) +#define BMI08_POWER_CONFIG_DELAY UINT8_C(5) +#define BMI08_GYRO_SET_CONFIG_DELAY UINT8_C(10) +#define BMI08_SENSOR_SETTLE_TIME_MS UINT8_C(30) +#define BMI08_SET_ACCEL_CONF_DELAY UINT8_C(40) +#define BMI08_SELF_TEST_DATA_READ_MS UINT8_C(50) +#define BMI08_ASIC_INIT_TIME_MS UINT8_C(150) + +#define BMI08_CONFIG_STREAM_SIZE UINT16_C(6144) /**\name Sensor time array parameter definitions */ -#define BMI08_SENSOR_TIME_MSB_BYTE UINT8_C(2) -#define BMI08_SENSOR_TIME_XLSB_BYTE UINT8_C(1) -#define BMI08_SENSOR_TIME_LSB_BYTE UINT8_C(0) +#define BMI08_SENSOR_TIME_MSB_BYTE UINT8_C(2) +#define BMI08_SENSOR_TIME_XLSB_BYTE UINT8_C(1) +#define BMI08_SENSOR_TIME_LSB_BYTE UINT8_C(0) /**\name int pin active state */ -#define BMI08_INT_ACTIVE_LOW UINT8_C(0) -#define BMI08_INT_ACTIVE_HIGH UINT8_C(1) +#define BMI08_INT_ACTIVE_LOW UINT8_C(0) +#define BMI08_INT_ACTIVE_HIGH UINT8_C(1) /**\name interrupt pin output definition */ -#define BMI08_INT_MODE_PUSH_PULL UINT8_C(0) -#define BMI08_INT_MODE_OPEN_DRAIN UINT8_C(1) +#define BMI08_INT_MODE_PUSH_PULL UINT8_C(0) +#define BMI08_INT_MODE_OPEN_DRAIN UINT8_C(1) /**\name Sensor bit resolution */ -#define BMI08_16_BIT_RESOLUTION UINT8_C(16) +#define BMI08_16_BIT_RESOLUTION UINT8_C(16) /**\name Convert milliseconds to microseconds */ -#define BMI08_MS_TO_US(X) UINT32_C(X * 1000) +#define BMI08_MS_TO_US(X) UINT32_C(X * 1000) /*********************************BMI08 FIFO Macros**********************************/ /** Register map */ /*! @name FIFO Header Mask definitions */ -#define BMI08_FIFO_HEADER_ACC_FRM UINT8_C(0x84) -#define BMI08_FIFO_HEADER_ALL_FRM UINT8_C(0x9C) -#define BMI08_FIFO_HEADER_SENS_TIME_FRM UINT8_C(0x44) -#define BMI08_FIFO_HEADER_SKIP_FRM UINT8_C(0x40) -#define BMI08_FIFO_HEADER_INPUT_CFG_FRM UINT8_C(0x48) -#define BMI08_FIFO_HEAD_OVER_READ_MSB UINT8_C(0x80) -#define BMI08_FIFO_SAMPLE_DROP_FRM UINT8_C(0x50) +#define BMI08_FIFO_HEADER_ACC_FRM UINT8_C(0x84) +#define BMI08_FIFO_HEADER_ALL_FRM UINT8_C(0x9C) +#define BMI08_FIFO_HEADER_SENS_TIME_FRM UINT8_C(0x44) +#define BMI08_FIFO_HEADER_SKIP_FRM UINT8_C(0x40) +#define BMI08_FIFO_HEADER_INPUT_CFG_FRM UINT8_C(0x48) +#define BMI08_FIFO_HEAD_OVER_READ_MSB UINT8_C(0x80) +#define BMI08_FIFO_SAMPLE_DROP_FRM UINT8_C(0x50) /* Accel registers */ -#define BMI08_FIFO_LENGTH_0_ADDR UINT8_C(0x24) -#define BMI08_FIFO_LENGTH_1_ADDR UINT8_C(0x25) -#define BMI08_FIFO_DATA_ADDR UINT8_C(0x26) -#define BMI08_FIFO_DOWNS_ADDR UINT8_C(0x45) -#define BMI08_FIFO_WTM_0_ADDR UINT8_C(0x46) -#define BMI08_FIFO_WTM_1_ADDR UINT8_C(0x47) -#define BMI08_FIFO_CONFIG_0_ADDR UINT8_C(0x48) -#define BMI08_FIFO_CONFIG_1_ADDR UINT8_C(0x49) +#define BMI08_FIFO_LENGTH_0_ADDR UINT8_C(0x24) +#define BMI08_FIFO_LENGTH_1_ADDR UINT8_C(0x25) +#define BMI08_FIFO_DATA_ADDR UINT8_C(0x26) +#define BMI08_FIFO_DOWNS_ADDR UINT8_C(0x45) +#define BMI08_FIFO_WTM_0_ADDR UINT8_C(0x46) +#define BMI08_FIFO_WTM_1_ADDR UINT8_C(0x47) +#define BMI08_FIFO_CONFIG_0_ADDR UINT8_C(0x48) +#define BMI08_FIFO_CONFIG_1_ADDR UINT8_C(0x49) /*! @name FIFO sensor data lengths */ -#define BMI08_FIFO_ACCEL_LENGTH UINT8_C(6) -#define BMI08_FIFO_WTM_LENGTH UINT8_C(2) -#define BMI08_FIFO_LENGTH_MSB_BYTE UINT8_C(1) -#define BMI08_FIFO_DATA_LENGTH UINT8_C(2) -#define BMI08_FIFO_CONFIG_LENGTH UINT8_C(2) -#define BMI08_SENSOR_TIME_LENGTH UINT8_C(3) -#define BMI08_FIFO_SKIP_FRM_LENGTH UINT8_C(1) -#define BMI08_FIFO_INPUT_CFG_LENGTH UINT8_C(1) +#define BMI08_FIFO_ACCEL_LENGTH UINT8_C(6) +#define BMI08_FIFO_WTM_LENGTH UINT8_C(2) +#define BMI08_FIFO_LENGTH_MSB_BYTE UINT8_C(1) +#define BMI08_FIFO_DATA_LENGTH UINT8_C(2) +#define BMI08_FIFO_CONFIG_LENGTH UINT8_C(2) +#define BMI08_SENSOR_TIME_LENGTH UINT8_C(3) +#define BMI08_FIFO_SKIP_FRM_LENGTH UINT8_C(1) +#define BMI08_FIFO_INPUT_CFG_LENGTH UINT8_C(1) /*! @name FIFO byte counter mask definition */ -#define BMI08_FIFO_BYTE_COUNTER_MSB_MASK UINT8_C(0x3F) +#define BMI08_FIFO_BYTE_COUNTER_MSB_MASK UINT8_C(0x3F) /*! @name FIFO frame masks */ -#define BMI08_FIFO_LSB_CONFIG_CHECK UINT8_C(0x00) -#define BMI08_FIFO_MSB_CONFIG_CHECK UINT8_C(0x80) -#define BMI08_FIFO_INTR_MASK UINT8_C(0x5C) +#define BMI08_FIFO_LSB_CONFIG_CHECK UINT8_C(0x00) +#define BMI08_FIFO_MSB_CONFIG_CHECK UINT8_C(0x80) +#define BMI08_FIFO_INTR_MASK UINT8_C(0x5C) /*name FIFO config modes */ -#define BMI08_ACC_STREAM_MODE UINT8_C(0x00) -#define BMI08_ACC_FIFO_MODE UINT8_C(0x01) +#define BMI08_ACC_STREAM_MODE UINT8_C(0x00) +#define BMI08_ACC_FIFO_MODE UINT8_C(0x01) /*name Mask definitions for FIFO configuration modes */ -#define BMI08_ACC_FIFO_MODE_CONFIG_MASK UINT8_C(0x01) +#define BMI08_ACC_FIFO_MODE_CONFIG_MASK UINT8_C(0x01) /*! @name Mask definitions for FIFO_CONFIG_1 register */ -#define BMI08_ACCEL_EN_MASK UINT8_C(0x40) -#define BMI08_ACCEL_INT1_EN_MASK UINT8_C(0x08) -#define BMI08_ACCEL_INT2_EN_MASK UINT8_C(0x04) +#define BMI08_ACCEL_EN_MASK UINT8_C(0x40) +#define BMI08_ACCEL_INT1_EN_MASK UINT8_C(0x08) +#define BMI08_ACCEL_INT2_EN_MASK UINT8_C(0x04) /*name Position definitions for FIFO_CONFIG_1 register */ -#define BMI08_ACCEL_EN_POS UINT8_C(6) -#define BMI08_ACCEL_INT1_EN_POS UINT8_C(3) -#define BMI08_ACCEL_INT2_EN_POS UINT8_C(2) +#define BMI08_ACCEL_EN_POS UINT8_C(6) +#define BMI08_ACCEL_INT1_EN_POS UINT8_C(3) +#define BMI08_ACCEL_INT2_EN_POS UINT8_C(2) /*! @name Position definitions for FIFO_DOWNS register */ -#define BMI08_ACC_FIFO_DOWNS_MASK UINT8_C(0xF0) +#define BMI08_ACC_FIFO_DOWNS_MASK UINT8_C(0xF0) /*! @name FIFO down sampling bit positions */ -#define BMI08_ACC_FIFO_DOWNS_POS UINT8_C(0x04) +#define BMI08_ACC_FIFO_DOWNS_POS UINT8_C(0x04) /*! @name FIFO down sampling user macros */ -#define BMI08_ACC_FIFO_DOWN_SAMPLE_0 UINT8_C(0) -#define BMI08_ACC_FIFO_DOWN_SAMPLE_1 UINT8_C(1) -#define BMI08_ACC_FIFO_DOWN_SAMPLE_2 UINT8_C(2) -#define BMI08_ACC_FIFO_DOWN_SAMPLE_3 UINT8_C(3) -#define BMI08_ACC_FIFO_DOWN_SAMPLE_4 UINT8_C(4) -#define BMI08_ACC_FIFO_DOWN_SAMPLE_5 UINT8_C(5) -#define BMI08_ACC_FIFO_DOWN_SAMPLE_6 UINT8_C(6) -#define BMI08_ACC_FIFO_DOWN_SAMPLE_7 UINT8_C(7) +#define BMI08_ACC_FIFO_DOWN_SAMPLE_0 UINT8_C(0) +#define BMI08_ACC_FIFO_DOWN_SAMPLE_1 UINT8_C(1) +#define BMI08_ACC_FIFO_DOWN_SAMPLE_2 UINT8_C(2) +#define BMI08_ACC_FIFO_DOWN_SAMPLE_3 UINT8_C(3) +#define BMI08_ACC_FIFO_DOWN_SAMPLE_4 UINT8_C(4) +#define BMI08_ACC_FIFO_DOWN_SAMPLE_5 UINT8_C(5) +#define BMI08_ACC_FIFO_DOWN_SAMPLE_6 UINT8_C(6) +#define BMI08_ACC_FIFO_DOWN_SAMPLE_7 UINT8_C(7) /*! @name Mask definitions for INT1_INT2_MAP_DATA register */ -#define BMI08_ACCEL_INT2_FWM_MASK UINT8_C(0x20) -#define BMI08_ACCEL_INT2_FFULL_MASK UINT8_C(0x10) -#define BMI08_ACCEL_INT1_FWM_MASK UINT8_C(0x02) -#define BMI08_ACCEL_INT1_FFULL_MASK UINT8_C(0x01) +#define BMI08_ACCEL_INT2_FWM_MASK UINT8_C(0x20) +#define BMI08_ACCEL_INT2_FFULL_MASK UINT8_C(0x10) +#define BMI08_ACCEL_INT1_FWM_MASK UINT8_C(0x02) +#define BMI08_ACCEL_INT1_FFULL_MASK UINT8_C(0x01) /*! @name Positions definitions for INT1_INT2_MAP_DATA register */ -#define BMI08_ACCEL_INT1_FWM_POS UINT8_C(1) -#define BMI08_ACCEL_INT2_FFULL_POS UINT8_C(4) -#define BMI08_ACCEL_INT2_FWM_POS UINT8_C(5) +#define BMI08_ACCEL_INT1_FWM_POS UINT8_C(1) +#define BMI08_ACCEL_INT2_FFULL_POS UINT8_C(4) +#define BMI08_ACCEL_INT2_FWM_POS UINT8_C(5) /*! @name Accel Data sync */ -#define BMI08_ACCEL_DATA_SYNC_ADR UINT8_C(0x02) -#define BMI08_ACCEL_DATA_SYNC_LEN UINT8_C(1) -#define BMI08_ACCEL_DATA_SYNC_MODE_MASK UINT16_C(0x0003) -#define BMI08_ACCEL_DATA_SYNC_MODE_SHIFT UINT8_C(0) +#define BMI08_ACCEL_DATA_SYNC_ADR UINT8_C(0x02) +#define BMI08_ACCEL_DATA_SYNC_LEN UINT8_C(1) +#define BMI08_ACCEL_DATA_SYNC_MODE_MASK UINT16_C(0x0003) +#define BMI08_ACCEL_DATA_SYNC_MODE_SHIFT UINT8_C(0) -#define BMI08_ACCEL_DATA_SYNC_MODE_OFF UINT8_C(0x00) -#define BMI08_ACCEL_DATA_SYNC_MODE_400HZ UINT8_C(0x01) -#define BMI08_ACCEL_DATA_SYNC_MODE_1000HZ UINT8_C(0x02) -#define BMI08_ACCEL_DATA_SYNC_MODE_2000HZ UINT8_C(0x03) +#define BMI08_ACCEL_DATA_SYNC_MODE_OFF UINT8_C(0x00) +#define BMI08_ACCEL_DATA_SYNC_MODE_400HZ UINT8_C(0x01) +#define BMI08_ACCEL_DATA_SYNC_MODE_1000HZ UINT8_C(0x02) +#define BMI08_ACCEL_DATA_SYNC_MODE_2000HZ UINT8_C(0x03) /**\name Absolute value */ #ifndef BMI08_ABS -#define BMI08_ABS(a) ((a) > 0 ? (a) : -(a)) +#define BMI08_ABS(a) ((a) > 0 ? (a) : -(a)) #endif /**\name Utility Macros */ -#define BMI08_SET_LOW_BYTE UINT16_C(0x00FF) -#define BMI08_SET_HIGH_BYTE UINT16_C(0xFF00) -#define BMI08_SET_LOW_NIBBLE UINT8_C(0x0F) +#define BMI08_SET_LOW_BYTE UINT16_C(0x00FF) +#define BMI08_SET_HIGH_BYTE UINT16_C(0xFF00) +#define BMI08_SET_LOW_NIBBLE UINT8_C(0x0F) /**\name Macro to SET and GET BITS of a register */ #define BMI08_SET_BITS(reg_var, bitname, val) \ - ((reg_var & ~(bitname##_MASK)) | \ - ((val << bitname##_POS) & bitname##_MASK)) + ((reg_var & ~(bitname##_MASK)) | ((val << bitname##_POS) & bitname##_MASK)) -#define BMI08_GET_BITS(reg_var, bitname) ((reg_var & (bitname##_MASK)) >> \ - (bitname##_POS)) +#define BMI08_GET_BITS(reg_var, bitname) ((reg_var & (bitname##_MASK)) >> (bitname##_POS)) -#define BMI08_SET_BITS_POS_0(reg_var, bitname, val) \ - ((reg_var & ~(bitname##_MASK)) | \ - (val & bitname##_MASK)) +#define BMI08_SET_BITS_POS_0(reg_var, bitname, val) ((reg_var & ~(bitname##_MASK)) | (val & bitname##_MASK)) -#define BMI08_GET_BITS_POS_0(reg_var, bitname) (reg_var & (bitname##_MASK)) +#define BMI08_GET_BITS_POS_0(reg_var, bitname) (reg_var & (bitname##_MASK)) -#define BMI08_SET_BIT_VAL_0(reg_var, bitname) (reg_var & ~(bitname##_MASK)) +#define BMI08_SET_BIT_VAL_0(reg_var, bitname) (reg_var & ~(bitname##_MASK)) /**\name Macro definition for difference between 2 values */ -#define BMI08_GET_DIFF(x, y) ((x) - (y)) +#define BMI08_GET_DIFF(x, y) ((x) - (y)) /**\name Macro definition to get LSB of 16 bit variable */ -#define BMI08_GET_LSB(var) (uint8_t)(var & BMI08_SET_LOW_BYTE) +#define BMI08_GET_LSB(var) (uint8_t)(var & BMI08_SET_LOW_BYTE) /**\name Macro definition to get MSB of 16 bit variable */ -#define BMI08_GET_MSB(var) (uint8_t)((var & BMI08_SET_HIGH_BYTE) >> 8) +#define BMI08_GET_MSB(var) (uint8_t)((var & BMI08_SET_HIGH_BYTE) >> 8) /*************************************************************************/ /*! * @brief Interface selection enums */ -enum bmi08_intf { - /*! I2C interface */ - BMI08_I2C_INTF, - /*! SPI interface */ - BMI08_SPI_INTF +enum bmi08_intf +{ + /*! I2C interface */ + BMI08_I2C_INTF, + /*! SPI interface */ + BMI08_SPI_INTF }; /*! * @brief Enum to define variants */ -enum bmi08_variant { - BMI085_VARIANT = 0, - BMI088_VARIANT = 1 +enum bmi08_variant +{ + BMI085_VARIANT = 0, + BMI088_VARIANT = 1 }; /*************************** Data structures *****************************/ @@ -778,7 +776,9 @@ typedef BMI08_INTF_RET_TYPE (*bmi08_read_fptr_t)(uint8_t reg_addr, uint8_t *reg_ * @retval Non-zero for Failure * */ -typedef BMI08_INTF_RET_TYPE (*bmi08_write_fptr_t)(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, +typedef BMI08_INTF_RET_TYPE (*bmi08_write_fptr_t)(uint8_t reg_addr, + const uint8_t *reg_data, + uint32_t len, void *intf_ptr); /*! @@ -799,14 +799,14 @@ typedef void (*bmi08_delay_us_fptr_t)(uint32_t period, void *intf_ptr); */ struct bmi08_sensor_data { - /*! X-axis sensor data */ - int16_t x; + /*! X-axis sensor data */ + int16_t x; - /*! Y-axis sensor data */ - int16_t y; + /*! Y-axis sensor data */ + int16_t y; - /*! Z-axis sensor data */ - int16_t z; + /*! Z-axis sensor data */ + int16_t z; }; /*! @@ -814,14 +814,14 @@ struct bmi08_sensor_data */ struct bmi08_sensor_data_f { - /*! X-axis sensor data */ - float x; + /*! X-axis sensor data */ + float x; - /*! Y-axis sensor data */ - float y; + /*! Y-axis sensor data */ + float y; - /*! Z-axis sensor data */ - float z; + /*! Z-axis sensor data */ + float z; }; /*! @@ -829,17 +829,17 @@ struct bmi08_sensor_data_f */ struct bmi08_cfg { - /*! power mode */ - uint8_t power; + /*! power mode */ + uint8_t power; - /*! range */ - uint8_t range; + /*! range */ + uint8_t range; - /*! bandwidth */ - uint8_t bw; + /*! bandwidth */ + uint8_t bw; - /*! output data rate */ - uint8_t odr; + /*! output data rate */ + uint8_t odr; }; /*! @@ -847,11 +847,11 @@ struct bmi08_cfg */ struct bmi08_err_reg { - /*! Indicates fatal error */ - uint8_t fatal_err; + /*! Indicates fatal error */ + uint8_t fatal_err; - /*! Indicates error code */ - uint8_t err_code; + /*! Indicates error code */ + uint8_t err_code; }; /*! @@ -859,55 +859,59 @@ struct bmi08_err_reg */ struct bmi08_data_sync_cfg { - /*! Mode (0 = off, 1 = 400Hz, 2 = 1kHz, 3 = 2kHz) */ - uint8_t mode; + /*! Mode (0 = off, 1 = 400Hz, 2 = 1kHz, 3 = 2kHz) */ + uint8_t mode; }; /*! * @brief Enum to select accelerometer Interrupt pins */ -enum bmi08_accel_int_channel { - /* interrupt Channel 1 for accel sensor */ - BMI08_INT_CHANNEL_1, - /* interrupt Channel 2 for accel sensor */ - BMI08_INT_CHANNEL_2 +enum bmi08_accel_int_channel +{ + /* interrupt Channel 1 for accel sensor */ + BMI08_INT_CHANNEL_1, + /* interrupt Channel 2 for accel sensor */ + BMI08_INT_CHANNEL_2 }; /*! * @brief Enum to select gyroscope Interrupt pins */ -enum bmi08_gyro_int_channel { - /* interrupt Channel 3 for gyro sensor */ - BMI08_INT_CHANNEL_3, - /* interrupt Channel 4 for gyro sensor */ - BMI08_INT_CHANNEL_4 +enum bmi08_gyro_int_channel +{ + /* interrupt Channel 3 for gyro sensor */ + BMI08_INT_CHANNEL_3, + /* interrupt Channel 4 for gyro sensor */ + BMI08_INT_CHANNEL_4 }; /*! * @brief Enum to select accelerometer interrupts */ -enum bmi08_accel_int_types { - BMI08_ACCEL_INT_DATA_RDY, - /* Accel data ready interrupt */ - BMI08_ACCEL_INT_SYNC_DATA_RDY, - /* Accel synchronized data ready interrupt */ - BMI08_ACCEL_SYNC_INPUT, - /* Accel FIFO watermark interrupt */ - BMI08_ACCEL_INT_FIFO_WM, - /* Accel FIFO full interrupt */ - BMI08_ACCEL_INT_FIFO_FULL +enum bmi08_accel_int_types +{ + BMI08_ACCEL_INT_DATA_RDY, + /* Accel data ready interrupt */ + BMI08_ACCEL_INT_SYNC_DATA_RDY, + /* Accel synchronized data ready interrupt */ + BMI08_ACCEL_SYNC_INPUT, + /* Accel FIFO watermark interrupt */ + BMI08_ACCEL_INT_FIFO_WM, + /* Accel FIFO full interrupt */ + BMI08_ACCEL_INT_FIFO_FULL }; /*! * @brief Enum to select gyroscope interrupts */ -enum bmi08_gyro_int_types { - /* Gyro data ready interrupt */ - BMI08_GYRO_INT_DATA_RDY, - /* Gyro FIFO watermark interrupt */ - BMI08_GYRO_INT_FIFO_WM, - /* Gyro FIFO full interrupt */ - BMI08_GYRO_INT_FIFO_FULL +enum bmi08_gyro_int_types +{ + /* Gyro data ready interrupt */ + BMI08_GYRO_INT_DATA_RDY, + /* Gyro FIFO watermark interrupt */ + BMI08_GYRO_INT_FIFO_WM, + /* Gyro FIFO full interrupt */ + BMI08_GYRO_INT_FIFO_FULL }; /*! @@ -915,26 +919,26 @@ enum bmi08_gyro_int_types { */ struct bmi08_int_pin_cfg { - /*! interrupt pin level configuration - * Assignable macros : - * - BMI08_INT_ACTIVE_LOW - * - BMI08_INT_ACTIVE_HIGH - */ - uint8_t lvl; - - /*! interrupt pin mode configuration - * Assignable macros : - * - BMI08_INT_MODE_PUSH_PULL - * - BMI08_INT_MODE_OPEN_DRAIN - */ - uint8_t output_mode; - - /*! Enable interrupt pin - * Assignable Macros : - * - BMI08_ENABLE - * - BMI08_DISABLE - */ - uint8_t enable_int_pin; + /*! interrupt pin level configuration + * Assignable macros : + * - BMI08_INT_ACTIVE_LOW + * - BMI08_INT_ACTIVE_HIGH + */ + uint8_t lvl; + + /*! interrupt pin mode configuration + * Assignable macros : + * - BMI08_INT_MODE_PUSH_PULL + * - BMI08_INT_MODE_OPEN_DRAIN + */ + uint8_t output_mode; + + /*! Enable interrupt pin + * Assignable Macros : + * - BMI08_ENABLE + * - BMI08_DISABLE + */ + uint8_t enable_int_pin; }; /*! @@ -942,14 +946,14 @@ struct bmi08_int_pin_cfg */ struct bmi08_accel_int_channel_cfg { - /*! Accel Interrupt channel */ - enum bmi08_accel_int_channel int_channel; + /*! Accel Interrupt channel */ + enum bmi08_accel_int_channel int_channel; - /*! Select Accel Interrupt type */ - enum bmi08_accel_int_types int_type; + /*! Select Accel Interrupt type */ + enum bmi08_accel_int_types int_type; - /*! Structure to configure accel interrupt pins */ - struct bmi08_int_pin_cfg int_pin_cfg; + /*! Structure to configure accel interrupt pins */ + struct bmi08_int_pin_cfg int_pin_cfg; }; /*! @@ -957,14 +961,14 @@ struct bmi08_accel_int_channel_cfg */ struct bmi08_gyro_int_channel_cfg { - /*! Gyro Interrupt channel */ - enum bmi08_gyro_int_channel int_channel; + /*! Gyro Interrupt channel */ + enum bmi08_gyro_int_channel int_channel; - /*! Select Gyro Interrupt type */ - enum bmi08_gyro_int_types int_type; + /*! Select Gyro Interrupt type */ + enum bmi08_gyro_int_types int_type; - /*! Structure to configure gyro interrupt pins */ - struct bmi08_int_pin_cfg int_pin_cfg; + /*! Structure to configure gyro interrupt pins */ + struct bmi08_int_pin_cfg int_pin_cfg; }; /*! @@ -972,17 +976,17 @@ struct bmi08_gyro_int_channel_cfg */ struct bmi08_int_cfg { - /*! Configuration of first accel interrupt channel */ - struct bmi08_accel_int_channel_cfg accel_int_config_1; + /*! Configuration of first accel interrupt channel */ + struct bmi08_accel_int_channel_cfg accel_int_config_1; - /*! Configuration of second accel interrupt channel */ - struct bmi08_accel_int_channel_cfg accel_int_config_2; + /*! Configuration of second accel interrupt channel */ + struct bmi08_accel_int_channel_cfg accel_int_config_2; - /*! Configuration of first gyro interrupt channel */ - struct bmi08_gyro_int_channel_cfg gyro_int_config_1; + /*! Configuration of first gyro interrupt channel */ + struct bmi08_gyro_int_channel_cfg gyro_int_config_1; - /*! Configuration of second gyro interrupt channel */ - struct bmi08_gyro_int_channel_cfg gyro_int_config_2; + /*! Configuration of second gyro interrupt channel */ + struct bmi08_gyro_int_channel_cfg gyro_int_config_2; }; /*! @@ -990,17 +994,17 @@ struct bmi08_int_cfg */ struct bmi08_accel_fifo_config { - /*! Configure the fifo mode (0 = Stream mode, 1 = FIFO mode) */ - uint8_t mode; + /*! Configure the fifo mode (0 = Stream mode, 1 = FIFO mode) */ + uint8_t mode; - /*! To enable the accel */ - uint8_t accel_en; + /*! To enable the accel */ + uint8_t accel_en; - /*! To enable the interrupt_1 */ - uint8_t int1_en; + /*! To enable the interrupt_1 */ + uint8_t int1_en; - /*! To enable the interrupt_2 */ - uint8_t int2_en; + /*! To enable the interrupt_2 */ + uint8_t int2_en; }; /*! @@ -1008,79 +1012,79 @@ struct bmi08_accel_fifo_config */ struct bmi08_gyr_fifo_config { - /*! Configure the fifo mode (0 = Stream mode, 1 = FIFO mode) */ - uint8_t mode; + /*! Configure the fifo mode (0 = Stream mode, 1 = FIFO mode) */ + uint8_t mode; - /*! Selection of axis for data */ - uint8_t data_select; + /*! Selection of axis for data */ + uint8_t data_select; - /*! Tag to include/exclude interrupt in FIFO data bytes */ - uint8_t tag; + /*! Tag to include/exclude interrupt in FIFO data bytes */ + uint8_t tag; - /*! Frame count of fifo data */ - uint8_t frame_count; + /*! Frame count of fifo data */ + uint8_t frame_count; - /*! Water-mark level for FIFO */ - uint16_t wm_level; + /*! Water-mark level for FIFO */ + uint16_t wm_level; }; /*! @name Structure to define FIFO frame configuration */ struct bmi08_fifo_frame { - /*! Pointer to FIFO data */ - uint8_t *data; + /*! Pointer to FIFO data */ + uint8_t *data; - /*! Number of user defined bytes of FIFO to be read */ - uint16_t length; + /*! Number of user defined bytes of FIFO to be read */ + uint16_t length; - /*! Enables type of data to be streamed - accelerometer */ - uint16_t acc_data_enable; + /*! Enables type of data to be streamed - accelerometer */ + uint16_t acc_data_enable; - /*! Enables type of data to be streamed - gyroscope */ - uint16_t gyr_data_enable; + /*! Enables type of data to be streamed - gyroscope */ + uint16_t gyr_data_enable; - /*! To index accelerometer bytes */ - uint16_t acc_byte_start_idx; + /*! To index accelerometer bytes */ + uint16_t acc_byte_start_idx; - /*! To index gyroscope bytes */ - uint16_t gyr_byte_start_idx; + /*! To index gyroscope bytes */ + uint16_t gyr_byte_start_idx; - /*! FIFO sensor time */ - uint32_t sensor_time; + /*! FIFO sensor time */ + uint32_t sensor_time; - /*! Skipped frame count */ - uint8_t skipped_frame_count; + /*! Skipped frame count */ + uint8_t skipped_frame_count; - /*! Type of data interrupt to be mapped */ - uint8_t data_int_map; + /*! Type of data interrupt to be mapped */ + uint8_t data_int_map; - /*! FIFO accelerometer configurations */ - struct bmi08_accel_fifo_config acc_fifo_conf; + /*! FIFO accelerometer configurations */ + struct bmi08_accel_fifo_config acc_fifo_conf; - /*! FIFO gyroscope configurations */ - struct bmi08_gyr_fifo_config gyr_fifo_conf; + /*! FIFO gyroscope configurations */ + struct bmi08_gyr_fifo_config gyr_fifo_conf; }; /*! @name Structure to store the value of re-mapped axis and its sign */ struct bmi08_axes_remap { - /*! Re-mapped x-axis */ - uint8_t x_axis; + /*! Re-mapped x-axis */ + uint8_t x_axis; - /*! Re-mapped y-axis */ - uint8_t y_axis; + /*! Re-mapped y-axis */ + uint8_t y_axis; - /*! Re-mapped z-axis */ - uint8_t z_axis; + /*! Re-mapped z-axis */ + uint8_t z_axis; - /*! Re-mapped x-axis sign */ - uint8_t x_axis_sign; + /*! Re-mapped x-axis sign */ + uint8_t x_axis_sign; - /*! Re-mapped y-axis sign */ - uint8_t y_axis_sign; + /*! Re-mapped y-axis sign */ + uint8_t y_axis_sign; - /*! Re-mapped z-axis sign */ - uint8_t z_axis_sign; + /*! Re-mapped z-axis sign */ + uint8_t z_axis_sign; }; /*! @@ -1088,57 +1092,57 @@ struct bmi08_axes_remap */ struct bmi08_dev { - /*! Accel chip Id */ - uint8_t accel_chip_id; + /*! Accel chip Id */ + uint8_t accel_chip_id; - /*! Gyro chip Id */ - uint8_t gyro_chip_id; + /*! Gyro chip Id */ + uint8_t gyro_chip_id; - /*! Interface function pointer used to enable the device address for I2C and chip selection for SPI */ - void *intf_ptr_accel; + /*! Interface function pointer used to enable the device address for I2C and chip selection for SPI */ + void *intf_ptr_accel; - /*! Interface function pointer used to enable the device address for I2C and chip selection for SPI */ - void *intf_ptr_gyro; + /*! Interface function pointer used to enable the device address for I2C and chip selection for SPI */ + void *intf_ptr_gyro; - /*! Interface Selection - * For SPI, interface = BMI08_SPI_INTF - * For I2C, interface = BMI08_I2C_INTF - **/ - enum bmi08_intf intf; + /*! Interface Selection + * For SPI, interface = BMI08_SPI_INTF + * For I2C, interface = BMI08_I2C_INTF + **/ + enum bmi08_intf intf; - /*! Define the BMI08 variant BMI085 or BMI088 */ - enum bmi08_variant variant; + /*! Define the BMI08 variant BMI085 or BMI088 */ + enum bmi08_variant variant; - /*! Decide SPI or I2C read mechanism */ - uint8_t dummy_byte; + /*! Decide SPI or I2C read mechanism */ + uint8_t dummy_byte; - /*! Structure to configure accel sensor */ - struct bmi08_cfg accel_cfg; + /*! Structure to configure accel sensor */ + struct bmi08_cfg accel_cfg; - /*! Structure to configure gyro sensor */ - struct bmi08_cfg gyro_cfg; + /*! Structure to configure gyro sensor */ + struct bmi08_cfg gyro_cfg; - /*! Structure to maintain a copy of the re-mapped axis */ - struct bmi08_axes_remap remap; + /*! Structure to maintain a copy of the re-mapped axis */ + struct bmi08_axes_remap remap; - /*! Config stream data buffer address will be assigned */ - const uint8_t *config_file_ptr; + /*! Config stream data buffer address will be assigned */ + const uint8_t *config_file_ptr; - /*! Max read/write length - * To be set by the user */ - uint8_t read_write_len; + /*! Max read/write length + * To be set by the user */ + uint8_t read_write_len; - /*! Read function pointer */ - bmi08_read_fptr_t read; + /*! Read function pointer */ + bmi08_read_fptr_t read; - /*! Write function pointer */ - bmi08_write_fptr_t write; + /*! Write function pointer */ + bmi08_write_fptr_t write; - /*! Delay function pointer */ - bmi08_delay_us_fptr_t delay_us; + /*! Delay function pointer */ + bmi08_delay_us_fptr_t delay_us; - /*! Variable to store result of read/write function */ - BMI08_INTF_RET_TYPE intf_rslt; + /*! Variable to store result of read/write function */ + BMI08_INTF_RET_TYPE intf_rslt; }; #endif /* BMI08_DEFS_H_ */ diff --git a/boards/varmint/include/board/BoardConfig.h b/boards/varmint/include/board/BoardConfig.h index 128ce37f..e8b288d9 100644 --- a/boards/varmint/include/board/BoardConfig.h +++ b/boards/varmint/include/board/BoardConfig.h @@ -42,203 +42,200 @@ #include - -#define EPOCH_HZ (400) -#define EPOCH_US (1000000/EPOCH_HZ) +#define EPOCH_HZ (400) +#define EPOCH_US (1000000 / EPOCH_HZ) #define FIFO_MIN_BUFFERS 2 // Defaults to double buffered with really slow data. - #define I2C_DMA_MAX_BUFFER_SIZE 64 #define SPI_DMA_MAX_BUFFER_SIZE 64 -#define HTIM_LOW (&htim5) // least significant word of 64-bit us timer -#define HTIM_LOW_INSTANCE (TIM5) -#define HTIM_HIGH (&htim8) // most significant word of 64-bit us timer -#define HTIM_HIGH_INSTANCE (TIM8) +#define HTIM_LOW (&htim5) // least significant word of 64-bit us timer +#define HTIM_LOW_INSTANCE (TIM5) +#define HTIM_HIGH (&htim8) // most significant word of 64-bit us timer +#define HTIM_HIGH_INSTANCE (TIM8) -#define POLL_HTIM (&htim7) // High rate periodic interrupt timer (PITR) -#define POLL_TIM_CHANNEL TIM_CHANNEL_1 -#define POLL_HTIM_INSTANCE (TIM7) -#define POLLING_PERIOD_US (100) // 100us, 10kHz -#define POLLING_FREQ_HZ (1000000/POLLING_PERIOD_US) // 10000 Hz +#define POLL_HTIM (&htim7) // High rate periodic interrupt timer (PITR) +#define POLL_TIM_CHANNEL TIM_CHANNEL_1 +#define POLL_HTIM_INSTANCE (TIM7) +#define POLLING_PERIOD_US (100) // 100us, 10kHz +#define POLLING_FREQ_HZ (1000000 / POLLING_PERIOD_US) // 10000 Hz // Pwm's -#define PWM_CHANNELS (10) // Number of PWM output channels on the board -#define SERVO_PWM_CLK_DIV (200-1) // 200MHz/200 = 1Hz or 1us per count -#define SERVO_PWM_PERIOD (20000) // For 1000000Hz/50Hz = 20000 -#define SERVO_PWM_CENTER (1500) // 1us * 1500 = 1500 us -#define PWM_HTIM_0 (&htim1) -#define PWM_HTIM_1 (&htim1) -#define PWM_HTIM_2 (&htim1) -#define PWM_HTIM_3 (&htim1) -#define PWM_HTIM_4 (&htim4) -#define PWM_HTIM_5 (&htim4) -#define PWM_HTIM_6 (&htim4) -#define PWM_HTIM_7 (&htim4) -#define PWM_HTIM_8 (&htim3) -#define PWM_HTIM_9 (&htim3) -#define PWM_CHAN_0 (TIM_CHANNEL_1) -#define PWM_CHAN_1 (TIM_CHANNEL_2) -#define PWM_CHAN_2 (TIM_CHANNEL_3) -#define PWM_CHAN_3 (TIM_CHANNEL_4) -#define PWM_CHAN_4 (TIM_CHANNEL_3) -#define PWM_CHAN_5 (TIM_CHANNEL_2) -#define PWM_CHAN_6 (TIM_CHANNEL_1) -#define PWM_CHAN_7 (TIM_CHANNEL_4) -#define PWM_CHAN_8 (TIM_CHANNEL_1) -#define PWM_CHAN_9 (TIM_CHANNEL_2) -#define PWM_MIN (1000) -#define PWM_CENTER (1500) -#define PWM_MAX (2000) +#define PWM_CHANNELS (10) // Number of PWM output channels on the board +#define SERVO_PWM_CLK_DIV (200 - 1) // 200MHz/200 = 1Hz or 1us per count +#define SERVO_PWM_PERIOD (20000) // For 1000000Hz/50Hz = 20000 +#define SERVO_PWM_CENTER (1500) // 1us * 1500 = 1500 us +#define PWM_HTIM_0 (&htim1) +#define PWM_HTIM_1 (&htim1) +#define PWM_HTIM_2 (&htim1) +#define PWM_HTIM_3 (&htim1) +#define PWM_HTIM_4 (&htim4) +#define PWM_HTIM_5 (&htim4) +#define PWM_HTIM_6 (&htim4) +#define PWM_HTIM_7 (&htim4) +#define PWM_HTIM_8 (&htim3) +#define PWM_HTIM_9 (&htim3) +#define PWM_CHAN_0 (TIM_CHANNEL_1) +#define PWM_CHAN_1 (TIM_CHANNEL_2) +#define PWM_CHAN_2 (TIM_CHANNEL_3) +#define PWM_CHAN_3 (TIM_CHANNEL_4) +#define PWM_CHAN_4 (TIM_CHANNEL_3) +#define PWM_CHAN_5 (TIM_CHANNEL_2) +#define PWM_CHAN_6 (TIM_CHANNEL_1) +#define PWM_CHAN_7 (TIM_CHANNEL_4) +#define PWM_CHAN_8 (TIM_CHANNEL_1) +#define PWM_CHAN_9 (TIM_CHANNEL_2) +#define PWM_MIN (1000) +#define PWM_CENTER (1500) +#define PWM_MAX (2000) // Imu1 is BMI088 on spi1 -#define BMI088_HZ (400) // 400, 1000, 2000 are the only options -#define BMI088_RANGE_A (3) // 0,1,2,3 --> 3,6,12,24g -#define BMI088_RANGE_G (2) // 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s -#define BMI088_FIFO_BUFFERS (FIFO_MIN_BUFFERS + BMI088_HZ/EPOCH_HZ) -#define IMU1_DRDY_PORT (IMU1_ACCEL_DRDY_GPIO_Port) -#define IMU1_DRDY_PIN (IMU1_ACCEL_DRDY_Pin) -//#define IMU1_DRDY_PORT_A (IMU1_ACCEL_DRDY_GPIO_Port) -//#define IMU1_DRDY_PIN_A (IMU1_ACCEL_DRDY_Pin) -//#define IMU1_DRDY_PORT_G (IMU1_GYRO_DRDY_GPIO_Port) -//#define IMU1_DRDY_PIN_G (IMU1_GYRO_DRDY_Pin) -#define IMU1_HZ (BMI088_HZ) -#define IMU1_SPI (&hspi1) -#define IMU1_CS_PORT_A (IMU1_SPI1_CS_ACCEL_GPIO_Port) -#define IMU1_CS_PIN_A (IMU1_SPI1_CS_ACCEL_Pin) -#define IMU1_CS_PORT_G (IMU1_SPI1_CS_GYRO_GPIO_Port) -#define IMU1_CS_PIN_G (IMU1_SPI1_CS_GYRO_Pin) -#define IMU1_RANGE_A (BMI088_RANGE_A) -#define IMU1_RANGE_G (BMI088_RANGE_G) +#define BMI088_HZ (400) // 400, 1000, 2000 are the only options +#define BMI088_RANGE_A (3) // 0,1,2,3 --> 3,6,12,24g +#define BMI088_RANGE_G (2) // 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s +#define BMI088_FIFO_BUFFERS (FIFO_MIN_BUFFERS + BMI088_HZ / EPOCH_HZ) +#define IMU1_DRDY_PORT (IMU1_ACCEL_DRDY_GPIO_Port) +#define IMU1_DRDY_PIN (IMU1_ACCEL_DRDY_Pin) +// #define IMU1_DRDY_PORT_A (IMU1_ACCEL_DRDY_GPIO_Port) +// #define IMU1_DRDY_PIN_A (IMU1_ACCEL_DRDY_Pin) +// #define IMU1_DRDY_PORT_G (IMU1_GYRO_DRDY_GPIO_Port) +// #define IMU1_DRDY_PIN_G (IMU1_GYRO_DRDY_Pin) +#define IMU1_HZ (BMI088_HZ) +#define IMU1_SPI (&hspi1) +#define IMU1_CS_PORT_A (IMU1_SPI1_CS_ACCEL_GPIO_Port) +#define IMU1_CS_PIN_A (IMU1_SPI1_CS_ACCEL_Pin) +#define IMU1_CS_PORT_G (IMU1_SPI1_CS_GYRO_GPIO_Port) +#define IMU1_CS_PIN_G (IMU1_SPI1_CS_GYRO_Pin) +#define IMU1_RANGE_A (BMI088_RANGE_A) +#define IMU1_RANGE_G (BMI088_RANGE_G) // Imu0 is ADIS on spi4 -#define ADIS165XX_HZ (400) -#define ADIS165XX_FIFO_BUFFERS (FIFO_MIN_BUFFERS + ADIS165XX_HZ/EPOCH_HZ) - -#define IMU0_DRDY_PORT (IMU0_DRDY_GPIO_Port) -#define IMU0_DRDY_PIN (IMU0_DRDY_Pin) -#define IMU0_HZ (ADIS165XX_HZ) -#define IMU0_SPI (&hspi4) -#define IMU0_HTIM (&htim12) // ADIS 16500 ExtClk -#define IMU0_TIM_CHANNEL (TIM_CHANNEL_1) // ADIS 16500 ExtClk -#define IMU0_TIM_INSTANCE (TIM12) // ADIS 16500 ExtClk -#define IMU0_TIM_PERIOD_US (500) // 500 us, 2kHz -#define IMU0_CS_PORT (IMU0_SPI4_CS_GPIO_Port) -#define IMU0_CS_PIN (IMU0_SPI4_CS_Pin) -#define IMU0_RESET_PORT (IMU0_RST_GPIO_Port) -#define IMU0_RESET_PIN (IMU0_RST_Pin) +#define ADIS165XX_HZ (400) +#define ADIS165XX_FIFO_BUFFERS (FIFO_MIN_BUFFERS + ADIS165XX_HZ / EPOCH_HZ) + +#define IMU0_DRDY_PORT (IMU0_DRDY_GPIO_Port) +#define IMU0_DRDY_PIN (IMU0_DRDY_Pin) +#define IMU0_HZ (ADIS165XX_HZ) +#define IMU0_SPI (&hspi4) +#define IMU0_HTIM (&htim12) // ADIS 16500 ExtClk +#define IMU0_TIM_CHANNEL (TIM_CHANNEL_1) // ADIS 16500 ExtClk +#define IMU0_TIM_INSTANCE (TIM12) // ADIS 16500 ExtClk +#define IMU0_TIM_PERIOD_US (500) // 500 us, 2kHz +#define IMU0_CS_PORT (IMU0_SPI4_CS_GPIO_Port) +#define IMU0_CS_PIN (IMU0_SPI4_CS_Pin) +#define IMU0_RESET_PORT (IMU0_RST_GPIO_Port) +#define IMU0_RESET_PIN (IMU0_RST_Pin) // Pitot is DLHR on i2c1 -#define DLHRL20G_HZ (100) -#define DLHRL20G_FIFO_BUFFERS (FIFO_MIN_BUFFERS + DLHRL20G_HZ/EPOCH_HZ) -#define PITOT_DRDY_PORT (PITOT_DRDY_GPIO_Port) -#define PITOT_DRDY_PIN (PITOT_DRDY_Pin) +#define DLHRL20G_HZ (100) +#define DLHRL20G_FIFO_BUFFERS (FIFO_MIN_BUFFERS + DLHRL20G_HZ / EPOCH_HZ) +#define PITOT_DRDY_PORT (PITOT_DRDY_GPIO_Port) +#define PITOT_DRDY_PIN (PITOT_DRDY_Pin) -#define PITOT_HZ (DLHRL20G_HZ) -#define PITOT_I2C (&hi2c1) -#define PITOT_I2C_ADDRESS (0x29) +#define PITOT_HZ (DLHRL20G_HZ) +#define PITOT_I2C (&hi2c1) +#define PITOT_I2C_ADDRESS (0x29) // Baro is DPS310 on spi3 -#define DPS310_HZ (100) // real value is lower -#define DPS310_FIFO_BUFFERS (FIFO_MIN_BUFFERS + DPS310_HZ/EPOCH_HZ) -#define BARO_DRDY_PORT (BARO_DRDY_GPIO_Port) -#define BARO_DRDY_PIN (BARO_DRDY_Pin) -#define BARO_CS_PORT (BARO_CS_GPIO_Port) -#define BARO_CS_PIN (BARO_CS_Pin) -#define BARO_HZ (DPS310_HZ) -#define BARO_SPI (&hspi3) +#define DPS310_HZ (100) // real value is lower +#define DPS310_FIFO_BUFFERS (FIFO_MIN_BUFFERS + DPS310_HZ / EPOCH_HZ) +#define BARO_DRDY_PORT (BARO_DRDY_GPIO_Port) +#define BARO_DRDY_PIN (BARO_DRDY_Pin) +#define BARO_CS_PORT (BARO_CS_GPIO_Port) +#define BARO_CS_PIN (BARO_CS_Pin) +#define BARO_HZ (DPS310_HZ) +#define BARO_SPI (&hspi3) // Mag is IIS2MDC on spi2 -#define IIS2MDC_HZ (100) // 10, 20, 50, 100 are the only options -#define IIS2MDC_FIFO_BUFFERS (FIFO_MIN_BUFFERS + IIS2MDC_HZ/EPOCH_HZ) -#define MAG_DRDY_PORT (MAG_DRDY_GPIO_Port) -#define MAG_DRDY_PIN (MAG_DRDY_Pin) -#define MAG_CS_PORT (MAG_CS_GPIO_Port) -#define MAG_CS_PIN (MAG_CS_Pin) -#define MAG_HZ (IIS2MDC_HZ) -#define MAG_SPI (&hspi2) +#define IIS2MDC_HZ (100) // 10, 20, 50, 100 are the only options +#define IIS2MDC_FIFO_BUFFERS (FIFO_MIN_BUFFERS + IIS2MDC_HZ / EPOCH_HZ) +#define MAG_DRDY_PORT (MAG_DRDY_GPIO_Port) +#define MAG_DRDY_PIN (MAG_DRDY_Pin) +#define MAG_CS_PORT (MAG_CS_GPIO_Port) +#define MAG_CS_PIN (MAG_CS_Pin) +#define MAG_HZ (IIS2MDC_HZ) +#define MAG_SPI (&hspi2) // SBus is on UART3 -#define SBUS_HZ (112) // 1000/9ms = 111.1Hz, 112 is rounds up -#define SBUS_FIFO_BUFFERS (FIFO_MIN_BUFFERS + SBUS_HZ/EPOCH_HZ) -#define SBUS_BAUD (100000) +#define SBUS_HZ (112) // 1000/9ms = 111.1Hz, 112 is rounds up +#define SBUS_FIFO_BUFFERS (FIFO_MIN_BUFFERS + SBUS_HZ / EPOCH_HZ) +#define SBUS_BAUD (100000) // -#define RC_HZ (SBUS_HZ) -#define RC_UART (&huart3) -#define RC_UART_INSTANCE (USART3) -#define RC_UART_DMA (&hdma_usart3_rx) -#define RC_BAUD (SBUS_BAUD) +#define RC_HZ (SBUS_HZ) +#define RC_UART (&huart3) +#define RC_UART_INSTANCE (USART3) +#define RC_UART_DMA (&hdma_usart3_rx) +#define RC_BAUD (SBUS_BAUD) // uBlox on UART1 -#define UBX_HZ (10) -#define UBX_NUM (3) // number of different types of packets -#define UBX_FIFO_BUFFERS (UBX_NUM*(FIFO_MIN_BUFFERS + UBX_HZ/EPOCH_HZ)) -#define UBX_BAUD (115200) +#define UBX_HZ (10) +#define UBX_NUM (3) // number of different types of packets +#define UBX_FIFO_BUFFERS (UBX_NUM * (FIFO_MIN_BUFFERS + UBX_HZ / EPOCH_HZ)) +#define UBX_BAUD (115200) // -#define GPS_HZ (UBX_HZ) -#define GPS_PPS_PORT (GPS_1PPS_GPIO_Port) -#define GPS_PPS_PIN (GPS_1PPS_Pin) -#define GPS_UART (&huart1) -#define GPS_UART_INSTANCE (USART1) -#define GPS_UART_DMA (&hdma_usart1_rx) -#define GPS_BAUD (115200) +#define GPS_HZ (UBX_HZ) +#define GPS_PPS_PORT (GPS_1PPS_GPIO_Port) +#define GPS_PPS_PIN (GPS_1PPS_Pin) +#define GPS_UART (&huart1) +#define GPS_UART_INSTANCE (USART1) +#define GPS_UART_DMA (&hdma_usart1_rx) +#define GPS_BAUD (115200) // Telemetry UART & VCP // Serial -#define SERIAL_HZ (IMU0_HZ) -#define SERIAL_QOS_FIFOS (3) -#define SERIAL_TX_FIFO_BUFFERS (PACKET_FIFO_MAX_BUFFERS) +#define SERIAL_HZ (IMU0_HZ) +#define SERIAL_QOS_FIFOS (3) +#define SERIAL_TX_FIFO_BUFFERS (PACKET_FIFO_MAX_BUFFERS) #define SERIAL_RX_FIFO_BUFFER_BYTES (4096) // Telem (USART2) -#define TELEM_HZ (SERIAL_HZ) -#define TELEM_BAUD (921600) -#define TELEM_UART (&huart2) -#define TELEM_UART_INSTANCE (USART2) -#define TELEM_UART_DMA (0) //(&hdma_usart2_rx) +#define TELEM_HZ (SERIAL_HZ) +#define TELEM_BAUD (921600) +#define TELEM_UART (&huart2) +#define TELEM_UART_INSTANCE (USART2) +#define TELEM_UART_DMA (0) //(&hdma_usart2_rx) // VCP -#define VCP_HZ (SERIAL_HZ) +#define VCP_HZ (SERIAL_HZ) // SDMMC -#define SD_HSD (&hsd1) -#define SD_HSD_INSTANCE (SDMMC1) +#define SD_HSD (&hsd1) +#define SD_HSD_INSTANCE (SDMMC1) // Onboard ADC's -#define ADC_HZ (10) // Maximum is 500 Hz. -#define ADC_FIFO_BUFFERS (FIFO_MIN_BUFFERS) - -#define ADC_ADC_EXTERNAL (&hadc1) -#define ADC_ADC_INSTANCE_EXTERNAL (ADC1) -#define ADC_CHANNELS_EXT (6) -#define ADC_BATTERY_VOLTS (5) -#define ADC_BATTERY_CURR (4) -#define ADC_STM_5V0 (3) -#define ADC_STM_12V (2) -#define ADC_SERVO_VOLTS (1) -#define ADC_JETSON_3V3 (0) - -#define ADC_ADC_INTERNAL (&hadc3) -#define ADC_ADC_INSTANCE_INTERNAL (ADC3) -#define ADC_CHANNELS_INT (3) -#define ADC_STM_TEMPERATURE (0) -#define ADC_STM_VBAT (1) -#define ADC_STM_VREFINT (2) +#define ADC_HZ (10) // Maximum is 500 Hz. +#define ADC_FIFO_BUFFERS (FIFO_MIN_BUFFERS) + +#define ADC_ADC_EXTERNAL (&hadc1) +#define ADC_ADC_INSTANCE_EXTERNAL (ADC1) +#define ADC_CHANNELS_EXT (6) +#define ADC_BATTERY_VOLTS (5) +#define ADC_BATTERY_CURR (4) +#define ADC_STM_5V0 (3) +#define ADC_STM_12V (2) +#define ADC_SERVO_VOLTS (1) +#define ADC_JETSON_3V3 (0) + +#define ADC_ADC_INTERNAL (&hadc3) +#define ADC_ADC_INSTANCE_INTERNAL (ADC3) +#define ADC_CHANNELS_INT (3) +#define ADC_STM_TEMPERATURE (0) +#define ADC_STM_VBAT (1) +#define ADC_STM_VREFINT (2) // Red LED PE7 -#define RED_HI HAL_GPIO_WritePin(GPIOE,GPIO_PIN_7, GPIO_PIN_SET) -#define RED_LO HAL_GPIO_WritePin(GPIOE,GPIO_PIN_7, GPIO_PIN_RESET) -#define RED_TOG HAL_GPIO_TogglePin(GPIOE,GPIO_PIN_7) +#define RED_HI HAL_GPIO_WritePin(GPIOE, GPIO_PIN_7, GPIO_PIN_SET) +#define RED_LO HAL_GPIO_WritePin(GPIOE, GPIO_PIN_7, GPIO_PIN_RESET) +#define RED_TOG HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_7) // Green LED PE15 -#define GRN_HI HAL_GPIO_WritePin(GPIOE,GPIO_PIN_15, GPIO_PIN_SET) -#define GRN_LO HAL_GPIO_WritePin(GPIOE,GPIO_PIN_15, GPIO_PIN_RESET) -#define GRN_TOG HAL_GPIO_TogglePin(GPIOE,GPIO_PIN_15) +#define GRN_HI HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15, GPIO_PIN_SET) +#define GRN_LO HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15, GPIO_PIN_RESET) +#define GRN_TOG HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_15) // Blue LED PE8 -#define BLU_HI HAL_GPIO_WritePin(GPIOE,GPIO_PIN_8, GPIO_PIN_SET) -#define BLU_LO HAL_GPIO_WritePin(GPIOE,GPIO_PIN_8, GPIO_PIN_RESET) -#define BLU_TOG HAL_GPIO_TogglePin(GPIOE,GPIO_PIN_8) - +#define BLU_HI HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_SET) +#define BLU_LO HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_RESET) +#define BLU_TOG HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_8) extern ADC_HandleTypeDef hadc1; extern ADC_HandleTypeDef hadc3; @@ -290,88 +287,88 @@ extern DMA_HandleTypeDef hdma_usart3_rx; extern DMA_HandleTypeDef hdma_usart3_tx; #if 1 // probes - // CN11 34 TP5 - #define PB0_HI HAL_GPIO_WritePin(GPIOB,GPIO_PIN_0, GPIO_PIN_SET) - #define PB0_LO HAL_GPIO_WritePin(GPIOB,GPIO_PIN_0, GPIO_PIN_RESET) - #define PB0_TOG HAL_GPIO_TogglePin(GPIOB,GPIO_PIN_0) - - // CN12 19 TP6 - #define PC7_HI HAL_GPIO_WritePin(GPIOC,GPIO_PIN_7, GPIO_PIN_SET) - #define PC7_LO HAL_GPIO_WritePin(GPIOC,GPIO_PIN_7, GPIO_PIN_RESET) - #define PC7_TOG HAL_GPIO_TogglePin(GPIOC,GPIO_PIN_7) - - // CN11 47 (Jetson DRDY) - #define PE5_HI HAL_GPIO_WritePin(GPIOE,GPIO_PIN_5, GPIO_PIN_SET) - #define PE5_LO HAL_GPIO_WritePin(GPIOE,GPIO_PIN_5, GPIO_PIN_RESET) - #define PE5_TOG HAL_GPIO_TogglePin(GPIOE,GPIO_PIN_5) - - // Real Board - PH1 J105 pin 18 RST - #define PH1_HI HAL_GPIO_WritePin(GPIOH,GPIO_PIN_1, GPIO_PIN_SET) - #define PH1_LO HAL_GPIO_WritePin(GPIOH,GPIO_PIN_1, GPIO_PIN_RESET) - #define PH1_TOG HAL_GPIO_TogglePin(GPIOH,GPIO_PIN_1) - - // Real Board - PE3 J105 pin 19 CS - #define PE3_HI HAL_GPIO_WritePin(GPIOE,GPIO_PIN_3, GPIO_PIN_SET) - #define PE3_LO HAL_GPIO_WritePin(GPIOE,GPIO_PIN_3, GPIO_PIN_RESET) - #define PE3_TOG HAL_GPIO_TogglePin(GPIOE,GPIO_PIN_3) - - // Real Board - PB1 J105 pin 23/25 Sync Bus (CAN XCVR) - #define PB1_HI HAL_GPIO_WritePin(GPIOB,GPIO_PIN_1, GPIO_PIN_SET) - #define PB1_LO HAL_GPIO_WritePin(GPIOB,GPIO_PIN_1, GPIO_PIN_RESET) - #define PB1_TOG HAL_GPIO_TogglePin(GPIOB,GPIO_PIN_1) - - // PB15 - #define PB15_HI HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15, GPIO_PIN_SET) - #define PB15_LO HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15, GPIO_PIN_RESET) - #define PB15_TOG HAL_GPIO_TogglePin(GPIOB,GPIO_PIN_15) + // CN11 34 TP5 +#define PB0_HI HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_SET) +#define PB0_LO HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET) +#define PB0_TOG HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_0) + +// CN12 19 TP6 +#define PC7_HI HAL_GPIO_WritePin(GPIOC, GPIO_PIN_7, GPIO_PIN_SET) +#define PC7_LO HAL_GPIO_WritePin(GPIOC, GPIO_PIN_7, GPIO_PIN_RESET) +#define PC7_TOG HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_7) + +// CN11 47 (Jetson DRDY) +#define PE5_HI HAL_GPIO_WritePin(GPIOE, GPIO_PIN_5, GPIO_PIN_SET) +#define PE5_LO HAL_GPIO_WritePin(GPIOE, GPIO_PIN_5, GPIO_PIN_RESET) +#define PE5_TOG HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_5) + +// Real Board - PH1 J105 pin 18 RST +#define PH1_HI HAL_GPIO_WritePin(GPIOH, GPIO_PIN_1, GPIO_PIN_SET) +#define PH1_LO HAL_GPIO_WritePin(GPIOH, GPIO_PIN_1, GPIO_PIN_RESET) +#define PH1_TOG HAL_GPIO_TogglePin(GPIOH, GPIO_PIN_1) + +// Real Board - PE3 J105 pin 19 CS +#define PE3_HI HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, GPIO_PIN_SET) +#define PE3_LO HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, GPIO_PIN_RESET) +#define PE3_TOG HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_3) + +// Real Board - PB1 J105 pin 23/25 Sync Bus (CAN XCVR) +#define PB1_HI HAL_GPIO_WritePin(GPIOB, GPIO_PIN_1, GPIO_PIN_SET) +#define PB1_LO HAL_GPIO_WritePin(GPIOB, GPIO_PIN_1, GPIO_PIN_RESET) +#define PB1_TOG HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_1) + +// PB15 +#define PB15_HI HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET) +#define PB15_LO HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET) +#define PB15_TOG HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_15) #else - // CN11 34 TP5 - #define PB0_HI - #define PB0_LO - #define PB0_TOG - - // CN12 19 TP6 - #define PC7_HI - #define PC7_LO - #define PC7_TOG - - // CN11 47 (Jetson DRDY) - #define PE5_HI - #define PE5_LO - #define PE5_TOG _5) - - // Real Board - PH1 J105 pin 18 RST - #define PH1_HI - #define PH1_LO - #define PH1_TOG - - // Real Board - PE3 J105 pin 19 CS - #define PE3_HI - #define PE3_LO - #define PE3_TOG - - // Real Board - PB1 J105 pin 23/25 Sync Bus (CAN XCVR) - #define PB1_HI - #define PB1_LO - #define PB1_TOG - - // PB15 - #define PB15_HI - #define PB15_LO - #define PB15_TOG +// CN11 34 TP5 +#define PB0_HI +#define PB0_LO +#define PB0_TOG + +// CN12 19 TP6 +#define PC7_HI +#define PC7_LO +#define PC7_TOG + +// CN11 47 (Jetson DRDY) +#define PE5_HI +#define PE5_LO +#define PE5_TOG _5) + +// Real Board - PH1 J105 pin 18 RST +#define PH1_HI +#define PH1_LO +#define PH1_TOG + +// Real Board - PE3 J105 pin 19 CS +#define PE3_HI +#define PE3_LO +#define PE3_TOG + +// Real Board - PB1 J105 pin 23/25 Sync Bus (CAN XCVR) +#define PB1_HI +#define PB1_LO +#define PB1_TOG + +// PB15 +#define PB15_HI +#define PB15_LO +#define PB15_TOG #endif -#define DRIVER_OK (0x00000000) -#define DRIVER_ID_MISMATCH (0x00000002) -#define DRIVER_SELF_DIAG_ERROR (0x00000004) -#define DRIVER_HAL_ERROR (0x00000008) -#define DRIVER_HAL_ERROR2 (0x00000010) -#define DRIVER_FIFO_INIT_ERROR (0x00000020) -#define UBX_ACK (0x00000040) -#define UBX_NAK (0x00000080) -#define UBX_ACKNAK_FAIL (0x00000100) -#define UBX_SUCCESS (0x00000200) -#define UBX_FAIL_BAUD_CHANGE (0x00000400) +#define DRIVER_OK (0x00000000) +#define DRIVER_ID_MISMATCH (0x00000002) +#define DRIVER_SELF_DIAG_ERROR (0x00000004) +#define DRIVER_HAL_ERROR (0x00000008) +#define DRIVER_HAL_ERROR2 (0x00000010) +#define DRIVER_FIFO_INIT_ERROR (0x00000020) +#define UBX_ACK (0x00000040) +#define UBX_NAK (0x00000080) +#define UBX_ACKNAK_FAIL (0x00000100) +#define UBX_SUCCESS (0x00000200) +#define UBX_FAIL_BAUD_CHANGE (0x00000400) #endif /* BOARDCONFIG_H_ */ diff --git a/boards/varmint/include/board/ByteFifo.h b/boards/varmint/include/board/ByteFifo.h index 02f07470..da7e8222 100644 --- a/boards/varmint/include/board/ByteFifo.h +++ b/boards/varmint/include/board/ByteFifo.h @@ -41,50 +41,58 @@ class ByteFifo { public: + void init(uint16_t buffer_size, uint8_t *buffer) + { + bufferSize_ = buffer_size; + buffer_ = buffer; + bufferEnd_ = buffer_ + bufferSize_ - 1; + flush(); + } - void init(uint16_t buffer_size, uint8_t *buffer) - { - bufferSize_ = buffer_size; - buffer_ = buffer; - bufferEnd_ = buffer_ + bufferSize_-1; - flush(); - } + void flush(void) + { + head_ = buffer_; + tail_ = buffer_; + } - void flush(void) { head_ = buffer_; tail_ = buffer_; } + bool write(uint8_t data) + { + if ((bufferSize_ + tail_ - head_) % bufferSize_ == 1) + return false; // buffer full + *(head_++) = data; - bool write(uint8_t data) - { - if((bufferSize_+tail_-head_)%bufferSize_ == 1) return false; // buffer full - *(head_++) = data; + if (head_ > bufferEnd_) + head_ = buffer_; - if(head_>bufferEnd_) head_ = buffer_; + return true; + } + bool writeBlock(uint8_t *data, uint32_t size) + { + uint32_t i; + for (i = 0; i < size; i++) + { + if (!write(data[i])) + break; + } + return i; + } - return true; - } - bool writeBlock(uint8_t *data, uint32_t size) - { - uint32_t i; - for(i = 0; i bufferEnd_) + tail_ = buffer_; + return true; + } - bool read(uint8_t *data) - { - if(head_ == tail_) return false; // buffer is empty - *data = *(tail_++); - if(tail_>bufferEnd_) tail_ = buffer_; - return true; - } - - uint16_t byteCount(void) { return (bufferSize_+head_-tail_)%bufferSize_;} + uint16_t byteCount(void) { return (bufferSize_ + head_ - tail_) % bufferSize_; } private: - volatile uint8_t *head_, *tail_; - volatile uint8_t *buffer_, *bufferEnd_; - uint32_t bufferSize_; + volatile uint8_t *head_, *tail_; + volatile uint8_t *buffer_, *bufferEnd_; + uint32_t bufferSize_; }; #endif /* BYTEFIFO_H_ */ diff --git a/boards/varmint/include/board/Callbacks.h b/boards/varmint/include/board/Callbacks.h index b826c835..74dbf7ad 100644 --- a/boards/varmint/include/board/Callbacks.h +++ b/boards/varmint/include/board/Callbacks.h @@ -41,15 +41,15 @@ #include #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif -void CDC_Receive_FS_Callback(uint8_t* buffer, uint16_t size); -void CDC_TransmitCplt_FS_Callback(uint8_t* buffer, uint16_t size); - -//void UART_RxIdleCallback(UART_HandleTypeDef *huart); -void RxIsrCallback(UART_HandleTypeDef *huart); + void CDC_Receive_FS_Callback(uint8_t* buffer, uint16_t size); + void CDC_TransmitCplt_FS_Callback(uint8_t* buffer, uint16_t size); + // void UART_RxIdleCallback(UART_HandleTypeDef *huart); + void RxIsrCallback(UART_HandleTypeDef* huart); #ifdef __cplusplus } diff --git a/boards/varmint/include/board/CubeMX.h b/boards/varmint/include/board/CubeMX.h index fd7e5d43..b2086b55 100644 --- a/boards/varmint/include/board/CubeMX.h +++ b/boards/varmint/include/board/CubeMX.h @@ -217,47 +217,48 @@ #define J106_PWM07_GPIO_Port GPIOD #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif #include -void SystemClock_Config(void); -void PeriphCommonClock_Config(void); -void MPU_Initialize(void); -void MPU_Config(void); + void SystemClock_Config(void); + void PeriphCommonClock_Config(void); + void MPU_Initialize(void); + void MPU_Config(void); -void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); -void Error_Handler(void); -void SystemClock_Config(void); -void PeriphCommonClock_Config(void); -void MPU_Config(void); -void MX_GPIO_Init(void); -void MX_DMA_Init(void); -void MX_BDMA_Init(void); -void MX_I2C1_Init(void); -void MX_I2C2_Init(void); -void MX_SPI1_Init(void); -void MX_SPI3_Init(void); -void MX_TIM3_Init(void); -void MX_TIM4_Init(void); -void MX_USART1_UART_Init(void); -void MX_USART2_UART_Init(void); -void MX_USART3_UART_Init(void); -void MX_FDCAN1_Init(void); -void MX_SPI4_Init(void); -void MX_TIM1_Init(void); -void MX_TIM12_Init(void); -void MX_ADC1_Init(void); -void MX_RTC_Init(void); -void MX_TIM7_Init(void); -void MX_TIM5_Init(void); -void MX_SDMMC1_SD_Init(void); -void MX_ADC3_Init(void); -void MX_SPI2_Init(void); -void MX_RNG_Init(void); -void MX_TIM8_Init(void); -void MX_CRC_Init(void); + void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); + void Error_Handler(void); + void SystemClock_Config(void); + void PeriphCommonClock_Config(void); + void MPU_Config(void); + void MX_GPIO_Init(void); + void MX_DMA_Init(void); + void MX_BDMA_Init(void); + void MX_I2C1_Init(void); + void MX_I2C2_Init(void); + void MX_SPI1_Init(void); + void MX_SPI3_Init(void); + void MX_TIM3_Init(void); + void MX_TIM4_Init(void); + void MX_USART1_UART_Init(void); + void MX_USART2_UART_Init(void); + void MX_USART3_UART_Init(void); + void MX_FDCAN1_Init(void); + void MX_SPI4_Init(void); + void MX_TIM1_Init(void); + void MX_TIM12_Init(void); + void MX_ADC1_Init(void); + void MX_RTC_Init(void); + void MX_TIM7_Init(void); + void MX_TIM5_Init(void); + void MX_SDMMC1_SD_Init(void); + void MX_ADC3_Init(void); + void MX_SPI2_Init(void); + void MX_RNG_Init(void); + void MX_TIM8_Init(void); + void MX_CRC_Init(void); #ifdef __cplusplus } diff --git a/boards/varmint/include/board/DlhrL20G.h b/boards/varmint/include/board/DlhrL20G.h index 19b921e9..2eda836c 100644 --- a/boards/varmint/include/board/DlhrL20G.h +++ b/boards/varmint/include/board/DlhrL20G.h @@ -39,44 +39,43 @@ #define DLHRL20G_H_ #include -#include #include #include +#include /* * */ class DlhrL20G : public Driver { - /** - * \brief - * - * - */ + /** + * \brief + * + * + */ public: - uint32_t init - ( - // Driver initializers - uint16_t sample_rate_hz, - GPIO_TypeDef *drdy_port, // Reset GPIO Port - uint16_t drdy_pin, // Reset GPIO Pin - // I2C initializers - I2C_HandleTypeDef *hi2c, // The SPI handle - uint16_t i2c_address // Chip select Port - ); - bool poll(uint16_t poll_offset); - bool startDma(void) override; - void endDma(void) override; - bool display(void); - //I2C_HandleTypeDef* hi2c(void) {return hi2c_;} - bool isMy(I2C_HandleTypeDef* hi2c) {return hi2c_==hi2c;} + uint32_t init( + // Driver initializers + uint16_t sample_rate_hz, + GPIO_TypeDef *drdy_port, // Reset GPIO Port + uint16_t drdy_pin, // Reset GPIO Pin + // I2C initializers + I2C_HandleTypeDef *hi2c, // The SPI handle + uint16_t i2c_address // Chip select Port + ); + bool poll(uint16_t poll_offset); + bool startDma(void) override; + void endDma(void) override; + bool display(void); + // I2C_HandleTypeDef* hi2c(void) {return hi2c_;} + bool isMy(I2C_HandleTypeDef *hi2c) { return hi2c_ == hi2c; } private: - I2C_HandleTypeDef *hi2c_; - uint16_t address_; - uint8_t cmdByte_; - double dtMs_; - uint64_t launchUs_; + I2C_HandleTypeDef *hi2c_; + uint16_t address_; + uint8_t cmdByte_; + double dtMs_; + uint64_t launchUs_; }; #endif /* DLHRL20G_H_ */ diff --git a/boards/varmint/include/board/Dps310.h b/boards/varmint/include/board/Dps310.h index 8bf17a38..f9023e84 100644 --- a/boards/varmint/include/board/Dps310.h +++ b/boards/varmint/include/board/Dps310.h @@ -39,54 +39,53 @@ #define DPS310_H_ #include -#include #include #include #include +#include /* * */ class Dps310 : public Driver { - /** - * \brief - * - * - */ + /** + * \brief + * + * + */ public: - uint32_t init - ( - // Driver initializers - uint16_t sample_rate_hz, - GPIO_TypeDef *drdy_port, // Reset GPIO Port - uint16_t drdy_pin, // Reset GPIO Pin - // SPI initializers - SPI_HandleTypeDef *hspi, - GPIO_TypeDef *cs_port, // Chip Select GPIO Port - uint16_t cs_pin // Chip Select GPIO Pin - ); + uint32_t init( + // Driver initializers + uint16_t sample_rate_hz, + GPIO_TypeDef *drdy_port, // Reset GPIO Port + uint16_t drdy_pin, // Reset GPIO Pin + // SPI initializers + SPI_HandleTypeDef *hspi, + GPIO_TypeDef *cs_port, // Chip Select GPIO Port + uint16_t cs_pin // Chip Select GPIO Pin + ); - bool poll(void); - void endDma(void) override; - bool startDma(void) override; - bool display(void) override; + bool poll(void); + void endDma(void) override; + bool startDma(void) override; + bool display(void) override; - bool isMy(uint16_t exti_pin) { return drdyPin_== exti_pin; } - bool isMy(SPI_HandleTypeDef *hspi) { return hspi==spi_.hspi(); } - SPI_HandleTypeDef *hspi(void) { return spi_.hspi(); } + bool isMy(uint16_t exti_pin) { return drdyPin_ == exti_pin; } + bool isMy(SPI_HandleTypeDef *hspi) { return hspi == spi_.hspi(); } + SPI_HandleTypeDef *hspi(void) { return spi_.hspi(); } private: - // SPI Stuff - Spi spi_; - uint16_t timeoutMs_; - uint64_t launchUs_; + // SPI Stuff + Spi spi_; + uint16_t timeoutMs_; + uint64_t launchUs_; -// Dps310 Stuff - double C0_, C1_, C01_, C11_, C20_, C21_, C30_, C00_, C10_; + // Dps310 Stuff + double C0_, C1_, C01_, C11_, C20_, C21_, C30_, C00_, C10_; - void writeRegister(uint8_t address, uint8_t value); - uint8_t readRegister(uint8_t address); + void writeRegister(uint8_t address, uint8_t value); + uint8_t readRegister(uint8_t address); }; #endif /* DPS310_H_ */ diff --git a/boards/varmint/include/board/Driver.h b/boards/varmint/include/board/Driver.h index d097abc2..ece67296 100644 --- a/boards/varmint/include/board/Driver.h +++ b/boards/varmint/include/board/Driver.h @@ -40,30 +40,27 @@ #include "stm32h7xx_hal.h" -#include #include +#include class Driver { public: - virtual bool startDma(void)=0; // Called in response to a data ready signal - virtual void endDma(void)=0; // Called when DMA read is complete - virtual bool display(void)=0; + virtual bool startDma(void) = 0; // Called in response to a data ready signal + virtual void endDma(void) = 0; // Called when DMA read is complete + virtual bool display(void) = 0; - uint16_t rxFifoCount(void) { return rxFifo_.packetCount(); } - uint16_t rxFifoRead( uint8_t *data, uint16_t size) { return rxFifo_.read(data, size); } - uint16_t rxFifoReadMostRecent( uint8_t *data, uint16_t size) { return rxFifo_.readMostRecent(data, size); } + uint16_t rxFifoCount(void) { return rxFifo_.packetCount(); } + uint16_t rxFifoRead(uint8_t *data, uint16_t size) { return rxFifo_.read(data, size); } + uint16_t rxFifoReadMostRecent(uint8_t *data, uint16_t size) { return rxFifo_.readMostRecent(data, size); } protected: - PacketFifo rxFifo_; - GPIO_TypeDef* drdyPort_; - uint16_t drdyPin_; - uint16_t sampleRateHz_; - uint64_t timestamp_, drdy_, timeout_; - uint64_t groupDelay_=0; + PacketFifo rxFifo_; + GPIO_TypeDef *drdyPort_; + uint16_t drdyPin_; + uint16_t sampleRateHz_; + uint64_t timestamp_, drdy_, timeout_; + uint64_t groupDelay_ = 0; }; - - - #endif /* DRIVER_H_ */ diff --git a/boards/varmint/include/board/Iis2mdc.h b/boards/varmint/include/board/Iis2mdc.h index 64846313..57361499 100644 --- a/boards/varmint/include/board/Iis2mdc.h +++ b/boards/varmint/include/board/Iis2mdc.h @@ -48,39 +48,38 @@ */ class Iis2mdc : public Driver { - /** - * \brief - * - * - */ + /** + * \brief + * + * + */ public: - uint32_t init - ( - // Driver initializers - uint16_t sample_rate_hz, - GPIO_TypeDef *drdy_port, // Reset GPIO Port - uint16_t drdy_pin, // Reset GPIO Pin - // SPI initializers - SPI_HandleTypeDef *hspi, - GPIO_TypeDef *cs_port, // Chip Select GPIO Port - uint16_t cs_pin // Chip Select GPIO Pin - ); - bool poll(void); - void endDma(void) override; - bool startDma(void) override; - bool display(void) override; + uint32_t init( + // Driver initializers + uint16_t sample_rate_hz, + GPIO_TypeDef *drdy_port, // Reset GPIO Port + uint16_t drdy_pin, // Reset GPIO Pin + // SPI initializers + SPI_HandleTypeDef *hspi, + GPIO_TypeDef *cs_port, // Chip Select GPIO Port + uint16_t cs_pin // Chip Select GPIO Pin + ); + bool poll(void); + void endDma(void) override; + bool startDma(void) override; + bool display(void) override; - bool isMy(uint16_t exti_pin) { return drdyPin_== exti_pin; } - bool isMy(SPI_HandleTypeDef *hspi) { return hspi==spi_.hspi(); } - SPI_HandleTypeDef *hspi(void) { return spi_.hspi(); } + bool isMy(uint16_t exti_pin) { return drdyPin_ == exti_pin; } + bool isMy(SPI_HandleTypeDef *hspi) { return hspi == spi_.hspi(); } + SPI_HandleTypeDef *hspi(void) { return spi_.hspi(); } private: - // SPI Stuff - Spi spi_; - uint16_t seqCount_; + // SPI Stuff + Spi spi_; + uint16_t seqCount_; - void writeRegister(uint8_t address, uint8_t value); - uint8_t readRegister(uint8_t address); + void writeRegister(uint8_t address, uint8_t value); + uint8_t readRegister(uint8_t address); }; #endif /* IIS2MDC_H_ */ diff --git a/boards/varmint/include/board/PacketFifo.h b/boards/varmint/include/board/PacketFifo.h index cd341281..9a9e9c26 100644 --- a/boards/varmint/include/board/PacketFifo.h +++ b/boards/varmint/include/board/PacketFifo.h @@ -1,4 +1,4 @@ - /** +/** ****************************************************************************** * File : PacketFifo.h * Date : Sep 20, 2023 @@ -41,104 +41,108 @@ #include #include - #define PACKET_FIFO_MAX_BUFFERS 64 typedef struct { - uint16_t size; - uint8_t *data; + uint16_t size; + uint8_t *data; } Packet; - class PacketFifo { public: - void init - ( - uint16_t max_packets, - uint16_t max_data_size, - uint8_t *buffer_head - ) - { - if(max_packets> PACKET_FIFO_MAX_BUFFERS) packetCountMax_ = PACKET_FIFO_MAX_BUFFERS; - else packetCountMax_ = max_packets; - dataSizeMax_ = max_data_size; - reset(); + void init(uint16_t max_packets, uint16_t max_data_size, uint8_t *buffer_head) + { + if (max_packets > PACKET_FIFO_MAX_BUFFERS) + packetCountMax_ = PACKET_FIFO_MAX_BUFFERS; + else + packetCountMax_ = max_packets; + dataSizeMax_ = max_data_size; + reset(); - for(uint16_t i=0; i dataSizeMax_) ? dataSizeMax_ : size; - packet_[head_].size = size; - memcpy(packet_[head_].data, data, size); - if(++head_ == packetCountMax_) head_ = 0; - return size; - } -/** - * @fn uint16_t read(uint8_t*, uint16_t) - * @brief Gets oldest buffer in the Fifo. - * - * @param data Data gets copied here - * @param size Maximum size of data buffer - * @return Actual size of data buffer read - */ - uint16_t read(uint8_t *data, uint16_t size) - { - if(head_ == tail_) return 0; // buffer is empty - if(size > packet_[tail_].size) size = packet_[tail_].size; - memcpy(data,packet_[tail_].data, size); - if(++tail_ == packetCountMax_) tail_ = 0; - return size; - } -/** - * @fn uint16_t readMostRecent(uint8_t*, uint16_t) - * @brief Get the most recent data in the Fifo, drop the older buffers. - * - * @param data Data gets copied here - * @param size Maximum size of data buffer - * @return Actual size of data buffer read - */ - uint16_t readMostRecent(uint8_t *data, uint16_t size) - { - if(head_ == tail_) return 0; // buffer is empty - if(head_ == 0) tail_ = packetCountMax_-1; - else tail_ = head_-1; - return read(data,size); - } + uint16_t write(uint8_t *data, uint16_t size) + { + if (tail_ == (head_ + 1) % packetCountMax_) + return false; + size = (size > dataSizeMax_) ? dataSizeMax_ : size; + packet_[head_].size = size; + memcpy(packet_[head_].data, data, size); + if (++head_ == packetCountMax_) + head_ = 0; + return size; + } + /** + * @fn uint16_t read(uint8_t*, uint16_t) + * @brief Gets oldest buffer in the Fifo. + * + * @param data Data gets copied here + * @param size Maximum size of data buffer + * @return Actual size of data buffer read + */ + uint16_t read(uint8_t *data, uint16_t size) + { + if (head_ == tail_) + return 0; // buffer is empty + if (size > packet_[tail_].size) + size = packet_[tail_].size; + memcpy(data, packet_[tail_].data, size); + if (++tail_ == packetCountMax_) + tail_ = 0; + return size; + } + /** + * @fn uint16_t readMostRecent(uint8_t*, uint16_t) + * @brief Get the most recent data in the Fifo, drop the older buffers. + * + * @param data Data gets copied here + * @param size Maximum size of data buffer + * @return Actual size of data buffer read + */ + uint16_t readMostRecent(uint8_t *data, uint16_t size) + { + if (head_ == tail_) + return 0; // buffer is empty + if (head_ == 0) + tail_ = packetCountMax_ - 1; + else + tail_ = head_ - 1; + return read(data, size); + } -/** - * @fn Packet peek*(void) - * @brief Get a pointer to the current read buffer. - * User must check packetCount()>0 before calling this to ensure data is not volatile. - * - * @return Pointer to the packet with index dataOut_. - * - */ - Packet *peek(void) - { - return &(packet_[tail_]); - } + /** + * @fn Packet peek*(void) + * @brief Get a pointer to the current read buffer. + * User must check packetCount()>0 before calling this to ensure data is not volatile. + * + * @return Pointer to the packet with index dataOut_. + * + */ + Packet *peek(void) { return &(packet_[tail_]); } - uint16_t packetCount(void) { return (packetCountMax_+head_-tail_)%packetCountMax_; } - uint16_t packetCountMax(void) { return packetCountMax_; } + uint16_t packetCount(void) { return (packetCountMax_ + head_ - tail_) % packetCountMax_; } + uint16_t packetCountMax(void) { return packetCountMax_; } private: - volatile uint16_t head_ , tail_; - uint16_t dataSizeMax_; - uint16_t packetCountMax_; + volatile uint16_t head_, tail_; + uint16_t dataSizeMax_; + uint16_t packetCountMax_; - volatile uint16_t bufferSize_; - Packet packet_[PACKET_FIFO_MAX_BUFFERS]; + volatile uint16_t bufferSize_; + Packet packet_[PACKET_FIFO_MAX_BUFFERS]; }; #endif /* PACKETFIFO_H_ */ diff --git a/boards/varmint/include/board/Packets.h b/boards/varmint/include/board/Packets.h index 3b08390d..00660942 100644 --- a/boards/varmint/include/board/Packets.h +++ b/boards/varmint/include/board/Packets.h @@ -38,95 +38,89 @@ #ifndef DRIVERPACKETS_H_ #define DRIVERPACKETS_H_ -#include #include +#include -#define SERIAL_MAX_PAYLOAD_SIZE (256+8) //for MAVLINK1, really 255+8, added 1 byte to make it an even multiple of 8 +#define SERIAL_MAX_PAYLOAD_SIZE (256 + 8) // for MAVLINK1, really 255+8, added 1 byte to make it an even multiple of 8 typedef struct __attribute__((__packed__)) { - uint64_t timestamp; // us, time of data read complete - uint16_t qos; - uint16_t packetSize; - uint16_t payloadSize; - uint16_t reserved; - uint8_t payload[SERIAL_MAX_PAYLOAD_SIZE]; + uint64_t timestamp; // us, time of data read complete + uint16_t qos; + uint16_t packetSize; + uint16_t payloadSize; + uint16_t reserved; + uint8_t payload[SERIAL_MAX_PAYLOAD_SIZE]; } SerialTxPacket; typedef struct __attribute__((__packed__)) { - uint64_t timestamp; // us, time of data read complete - uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) - uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) - double temperature; - double vBku; - double vRef; - double volts_ext[ADC_CHANNELS_EXT]; + uint64_t timestamp; // us, time of data read complete + uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) + uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) + double temperature; + double vBku; + double vRef; + double volts_ext[ADC_CHANNELS_EXT]; } AdcPacket; typedef struct __attribute__((__packed__)) { - uint64_t timestamp; // us, time of data read complete - uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) - uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) - uint16_t status; // sensor specific -// - double gyro[3]; // rad/s - double accel[3]; // rad/s - double temperature; // K - double dataTime; // s + uint64_t timestamp; // us, time of data read complete + uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) + uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) + uint16_t status; // sensor specific + // + double gyro[3]; // rad/s + double accel[3]; // rad/s + double temperature; // K + double dataTime; // s } ImuPacket; typedef struct __attribute__((__packed__)) { - uint64_t timestamp; // us, time of data read complete - uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) - uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) - uint16_t status; // sensor specific -// - double pressure; // Pa - double temperature; // K + uint64_t timestamp; // us, time of data read complete + uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) + uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) + uint16_t status; // sensor specific + // + double pressure; // Pa + double temperature; // K } PitotPacket; typedef struct __attribute__((__packed__)) { - uint64_t timestamp; // us, time of data read complete - uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) - uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) - uint16_t status; // sensor specific -// - double pressure; // Pa - double temperature; // K + uint64_t timestamp; // us, time of data read complete + uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) + uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) + uint16_t status; // sensor specific + // + double pressure; // Pa + double temperature; // K } BaroPacket; typedef struct __attribute__((packed)) { - uint64_t timestamp; // us, time of data read complete - uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) - uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) - uint16_t status; // sensor specific -// - double flux[3]; // T, magnetic flux density - double temperature; // K + uint64_t timestamp; // us, time of data read complete + uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) + uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) + uint16_t status; // sensor specific + // + double flux[3]; // T, magnetic flux density + double temperature; // K } MagPacket; #define RC_PACKET_CHANNELS 24 // 16 analog + 8 digital typedef struct __attribute__((__packed__)) { - uint64_t timestamp; // us, time of data read complete - uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) - uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) - uint16_t status; // sensor specific -// - uint8_t nChan; - float chan[RC_PACKET_CHANNELS]; - bool frameLost; - bool failsafeActivated; + uint64_t timestamp; // us, time of data read complete + uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) + uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) + uint16_t status; // sensor specific + // + uint8_t nChan; + float chan[RC_PACKET_CHANNELS]; + bool frameLost; + bool failsafeActivated; } RcPacket; - - - - - - #endif /* DRIVERPACKETS_H_ */ diff --git a/boards/varmint/include/board/Pwm.h b/boards/varmint/include/board/Pwm.h index f52ee64c..0e48128a 100644 --- a/boards/varmint/include/board/Pwm.h +++ b/boards/varmint/include/board/Pwm.h @@ -45,62 +45,64 @@ uint32_t init_pwm_timers_1_3_4(uint32_t servo_pwm_period_us); class Pwm { public: - uint32_t init(TIM_HandleTypeDef *htim, uint16_t chan, uint16_t min, uint16_t center, uint16_t max) - { - htim_ = htim; - chan_ = chan; - min_ = min; - center_ = center; - max_ = max; + uint32_t init(TIM_HandleTypeDef *htim, uint16_t chan, uint16_t min, uint16_t center, uint16_t max) + { + htim_ = htim; + chan_ = chan; + min_ = min; + center_ = center; + max_ = max; - disable(); - writeUs(center_); - return DRIVER_OK; - } - void enable() - { - if(htim_) - { - HAL_TIM_PWM_Start(htim_, chan_); - } - } + disable(); + writeUs(center_); + return DRIVER_OK; + } + void enable() + { + if (htim_) + { + HAL_TIM_PWM_Start(htim_, chan_); + } + } - void disable() - { - if(htim_) HAL_TIM_PWM_Stop(htim_, chan_); - } + void disable() + { + if (htim_) + HAL_TIM_PWM_Stop(htim_, chan_); + } - void writeUs(uint16_t us) - { - if(htim_) - { - us = (usmax_)?max_:us; - __HAL_TIM_SET_COMPARE(htim_, chan_, us); - } - } - void write(double val) - { - if(htim_) - { - val = (val<0)?0:val; - val = (val>1)?1:val; - uint16_t us = val*(double)(max_-min_)+min_; - __HAL_TIM_SET_COMPARE(htim_, chan_, us); - } - } + void writeUs(uint16_t us) + { + if (htim_) + { + us = (us < min_) ? min_ : us; + us = (us > max_) ? max_ : us; + __HAL_TIM_SET_COMPARE(htim_, chan_, us); + } + } + void write(double val) + { + if (htim_) + { + val = (val < 0) ? 0 : val; + val = (val > 1) ? 1 : val; + uint16_t us = val * (double)(max_ - min_) + min_; + __HAL_TIM_SET_COMPARE(htim_, chan_, us); + } + } - void set_rate(uint32_t rate) - { - if((rate>0) && (rate <=400)) htim_->Instance->ARR = 1000000/rate; - } + void set_rate(uint32_t rate) + { + if ((rate > 0) && (rate <= 400)) + htim_->Instance->ARR = 1000000 / rate; + } private: - TIM_HandleTypeDef *htim_; - uint32_t chan_; - uint16_t min_; // us - uint16_t center_; // us - uint16_t max_; // us + TIM_HandleTypeDef *htim_; + uint32_t chan_; + uint16_t min_; // us + uint16_t center_; // us + uint16_t max_; // us }; #endif /* PWM_H_ */ diff --git a/boards/varmint/include/board/Sbus.h b/boards/varmint/include/board/Sbus.h index c6fe20c7..827f197c 100644 --- a/boards/varmint/include/board/Sbus.h +++ b/boards/varmint/include/board/Sbus.h @@ -42,43 +42,40 @@ #include #include - /* * */ class Sbus : public Driver { - /** - * \brief - * - * - */ + /** + * \brief + * + * + */ public: - uint32_t init - ( - // Driver initializers - uint16_t sample_rate_hz, - // UART initializers - UART_HandleTypeDef *huart, - USART_TypeDef *huart_instance, - DMA_HandleTypeDef *hdma_uart_rx, - uint32_t baud - ); + uint32_t init( + // Driver initializers + uint16_t sample_rate_hz, + // UART initializers + UART_HandleTypeDef *huart, + USART_TypeDef *huart_instance, + DMA_HandleTypeDef *hdma_uart_rx, + uint32_t baud); - bool poll(void); - void endDma(void) override; - bool startDma(void) override; - bool display(void) override; - bool lol(void) { return lol_;} + bool poll(void); + void endDma(void) override; + bool startDma(void) override; + bool display(void) override; + bool lol(void) { return lol_; } - UART_HandleTypeDef* huart(void) {return huart_;} - bool isMy(UART_HandleTypeDef* huart) {return huart_==huart;} + UART_HandleTypeDef *huart(void) { return huart_; } + bool isMy(UART_HandleTypeDef *huart) { return huart_ == huart; } private: - bool lol_; - uint64_t timeout_, dtimeout_; - UART_HandleTypeDef *huart_; - DMA_HandleTypeDef *hdmaUartRx_; + bool lol_; + uint64_t timeout_, dtimeout_; + UART_HandleTypeDef *huart_; + DMA_HandleTypeDef *hdmaUartRx_; }; #endif /* SBUS_H_ */ diff --git a/boards/varmint/include/board/Sd.h b/boards/varmint/include/board/Sd.h index 22cdbbc7..f55ddaf8 100644 --- a/boards/varmint/include/board/Sd.h +++ b/boards/varmint/include/board/Sd.h @@ -41,24 +41,23 @@ #include #include - /* * */ class Sd { - /** - * \brief - * - * - */ + /** + * \brief + * + * + */ public: - uint32_t init(SD_HandleTypeDef *hsd, SD_TypeDef *hsd_instance); - bool read(uint8_t *dest, size_t len); - bool write(uint8_t *src, size_t len); + uint32_t init(SD_HandleTypeDef *hsd, SD_TypeDef *hsd_instance); + bool read(uint8_t *dest, size_t len); + bool write(uint8_t *src, size_t len); private: - SD_HandleTypeDef *hsd_; + SD_HandleTypeDef *hsd_; }; #endif /* SD_H_ */ diff --git a/boards/varmint/include/board/Spi.h b/boards/varmint/include/board/Spi.h index 9a00cdd4..3883c47c 100644 --- a/boards/varmint/include/board/Spi.h +++ b/boards/varmint/include/board/Spi.h @@ -40,12 +40,10 @@ #include "stm32h7xx_hal.h" -#include // for memset - -#include - #include #include +#include +#include // for memset extern Time64 time64; @@ -55,94 +53,92 @@ extern Time64 time64; class Spi { - /** - * \brief - * - * - */ + /** + * \brief + * + * + */ private: - SPI_HandleTypeDef *hspi_; - uint8_t *txBuffer_; - uint8_t *rxBuffer_; - + SPI_HandleTypeDef *hspi_; + uint8_t *txBuffer_; + uint8_t *rxBuffer_; public: - //uint16_t size_; - GPIO_TypeDef *port_; - uint16_t pin_; - uint32_t init - ( - SPI_HandleTypeDef *hspi, - uint8_t *tx_buffer, - uint8_t *rx_buffer, - GPIO_TypeDef *cs_port, - uint16_t cs_pin - ) - { - hspi_ = hspi; - txBuffer_ = tx_buffer; - rxBuffer_ = rx_buffer; - port_ = cs_port; - pin_ = cs_pin; - return DRIVER_OK; - } - - SPI_HandleTypeDef *hspi(void) { return hspi_; } - - uint8_t *endDma(void) - { - bool software_nss = !(hspi_->Init.NSS == SPI_NSS_HARD_OUTPUT); - - if(software_nss) HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_SET); - - #if CACHE_MANAGEMENT_FUNCTIONS - SCB_InvalidateDCache_by_Addr((uint32_t *)rxBuffer_, SPI_DMA_MAX_BUFFER_SIZE); // Force read from SRAM - #endif - time64.dUs(2); - return rxBuffer_; - } - - HAL_StatusTypeDef startDma(uint8_t tx_byte, uint16_t size) - { - bool software_nss = !(hspi_->Init.NSS == SPI_NSS_HARD_OUTPUT); - - memset(txBuffer_,0,SPI_DMA_MAX_BUFFER_SIZE); - memset(rxBuffer_,0xFF,SPI_DMA_MAX_BUFFER_SIZE); - txBuffer_[0] = tx_byte; - - #if CACHE_MANAGEMENT_FUNCTIONS - SCB_CleanDCache_by_Addr((uint32_t *)txBuffer_, SPI_DMA_MAX_BUFFER_SIZE) ; // Force data from cache to SRAM - #endif - - if(software_nss) HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_RESET); - HAL_StatusTypeDef hal_status = HAL_SPI_TransmitReceive_DMA(hspi_, txBuffer_, rxBuffer_, size); - if((HAL_OK!=hal_status) && (software_nss)) HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_SET); - time64.dUs(2); - - return hal_status; - } - - - HAL_StatusTypeDef rx(uint8_t *tx_buffer, uint8_t *rx_buffer, uint16_t size, uint16_t timeout_ms) - { - bool software_nss = !(hspi_->Init.NSS == SPI_NSS_HARD_OUTPUT); - if(software_nss) HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_RESET); - HAL_StatusTypeDef hal_status = HAL_SPI_TransmitReceive(hspi_, tx_buffer, rx_buffer, size, timeout_ms); - if(software_nss) HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_SET); - time64.dUs(2); - return hal_status; - } - - HAL_StatusTypeDef tx(uint8_t *tx_buffer, uint16_t size, uint16_t timeout_ms) - { - bool software_nss = !(hspi_->Init.NSS == SPI_NSS_HARD_OUTPUT); - if(software_nss) HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_RESET); - HAL_StatusTypeDef hal_status = HAL_SPI_Transmit(hspi_, tx_buffer, size, timeout_ms); - if(software_nss) HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_SET); - time64.dUs(2); - return hal_status; - } + // uint16_t size_; + GPIO_TypeDef *port_; + uint16_t pin_; + uint32_t init(SPI_HandleTypeDef *hspi, uint8_t *tx_buffer, uint8_t *rx_buffer, GPIO_TypeDef *cs_port, uint16_t cs_pin) + { + hspi_ = hspi; + txBuffer_ = tx_buffer; + rxBuffer_ = rx_buffer; + port_ = cs_port; + pin_ = cs_pin; + return DRIVER_OK; + } + + SPI_HandleTypeDef *hspi(void) { return hspi_; } + + uint8_t *endDma(void) + { + bool software_nss = !(hspi_->Init.NSS == SPI_NSS_HARD_OUTPUT); + + if (software_nss) + HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_SET); + +#if CACHE_MANAGEMENT_FUNCTIONS + SCB_InvalidateDCache_by_Addr((uint32_t *)rxBuffer_, SPI_DMA_MAX_BUFFER_SIZE); // Force read from SRAM +#endif + time64.dUs(2); + return rxBuffer_; + } + + HAL_StatusTypeDef startDma(uint8_t tx_byte, uint16_t size) + { + bool software_nss = !(hspi_->Init.NSS == SPI_NSS_HARD_OUTPUT); + + memset(txBuffer_, 0, SPI_DMA_MAX_BUFFER_SIZE); + memset(rxBuffer_, 0xFF, SPI_DMA_MAX_BUFFER_SIZE); + txBuffer_[0] = tx_byte; + +#if CACHE_MANAGEMENT_FUNCTIONS + SCB_CleanDCache_by_Addr((uint32_t *)txBuffer_, SPI_DMA_MAX_BUFFER_SIZE); // Force data from cache to SRAM +#endif + + if (software_nss) + HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_RESET); + HAL_StatusTypeDef hal_status = HAL_SPI_TransmitReceive_DMA(hspi_, txBuffer_, rxBuffer_, size); + if ((HAL_OK != hal_status) && (software_nss)) + HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_SET); + time64.dUs(2); + + return hal_status; + } + + HAL_StatusTypeDef rx(uint8_t *tx_buffer, uint8_t *rx_buffer, uint16_t size, uint16_t timeout_ms) + { + bool software_nss = !(hspi_->Init.NSS == SPI_NSS_HARD_OUTPUT); + if (software_nss) + HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_RESET); + HAL_StatusTypeDef hal_status = HAL_SPI_TransmitReceive(hspi_, tx_buffer, rx_buffer, size, timeout_ms); + if (software_nss) + HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_SET); + time64.dUs(2); + return hal_status; + } + + HAL_StatusTypeDef tx(uint8_t *tx_buffer, uint16_t size, uint16_t timeout_ms) + { + bool software_nss = !(hspi_->Init.NSS == SPI_NSS_HARD_OUTPUT); + if (software_nss) + HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_RESET); + HAL_StatusTypeDef hal_status = HAL_SPI_Transmit(hspi_, tx_buffer, size, timeout_ms); + if (software_nss) + HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_SET); + time64.dUs(2); + return hal_status; + } }; #endif /* SPI_H_ */ diff --git a/boards/varmint/include/board/Telem.h b/boards/varmint/include/board/Telem.h index 5b8ceb3a..e7b8ee4c 100644 --- a/boards/varmint/include/board/Telem.h +++ b/boards/varmint/include/board/Telem.h @@ -39,14 +39,18 @@ #define TELEM_H_ #include -#include #include +#include #include #include extern Time64 time64; -enum DmaItType { HALF, FULL, IDLE}; - +enum DmaItType +{ + HALF, + FULL, + IDLE +}; /** * @class Telem @@ -55,54 +59,51 @@ enum DmaItType { HALF, FULL, IDLE}; */ class Telem { - /** - * \brief - * - * - */ - + /** + * \brief + * + * + */ public: + uint32_t init( + // Driver initializers + uint16_t sample_rate_hz, + // UART initializers + UART_HandleTypeDef *huart, + USART_TypeDef *huart_instance, + DMA_HandleTypeDef *hdma_uart_rx, + uint32_t baud, + void (*RxISR)(UART_HandleTypeDef *huart)); + uint32_t reset_baud(uint32_t baud); - uint32_t init - ( - // Driver initializers - uint16_t sample_rate_hz, - // UART initializers - UART_HandleTypeDef *huart, - USART_TypeDef *huart_instance, - DMA_HandleTypeDef *hdma_uart_rx, - uint32_t baud, - void (*RxISR)(UART_HandleTypeDef *huart) - ); - uint32_t reset_baud(uint32_t baud); + void poll(void); + bool display(void) { return 0; } + uint16_t byteCount(void) { return rxFifo_.byteCount(); } + bool readByte(uint8_t *data) { return rxFifo_.read(data); } - void poll(void) ; - bool display(void) {return 0;} - uint16_t byteCount(void) {return rxFifo_.byteCount();} - bool readByte(uint8_t *data) { return rxFifo_.read(data);} + uint16_t writePacket(SerialTxPacket *p); + bool newPacket(SerialTxPacket *p); - uint16_t writePacket(SerialTxPacket* p); - bool newPacket(SerialTxPacket* p); + UART_HandleTypeDef *huart(void) { return huart_; } + bool isMy(UART_HandleTypeDef *huart) { return huart_ == huart; } + bool rxStart(void); + void rxIsrCallback(UART_HandleTypeDef *huart); + bool txStart(void); - UART_HandleTypeDef* huart(void) {return huart_;} - bool isMy(UART_HandleTypeDef* huart) {return huart_==huart;} - bool rxStart(void); - void rxIsrCallback(UART_HandleTypeDef *huart); - bool txStart(void); private: - int16_t sampleRateHz_; - uint32_t baud_; + int16_t sampleRateHz_; + uint32_t baud_; - PacketFifo *txFifo_; - int64_t txFrameSizeUs_, txFrameEndUs_,usPerByte_; - volatile bool txIdle_; - uint16_t retry_; + PacketFifo *txFifo_; + int64_t txFrameSizeUs_, txFrameEndUs_, usPerByte_; + volatile bool txIdle_; + uint16_t retry_; - ByteFifo rxFifo_; + ByteFifo rxFifo_; - UART_HandleTypeDef *huart_; - DMA_HandleTypeDef *hdmaUartRx_; + UART_HandleTypeDef *huart_; + DMA_HandleTypeDef *hdmaUartRx_; }; #endif /* TELEM_H_ */ diff --git a/boards/varmint/include/board/Time64.h b/boards/varmint/include/board/Time64.h index 30bd9dc8..994570be 100644 --- a/boards/varmint/include/board/Time64.h +++ b/boards/varmint/include/board/Time64.h @@ -39,93 +39,103 @@ #define TIME64_H_ #include -#include #include +#include class Time64 { public: - Time64(){}; - uint32_t init - ( - TIM_HandleTypeDef *htim_low, TIM_TypeDef *instance_low, - TIM_HandleTypeDef *htim_high, TIM_TypeDef *instance_high - ) - { - htimLow_ = htim_low; - htimHigh_ = htim_high; + Time64(){}; + uint32_t init(TIM_HandleTypeDef *htim_low, + TIM_TypeDef *instance_low, + TIM_HandleTypeDef *htim_high, + TIM_TypeDef *instance_high) + { + htimLow_ = htim_low; + htimHigh_ = htim_high; - { - TIM_ClockConfigTypeDef sClockSourceConfig = {0}; - TIM_MasterConfigTypeDef sMasterConfig = {0}; - htimLow_->Instance = instance_low; - htimLow_->Init.Prescaler = 199; - htimLow_->Init.CounterMode = TIM_COUNTERMODE_UP; - htimLow_->Init.Period = 0xffffffff; - htimLow_->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htimLow_->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - if (HAL_TIM_Base_Init(htimLow_) != HAL_OK) return DRIVER_HAL_ERROR; - sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; - if (HAL_TIM_ConfigClockSource(htimLow_, &sClockSourceConfig) != HAL_OK) return DRIVER_HAL_ERROR; - sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(htimLow_, &sMasterConfig) != HAL_OK) return DRIVER_HAL_ERROR; - } + { + TIM_ClockConfigTypeDef sClockSourceConfig = {0}; + TIM_MasterConfigTypeDef sMasterConfig = {0}; + htimLow_->Instance = instance_low; + htimLow_->Init.Prescaler = 199; + htimLow_->Init.CounterMode = TIM_COUNTERMODE_UP; + htimLow_->Init.Period = 0xffffffff; + htimLow_->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htimLow_->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_Base_Init(htimLow_) != HAL_OK) + return DRIVER_HAL_ERROR; + sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; + if (HAL_TIM_ConfigClockSource(htimLow_, &sClockSourceConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(htimLow_, &sMasterConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + } - { - TIM_SlaveConfigTypeDef sSlaveConfig = {0}; - TIM_MasterConfigTypeDef sMasterConfig = {0}; - htimHigh_->Instance = instance_high; - htimHigh_->Init.Prescaler = 0; - htimHigh_->Init.CounterMode = TIM_COUNTERMODE_UP; - htimHigh_->Init.Period = 65535; - htimHigh_->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htimHigh_->Init.RepetitionCounter = 0; - htimHigh_->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - if (HAL_TIM_Base_Init(htimHigh_) != HAL_OK) return DRIVER_HAL_ERROR; - sSlaveConfig.SlaveMode = TIM_SLAVEMODE_EXTERNAL1; - sSlaveConfig.InputTrigger = TIM_TS_ITR3; - if (HAL_TIM_SlaveConfigSynchro(htimHigh_, &sSlaveConfig) != HAL_OK) return DRIVER_HAL_ERROR; - sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; - sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(htimHigh_, &sMasterConfig) != HAL_OK) return DRIVER_HAL_ERROR; - } + { + TIM_SlaveConfigTypeDef sSlaveConfig = {0}; + TIM_MasterConfigTypeDef sMasterConfig = {0}; + htimHigh_->Instance = instance_high; + htimHigh_->Init.Prescaler = 0; + htimHigh_->Init.CounterMode = TIM_COUNTERMODE_UP; + htimHigh_->Init.Period = 65535; + htimHigh_->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htimHigh_->Init.RepetitionCounter = 0; + htimHigh_->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_Base_Init(htimHigh_) != HAL_OK) + return DRIVER_HAL_ERROR; + sSlaveConfig.SlaveMode = TIM_SLAVEMODE_EXTERNAL1; + sSlaveConfig.InputTrigger = TIM_TS_ITR3; + if (HAL_TIM_SlaveConfigSynchro(htimHigh_, &sSlaveConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(htimHigh_, &sMasterConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + } - // Note priority is set in HAL_TIM_Base_MspInit(). + // Note priority is set in HAL_TIM_Base_MspInit(). - __HAL_TIM_SET_COUNTER(htimHigh_,0); - __HAL_TIM_SET_COUNTER(htimLow_, 0); - HAL_TIM_Base_Start_IT(htimHigh_); // Startup our overflow counter. - HAL_TIM_Base_Start_IT(htimLow_); // Startup our us counter. - return DRIVER_OK; - } + __HAL_TIM_SET_COUNTER(htimHigh_, 0); + __HAL_TIM_SET_COUNTER(htimLow_, 0); + HAL_TIM_Base_Start_IT(htimHigh_); // Startup our overflow counter. + HAL_TIM_Base_Start_IT(htimLow_); // Startup our us counter. + return DRIVER_OK; + } - uint64_t Us(void) - { - volatile uint32_t low1 = __HAL_TIM_GET_COUNTER(htimLow_); // htimLow_->Instance->CNT; - volatile uint32_t high1 = __HAL_TIM_GET_COUNTER(htimHigh_); - volatile uint32_t low2 = __HAL_TIM_GET_COUNTER(htimLow_); - volatile uint32_t high2 = __HAL_TIM_GET_COUNTER(htimHigh_); - if( (low1 > low2) && (high1 == high2)) high1--; // rollover correction - return (uint64_t)high1<<32 | (uint64_t)low1; - } + uint64_t Us(void) + { + volatile uint32_t low1 = __HAL_TIM_GET_COUNTER(htimLow_); // htimLow_->Instance->CNT; + volatile uint32_t high1 = __HAL_TIM_GET_COUNTER(htimHigh_); + volatile uint32_t low2 = __HAL_TIM_GET_COUNTER(htimLow_); + volatile uint32_t high2 = __HAL_TIM_GET_COUNTER(htimHigh_); + if ((low1 > low2) && (high1 == high2)) + high1--; // rollover correction + return (uint64_t)high1 << 32 | (uint64_t)low1; + } - void dUs(uint32_t dt) - { - uint64_t endtimestamp = Us()+dt; - while (Us() -#include #include +#include +#include #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif -void misc_printf(const char* format, ...); -void misc_header(char *name, uint64_t drdy, uint64_t timestamp, uint64_t delay); -uint16_t misc_bytes_in_dma( DMA_HandleTypeDef *hdma_uart_rx, uint16_t dma_buffer_size); -void misc_exit_status(uint32_t status); + void misc_printf(const char *format, ...); + void misc_header(char *name, uint64_t drdy, uint64_t timestamp, uint64_t delay); + uint16_t misc_bytes_in_dma(DMA_HandleTypeDef *hdma_uart_rx, uint16_t dma_buffer_size); + void misc_exit_status(uint32_t status); #ifdef __cplusplus } #endif - #endif /* MISC_H_ */ diff --git a/boards/varmint/include/main.h b/boards/varmint/include/main.h index 25b2a106..b86befa1 100644 --- a/boards/varmint/include/main.h +++ b/boards/varmint/include/main.h @@ -1,21 +1,21 @@ /* USER CODE BEGIN Header */ /** - ****************************************************************************** - * @file : main.h - * @brief : Header for main.c file. - * This file contains the common defines of the application. - ****************************************************************************** - * @attention - * - * Copyright (c) 2023 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ + ****************************************************************************** + * @file : main.h + * @brief : Header for main.c file. + * This file contains the common defines of the application. + ****************************************************************************** + * @attention + * + * Copyright (c) 2023 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ /* USER CODE END Header */ /* Define to prevent recursive inclusion -------------------------------------*/ @@ -23,62 +23,63 @@ #define __MAIN_H #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif /* Includes ------------------------------------------------------------------*/ #include "stm32h7xx_hal.h" -/* Private includes ----------------------------------------------------------*/ -/* USER CODE BEGIN Includes */ + /* Private includes ----------------------------------------------------------*/ + /* USER CODE BEGIN Includes */ -/* USER CODE END Includes */ + /* USER CODE END Includes */ -/* Exported types ------------------------------------------------------------*/ -/* USER CODE BEGIN ET */ + /* Exported types ------------------------------------------------------------*/ + /* USER CODE BEGIN ET */ -/* USER CODE END ET */ + /* USER CODE END ET */ -/* Exported constants --------------------------------------------------------*/ -/* USER CODE BEGIN EC */ + /* Exported constants --------------------------------------------------------*/ + /* USER CODE BEGIN EC */ -/* USER CODE END EC */ + /* USER CODE END EC */ -/* Exported macro ------------------------------------------------------------*/ -/* USER CODE BEGIN EM */ + /* Exported macro ------------------------------------------------------------*/ + /* USER CODE BEGIN EM */ -/* USER CODE END EM */ + /* USER CODE END EM */ -void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); + void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); -/* Exported functions prototypes ---------------------------------------------*/ -void Error_Handler(void); -void MX_GPIO_Init(void); -void MX_DMA_Init(void); -void MX_BDMA_Init(void); -void MX_I2C2_Init(void); -void MX_SPI1_Init(void); -void MX_SPI2_Init(void); -void MX_SPI3_Init(void); -void MX_SPI4_Init(void); -void MX_TIM1_Init(void); -void MX_TIM3_Init(void); -void MX_TIM4_Init(void); -void MX_TIM5_Init(void); -void MX_TIM8_Init(void); -void MX_TIM7_Init(void); -void MX_TIM12_Init(void); -void MX_USART1_UART_Init(void); -void MX_USART2_UART_Init(void); -void MX_USART3_UART_Init(void); -void MX_ADC1_Init(void); -void MX_ADC3_Init(void); -void MX_FDCAN1_Init(void); -void MX_SDMMC1_SD_Init(void); -void MX_RTC_Init(void); -void MX_I2C1_Init(void); -void MX_CRC_Init(void); -void MX_RNG_Init(void); + /* Exported functions prototypes ---------------------------------------------*/ + void Error_Handler(void); + void MX_GPIO_Init(void); + void MX_DMA_Init(void); + void MX_BDMA_Init(void); + void MX_I2C2_Init(void); + void MX_SPI1_Init(void); + void MX_SPI2_Init(void); + void MX_SPI3_Init(void); + void MX_SPI4_Init(void); + void MX_TIM1_Init(void); + void MX_TIM3_Init(void); + void MX_TIM4_Init(void); + void MX_TIM5_Init(void); + void MX_TIM8_Init(void); + void MX_TIM7_Init(void); + void MX_TIM12_Init(void); + void MX_USART1_UART_Init(void); + void MX_USART2_UART_Init(void); + void MX_USART3_UART_Init(void); + void MX_ADC1_Init(void); + void MX_ADC3_Init(void); + void MX_FDCAN1_Init(void); + void MX_SDMMC1_SD_Init(void); + void MX_RTC_Init(void); + void MX_I2C1_Init(void); + void MX_CRC_Init(void); + void MX_RNG_Init(void); /* USER CODE BEGIN EFP */ @@ -260,9 +261,9 @@ void MX_RNG_Init(void); #define J106_PWM07_Pin GPIO_PIN_12 #define J106_PWM07_GPIO_Port GPIOD -/* USER CODE BEGIN Private defines */ + /* USER CODE BEGIN Private defines */ -/* USER CODE END Private defines */ + /* USER CODE END Private defines */ #ifdef __cplusplus } diff --git a/boards/varmint/include/stm32h7xx_hal_conf.h b/boards/varmint/include/stm32h7xx_hal_conf.h index 541c06c6..c965da42 100644 --- a/boards/varmint/include/stm32h7xx_hal_conf.h +++ b/boards/varmint/include/stm32h7xx_hal_conf.h @@ -1,28 +1,29 @@ /* USER CODE BEGIN Header */ /** - ****************************************************************************** - * @file stm32h7xx_hal_conf.h - * @author MCD Application Team - * @brief HAL configuration file. - ****************************************************************************** - * @attention - * - * Copyright (c) 2017 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ + ****************************************************************************** + * @file stm32h7xx_hal_conf.h + * @author MCD Application Team + * @brief HAL configuration file. + ****************************************************************************** + * @attention + * + * Copyright (c) 2017 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ /* USER CODE END Header */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef STM32H7xx_HAL_CONF_H #define STM32H7xx_HAL_CONF_H #ifdef __cplusplus - extern "C" { +extern "C" +{ #endif /* Exported types ------------------------------------------------------------*/ @@ -30,11 +31,11 @@ /* ########################## Module Selection ############################## */ /** - * @brief This is the list of modules to be used in the HAL driver - */ + * @brief This is the list of modules to be used in the HAL driver + */ #define HAL_MODULE_ENABLED - #define HAL_ADC_MODULE_ENABLED +#define HAL_ADC_MODULE_ENABLED #define HAL_FDCAN_MODULE_ENABLED /* #define HAL_FMAC_MODULE_ENABLED */ /* #define HAL_CEC_MODULE_ENABLED */ @@ -102,289 +103,289 @@ /* ########################## Oscillator Values adaptation ####################*/ /** - * @brief Adjust the value of External High Speed oscillator (HSE) used in your application. - * This value is used by the RCC HAL module to compute the system frequency - * (when HSE is used as system clock source, directly or through the PLL). - */ -#if !defined (HSE_VALUE) -#define HSE_VALUE (50000000UL) /*!< Value of the External oscillator in Hz : FPGA case fixed to 60MHZ */ -#endif /* HSE_VALUE */ - -#if !defined (HSE_STARTUP_TIMEOUT) - #define HSE_STARTUP_TIMEOUT (100UL) /*!< Time out for HSE start up, in ms */ -#endif /* HSE_STARTUP_TIMEOUT */ + * @brief Adjust the value of External High Speed oscillator (HSE) used in your application. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSE is used as system clock source, directly or through the PLL). + */ +#if !defined(HSE_VALUE) +#define HSE_VALUE (50000000UL) /*!< Value of the External oscillator in Hz : FPGA case fixed to 60MHZ */ +#endif /* HSE_VALUE */ + +#if !defined(HSE_STARTUP_TIMEOUT) +#define HSE_STARTUP_TIMEOUT (100UL) /*!< Time out for HSE start up, in ms */ +#endif /* HSE_STARTUP_TIMEOUT */ /** - * @brief Internal oscillator (CSI) default value. - * This value is the default CSI value after Reset. - */ -#if !defined (CSI_VALUE) - #define CSI_VALUE (4000000UL) /*!< Value of the Internal oscillator in Hz*/ -#endif /* CSI_VALUE */ + * @brief Internal oscillator (CSI) default value. + * This value is the default CSI value after Reset. + */ +#if !defined(CSI_VALUE) +#define CSI_VALUE (4000000UL) /*!< Value of the Internal oscillator in Hz*/ +#endif /* CSI_VALUE */ /** - * @brief Internal High Speed oscillator (HSI) value. - * This value is used by the RCC HAL module to compute the system frequency - * (when HSI is used as system clock source, directly or through the PLL). - */ -#if !defined (HSI_VALUE) - #define HSI_VALUE (64000000UL) /*!< Value of the Internal oscillator in Hz*/ -#endif /* HSI_VALUE */ + * @brief Internal High Speed oscillator (HSI) value. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSI is used as system clock source, directly or through the PLL). + */ +#if !defined(HSI_VALUE) +#define HSI_VALUE (64000000UL) /*!< Value of the Internal oscillator in Hz*/ +#endif /* HSI_VALUE */ /** - * @brief External Low Speed oscillator (LSE) value. - * This value is used by the UART, RTC HAL module to compute the system frequency - */ -#if !defined (LSE_VALUE) - #define LSE_VALUE (32768UL) /*!< Value of the External oscillator in Hz*/ -#endif /* LSE_VALUE */ - -#if !defined (LSE_STARTUP_TIMEOUT) - #define LSE_STARTUP_TIMEOUT (5000UL) /*!< Time out for LSE start up, in ms */ -#endif /* LSE_STARTUP_TIMEOUT */ - -#if !defined (LSI_VALUE) - #define LSI_VALUE (32000UL) /*!< LSI Typical Value in Hz*/ -#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz - The real value may vary depending on the variations - in voltage and temperature.*/ + * @brief External Low Speed oscillator (LSE) value. + * This value is used by the UART, RTC HAL module to compute the system frequency + */ +#if !defined(LSE_VALUE) +#define LSE_VALUE (32768UL) /*!< Value of the External oscillator in Hz*/ +#endif /* LSE_VALUE */ + +#if !defined(LSE_STARTUP_TIMEOUT) +#define LSE_STARTUP_TIMEOUT (5000UL) /*!< Time out for LSE start up, in ms */ +#endif /* LSE_STARTUP_TIMEOUT */ + +#if !defined(LSI_VALUE) +#define LSI_VALUE (32000UL) /*!< LSI Typical Value in Hz*/ +#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz \ + The real value may vary depending on the variations \ + in voltage and temperature.*/ /** - * @brief External clock source for I2S peripheral - * This value is used by the I2S HAL module to compute the I2S clock source - * frequency, this source is inserted directly through I2S_CKIN pad. - */ -#if !defined (EXTERNAL_CLOCK_VALUE) - #define EXTERNAL_CLOCK_VALUE 12288000UL /*!< Value of the External clock in Hz*/ -#endif /* EXTERNAL_CLOCK_VALUE */ + * @brief External clock source for I2S peripheral + * This value is used by the I2S HAL module to compute the I2S clock source + * frequency, this source is inserted directly through I2S_CKIN pad. + */ +#if !defined(EXTERNAL_CLOCK_VALUE) +#define EXTERNAL_CLOCK_VALUE 12288000UL /*!< Value of the External clock in Hz*/ +#endif /* EXTERNAL_CLOCK_VALUE */ /* Tip: To avoid modifying this file each time you need to use different HSE, === you can define the HSE value in your toolchain compiler preprocessor. */ /* ########################### System Configuration ######################### */ /** - * @brief This is the HAL system configuration section - */ -#define VDD_VALUE (3300UL) /*!< Value of VDD in mv */ -#define TICK_INT_PRIORITY (15UL) /*!< tick interrupt priority */ -#define USE_RTOS 0 -#define USE_SD_TRANSCEIVER 0U /*!< use uSD Transceiver */ -#define USE_SPI_CRC 0U /*!< use CRC in SPI */ - -#define USE_HAL_ADC_REGISTER_CALLBACKS 0U /* ADC register callback disabled */ -#define USE_HAL_CEC_REGISTER_CALLBACKS 0U /* CEC register callback disabled */ -#define USE_HAL_COMP_REGISTER_CALLBACKS 0U /* COMP register callback disabled */ -#define USE_HAL_CORDIC_REGISTER_CALLBACKS 0U /* CORDIC register callback disabled */ -#define USE_HAL_CRYP_REGISTER_CALLBACKS 0U /* CRYP register callback disabled */ -#define USE_HAL_DAC_REGISTER_CALLBACKS 0U /* DAC register callback disabled */ -#define USE_HAL_DCMI_REGISTER_CALLBACKS 0U /* DCMI register callback disabled */ -#define USE_HAL_DFSDM_REGISTER_CALLBACKS 0U /* DFSDM register callback disabled */ -#define USE_HAL_DMA2D_REGISTER_CALLBACKS 0U /* DMA2D register callback disabled */ -#define USE_HAL_DSI_REGISTER_CALLBACKS 0U /* DSI register callback disabled */ -#define USE_HAL_DTS_REGISTER_CALLBACKS 0U /* DTS register callback disabled */ -#define USE_HAL_ETH_REGISTER_CALLBACKS 0U /* ETH register callback disabled */ -#define USE_HAL_FDCAN_REGISTER_CALLBACKS 0U /* FDCAN register callback disabled */ -#define USE_HAL_FMAC_REGISTER_CALLBACKS 0U /* FMAC register callback disabled */ -#define USE_HAL_NAND_REGISTER_CALLBACKS 0U /* NAND register callback disabled */ -#define USE_HAL_NOR_REGISTER_CALLBACKS 0U /* NOR register callback disabled */ -#define USE_HAL_SDRAM_REGISTER_CALLBACKS 0U /* SDRAM register callback disabled */ -#define USE_HAL_SRAM_REGISTER_CALLBACKS 0U /* SRAM register callback disabled */ -#define USE_HAL_HASH_REGISTER_CALLBACKS 0U /* HASH register callback disabled */ -#define USE_HAL_HCD_REGISTER_CALLBACKS 0U /* HCD register callback disabled */ -#define USE_HAL_GFXMMU_REGISTER_CALLBACKS 0U /* GFXMMU register callback disabled */ -#define USE_HAL_HRTIM_REGISTER_CALLBACKS 0U /* HRTIM register callback disabled */ -#define USE_HAL_I2C_REGISTER_CALLBACKS 0U /* I2C register callback disabled */ -#define USE_HAL_I2S_REGISTER_CALLBACKS 0U /* I2S register callback disabled */ -#define USE_HAL_IRDA_REGISTER_CALLBACKS 0U /* IRDA register callback disabled */ -#define USE_HAL_JPEG_REGISTER_CALLBACKS 0U /* JPEG register callback disabled */ -#define USE_HAL_LPTIM_REGISTER_CALLBACKS 0U /* LPTIM register callback disabled */ -#define USE_HAL_LTDC_REGISTER_CALLBACKS 0U /* LTDC register callback disabled */ -#define USE_HAL_MDIOS_REGISTER_CALLBACKS 0U /* MDIO register callback disabled */ -#define USE_HAL_MMC_REGISTER_CALLBACKS 0U /* MMC register callback disabled */ -#define USE_HAL_OPAMP_REGISTER_CALLBACKS 0U /* MDIO register callback disabled */ -#define USE_HAL_OSPI_REGISTER_CALLBACKS 0U /* OSPI register callback disabled */ -#define USE_HAL_OTFDEC_REGISTER_CALLBACKS 0U /* OTFDEC register callback disabled */ -#define USE_HAL_PCD_REGISTER_CALLBACKS 0U /* PCD register callback disabled */ -#define USE_HAL_QSPI_REGISTER_CALLBACKS 0U /* QSPI register callback disabled */ -#define USE_HAL_RNG_REGISTER_CALLBACKS 0U /* RNG register callback disabled */ -#define USE_HAL_RTC_REGISTER_CALLBACKS 0U /* RTC register callback disabled */ -#define USE_HAL_SAI_REGISTER_CALLBACKS 0U /* SAI register callback disabled */ -#define USE_HAL_SD_REGISTER_CALLBACKS 0U /* SD register callback disabled */ -#define USE_HAL_SMARTCARD_REGISTER_CALLBACKS 0U /* SMARTCARD register callback disabled */ -#define USE_HAL_SPDIFRX_REGISTER_CALLBACKS 0U /* SPDIFRX register callback disabled */ -#define USE_HAL_SMBUS_REGISTER_CALLBACKS 0U /* SMBUS register callback disabled */ -#define USE_HAL_SPI_REGISTER_CALLBACKS 0U /* SPI register callback disabled */ -#define USE_HAL_SWPMI_REGISTER_CALLBACKS 0U /* SWPMI register callback disabled */ -#define USE_HAL_TIM_REGISTER_CALLBACKS 0U /* TIM register callback disabled */ -#define USE_HAL_UART_REGISTER_CALLBACKS 0U /* UART register callback disabled */ -#define USE_HAL_USART_REGISTER_CALLBACKS 0U /* USART register callback disabled */ -#define USE_HAL_WWDG_REGISTER_CALLBACKS 0U /* WWDG register callback disabled */ + * @brief This is the HAL system configuration section + */ +#define VDD_VALUE (3300UL) /*!< Value of VDD in mv */ +#define TICK_INT_PRIORITY (15UL) /*!< tick interrupt priority */ +#define USE_RTOS 0 +#define USE_SD_TRANSCEIVER 0U /*!< use uSD Transceiver */ +#define USE_SPI_CRC 0U /*!< use CRC in SPI */ + +#define USE_HAL_ADC_REGISTER_CALLBACKS 0U /* ADC register callback disabled */ +#define USE_HAL_CEC_REGISTER_CALLBACKS 0U /* CEC register callback disabled */ +#define USE_HAL_COMP_REGISTER_CALLBACKS 0U /* COMP register callback disabled */ +#define USE_HAL_CORDIC_REGISTER_CALLBACKS 0U /* CORDIC register callback disabled */ +#define USE_HAL_CRYP_REGISTER_CALLBACKS 0U /* CRYP register callback disabled */ +#define USE_HAL_DAC_REGISTER_CALLBACKS 0U /* DAC register callback disabled */ +#define USE_HAL_DCMI_REGISTER_CALLBACKS 0U /* DCMI register callback disabled */ +#define USE_HAL_DFSDM_REGISTER_CALLBACKS 0U /* DFSDM register callback disabled */ +#define USE_HAL_DMA2D_REGISTER_CALLBACKS 0U /* DMA2D register callback disabled */ +#define USE_HAL_DSI_REGISTER_CALLBACKS 0U /* DSI register callback disabled */ +#define USE_HAL_DTS_REGISTER_CALLBACKS 0U /* DTS register callback disabled */ +#define USE_HAL_ETH_REGISTER_CALLBACKS 0U /* ETH register callback disabled */ +#define USE_HAL_FDCAN_REGISTER_CALLBACKS 0U /* FDCAN register callback disabled */ +#define USE_HAL_FMAC_REGISTER_CALLBACKS 0U /* FMAC register callback disabled */ +#define USE_HAL_NAND_REGISTER_CALLBACKS 0U /* NAND register callback disabled */ +#define USE_HAL_NOR_REGISTER_CALLBACKS 0U /* NOR register callback disabled */ +#define USE_HAL_SDRAM_REGISTER_CALLBACKS 0U /* SDRAM register callback disabled */ +#define USE_HAL_SRAM_REGISTER_CALLBACKS 0U /* SRAM register callback disabled */ +#define USE_HAL_HASH_REGISTER_CALLBACKS 0U /* HASH register callback disabled */ +#define USE_HAL_HCD_REGISTER_CALLBACKS 0U /* HCD register callback disabled */ +#define USE_HAL_GFXMMU_REGISTER_CALLBACKS 0U /* GFXMMU register callback disabled */ +#define USE_HAL_HRTIM_REGISTER_CALLBACKS 0U /* HRTIM register callback disabled */ +#define USE_HAL_I2C_REGISTER_CALLBACKS 0U /* I2C register callback disabled */ +#define USE_HAL_I2S_REGISTER_CALLBACKS 0U /* I2S register callback disabled */ +#define USE_HAL_IRDA_REGISTER_CALLBACKS 0U /* IRDA register callback disabled */ +#define USE_HAL_JPEG_REGISTER_CALLBACKS 0U /* JPEG register callback disabled */ +#define USE_HAL_LPTIM_REGISTER_CALLBACKS 0U /* LPTIM register callback disabled */ +#define USE_HAL_LTDC_REGISTER_CALLBACKS 0U /* LTDC register callback disabled */ +#define USE_HAL_MDIOS_REGISTER_CALLBACKS 0U /* MDIO register callback disabled */ +#define USE_HAL_MMC_REGISTER_CALLBACKS 0U /* MMC register callback disabled */ +#define USE_HAL_OPAMP_REGISTER_CALLBACKS 0U /* MDIO register callback disabled */ +#define USE_HAL_OSPI_REGISTER_CALLBACKS 0U /* OSPI register callback disabled */ +#define USE_HAL_OTFDEC_REGISTER_CALLBACKS 0U /* OTFDEC register callback disabled */ +#define USE_HAL_PCD_REGISTER_CALLBACKS 0U /* PCD register callback disabled */ +#define USE_HAL_QSPI_REGISTER_CALLBACKS 0U /* QSPI register callback disabled */ +#define USE_HAL_RNG_REGISTER_CALLBACKS 0U /* RNG register callback disabled */ +#define USE_HAL_RTC_REGISTER_CALLBACKS 0U /* RTC register callback disabled */ +#define USE_HAL_SAI_REGISTER_CALLBACKS 0U /* SAI register callback disabled */ +#define USE_HAL_SD_REGISTER_CALLBACKS 0U /* SD register callback disabled */ +#define USE_HAL_SMARTCARD_REGISTER_CALLBACKS 0U /* SMARTCARD register callback disabled */ +#define USE_HAL_SPDIFRX_REGISTER_CALLBACKS 0U /* SPDIFRX register callback disabled */ +#define USE_HAL_SMBUS_REGISTER_CALLBACKS 0U /* SMBUS register callback disabled */ +#define USE_HAL_SPI_REGISTER_CALLBACKS 0U /* SPI register callback disabled */ +#define USE_HAL_SWPMI_REGISTER_CALLBACKS 0U /* SWPMI register callback disabled */ +#define USE_HAL_TIM_REGISTER_CALLBACKS 0U /* TIM register callback disabled */ +#define USE_HAL_UART_REGISTER_CALLBACKS 0U /* UART register callback disabled */ +#define USE_HAL_USART_REGISTER_CALLBACKS 0U /* USART register callback disabled */ +#define USE_HAL_WWDG_REGISTER_CALLBACKS 0U /* WWDG register callback disabled */ /* ########################### Ethernet Configuration ######################### */ -#define ETH_TX_DESC_CNT 4U /* number of Ethernet Tx DMA descriptors */ -#define ETH_RX_DESC_CNT 4U /* number of Ethernet Rx DMA descriptors */ - -#define ETH_MAC_ADDR0 (0x02UL) -#define ETH_MAC_ADDR1 (0x00UL) -#define ETH_MAC_ADDR2 (0x00UL) -#define ETH_MAC_ADDR3 (0x00UL) -#define ETH_MAC_ADDR4 (0x00UL) -#define ETH_MAC_ADDR5 (0x00UL) - -/* ########################## Assert Selection ############################## */ -/** - * @brief Uncomment the line below to expanse the "assert_param" macro in the - * HAL drivers code - */ -/* #define USE_FULL_ASSERT 1U */ - -/* Includes ------------------------------------------------------------------*/ -/** - * @brief Include module's header file - */ +#define ETH_TX_DESC_CNT 4U /* number of Ethernet Tx DMA descriptors */ +#define ETH_RX_DESC_CNT 4U /* number of Ethernet Rx DMA descriptors */ + +#define ETH_MAC_ADDR0 (0x02UL) +#define ETH_MAC_ADDR1 (0x00UL) +#define ETH_MAC_ADDR2 (0x00UL) +#define ETH_MAC_ADDR3 (0x00UL) +#define ETH_MAC_ADDR4 (0x00UL) +#define ETH_MAC_ADDR5 (0x00UL) + + /* ########################## Assert Selection ############################## */ + /** + * @brief Uncomment the line below to expanse the "assert_param" macro in the + * HAL drivers code + */ + /* #define USE_FULL_ASSERT 1U */ + + /* Includes ------------------------------------------------------------------*/ + /** + * @brief Include module's header file + */ #ifdef HAL_RCC_MODULE_ENABLED - #include "stm32h7xx_hal_rcc.h" +#include "stm32h7xx_hal_rcc.h" #endif /* HAL_RCC_MODULE_ENABLED */ #ifdef HAL_GPIO_MODULE_ENABLED - #include "stm32h7xx_hal_gpio.h" +#include "stm32h7xx_hal_gpio.h" #endif /* HAL_GPIO_MODULE_ENABLED */ #ifdef HAL_DMA_MODULE_ENABLED - #include "stm32h7xx_hal_dma.h" +#include "stm32h7xx_hal_dma.h" #endif /* HAL_DMA_MODULE_ENABLED */ #ifdef HAL_MDMA_MODULE_ENABLED - #include "stm32h7xx_hal_mdma.h" +#include "stm32h7xx_hal_mdma.h" #endif /* HAL_MDMA_MODULE_ENABLED */ #ifdef HAL_HASH_MODULE_ENABLED - #include "stm32h7xx_hal_hash.h" +#include "stm32h7xx_hal_hash.h" #endif /* HAL_HASH_MODULE_ENABLED */ #ifdef HAL_DCMI_MODULE_ENABLED - #include "stm32h7xx_hal_dcmi.h" +#include "stm32h7xx_hal_dcmi.h" #endif /* HAL_DCMI_MODULE_ENABLED */ #ifdef HAL_DMA2D_MODULE_ENABLED - #include "stm32h7xx_hal_dma2d.h" +#include "stm32h7xx_hal_dma2d.h" #endif /* HAL_DMA2D_MODULE_ENABLED */ #ifdef HAL_DSI_MODULE_ENABLED - #include "stm32h7xx_hal_dsi.h" +#include "stm32h7xx_hal_dsi.h" #endif /* HAL_DSI_MODULE_ENABLED */ #ifdef HAL_DFSDM_MODULE_ENABLED - #include "stm32h7xx_hal_dfsdm.h" +#include "stm32h7xx_hal_dfsdm.h" #endif /* HAL_DFSDM_MODULE_ENABLED */ #ifdef HAL_DTS_MODULE_ENABLED - #include "stm32h7xx_hal_dts.h" +#include "stm32h7xx_hal_dts.h" #endif /* HAL_DTS_MODULE_ENABLED */ #ifdef HAL_ETH_MODULE_ENABLED - #include "stm32h7xx_hal_eth.h" +#include "stm32h7xx_hal_eth.h" #endif /* HAL_ETH_MODULE_ENABLED */ #ifdef HAL_ETH_LEGACY_MODULE_ENABLED - #include "stm32h7xx_hal_eth_legacy.h" +#include "stm32h7xx_hal_eth_legacy.h" #endif /* HAL_ETH_LEGACY_MODULE_ENABLED */ #ifdef HAL_EXTI_MODULE_ENABLED - #include "stm32h7xx_hal_exti.h" +#include "stm32h7xx_hal_exti.h" #endif /* HAL_EXTI_MODULE_ENABLED */ #ifdef HAL_CORTEX_MODULE_ENABLED - #include "stm32h7xx_hal_cortex.h" +#include "stm32h7xx_hal_cortex.h" #endif /* HAL_CORTEX_MODULE_ENABLED */ #ifdef HAL_ADC_MODULE_ENABLED - #include "stm32h7xx_hal_adc.h" +#include "stm32h7xx_hal_adc.h" #endif /* HAL_ADC_MODULE_ENABLED */ #ifdef HAL_FDCAN_MODULE_ENABLED - #include "stm32h7xx_hal_fdcan.h" +#include "stm32h7xx_hal_fdcan.h" #endif /* HAL_FDCAN_MODULE_ENABLED */ #ifdef HAL_CEC_MODULE_ENABLED - #include "stm32h7xx_hal_cec.h" +#include "stm32h7xx_hal_cec.h" #endif /* HAL_CEC_MODULE_ENABLED */ #ifdef HAL_COMP_MODULE_ENABLED - #include "stm32h7xx_hal_comp.h" +#include "stm32h7xx_hal_comp.h" #endif /* HAL_COMP_MODULE_ENABLED */ #ifdef HAL_CORDIC_MODULE_ENABLED - #include "stm32h7xx_hal_cordic.h" +#include "stm32h7xx_hal_cordic.h" #endif /* HAL_CORDIC_MODULE_ENABLED */ #ifdef HAL_CRC_MODULE_ENABLED - #include "stm32h7xx_hal_crc.h" +#include "stm32h7xx_hal_crc.h" #endif /* HAL_CRC_MODULE_ENABLED */ #ifdef HAL_CRYP_MODULE_ENABLED - #include "stm32h7xx_hal_cryp.h" +#include "stm32h7xx_hal_cryp.h" #endif /* HAL_CRYP_MODULE_ENABLED */ #ifdef HAL_DAC_MODULE_ENABLED - #include "stm32h7xx_hal_dac.h" +#include "stm32h7xx_hal_dac.h" #endif /* HAL_DAC_MODULE_ENABLED */ #ifdef HAL_FLASH_MODULE_ENABLED - #include "stm32h7xx_hal_flash.h" +#include "stm32h7xx_hal_flash.h" #endif /* HAL_FLASH_MODULE_ENABLED */ #ifdef HAL_GFXMMU_MODULE_ENABLED - #include "stm32h7xx_hal_gfxmmu.h" +#include "stm32h7xx_hal_gfxmmu.h" #endif /* HAL_GFXMMU_MODULE_ENABLED */ #ifdef HAL_FMAC_MODULE_ENABLED - #include "stm32h7xx_hal_fmac.h" +#include "stm32h7xx_hal_fmac.h" #endif /* HAL_FMAC_MODULE_ENABLED */ #ifdef HAL_HRTIM_MODULE_ENABLED - #include "stm32h7xx_hal_hrtim.h" +#include "stm32h7xx_hal_hrtim.h" #endif /* HAL_HRTIM_MODULE_ENABLED */ #ifdef HAL_HSEM_MODULE_ENABLED - #include "stm32h7xx_hal_hsem.h" +#include "stm32h7xx_hal_hsem.h" #endif /* HAL_HSEM_MODULE_ENABLED */ #ifdef HAL_SRAM_MODULE_ENABLED - #include "stm32h7xx_hal_sram.h" +#include "stm32h7xx_hal_sram.h" #endif /* HAL_SRAM_MODULE_ENABLED */ #ifdef HAL_NOR_MODULE_ENABLED - #include "stm32h7xx_hal_nor.h" +#include "stm32h7xx_hal_nor.h" #endif /* HAL_NOR_MODULE_ENABLED */ #ifdef HAL_NAND_MODULE_ENABLED - #include "stm32h7xx_hal_nand.h" +#include "stm32h7xx_hal_nand.h" #endif /* HAL_NAND_MODULE_ENABLED */ #ifdef HAL_I2C_MODULE_ENABLED - #include "stm32h7xx_hal_i2c.h" +#include "stm32h7xx_hal_i2c.h" #endif /* HAL_I2C_MODULE_ENABLED */ #ifdef HAL_I2S_MODULE_ENABLED - #include "stm32h7xx_hal_i2s.h" +#include "stm32h7xx_hal_i2s.h" #endif /* HAL_I2S_MODULE_ENABLED */ #ifdef HAL_IWDG_MODULE_ENABLED - #include "stm32h7xx_hal_iwdg.h" +#include "stm32h7xx_hal_iwdg.h" #endif /* HAL_IWDG_MODULE_ENABLED */ #ifdef HAL_JPEG_MODULE_ENABLED - #include "stm32h7xx_hal_jpeg.h" +#include "stm32h7xx_hal_jpeg.h" #endif /* HAL_JPEG_MODULE_ENABLED */ #ifdef HAL_MDIOS_MODULE_ENABLED - #include "stm32h7xx_hal_mdios.h" +#include "stm32h7xx_hal_mdios.h" #endif /* HAL_MDIOS_MODULE_ENABLED */ #ifdef HAL_MMC_MODULE_ENABLED - #include "stm32h7xx_hal_mmc.h" +#include "stm32h7xx_hal_mmc.h" #endif /* HAL_MMC_MODULE_ENABLED */ #ifdef HAL_LPTIM_MODULE_ENABLED @@ -400,7 +401,7 @@ #endif /* HAL_OPAMP_MODULE_ENABLED */ #ifdef HAL_OSPI_MODULE_ENABLED - #include "stm32h7xx_hal_ospi.h" +#include "stm32h7xx_hal_ospi.h" #endif /* HAL_OSPI_MODULE_ENABLED */ #ifdef HAL_OTFDEC_MODULE_ENABLED @@ -408,104 +409,104 @@ #endif /* HAL_OTFDEC_MODULE_ENABLED */ #ifdef HAL_PSSI_MODULE_ENABLED - #include "stm32h7xx_hal_pssi.h" +#include "stm32h7xx_hal_pssi.h" #endif /* HAL_PSSI_MODULE_ENABLED */ #ifdef HAL_PWR_MODULE_ENABLED - #include "stm32h7xx_hal_pwr.h" +#include "stm32h7xx_hal_pwr.h" #endif /* HAL_PWR_MODULE_ENABLED */ #ifdef HAL_QSPI_MODULE_ENABLED - #include "stm32h7xx_hal_qspi.h" +#include "stm32h7xx_hal_qspi.h" #endif /* HAL_QSPI_MODULE_ENABLED */ #ifdef HAL_RAMECC_MODULE_ENABLED - #include "stm32h7xx_hal_ramecc.h" +#include "stm32h7xx_hal_ramecc.h" #endif /* HAL_RAMECC_MODULE_ENABLED */ #ifdef HAL_RNG_MODULE_ENABLED - #include "stm32h7xx_hal_rng.h" +#include "stm32h7xx_hal_rng.h" #endif /* HAL_RNG_MODULE_ENABLED */ #ifdef HAL_RTC_MODULE_ENABLED - #include "stm32h7xx_hal_rtc.h" +#include "stm32h7xx_hal_rtc.h" #endif /* HAL_RTC_MODULE_ENABLED */ #ifdef HAL_SAI_MODULE_ENABLED - #include "stm32h7xx_hal_sai.h" +#include "stm32h7xx_hal_sai.h" #endif /* HAL_SAI_MODULE_ENABLED */ #ifdef HAL_SD_MODULE_ENABLED - #include "stm32h7xx_hal_sd.h" +#include "stm32h7xx_hal_sd.h" #endif /* HAL_SD_MODULE_ENABLED */ #ifdef HAL_SDRAM_MODULE_ENABLED - #include "stm32h7xx_hal_sdram.h" +#include "stm32h7xx_hal_sdram.h" #endif /* HAL_SDRAM_MODULE_ENABLED */ #ifdef HAL_SPI_MODULE_ENABLED - #include "stm32h7xx_hal_spi.h" +#include "stm32h7xx_hal_spi.h" #endif /* HAL_SPI_MODULE_ENABLED */ #ifdef HAL_SPDIFRX_MODULE_ENABLED - #include "stm32h7xx_hal_spdifrx.h" +#include "stm32h7xx_hal_spdifrx.h" #endif /* HAL_SPDIFRX_MODULE_ENABLED */ #ifdef HAL_SWPMI_MODULE_ENABLED - #include "stm32h7xx_hal_swpmi.h" +#include "stm32h7xx_hal_swpmi.h" #endif /* HAL_SWPMI_MODULE_ENABLED */ #ifdef HAL_TIM_MODULE_ENABLED - #include "stm32h7xx_hal_tim.h" +#include "stm32h7xx_hal_tim.h" #endif /* HAL_TIM_MODULE_ENABLED */ #ifdef HAL_UART_MODULE_ENABLED - #include "stm32h7xx_hal_uart.h" +#include "stm32h7xx_hal_uart.h" #endif /* HAL_UART_MODULE_ENABLED */ #ifdef HAL_USART_MODULE_ENABLED - #include "stm32h7xx_hal_usart.h" +#include "stm32h7xx_hal_usart.h" #endif /* HAL_USART_MODULE_ENABLED */ #ifdef HAL_IRDA_MODULE_ENABLED - #include "stm32h7xx_hal_irda.h" +#include "stm32h7xx_hal_irda.h" #endif /* HAL_IRDA_MODULE_ENABLED */ #ifdef HAL_SMARTCARD_MODULE_ENABLED - #include "stm32h7xx_hal_smartcard.h" +#include "stm32h7xx_hal_smartcard.h" #endif /* HAL_SMARTCARD_MODULE_ENABLED */ #ifdef HAL_SMBUS_MODULE_ENABLED - #include "stm32h7xx_hal_smbus.h" +#include "stm32h7xx_hal_smbus.h" #endif /* HAL_SMBUS_MODULE_ENABLED */ #ifdef HAL_WWDG_MODULE_ENABLED - #include "stm32h7xx_hal_wwdg.h" +#include "stm32h7xx_hal_wwdg.h" #endif /* HAL_WWDG_MODULE_ENABLED */ #ifdef HAL_PCD_MODULE_ENABLED - #include "stm32h7xx_hal_pcd.h" +#include "stm32h7xx_hal_pcd.h" #endif /* HAL_PCD_MODULE_ENABLED */ #ifdef HAL_HCD_MODULE_ENABLED - #include "stm32h7xx_hal_hcd.h" +#include "stm32h7xx_hal_hcd.h" #endif /* HAL_HCD_MODULE_ENABLED */ /* Exported macro ------------------------------------------------------------*/ -#ifdef USE_FULL_ASSERT -/** - * @brief The assert_param macro is used for function's parameters check. - * @param expr: If expr is false, it calls assert_failed function - * which reports the name of the source file and the source - * line number of the call that failed. - * If expr is true, it returns no value. - * @retval None - */ - #define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__)) -/* Exported functions ------------------------------------------------------- */ +#ifdef USE_FULL_ASSERT + /** + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ +#define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__)) + /* Exported functions ------------------------------------------------------- */ void assert_failed(uint8_t *file, uint32_t line); #else - #define assert_param(expr) ((void)0U) +#define assert_param(expr) ((void)0U) #endif /* USE_FULL_ASSERT */ #ifdef __cplusplus diff --git a/boards/varmint/include/stm32h7xx_it.h b/boards/varmint/include/stm32h7xx_it.h index 4398690f..ecd74256 100644 --- a/boards/varmint/include/stm32h7xx_it.h +++ b/boards/varmint/include/stm32h7xx_it.h @@ -1,20 +1,20 @@ /* USER CODE BEGIN Header */ /** - ****************************************************************************** - * @file stm32h7xx_it.h - * @brief This file contains the headers of the interrupt handlers. - ****************************************************************************** - * @attention - * - * Copyright (c) 2023 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * ****************************************************************************** - */ + * @file stm32h7xx_it.h + * @brief This file contains the headers of the interrupt handlers. + ****************************************************************************** + * @attention + * + * Copyright (c) 2023 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ /* USER CODE END Header */ /* Define to prevent recursive inclusion -------------------------------------*/ @@ -22,80 +22,81 @@ #define __STM32H7xx_IT_H #ifdef __cplusplus - extern "C" { +extern "C" +{ #endif -/* Private includes ----------------------------------------------------------*/ -/* USER CODE BEGIN Includes */ + /* Private includes ----------------------------------------------------------*/ + /* USER CODE BEGIN Includes */ -/* USER CODE END Includes */ + /* USER CODE END Includes */ -/* Exported types ------------------------------------------------------------*/ -/* USER CODE BEGIN ET */ + /* Exported types ------------------------------------------------------------*/ + /* USER CODE BEGIN ET */ -/* USER CODE END ET */ + /* USER CODE END ET */ -/* Exported constants --------------------------------------------------------*/ -/* USER CODE BEGIN EC */ + /* Exported constants --------------------------------------------------------*/ + /* USER CODE BEGIN EC */ -/* USER CODE END EC */ + /* USER CODE END EC */ -/* Exported macro ------------------------------------------------------------*/ -/* USER CODE BEGIN EM */ + /* Exported macro ------------------------------------------------------------*/ + /* USER CODE BEGIN EM */ -/* USER CODE END EM */ + /* USER CODE END EM */ -/* Exported functions prototypes ---------------------------------------------*/ -void NMI_Handler(void); -void HardFault_Handler(void); -void MemManage_Handler(void); -void BusFault_Handler(void); -void UsageFault_Handler(void); -void SVC_Handler(void); -void DebugMon_Handler(void); -void PendSV_Handler(void); -void SysTick_Handler(void); -void RCC_IRQHandler(void); -void EXTI0_IRQHandler(void); -void EXTI1_IRQHandler(void); -void EXTI2_IRQHandler(void); -void EXTI3_IRQHandler(void); -void DMA1_Stream0_IRQHandler(void); -void DMA1_Stream1_IRQHandler(void); -void DMA1_Stream2_IRQHandler(void); -void DMA1_Stream3_IRQHandler(void); -void DMA1_Stream4_IRQHandler(void); -void DMA1_Stream5_IRQHandler(void); -void DMA1_Stream6_IRQHandler(void); -void ADC_IRQHandler(void); -void EXTI9_5_IRQHandler(void); -void I2C1_EV_IRQHandler(void); -void SPI1_IRQHandler(void); -void SPI2_IRQHandler(void); -void USART1_IRQHandler(void); -void USART2_IRQHandler(void); -void USART3_IRQHandler(void); -void EXTI15_10_IRQHandler(void); -void TIM8_BRK_TIM12_IRQHandler(void); -void DMA1_Stream7_IRQHandler(void); -void SDMMC1_IRQHandler(void); -void TIM5_IRQHandler(void); -void SPI3_IRQHandler(void); -void TIM7_IRQHandler(void); -void DMA2_Stream0_IRQHandler(void); -void DMA2_Stream1_IRQHandler(void); -void DMA2_Stream2_IRQHandler(void); -void DMA2_Stream3_IRQHandler(void); -void DMA2_Stream5_IRQHandler(void); -void DMA2_Stream6_IRQHandler(void); -void DMA2_Stream7_IRQHandler(void); -void SPI4_IRQHandler(void); -void OTG_FS_IRQHandler(void); -void ADC3_IRQHandler(void); -void BDMA_Channel0_IRQHandler(void); -/* USER CODE BEGIN EFP */ + /* Exported functions prototypes ---------------------------------------------*/ + void NMI_Handler(void); + void HardFault_Handler(void); + void MemManage_Handler(void); + void BusFault_Handler(void); + void UsageFault_Handler(void); + void SVC_Handler(void); + void DebugMon_Handler(void); + void PendSV_Handler(void); + void SysTick_Handler(void); + void RCC_IRQHandler(void); + void EXTI0_IRQHandler(void); + void EXTI1_IRQHandler(void); + void EXTI2_IRQHandler(void); + void EXTI3_IRQHandler(void); + void DMA1_Stream0_IRQHandler(void); + void DMA1_Stream1_IRQHandler(void); + void DMA1_Stream2_IRQHandler(void); + void DMA1_Stream3_IRQHandler(void); + void DMA1_Stream4_IRQHandler(void); + void DMA1_Stream5_IRQHandler(void); + void DMA1_Stream6_IRQHandler(void); + void ADC_IRQHandler(void); + void EXTI9_5_IRQHandler(void); + void I2C1_EV_IRQHandler(void); + void SPI1_IRQHandler(void); + void SPI2_IRQHandler(void); + void USART1_IRQHandler(void); + void USART2_IRQHandler(void); + void USART3_IRQHandler(void); + void EXTI15_10_IRQHandler(void); + void TIM8_BRK_TIM12_IRQHandler(void); + void DMA1_Stream7_IRQHandler(void); + void SDMMC1_IRQHandler(void); + void TIM5_IRQHandler(void); + void SPI3_IRQHandler(void); + void TIM7_IRQHandler(void); + void DMA2_Stream0_IRQHandler(void); + void DMA2_Stream1_IRQHandler(void); + void DMA2_Stream2_IRQHandler(void); + void DMA2_Stream3_IRQHandler(void); + void DMA2_Stream5_IRQHandler(void); + void DMA2_Stream6_IRQHandler(void); + void DMA2_Stream7_IRQHandler(void); + void SPI4_IRQHandler(void); + void OTG_FS_IRQHandler(void); + void ADC3_IRQHandler(void); + void BDMA_Channel0_IRQHandler(void); + /* USER CODE BEGIN EFP */ -/* USER CODE END EFP */ + /* USER CODE END EFP */ #ifdef __cplusplus } diff --git a/boards/varmint/src/Varmint.cpp b/boards/varmint/src/Varmint.cpp index 4893bbd2..ea093815 100644 --- a/boards/varmint/src/Varmint.cpp +++ b/boards/varmint/src/Varmint.cpp @@ -34,73 +34,67 @@ * ****************************************************************************** **/ -#include - -#include - -#include #include -#include +#include +#include #include - -#include - +#include +#include +#include +#include +#include #include #include - -#include - -#include +#include #include -#include +bool verbose = true; - -bool verbose=true; - -//#ifdef __cplusplus -//extern "C" { -//#endif +// #ifdef __cplusplus +// extern "C" { +// #endif // -//void __stack_chk_fail(void); +// void __stack_chk_fail(void); // -//#ifdef __cplusplus -//} -//#endif +// #ifdef __cplusplus +// } +// #endif // // -//void *__stack_chk_guard = (void *)0xDEADD00D; +// void *__stack_chk_guard = (void *)0xDEADD00D; // -//void __stack_chk_fail(void) +// void __stack_chk_fail(void) //{ -// misc_printf("Stack smashing detected.\n"); +// misc_printf("Stack smashing detected.\n"); // while(1){} -//} +// } Varmint varmint; Time64 time64; -static uint32_t init_poll_timer(TIM_HandleTypeDef *htim, TIM_TypeDef *instance, uint32_t channel); +static uint32_t init_poll_timer(TIM_HandleTypeDef *htim, TIM_TypeDef *instance, uint32_t channel); -uint32_t init_poll_timer(TIM_HandleTypeDef *htim, TIM_TypeDef *instance, uint32_t channel) +uint32_t init_poll_timer(TIM_HandleTypeDef *htim, TIM_TypeDef *instance, uint32_t channel) { TIM_MasterConfigTypeDef sMasterConfig = {0}; htim->Instance = instance; - htim->Init.Prescaler = 199; + htim->Init.Prescaler = 199; htim->Init.CounterMode = TIM_COUNTERMODE_UP; htim->Init.Period = POLLING_PERIOD_US; htim->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; - if (HAL_TIM_Base_Init(&htim7) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIM_Base_Init(&htim7) != HAL_OK) + return DRIVER_HAL_ERROR; sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig) != HAL_OK) + return DRIVER_HAL_ERROR; - HAL_TIM_PWM_Start( htim, channel); // (10kHz) to service polling routines - HAL_TIM_Base_Start_IT(htim); - return DRIVER_OK; + HAL_TIM_PWM_Start(htim, channel); // (10kHz) to service polling routines + HAL_TIM_Base_Start_IT(htim); + return DRIVER_OK; } //////////////////////////////////////////////////////////////////////////////////////// @@ -117,7 +111,7 @@ uint32_t init_poll_timer(TIM_HandleTypeDef *htim, TIM_TypeDef *instance, uint32 void Varmint::init_board(void) { - uint32_t init_status; + uint32_t init_status; SCB_EnableICache(); SCB_EnableDCache(); @@ -128,15 +122,15 @@ void Varmint::init_board(void) MX_GPIO_Init(); MX_DMA_Init(); MX_BDMA_Init(); - MX_I2C1_Init(); // PITOT, POT, EEPROM + MX_I2C1_Init(); // PITOT, POT, EEPROM // MX_I2C2_Init(); // not used MX_SPI1_Init(); // BMI IMU MX_SPI2_Init(); // MAG MX_SPI3_Init(); // Baro - MX_SPI4_Init(); // ADIS IMU - // MX_TIM1_Init(); // PWM - // MX_TIM3_Init(); // PWM - // MX_TIM4_Init(); // PWM + MX_SPI4_Init(); // ADIS IMU + // MX_TIM1_Init(); // PWM + // MX_TIM3_Init(); // PWM + // MX_TIM4_Init(); // PWM // MX_TIM5_Init(); // Time64, initialized elsewhere // MX_TIM8_Init(); // Time64, initialized elsewhere @@ -144,589 +138,627 @@ void Varmint::init_board(void) // MX_TIM12_Init(); // ADIS16500 Ext Clock, initialized elsewhere // MX_USART1_UART_Init(); // uBlox, initialized elsewhere MX_USART2_UART_Init(); // Telem &| Console, re-initialized elsewhere - // MX_USART3_UART_Init(); // S.Bus, initialized elsewhere - // MX_ADC1_Init(); // Battery and power supplies, initialized elsewhere - // MX_ADC3_Init(); // STM Temperature, initialized elsewhere - // MX_USB_DEVICE_Init(); // initialized elsewhere - // MX_FDCAN1_Init(); // not used - // MX_SDMMC1_SD_Init(); // initialized elsewhere - // MX_RTC_Init(); // not used - // MX_RNG_Init(); // not used - // MX_CRC_Init(); // not used - - - - - - - - //// Startup Chained Timestamp Timers 1us rolls over in 8.9 years. - misc_printf("\n\rStart Timestamp Timer\n\r"); - time64.init(HTIM_LOW, HTIM_LOW_INSTANCE, HTIM_HIGH, HTIM_HIGH_INSTANCE); - - #define ASCII_ESC 27 - misc_printf( "%c[H", ASCII_ESC ); // home - misc_printf( "%c[2J", ASCII_ESC ); // clear screen - - misc_printf("\n\rStarted Timestamp Timer\n\r"); - - // Zero GPIO outputs used as probes. // TODO remove these eventually and return the pins to their normal functions. - PE5_LO; // J000_JETSON_DRDY - PE3_LO; // J105_2_SPI4_EXT_CS - PB0_LO; // TP5 - PC7_LO; // TP6 - PH1_LO; // J105 pin 18 RST - PB1_HI; // J105 pin 23/25 Sync Bus Tx - PB15_LO; // J105_2_SPI4_EXT_CLK - - misc_printf("\n\r\n\rADIS165xx (imu0) Initialization\n\r"); - init_status = imu0_.init - ( - IMU0_HZ,IMU0_DRDY_PORT, IMU0_DRDY_PIN, // Driver - IMU0_SPI, IMU0_CS_PORT, IMU0_CS_PIN, // SPI - IMU0_RESET_PORT, IMU0_RESET_PIN, // Reset Pin - IMU0_HTIM, IMU0_TIM_INSTANCE, IMU0_TIM_CHANNEL, IMU0_TIM_PERIOD_US // ADIS external clock - ); - misc_exit_status(init_status); - - misc_printf("\n\r\n\rBMI088 (imu1) Initialization\n\r"); - init_status = imu1_.init - ( - IMU1_HZ,IMU1_DRDY_PORT, IMU1_DRDY_PIN, - IMU1_SPI, IMU1_CS_PORT_A, IMU1_CS_PIN_A, IMU1_CS_PORT_G, IMU1_CS_PIN_G, - IMU1_RANGE_A, IMU1_RANGE_G - ); - misc_exit_status(init_status); - - misc_printf("\n\r\n\rDLHRL20G (pitot) Initialization\n\r"); - init_status = pitot_.init - ( - PITOT_HZ, PITOT_DRDY_PORT, PITOT_DRDY_PIN, // Driver - PITOT_I2C, PITOT_I2C_ADDRESS // I2C - ); - misc_exit_status(init_status); - - misc_printf("\n\r\n\rDPS310 (bar0) Initialization\n\r"); - init_status = baro_.init - ( - BARO_HZ,BARO_DRDY_PORT, BARO_DRDY_PIN, // Driver - BARO_SPI, BARO_CS_PORT, BARO_CS_PIN // SPI - ); - misc_exit_status(init_status); - - misc_printf("\n\r\n\rIIS2MDC (mag) Initialization\n\r"); - init_status = mag_.init - ( - MAG_HZ, MAG_DRDY_PORT, MAG_DRDY_PIN, // Driver - MAG_SPI, MAG_CS_PORT, MAG_CS_PIN // SPI - ); - misc_exit_status(init_status); - - misc_printf("\n\r\n\rUbx (gps) Initialization\n\r"); - init_status = gps_.init(GPS_HZ, GPS_PPS_PORT, GPS_PPS_PIN, GPS_UART, GPS_UART_INSTANCE, GPS_UART_DMA, GPS_BAUD); - misc_exit_status(init_status); - - misc_printf("\n\r\nS.Bus (rc) Initialization\n\r"); - init_status = rc_.init(RC_HZ, RC_UART, RC_UART_INSTANCE, RC_UART_DMA, RC_BAUD ); - misc_exit_status(init_status); - - misc_printf("\n\r\n\rAdc (adc) Initialization\n\r"); - init_status = adc_.init(ADC_HZ, ADC_ADC_EXTERNAL, ADC_ADC_INSTANCE_EXTERNAL, ADC_ADC_INTERNAL, ADC_ADC_INSTANCE_INTERNAL); - misc_exit_status(init_status); - - misc_printf("\n\r\n\rVcp (vcp) Initialization\n\r"); - init_status = vcp_.init(VCP_HZ); - misc_exit_status(init_status); - - misc_printf("\n\r\n\rTelem (telem) Initialization\n\r"); - init_status = telem_.init(TELEM_HZ, TELEM_UART, TELEM_UART_INSTANCE, TELEM_UART_DMA, TELEM_BAUD,RxIsrCallback ); - misc_exit_status(init_status); - -//// Initialize PWM Timers - misc_printf("\n\r\n\rPWM (PWM) Initialization\n\r"); - init_status = pwm_init_timers(SERVO_PWM_PERIOD); - init_status |= pwm_[0].init( PWM_HTIM_0, PWM_CHAN_0, PWM_MIN, PWM_CENTER, PWM_MAX); - init_status |= pwm_[1].init( PWM_HTIM_1, PWM_CHAN_1, PWM_MIN, PWM_CENTER, PWM_MAX); - init_status |= pwm_[2].init( PWM_HTIM_2, PWM_CHAN_2, PWM_MIN, PWM_CENTER, PWM_MAX); - init_status |= pwm_[3].init( PWM_HTIM_3, PWM_CHAN_3, PWM_MIN, PWM_CENTER, PWM_MAX); - init_status |= pwm_[4].init( PWM_HTIM_4, PWM_CHAN_4, PWM_MIN, PWM_CENTER, PWM_MAX); - init_status |= pwm_[5].init( PWM_HTIM_5, PWM_CHAN_5, PWM_MIN, PWM_CENTER, PWM_MAX); - init_status |= pwm_[6].init( PWM_HTIM_6, PWM_CHAN_6, PWM_MIN, PWM_CENTER, PWM_MAX); - init_status |= pwm_[7].init( PWM_HTIM_7, PWM_CHAN_7, PWM_MIN, PWM_CENTER, PWM_MAX); - init_status |= pwm_[8].init( PWM_HTIM_8, PWM_CHAN_8, PWM_MIN, PWM_CENTER, PWM_MAX); - init_status |= pwm_[9].init( PWM_HTIM_9, PWM_CHAN_9, PWM_MIN, PWM_CENTER, PWM_MAX); - misc_exit_status(init_status); - -////// Initialize SD Card - misc_printf("\n\r\n\rSDMMC Initialization\n\r"); - init_status = sd_.init(SD_HSD,SD_HSD_INSTANCE); - misc_exit_status(init_status); - -//// Start the Periodic Polling Timer - // High Rate Polling Timer - misc_printf("\n\r\n\rPolling Timer Initialization\n\r"); - init_status = init_poll_timer(POLL_HTIM, POLL_HTIM_INSTANCE, POLL_TIM_CHANNEL); - misc_exit_status(init_status); - -//// Enable EXTI IRQ's - misc_printf("\n\r\n\rSet-up EXTI IRQ's\n\r"); - - verbose = false; - - HAL_NVIC_EnableIRQ(EXTI3_IRQn); // uBlox GPS PPS - HAL_NVIC_EnableIRQ(EXTI9_5_IRQn); // ADIS IMU DRDY - HAL_NVIC_EnableIRQ(EXTI15_10_IRQn); // Bosh IMU DRDY - - __HAL_UART_ENABLE_IT(gps_.huart(), UART_IT_IDLE); - __HAL_UART_ENABLE_IT(rc_.huart(), UART_IT_IDLE); - - telem_.rxStart(); // Also enables interrupts. + // MX_USART3_UART_Init(); // S.Bus, initialized elsewhere + // MX_ADC1_Init(); // Battery and power supplies, initialized elsewhere + // MX_ADC3_Init(); // STM Temperature, initialized elsewhere + // MX_USB_DEVICE_Init(); // initialized elsewhere + // MX_FDCAN1_Init(); // not used + // MX_SDMMC1_SD_Init(); // initialized elsewhere + // MX_RTC_Init(); // not used + // MX_RNG_Init(); // not used + // MX_CRC_Init(); // not used + + //// Startup Chained Timestamp Timers 1us rolls over in 8.9 years. + misc_printf("\n\rStart Timestamp Timer\n\r"); + time64.init(HTIM_LOW, HTIM_LOW_INSTANCE, HTIM_HIGH, HTIM_HIGH_INSTANCE); + +#define ASCII_ESC 27 + misc_printf("%c[H", ASCII_ESC); // home + misc_printf("%c[2J", ASCII_ESC); // clear screen + + misc_printf("\n\rStarted Timestamp Timer\n\r"); + + // Zero GPIO outputs used as probes. // TODO remove these eventually and return the pins to their normal functions. + PE5_LO; // J000_JETSON_DRDY + PE3_LO; // J105_2_SPI4_EXT_CS + PB0_LO; // TP5 + PC7_LO; // TP6 + PH1_LO; // J105 pin 18 RST + PB1_HI; // J105 pin 23/25 Sync Bus Tx + PB15_LO; // J105_2_SPI4_EXT_CLK + + misc_printf("\n\r\n\rADIS165xx (imu0) Initialization\n\r"); + init_status = imu0_.init(IMU0_HZ, IMU0_DRDY_PORT, IMU0_DRDY_PIN, // Driver + IMU0_SPI, IMU0_CS_PORT, IMU0_CS_PIN, // SPI + IMU0_RESET_PORT, IMU0_RESET_PIN, // Reset Pin + IMU0_HTIM, IMU0_TIM_INSTANCE, IMU0_TIM_CHANNEL, IMU0_TIM_PERIOD_US // ADIS external clock + ); + misc_exit_status(init_status); + + misc_printf("\n\r\n\rBMI088 (imu1) Initialization\n\r"); + init_status = imu1_.init(IMU1_HZ, IMU1_DRDY_PORT, IMU1_DRDY_PIN, IMU1_SPI, IMU1_CS_PORT_A, IMU1_CS_PIN_A, + IMU1_CS_PORT_G, IMU1_CS_PIN_G, IMU1_RANGE_A, IMU1_RANGE_G); + misc_exit_status(init_status); + + misc_printf("\n\r\n\rDLHRL20G (pitot) Initialization\n\r"); + init_status = pitot_.init(PITOT_HZ, PITOT_DRDY_PORT, PITOT_DRDY_PIN, // Driver + PITOT_I2C, PITOT_I2C_ADDRESS // I2C + ); + misc_exit_status(init_status); + + misc_printf("\n\r\n\rDPS310 (bar0) Initialization\n\r"); + init_status = baro_.init(BARO_HZ, BARO_DRDY_PORT, BARO_DRDY_PIN, // Driver + BARO_SPI, BARO_CS_PORT, BARO_CS_PIN // SPI + ); + misc_exit_status(init_status); + + misc_printf("\n\r\n\rIIS2MDC (mag) Initialization\n\r"); + init_status = mag_.init(MAG_HZ, MAG_DRDY_PORT, MAG_DRDY_PIN, // Driver + MAG_SPI, MAG_CS_PORT, MAG_CS_PIN // SPI + ); + misc_exit_status(init_status); + + misc_printf("\n\r\n\rUbx (gps) Initialization\n\r"); + init_status = gps_.init(GPS_HZ, GPS_PPS_PORT, GPS_PPS_PIN, GPS_UART, GPS_UART_INSTANCE, GPS_UART_DMA, GPS_BAUD); + misc_exit_status(init_status); + + misc_printf("\n\r\nS.Bus (rc) Initialization\n\r"); + init_status = rc_.init(RC_HZ, RC_UART, RC_UART_INSTANCE, RC_UART_DMA, RC_BAUD); + misc_exit_status(init_status); + + misc_printf("\n\r\n\rAdc (adc) Initialization\n\r"); + init_status = + adc_.init(ADC_HZ, ADC_ADC_EXTERNAL, ADC_ADC_INSTANCE_EXTERNAL, ADC_ADC_INTERNAL, ADC_ADC_INSTANCE_INTERNAL); + misc_exit_status(init_status); + + misc_printf("\n\r\n\rVcp (vcp) Initialization\n\r"); + init_status = vcp_.init(VCP_HZ); + misc_exit_status(init_status); + + misc_printf("\n\r\n\rTelem (telem) Initialization\n\r"); + init_status = telem_.init(TELEM_HZ, TELEM_UART, TELEM_UART_INSTANCE, TELEM_UART_DMA, TELEM_BAUD, RxIsrCallback); + misc_exit_status(init_status); + + //// Initialize PWM Timers + misc_printf("\n\r\n\rPWM (PWM) Initialization\n\r"); + init_status = pwm_init_timers(SERVO_PWM_PERIOD); + init_status |= pwm_[0].init(PWM_HTIM_0, PWM_CHAN_0, PWM_MIN, PWM_CENTER, PWM_MAX); + init_status |= pwm_[1].init(PWM_HTIM_1, PWM_CHAN_1, PWM_MIN, PWM_CENTER, PWM_MAX); + init_status |= pwm_[2].init(PWM_HTIM_2, PWM_CHAN_2, PWM_MIN, PWM_CENTER, PWM_MAX); + init_status |= pwm_[3].init(PWM_HTIM_3, PWM_CHAN_3, PWM_MIN, PWM_CENTER, PWM_MAX); + init_status |= pwm_[4].init(PWM_HTIM_4, PWM_CHAN_4, PWM_MIN, PWM_CENTER, PWM_MAX); + init_status |= pwm_[5].init(PWM_HTIM_5, PWM_CHAN_5, PWM_MIN, PWM_CENTER, PWM_MAX); + init_status |= pwm_[6].init(PWM_HTIM_6, PWM_CHAN_6, PWM_MIN, PWM_CENTER, PWM_MAX); + init_status |= pwm_[7].init(PWM_HTIM_7, PWM_CHAN_7, PWM_MIN, PWM_CENTER, PWM_MAX); + init_status |= pwm_[8].init(PWM_HTIM_8, PWM_CHAN_8, PWM_MIN, PWM_CENTER, PWM_MAX); + init_status |= pwm_[9].init(PWM_HTIM_9, PWM_CHAN_9, PWM_MIN, PWM_CENTER, PWM_MAX); + misc_exit_status(init_status); + + ////// Initialize SD Card + misc_printf("\n\r\n\rSDMMC Initialization\n\r"); + init_status = sd_.init(SD_HSD, SD_HSD_INSTANCE); + misc_exit_status(init_status); + + //// Start the Periodic Polling Timer + // High Rate Polling Timer + misc_printf("\n\r\n\rPolling Timer Initialization\n\r"); + init_status = init_poll_timer(POLL_HTIM, POLL_HTIM_INSTANCE, POLL_TIM_CHANNEL); + misc_exit_status(init_status); + + //// Enable EXTI IRQ's + misc_printf("\n\r\n\rSet-up EXTI IRQ's\n\r"); + + verbose = false; + + HAL_NVIC_EnableIRQ(EXTI3_IRQn); // uBlox GPS PPS + HAL_NVIC_EnableIRQ(EXTI9_5_IRQn); // ADIS IMU DRDY + HAL_NVIC_EnableIRQ(EXTI15_10_IRQn); // Bosh IMU DRDY + + __HAL_UART_ENABLE_IT(gps_.huart(), UART_IT_IDLE); + __HAL_UART_ENABLE_IT(rc_.huart(), UART_IT_IDLE); + + telem_.rxStart(); // Also enables interrupts. } void Varmint::board_reset(bool bootloader) { -// pwm_disable(); -// HAL_NVIC_SystemReset(); + // pwm_disable(); + // HAL_NVIC_SystemReset(); } // clock -uint32_t Varmint::clock_millis() {return time64.Us()/1000;} -uint64_t Varmint::clock_micros() {return time64.Us();} -void Varmint::clock_delay(uint32_t ms) {time64.dMs(ms);} +uint32_t Varmint::clock_millis() +{ + return time64.Us() / 1000; +} +uint64_t Varmint::clock_micros() +{ + return time64.Us(); +} +void Varmint::clock_delay(uint32_t ms) +{ + time64.dMs(ms); +} // serial comms to the Companion computer void Varmint::serial_init(uint32_t baud_rate, uint32_t dev) { - serial_device_ = 1;//dev;// 1 = serial, otherwise = VCP - if(dev==1) telem_.reset_baud(baud_rate); + serial_device_ = 1; // dev;// 1 = serial, otherwise = VCP + if (dev == 1) + telem_.reset_baud(baud_rate); } void Varmint::serial_write(const uint8_t *src, size_t len) { - SerialTxPacket p; - p.timestamp = time64.Us(); - p.payloadSize = len; - - if(len>(256+8)) len = (256+8); - memcpy(p.payload,src,len); - - uint8_t message_id = p.payload[5]; // Assumes Mavlink v1 - - switch(message_id) - { - // Real-time - case MAVLINK_MSG_ID_SMALL_IMU: - p.qos=0x00; break; // Start of Timeframe - case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: - case MAVLINK_MSG_ID_TIMESYNC: - p.qos=0x01; break; - // Secondary real-time - case MAVLINK_MSG_ID_HEARTBEAT: - case MAVLINK_MSG_ID_SMALL_BARO: - case MAVLINK_MSG_ID_DIFF_PRESSURE: - case MAVLINK_MSG_ID_ROSFLIGHT_GNSS: - case MAVLINK_MSG_ID_ROSFLIGHT_GNSS_FULL: - case MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS: - case MAVLINK_MSG_ID_SMALL_MAG: - case MAVLINK_MSG_ID_RC_CHANNELS: - case MAVLINK_MSG_ID_SMALL_RANGE: - case MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW: -// case MAVLINK_MSG_ID_ROSFLIGHT_STATUS: - case MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD: - p.qos=0x02; break; - // Otherwise send if there is bandwidth available - default: - p.qos = 0xFF; - } - - if(serial_device_==1) telem_.writePacket(&p); - else vcp_.writePacket(&p); -} + SerialTxPacket p; + p.timestamp = time64.Us(); + p.payloadSize = len; + + if (len > (256 + 8)) + len = (256 + 8); + memcpy(p.payload, src, len); + uint8_t message_id = p.payload[5]; // Assumes Mavlink v1 + + switch (message_id) + { + // Real-time + case MAVLINK_MSG_ID_SMALL_IMU: + p.qos = 0x00; + break; // Start of Timeframe + case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: + case MAVLINK_MSG_ID_TIMESYNC: + p.qos = 0x01; + break; + // Secondary real-time + case MAVLINK_MSG_ID_HEARTBEAT: + case MAVLINK_MSG_ID_SMALL_BARO: + case MAVLINK_MSG_ID_DIFF_PRESSURE: + case MAVLINK_MSG_ID_ROSFLIGHT_GNSS: + case MAVLINK_MSG_ID_ROSFLIGHT_GNSS_FULL: + case MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS: + case MAVLINK_MSG_ID_SMALL_MAG: + case MAVLINK_MSG_ID_RC_CHANNELS: + case MAVLINK_MSG_ID_SMALL_RANGE: + case MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW: + // case MAVLINK_MSG_ID_ROSFLIGHT_STATUS: + case MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD: + p.qos = 0x02; + break; + // Otherwise send if there is bandwidth available + default: + p.qos = 0xFF; + } + + if (serial_device_ == 1) + telem_.writePacket(&p); + else + vcp_.writePacket(&p); +} uint16_t Varmint::serial_bytes_available(void) { - if(serial_device_==1) return telem_.byteCount(); - else return vcp_.byteCount(); + if (serial_device_ == 1) + return telem_.byteCount(); + else + return vcp_.byteCount(); } uint8_t Varmint::serial_read(void) { - uint8_t c=0; - if(serial_device_==1) telem_.readByte(&c); - else vcp_.readByte(&c); - return c; + uint8_t c = 0; + if (serial_device_ == 1) + telem_.readByte(&c); + else + vcp_.readByte(&c); + return c; } void Varmint::serial_flush(void) { -// do nothing + // do nothing } // sensors -void Varmint::sensors_init(){} +void Varmint::sensors_init() {} uint16_t Varmint::num_sensor_errors() { - return 0; + return 0; } // IMU uint8_t Varmint::imu_has_new_data() { - return imu0_.rxFifoCount(); + return imu0_.rxFifoCount(); } bool Varmint::imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us) { - ImuPacket p; - if(imu0_.rxFifoReadMostRecent((uint8_t*)&p,sizeof(p))) - { - accel[0] = p.accel[0]; - accel[1] = p.accel[1]; - accel[2] = p.accel[2]; - gyro[0] = p.gyro[0]; - gyro[1] = p.gyro[1]; - gyro[2] = p.gyro[2]; - *temperature = p.temperature; - *time_us = p.drdy; - return true; - } - return false; + ImuPacket p; + if (imu0_.rxFifoReadMostRecent((uint8_t *)&p, sizeof(p))) + { + accel[0] = p.accel[0]; + accel[1] = p.accel[1]; + accel[2] = p.accel[2]; + gyro[0] = p.gyro[0]; + gyro[1] = p.gyro[1]; + gyro[2] = p.gyro[2]; + *temperature = p.temperature; + *time_us = p.drdy; + return true; + } + return false; } void Varmint::imu_not_responding_error() { - // do nothing for now. + // do nothing for now. } // MAG -bool Varmint::mag_present() {return 1;} +bool Varmint::mag_present() +{ + return 1; +} uint8_t Varmint::mag_has_new_data() { - return mag_.rxFifoCount(); + return mag_.rxFifoCount(); } bool Varmint::mag_read(float mag[3]) { - MagPacket p; - if(mag_.rxFifoReadMostRecent((uint8_t*)&p,sizeof(p))) - { - mag[0] = p.flux[0]; - mag[1] = p.flux[1]; - mag[2] = p.flux[2]; - return true; - } - return false; + MagPacket p; + if (mag_.rxFifoReadMostRecent((uint8_t *)&p, sizeof(p))) + { + mag[0] = p.flux[0]; + mag[1] = p.flux[1]; + mag[2] = p.flux[2]; + return true; + } + return false; } // Baro -bool Varmint::baro_present() {return 1;} +bool Varmint::baro_present() +{ + return 1; +} uint8_t Varmint::baro_has_new_data() { - return baro_.rxFifoCount(); + return baro_.rxFifoCount(); } bool Varmint::baro_read(float *pressure, float *temperature) { - BaroPacket p; - if(baro_.rxFifoReadMostRecent((uint8_t*)&p,sizeof(p))) - { - *pressure = p.pressure; - *temperature = p.temperature; - return true; - } - return false; + BaroPacket p; + if (baro_.rxFifoReadMostRecent((uint8_t *)&p, sizeof(p))) + { + *pressure = p.pressure; + *temperature = p.temperature; + return true; + } + return false; } // Pitot -bool Varmint::diff_pressure_present() {return 1;} +bool Varmint::diff_pressure_present() +{ + return 1; +} uint8_t Varmint::diff_pressure_has_new_data() { - return pitot_.rxFifoCount(); + return pitot_.rxFifoCount(); } bool Varmint::diff_pressure_read(float *diff_pressure, float *temperature) { - PitotPacket p; - if(pitot_.rxFifoReadMostRecent((uint8_t*)&p,sizeof(p))) - { - *diff_pressure = p.pressure; - *temperature = p.temperature; - return true; - } - return false; + PitotPacket p; + if (pitot_.rxFifoReadMostRecent((uint8_t *)&p, sizeof(p))) + { + *diff_pressure = p.pressure; + *temperature = p.temperature; + return true; + } + return false; } // Sonar -bool Varmint::sonar_present() {return 0;} -uint8_t Varmint::sonar_has_new_data(){ return 0;} -bool Varmint::sonar_read(float *range) { return false;} +bool Varmint::sonar_present() +{ + return 0; +} +uint8_t Varmint::sonar_has_new_data() +{ + return 0; +} +bool Varmint::sonar_read(float *range) +{ + return false; +} // Battery -bool Varmint::battery_present() {return 1;} +bool Varmint::battery_present() +{ + return 1; +} uint8_t Varmint::battery_has_new_data() { - return adc_.rxFifoCount(); + return adc_.rxFifoCount(); } -bool Varmint::battery_read( float *voltage, float *current) +bool Varmint::battery_read(float *voltage, float *current) { - AdcPacket p; - if(adc_.rxFifoReadMostRecent((uint8_t*)&p,sizeof(p))) - { - *current = p.volts_ext[ADC_BATTERY_CURR]; - *voltage = p.volts_ext[ADC_BATTERY_VOLTS]; - return true; - } - return false; + AdcPacket p; + if (adc_.rxFifoReadMostRecent((uint8_t *)&p, sizeof(p))) + { + *current = p.volts_ext[ADC_BATTERY_CURR]; + *voltage = p.volts_ext[ADC_BATTERY_VOLTS]; + return true; + } + return false; } void Varmint::battery_voltage_set_multiplier(double multiplier) { - if(multiplier==0) return; - adc_.setScaleFactor(ADC_BATTERY_VOLTS, multiplier); + if (multiplier == 0) + return; + adc_.setScaleFactor(ADC_BATTERY_VOLTS, multiplier); } void Varmint::battery_current_set_multiplier(double multiplier) { - if(multiplier==0) return; - adc_.setScaleFactor(ADC_BATTERY_CURR, multiplier); + if (multiplier == 0) + return; + adc_.setScaleFactor(ADC_BATTERY_CURR, multiplier); } // GNSS -bool Varmint::gnss_present() {return 1;} +bool Varmint::gnss_present() +{ + return 1; +} uint8_t Varmint::gnss_has_new_data() { - return gps_.rxFifoCount(); -} - -bool Varmint::gnss_read( rosflight_firmware::GNSSData *gnss, rosflight_firmware::GNSSFull *gnss_full) -{ - UbxPacket p; - - if(gps_.rxFifoReadMostRecent((uint8_t*)&p,sizeof(p))) - { - gnss_full->time_of_week = p.pvt.iTOW; - gnss_full->year = p.pvt.year; - gnss_full->month = p.pvt.month; - gnss_full->day = p.pvt.day; - gnss_full->hour = p.pvt.hour; - gnss_full->min = p.pvt.min; - gnss_full->sec = p.pvt.sec; - gnss_full->valid = p.pvt.valid; - gnss_full->t_acc = p.pvt.tAcc; - gnss_full->nano = p.pvt.nano; - //gnss_full->fix_type = p.pvt.fixType; // This is the Ubx fix type, not rosflight - if((p.pvt.fixType>1)&&(p.pvt.fixType<5)) gnss_full->fix_type = rosflight_firmware::GNSS_FIX_TYPE_FIX; - else gnss_full->fix_type = rosflight_firmware::GNSS_FIX_TYPE_NO_FIX; - gnss_full->num_sat = p.pvt.numSV; - gnss_full->lon = p.pvt.lon; - gnss_full->lat = p.pvt.lat; - gnss_full->height = p.pvt.height; - gnss_full->height_msl = p.pvt.hMSL; - gnss_full->h_acc = p.pvt.hAcc; - gnss_full->v_acc = p.pvt.vAcc; - gnss_full->vel_n = p.pvt.velN; - gnss_full->vel_e = p.pvt.velE; - gnss_full->vel_d = p.pvt.velD; - gnss_full->g_speed = p.pvt.gSpeed; - gnss_full->head_mot = p.pvt.headMot; - gnss_full->s_acc = p.pvt.sAcc; - gnss_full->head_acc = p.pvt.headAcc; - gnss_full->p_dop = p.pvt.pDOP; - gnss_full->rosflight_timestamp = p.drdy; - - gnss->time_of_week = p.nav.iTOW; - gnss->nanos = p.nav.fTOW; // TODO: Is this supposed to be the nanoseconds of the TOW, unix time??? - //gnss->fix_type = nav.gpsFix; - if((p.nav.fixType>1)&&(p.nav.fixType<5)) gnss->fix_type = rosflight_firmware::GNSS_FIX_TYPE_FIX; - else gnss->fix_type = rosflight_firmware::GNSS_FIX_TYPE_NO_FIX; - struct tm tm; - tm.tm_sec = p.pvt.sec; - tm.tm_min = p.pvt.min; - tm.tm_hour = p.pvt.hour; - tm.tm_mday = p.pvt.day; - tm.tm_mon = p.pvt.month-1; - tm.tm_year = p.pvt.year-1900; - gnss->time = mktime(&tm); - gnss->lat = p.pvt.lat; - gnss->lon = p.pvt.lon; - gnss->height = p.pvt.height; - gnss->vel_n = p.pvt.velN; - gnss->vel_e = p.pvt.velE; - gnss->vel_d = p.pvt.velD; - gnss->h_acc = p.pvt.hAcc; - gnss->v_acc = p.pvt.vAcc; - - gnss->ecef.x = p.nav.ecefX; - gnss->ecef.y = p.nav.ecefY; - gnss->ecef.z = p.nav.ecefZ; - gnss->ecef.p_acc = p.nav.pAcc; - gnss->ecef.vx = p.nav.ecefVX; - gnss->ecef.vy = p.nav.ecefVY; - gnss->ecef.vz = p.nav.ecefVZ; - gnss->ecef.s_acc = p.nav.sAcc; - gnss->rosflight_timestamp = p.drdy; - return true; - } - - return false; + return gps_.rxFifoCount(); +} + +bool Varmint::gnss_read(rosflight_firmware::GNSSData *gnss, rosflight_firmware::GNSSFull *gnss_full) +{ + UbxPacket p; + + if (gps_.rxFifoReadMostRecent((uint8_t *)&p, sizeof(p))) + { + gnss_full->time_of_week = p.pvt.iTOW; + gnss_full->year = p.pvt.year; + gnss_full->month = p.pvt.month; + gnss_full->day = p.pvt.day; + gnss_full->hour = p.pvt.hour; + gnss_full->min = p.pvt.min; + gnss_full->sec = p.pvt.sec; + gnss_full->valid = p.pvt.valid; + gnss_full->t_acc = p.pvt.tAcc; + gnss_full->nano = p.pvt.nano; + // gnss_full->fix_type = p.pvt.fixType; // This is the Ubx fix type, not rosflight + if ((p.pvt.fixType > 1) && (p.pvt.fixType < 5)) + gnss_full->fix_type = rosflight_firmware::GNSS_FIX_TYPE_FIX; + else + gnss_full->fix_type = rosflight_firmware::GNSS_FIX_TYPE_NO_FIX; + gnss_full->num_sat = p.pvt.numSV; + gnss_full->lon = p.pvt.lon; + gnss_full->lat = p.pvt.lat; + gnss_full->height = p.pvt.height; + gnss_full->height_msl = p.pvt.hMSL; + gnss_full->h_acc = p.pvt.hAcc; + gnss_full->v_acc = p.pvt.vAcc; + gnss_full->vel_n = p.pvt.velN; + gnss_full->vel_e = p.pvt.velE; + gnss_full->vel_d = p.pvt.velD; + gnss_full->g_speed = p.pvt.gSpeed; + gnss_full->head_mot = p.pvt.headMot; + gnss_full->s_acc = p.pvt.sAcc; + gnss_full->head_acc = p.pvt.headAcc; + gnss_full->p_dop = p.pvt.pDOP; + gnss_full->rosflight_timestamp = p.drdy; + + gnss->time_of_week = p.nav.iTOW; + gnss->nanos = p.nav.fTOW; // TODO: Is this supposed to be the nanoseconds of the TOW, unix time??? + // gnss->fix_type = nav.gpsFix; + if ((p.nav.fixType > 1) && (p.nav.fixType < 5)) + gnss->fix_type = rosflight_firmware::GNSS_FIX_TYPE_FIX; + else + gnss->fix_type = rosflight_firmware::GNSS_FIX_TYPE_NO_FIX; + struct tm tm; + tm.tm_sec = p.pvt.sec; + tm.tm_min = p.pvt.min; + tm.tm_hour = p.pvt.hour; + tm.tm_mday = p.pvt.day; + tm.tm_mon = p.pvt.month - 1; + tm.tm_year = p.pvt.year - 1900; + gnss->time = mktime(&tm); + gnss->lat = p.pvt.lat; + gnss->lon = p.pvt.lon; + gnss->height = p.pvt.height; + gnss->vel_n = p.pvt.velN; + gnss->vel_e = p.pvt.velE; + gnss->vel_d = p.pvt.velD; + gnss->h_acc = p.pvt.hAcc; + gnss->v_acc = p.pvt.vAcc; + + gnss->ecef.x = p.nav.ecefX; + gnss->ecef.y = p.nav.ecefY; + gnss->ecef.z = p.nav.ecefZ; + gnss->ecef.p_acc = p.nav.pAcc; + gnss->ecef.vx = p.nav.ecefVX; + gnss->ecef.vy = p.nav.ecefVY; + gnss->ecef.vz = p.nav.ecefVZ; + gnss->ecef.s_acc = p.nav.sAcc; + gnss->rosflight_timestamp = p.drdy; + return true; + } + + return false; } // RC void Varmint::rc_init(rc_type_t rc_type){}; bool Varmint::rc_lost() { - return rc_.lol(); + return rc_.lol(); } uint8_t Varmint::rc_has_new_data() { - return rc_.rxFifoCount(); + return rc_.rxFifoCount(); } float Varmint::rc_read(uint8_t chan) { - static RcPacket p; - static bool ever_read = false; + static RcPacket p; + static bool ever_read = false; - if(rc_.rxFifoReadMostRecent((uint8_t*)&p,sizeof(p))) - { - ever_read=true; - } + if (rc_.rxFifoReadMostRecent((uint8_t *)&p, sizeof(p))) + { + ever_read = true; + } - if((chanAHB Bridge */ -//#define D3_SRAM_BASE (0x38000000UL) /*!< Base address of : Backup SRAM(64 KB) over AXI->AHB Bridge +// #define D3_BKPSRAM_BASE (0x38800000UL) /*!< Base address of : Backup SRAM(4 KB) over AXI->AHB Bridge */ +// #define D3_SRAM_BASE (0x38000000UL) /*!< Base address of : Backup SRAM(64 KB) over AXI->AHB Bridge #define D3_BKPSRAM_BASE_LEN (4096U) -#define D3_SRAM_BASE_LEN (65536U) -void Varmint::backup_memory_init(){} +#define D3_SRAM_BASE_LEN (65536U) +void Varmint::backup_memory_init() {} bool Varmint::backup_memory_read(void *dest, size_t len) { -// if(len > D3_BKPSRAM_BASE_LEN) len = D3_BKPSRAM_BASE_LEN; -// HAL_PWR_EnableBkUpAccess(); -// memcpy(dest, (void*)D3_BKPSRAM_BASE, len); -// HAL_PWR_DisableBkUpAccess(); - return 1; + // if(len > D3_BKPSRAM_BASE_LEN) len = D3_BKPSRAM_BASE_LEN; + // HAL_PWR_EnableBkUpAccess(); + // memcpy(dest, (void*)D3_BKPSRAM_BASE, len); + // HAL_PWR_DisableBkUpAccess(); + return 1; } void Varmint::backup_memory_write(const void *src, size_t len) { -// if(len > D3_BKPSRAM_BASE_LEN) len = D3_BKPSRAM_BASE_LEN; -// HAL_PWR_EnableBkUpAccess(); -// memcpy((void*)D3_BKPSRAM_BASE, src, len); -// HAL_PWR_DisableBkUpAccess(); + // if(len > D3_BKPSRAM_BASE_LEN) len = D3_BKPSRAM_BASE_LEN; + // HAL_PWR_EnableBkUpAccess(); + // memcpy((void*)D3_BKPSRAM_BASE, src, len); + // HAL_PWR_DisableBkUpAccess(); } void Varmint::backup_memory_clear(size_t len) { -// HAL_PWR_EnableBkUpAccess(); -// memset((void*)D3_BKPSRAM_BASE, 0, D3_BKPSRAM_BASE_LEN); -// HAL_PWR_DisableBkUpAccess(); + // HAL_PWR_EnableBkUpAccess(); + // memset((void*)D3_BKPSRAM_BASE, 0, D3_BKPSRAM_BASE_LEN); + // HAL_PWR_DisableBkUpAccess(); } void Varmint::memory_init() {} // do nothing bool Varmint::memory_read(void *dest, size_t len) { - return sd_.read((uint8_t*)dest,len); + return sd_.read((uint8_t *)dest, len); } bool Varmint::memory_write(const void *src, size_t len) { - return sd_.write((uint8_t*)src,len); + return sd_.write((uint8_t *)src, len); } - uint32_t Varmint::pwm_init_timers(uint32_t servo_pwm_period_us) { - { - TIM_MasterConfigTypeDef sMasterConfig = {0}; - TIM_OC_InitTypeDef sConfigOC = {0}; - TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; - - htim1.Instance = TIM1; - htim1.Init.Prescaler = (SERVO_PWM_CLK_DIV); - htim1.Init.CounterMode = TIM_COUNTERMODE_UP; - htim1.Init.Period = servo_pwm_period_us; - htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim1.Init.RepetitionCounter = 0; - htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; - if (HAL_TIM_PWM_Init(&htim1) != HAL_OK) return DRIVER_HAL_ERROR; - sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; - sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK) return DRIVER_HAL_ERROR; - sConfigOC.OCMode = TIM_OCMODE_PWM1; - sConfigOC.Pulse = (SERVO_PWM_CENTER); - sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; - sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; - sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; - sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) return DRIVER_HAL_ERROR; - sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; - sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; - sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; - sBreakDeadTimeConfig.DeadTime = 0; - sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; - sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; - sBreakDeadTimeConfig.BreakFilter = 0; - sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE; - sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH; - sBreakDeadTimeConfig.Break2Filter = 0; - sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; - if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK) return DRIVER_HAL_ERROR; - HAL_TIM_MspPostInit(&htim1); - } - { + { + TIM_MasterConfigTypeDef sMasterConfig = {0}; + TIM_OC_InitTypeDef sConfigOC = {0}; + TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; + + htim1.Instance = TIM1; + htim1.Init.Prescaler = (SERVO_PWM_CLK_DIV); + htim1.Init.CounterMode = TIM_COUNTERMODE_UP; + htim1.Init.Period = servo_pwm_period_us; + htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim1.Init.RepetitionCounter = 0; + htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; + if (HAL_TIM_PWM_Init(&htim1) != HAL_OK) + return DRIVER_HAL_ERROR; + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + sConfigOC.OCMode = TIM_OCMODE_PWM1; + sConfigOC.Pulse = (SERVO_PWM_CENTER); + sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) + return DRIVER_HAL_ERROR; + sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; + sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; + sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; + sBreakDeadTimeConfig.DeadTime = 0; + sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; + sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; + sBreakDeadTimeConfig.BreakFilter = 0; + sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE; + sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH; + sBreakDeadTimeConfig.Break2Filter = 0; + sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; + if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + HAL_TIM_MspPostInit(&htim1); + } + { TIM_MasterConfigTypeDef sMasterConfig = {0}; TIM_OC_InitTypeDef sConfigOC = {0}; htim3.Instance = TIM3; @@ -735,16 +767,20 @@ uint32_t Varmint::pwm_init_timers(uint32_t servo_pwm_period_us) htim3.Init.Period = servo_pwm_period_us; htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; - if (HAL_TIM_PWM_Init(&htim3) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_Init(&htim3) != HAL_OK) + return DRIVER_HAL_ERROR; sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) + return DRIVER_HAL_ERROR; sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = (SERVO_PWM_CENTER); sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) + return DRIVER_HAL_ERROR; HAL_TIM_MspPostInit(&htim3); } { @@ -756,18 +792,24 @@ uint32_t Varmint::pwm_init_timers(uint32_t servo_pwm_period_us) htim4.Init.Period = servo_pwm_period_us; htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; - if (HAL_TIM_PWM_Init(&htim4) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_Init(&htim4) != HAL_OK) + return DRIVER_HAL_ERROR; sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) + return DRIVER_HAL_ERROR; sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = (SERVO_PWM_CENTER); sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) + return DRIVER_HAL_ERROR; HAL_TIM_MspPostInit(&htim4); } return DRIVER_OK; diff --git a/boards/varmint/src/board/Adc.cpp b/boards/varmint/src/board/Adc.cpp index de8c8290..4eccc7d2 100644 --- a/boards/varmint/src/board/Adc.cpp +++ b/boards/varmint/src/board/Adc.cpp @@ -36,228 +36,239 @@ **/ #include - #include #include - extern Time64 time64; -#define ADC_DMA_BUF_SIZE_INT (ADC_CHANNELS_INT*sizeof(uint32_t)) -#define ADC_DMA_BUF_SIZE_EXT (ADC_CHANNELS_EXT*sizeof(uint32_t)) +#define ADC_DMA_BUF_SIZE_INT (ADC_CHANNELS_INT * sizeof(uint32_t)) +#define ADC_DMA_BUF_SIZE_EXT (ADC_CHANNELS_EXT * sizeof(uint32_t)) -__attribute__((section("my_bdma_buffers"))) __attribute__((aligned (32))) static uint8_t adc_dma_buf_int[ADC_DMA_BUF_SIZE_INT]={0}; -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t adc_dma_buf_ext[ADC_DMA_BUF_SIZE_EXT]={0}; +__attribute__((section("my_bdma_buffers"))) +__attribute__((aligned(32))) static uint8_t adc_dma_buf_int[ADC_DMA_BUF_SIZE_INT] = {0}; +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t adc_dma_buf_ext[ADC_DMA_BUF_SIZE_EXT] = {0}; -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t adc_fifo_rx_buffer[ADC_FIFO_BUFFERS*sizeof(AdcPacket)]={0}; +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t adc_fifo_rx_buffer[ADC_FIFO_BUFFERS * sizeof(AdcPacket)] = {0}; - -uint32_t Adc::init -( - uint16_t sample_rate_hz, - ADC_HandleTypeDef *hadcExt_ext, ADC_TypeDef *adc_instance_ext, - ADC_HandleTypeDef *hadcExt_int, ADC_TypeDef *adc_instance_int -) +uint32_t Adc::init(uint16_t sample_rate_hz, + ADC_HandleTypeDef *hadcExt_ext, + ADC_TypeDef *adc_instance_ext, + ADC_HandleTypeDef *hadcExt_int, + ADC_TypeDef *adc_instance_int) { - sampleRateHz_ = sample_rate_hz; - hadcExt_ = hadcExt_ext; - hadcInt_ = hadcExt_int; - - groupDelay_ = 1000000/sampleRateHz_; - - rxFifo_.init(ADC_FIFO_BUFFERS, sizeof(AdcPacket), adc_fifo_rx_buffer); - - uint32_t clock_prescaler = ADC_CLOCK_ASYNC_DIV64; - uint32_t sampling_cycles = ADC_SAMPLETIME_810CYCLES_5; - uint32_t conversion_cycles = 8; - // ADC is being fed with 64 MHz which is divided by 2 to make the ADC clock. - // The sample time in us = 1/(64MHz/2)*clock_prescalar*(sampling_cycles+conversion_cycles)*ADC_MAX*oversample_ratio - - clock_prescaler = (64000000/2)/sampleRateHz_/((1621 + 2*conversion_cycles)/2)/ADC_CHANNELS_EXT; - if(clock_prescaler>256) clock_prescaler = ADC_CLOCK_ASYNC_DIV256; // ~39.3 ms - else if(clock_prescaler>128) clock_prescaler = ADC_CLOCK_ASYNC_DIV128; // ~19.6 ms - else clock_prescaler = ADC_CLOCK_ASYNC_DIV64; // ~ 9.8 ms - - { - - ADC_ChannelConfTypeDef sConfig = {0}; - - /** Common config */ - hadcExt_->Instance = adc_instance_ext; - hadcExt_->Init.ClockPrescaler = clock_prescaler; - hadcExt_->Init.Resolution = ADC_RESOLUTION_16B; - hadcExt_->Init.ScanConvMode = ADC_SCAN_ENABLE; - hadcExt_->Init.EOCSelection = ADC_EOC_SEQ_CONV; - hadcExt_->Init.LowPowerAutoWait = DISABLE; - hadcExt_->Init.ContinuousConvMode = DISABLE; - hadcExt_->Init.NbrOfConversion = ADC_CHANNELS_EXT; - hadcExt_->Init.DiscontinuousConvMode = DISABLE; - hadcExt_->Init.ExternalTrigConv = ADC_SOFTWARE_START; - hadcExt_->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; - hadcExt_->Init.ConversionDataManagement = ADC_CONVERSIONDATA_DMA_ONESHOT; - hadcExt_->Init.Overrun = ADC_OVR_DATA_PRESERVED; - hadcExt_->Init.LeftBitShift = ADC_LEFTBITSHIFT_NONE; - hadcExt_->Init.OversamplingMode = DISABLE; - - if (HAL_ADC_Init(hadcExt_) != HAL_OK) return DRIVER_HAL_ERROR; - - /** Configure the ADC multi-mode */ - ADC_MultiModeTypeDef multimode = {0}; - multimode.Mode = ADC_MODE_INDEPENDENT; - if (HAL_ADCEx_MultiModeConfigChannel(hadcExt_, &multimode) != HAL_OK) return DRIVER_HAL_ERROR; - - /** Configure Regular Channels */ - sConfig.SamplingTime = sampling_cycles; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; - sConfig.OffsetSignedSaturation = DISABLE; - - /** Configure Regular Channel */ - sConfig.Channel = ADC_CHANNEL_4; - sConfig.Rank = ADC_REGULAR_RANK_1; - if (HAL_ADC_ConfigChannel(hadcExt_, &sConfig) != HAL_OK) return DRIVER_HAL_ERROR; - /** Configure Regular Channel */ - sConfig.Channel = ADC_CHANNEL_7; - sConfig.Rank = ADC_REGULAR_RANK_2; - if (HAL_ADC_ConfigChannel(hadcExt_, &sConfig) != HAL_OK) return DRIVER_HAL_ERROR; - /** Configure Regular Channel */ - sConfig.Channel = ADC_CHANNEL_8; - sConfig.Rank = ADC_REGULAR_RANK_3; - if (HAL_ADC_ConfigChannel(hadcExt_, &sConfig) != HAL_OK) return DRIVER_HAL_ERROR; - /** Configure Regular Channel */ - sConfig.Channel = ADC_CHANNEL_10; - sConfig.Rank = ADC_REGULAR_RANK_4; - if (HAL_ADC_ConfigChannel(hadcExt_, &sConfig) != HAL_OK) return DRIVER_HAL_ERROR; - /** Configure Regular Channel */ - sConfig.Channel = ADC_CHANNEL_11; - sConfig.Rank = ADC_REGULAR_RANK_5; - if (HAL_ADC_ConfigChannel(hadcExt_, &sConfig) != HAL_OK) return DRIVER_HAL_ERROR; - sConfig.Channel = ADC_CHANNEL_16; - sConfig.Rank = ADC_REGULAR_RANK_6; - if (HAL_ADC_ConfigChannel(hadcExt_, &sConfig) != HAL_OK) return DRIVER_HAL_ERROR; - - HAL_ADCEx_Calibration_Start(hadcExt_, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED); - } - - { - ADC_ChannelConfTypeDef sConfig = {0}; - hadcInt_->Instance = adc_instance_int; - hadcInt_->Init.ClockPrescaler = clock_prescaler; - hadcInt_->Init.Resolution = ADC_RESOLUTION_16B; - hadcInt_->Init.ScanConvMode = ADC_SCAN_ENABLE; - hadcInt_->Init.EOCSelection = ADC_EOC_SEQ_CONV; - hadcInt_->Init.LowPowerAutoWait = DISABLE; - hadcInt_->Init.ContinuousConvMode = DISABLE; - hadcInt_->Init.NbrOfConversion = ADC_CHANNELS_INT; - hadcInt_->Init.DiscontinuousConvMode = DISABLE; - hadcInt_->Init.ExternalTrigConv = ADC_SOFTWARE_START; - hadcInt_->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; - hadcInt_->Init.ConversionDataManagement = ADC_CONVERSIONDATA_DMA_ONESHOT; - hadcInt_->Init.Overrun = ADC_OVR_DATA_PRESERVED; - hadcInt_->Init.LeftBitShift = ADC_LEFTBITSHIFT_NONE; - hadcInt_->Init.OversamplingMode = DISABLE; - if (HAL_ADC_Init(hadcInt_) != HAL_OK) return DRIVER_HAL_ERROR; - - sConfig.SamplingTime = sampling_cycles; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; - sConfig.OffsetSignedSaturation = DISABLE; - - sConfig.Channel = ADC_CHANNEL_TEMPSENSOR; - sConfig.Rank = ADC_REGULAR_RANK_1; - if (HAL_ADC_ConfigChannel(hadcInt_, &sConfig) != HAL_OK) return DRIVER_HAL_ERROR; - - sConfig.Channel = ADC_CHANNEL_VBAT; - sConfig.Rank = ADC_REGULAR_RANK_2; - if (HAL_ADC_ConfigChannel(hadcInt_, &sConfig) != HAL_OK) return DRIVER_HAL_ERROR; - - sConfig.Channel = ADC_CHANNEL_VREFINT; - sConfig.Rank = ADC_REGULAR_RANK_3; - if (HAL_ADC_ConfigChannel(hadcInt_, &sConfig) != HAL_OK) return DRIVER_HAL_ERROR; - - HAL_ADCEx_Calibration_Start(hadcInt_, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED); - - } - return DRIVER_OK; + sampleRateHz_ = sample_rate_hz; + hadcExt_ = hadcExt_ext; + hadcInt_ = hadcExt_int; + + groupDelay_ = 1000000 / sampleRateHz_; + + rxFifo_.init(ADC_FIFO_BUFFERS, sizeof(AdcPacket), adc_fifo_rx_buffer); + + uint32_t clock_prescaler = ADC_CLOCK_ASYNC_DIV64; + uint32_t sampling_cycles = ADC_SAMPLETIME_810CYCLES_5; + uint32_t conversion_cycles = 8; + // ADC is being fed with 64 MHz which is divided by 2 to make the ADC clock. + // The sample time in us = 1/(64MHz/2)*clock_prescalar*(sampling_cycles+conversion_cycles)*ADC_MAX*oversample_ratio + + clock_prescaler = (64000000 / 2) / sampleRateHz_ / ((1621 + 2 * conversion_cycles) / 2) / ADC_CHANNELS_EXT; + if (clock_prescaler > 256) + clock_prescaler = ADC_CLOCK_ASYNC_DIV256; // ~39.3 ms + else if (clock_prescaler > 128) + clock_prescaler = ADC_CLOCK_ASYNC_DIV128; // ~19.6 ms + else + clock_prescaler = ADC_CLOCK_ASYNC_DIV64; // ~ 9.8 ms + + { + ADC_ChannelConfTypeDef sConfig = {0}; + + /** Common config */ + hadcExt_->Instance = adc_instance_ext; + hadcExt_->Init.ClockPrescaler = clock_prescaler; + hadcExt_->Init.Resolution = ADC_RESOLUTION_16B; + hadcExt_->Init.ScanConvMode = ADC_SCAN_ENABLE; + hadcExt_->Init.EOCSelection = ADC_EOC_SEQ_CONV; + hadcExt_->Init.LowPowerAutoWait = DISABLE; + hadcExt_->Init.ContinuousConvMode = DISABLE; + hadcExt_->Init.NbrOfConversion = ADC_CHANNELS_EXT; + hadcExt_->Init.DiscontinuousConvMode = DISABLE; + hadcExt_->Init.ExternalTrigConv = ADC_SOFTWARE_START; + hadcExt_->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadcExt_->Init.ConversionDataManagement = ADC_CONVERSIONDATA_DMA_ONESHOT; + hadcExt_->Init.Overrun = ADC_OVR_DATA_PRESERVED; + hadcExt_->Init.LeftBitShift = ADC_LEFTBITSHIFT_NONE; + hadcExt_->Init.OversamplingMode = DISABLE; + + if (HAL_ADC_Init(hadcExt_) != HAL_OK) + return DRIVER_HAL_ERROR; + + /** Configure the ADC multi-mode */ + ADC_MultiModeTypeDef multimode = {0}; + multimode.Mode = ADC_MODE_INDEPENDENT; + if (HAL_ADCEx_MultiModeConfigChannel(hadcExt_, &multimode) != HAL_OK) + return DRIVER_HAL_ERROR; + + /** Configure Regular Channels */ + sConfig.SamplingTime = sampling_cycles; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + sConfig.OffsetSignedSaturation = DISABLE; + + /** Configure Regular Channel */ + sConfig.Channel = ADC_CHANNEL_4; + sConfig.Rank = ADC_REGULAR_RANK_1; + if (HAL_ADC_ConfigChannel(hadcExt_, &sConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + /** Configure Regular Channel */ + sConfig.Channel = ADC_CHANNEL_7; + sConfig.Rank = ADC_REGULAR_RANK_2; + if (HAL_ADC_ConfigChannel(hadcExt_, &sConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + /** Configure Regular Channel */ + sConfig.Channel = ADC_CHANNEL_8; + sConfig.Rank = ADC_REGULAR_RANK_3; + if (HAL_ADC_ConfigChannel(hadcExt_, &sConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + /** Configure Regular Channel */ + sConfig.Channel = ADC_CHANNEL_10; + sConfig.Rank = ADC_REGULAR_RANK_4; + if (HAL_ADC_ConfigChannel(hadcExt_, &sConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + /** Configure Regular Channel */ + sConfig.Channel = ADC_CHANNEL_11; + sConfig.Rank = ADC_REGULAR_RANK_5; + if (HAL_ADC_ConfigChannel(hadcExt_, &sConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + sConfig.Channel = ADC_CHANNEL_16; + sConfig.Rank = ADC_REGULAR_RANK_6; + if (HAL_ADC_ConfigChannel(hadcExt_, &sConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + + HAL_ADCEx_Calibration_Start(hadcExt_, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED); + } + + { + ADC_ChannelConfTypeDef sConfig = {0}; + hadcInt_->Instance = adc_instance_int; + hadcInt_->Init.ClockPrescaler = clock_prescaler; + hadcInt_->Init.Resolution = ADC_RESOLUTION_16B; + hadcInt_->Init.ScanConvMode = ADC_SCAN_ENABLE; + hadcInt_->Init.EOCSelection = ADC_EOC_SEQ_CONV; + hadcInt_->Init.LowPowerAutoWait = DISABLE; + hadcInt_->Init.ContinuousConvMode = DISABLE; + hadcInt_->Init.NbrOfConversion = ADC_CHANNELS_INT; + hadcInt_->Init.DiscontinuousConvMode = DISABLE; + hadcInt_->Init.ExternalTrigConv = ADC_SOFTWARE_START; + hadcInt_->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadcInt_->Init.ConversionDataManagement = ADC_CONVERSIONDATA_DMA_ONESHOT; + hadcInt_->Init.Overrun = ADC_OVR_DATA_PRESERVED; + hadcInt_->Init.LeftBitShift = ADC_LEFTBITSHIFT_NONE; + hadcInt_->Init.OversamplingMode = DISABLE; + if (HAL_ADC_Init(hadcInt_) != HAL_OK) + return DRIVER_HAL_ERROR; + + sConfig.SamplingTime = sampling_cycles; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + sConfig.OffsetSignedSaturation = DISABLE; + + sConfig.Channel = ADC_CHANNEL_TEMPSENSOR; + sConfig.Rank = ADC_REGULAR_RANK_1; + if (HAL_ADC_ConfigChannel(hadcInt_, &sConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + + sConfig.Channel = ADC_CHANNEL_VBAT; + sConfig.Rank = ADC_REGULAR_RANK_2; + if (HAL_ADC_ConfigChannel(hadcInt_, &sConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + + sConfig.Channel = ADC_CHANNEL_VREFINT; + sConfig.Rank = ADC_REGULAR_RANK_3; + if (HAL_ADC_ConfigChannel(hadcInt_, &sConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + + HAL_ADCEx_Calibration_Start(hadcInt_, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED); + } + return DRIVER_OK; } bool Adc::poll(uint16_t poll_offset) { - if(poll_offset==0) // launch a read - { - return startDma(); - } - return false; + if (poll_offset == 0) // launch a read + { + return startDma(); + } + return false; } bool Adc::startDma(void) { - drdy_ = time64.Us(); + drdy_ = time64.Us(); - HAL_StatusTypeDef hal_status_int = HAL_ADC_Start_DMA(hadcInt_, (uint32_t*)adc_dma_buf_int, ADC_CHANNELS_INT ); - HAL_StatusTypeDef hal_status_ext = HAL_ADC_Start_DMA(hadcExt_, (uint32_t*)adc_dma_buf_ext, ADC_CHANNELS_EXT ); + HAL_StatusTypeDef hal_status_int = HAL_ADC_Start_DMA(hadcInt_, (uint32_t *)adc_dma_buf_int, ADC_CHANNELS_INT); + HAL_StatusTypeDef hal_status_ext = HAL_ADC_Start_DMA(hadcExt_, (uint32_t *)adc_dma_buf_ext, ADC_CHANNELS_EXT); - return (HAL_OK == hal_status_int)&&(HAL_OK == hal_status_ext); + return (HAL_OK == hal_status_int) && (HAL_OK == hal_status_ext); } void Adc::endDma(void) { - #if USE_D_CACHE_MANAGEMENT_FUNCTIONS - SCB_InvalidateDCache_by_Addr ((uint32_t *)adc_dma_buf_ext, ADC_DMA_BUF_SIZE_EXT); - SCB_InvalidateDCache_by_Addr ((uint32_t *)adc_dma_buf_int, ADC_DMA_BUF_SIZE_INT); - #endif - - uint32_t *counts_ext = (uint32_t*)adc_dma_buf_ext; - uint32_t *counts_int = (uint32_t*)adc_dma_buf_int; - - AdcPacket p; - p.temperature = (double)(TEMPSENSOR_CAL2_TEMP-TEMPSENSOR_CAL1_TEMP) - /(double)( *TEMPSENSOR_CAL2_ADDR -*TEMPSENSOR_CAL1_ADDR) - *((double)counts_int[ADC_STM_TEMPERATURE]-(double)*TEMPSENSOR_CAL1_ADDR) - +(double)TEMPSENSOR_CAL1_TEMP; - - p.vRef = (double)VREFINT_CAL_VREF/1000.0*(double)*VREFINT_CAL_ADDR/(double)counts_int[ADC_STM_VREFINT]; - p.vBku = 4.0*(double)counts_int[ADC_STM_VBAT]*p.vRef/65535.0; - - for(int i=0;i< ADC_CHANNELS_EXT; i++) p.volts_ext[i] = (double)counts_ext[i]*(p.vRef/65535.0)*scaleFactor_[i]; - - p.timestamp = time64.Us(); - p.drdy = drdy_; - p.groupDelay = groupDelay_; - rxFifo_.write((uint8_t*)&p,sizeof(p)); +#if USE_D_CACHE_MANAGEMENT_FUNCTIONS + SCB_InvalidateDCache_by_Addr((uint32_t *)adc_dma_buf_ext, ADC_DMA_BUF_SIZE_EXT); + SCB_InvalidateDCache_by_Addr((uint32_t *)adc_dma_buf_int, ADC_DMA_BUF_SIZE_INT); +#endif + + uint32_t *counts_ext = (uint32_t *)adc_dma_buf_ext; + uint32_t *counts_int = (uint32_t *)adc_dma_buf_int; + + AdcPacket p; + p.temperature = (double)(TEMPSENSOR_CAL2_TEMP - TEMPSENSOR_CAL1_TEMP) + / (double)(*TEMPSENSOR_CAL2_ADDR - *TEMPSENSOR_CAL1_ADDR) + * ((double)counts_int[ADC_STM_TEMPERATURE] - (double)*TEMPSENSOR_CAL1_ADDR) + + (double)TEMPSENSOR_CAL1_TEMP; + + p.vRef = (double)VREFINT_CAL_VREF / 1000.0 * (double)*VREFINT_CAL_ADDR / (double)counts_int[ADC_STM_VREFINT]; + p.vBku = 4.0 * (double)counts_int[ADC_STM_VBAT] * p.vRef / 65535.0; + + for (int i = 0; i < ADC_CHANNELS_EXT; i++) + p.volts_ext[i] = (double)counts_ext[i] * (p.vRef / 65535.0) * scaleFactor_[i]; + + p.timestamp = time64.Us(); + p.drdy = drdy_; + p.groupDelay = groupDelay_; + rxFifo_.write((uint8_t *)&p, sizeof(p)); } bool Adc::display(void) { - AdcPacket p; - char name[] = "Adc (adc) "; - if(rxFifo_.readMostRecent((uint8_t*)&p,sizeof(p))) - { - misc_header(name, p.drdy, p.timestamp, p.groupDelay); - misc_printf(" Batt V %5.2fV, I %5.2fA, P %5.2fW\n\r",p.volts_ext[ADC_BATTERY_VOLTS],p.volts_ext[ADC_BATTERY_CURR],p.volts_ext[ADC_BATTERY_VOLTS]*p.volts_ext[ADC_BATTERY_CURR]); - misc_header(name, p.drdy, p.timestamp, p.groupDelay); - misc_printf(" PS's 3V3 %5.2fV, 5V0 %5.2fV, 12V %5.2fV, Servo %5.2fV\n\r",p.volts_ext[ADC_JETSON_3V3],p.volts_ext[ADC_STM_5V0],p.volts_ext[ADC_STM_12V],p.volts_ext[ADC_SERVO_VOLTS]); - misc_header(name, p.drdy, p.timestamp, p.groupDelay); - misc_printf(" Temp %7.2fC, Vbat %5.2fV, Vref %5.2fV\n\r",p.temperature, p.vBku,p.vRef); - return 1; - } - else - { - misc_printf("%s\n\r",name); - misc_printf("%s\n\r",name); - misc_printf("%s\n\r",name); - } - return 0; + AdcPacket p; + char name[] = "Adc (adc) "; + if (rxFifo_.readMostRecent((uint8_t *)&p, sizeof(p))) + { + misc_header(name, p.drdy, p.timestamp, p.groupDelay); + misc_printf(" Batt V %5.2fV, I %5.2fA, P %5.2fW\n\r", p.volts_ext[ADC_BATTERY_VOLTS], + p.volts_ext[ADC_BATTERY_CURR], p.volts_ext[ADC_BATTERY_VOLTS] * p.volts_ext[ADC_BATTERY_CURR]); + misc_header(name, p.drdy, p.timestamp, p.groupDelay); + misc_printf(" PS's 3V3 %5.2fV, 5V0 %5.2fV, 12V %5.2fV, Servo %5.2fV\n\r", p.volts_ext[ADC_JETSON_3V3], + p.volts_ext[ADC_STM_5V0], p.volts_ext[ADC_STM_12V], p.volts_ext[ADC_SERVO_VOLTS]); + misc_header(name, p.drdy, p.timestamp, p.groupDelay); + misc_printf(" Temp %7.2fC, Vbat %5.2fV, Vref %5.2fV\n\r", p.temperature, p.vBku, p.vRef); + return 1; + } + else + { + misc_printf("%s\n\r", name); + misc_printf("%s\n\r", name); + misc_printf("%s\n\r", name); + } + return 0; } void Adc::setScaleFactor(uint16_t n, float scale_factor) { - if(n -#include - #include +#include +#include #define SPI_WRITE 0x80 -#define SPI_READ 0x00 +#define SPI_READ 0x00 -#define ADIS_SPI_PAUSE_US 100 //16->20 us between spi transactions +#define ADIS_SPI_PAUSE_US 100 // 16->20 us between spi transactions -#define ADIS_BUFFBYTES32 34 -//#define ADIS_BUFFBYTES16 22 +#define ADIS_BUFFBYTES32 34 +// #define ADIS_BUFFBYTES16 22 #define SPI_WRITE 0x80 -#define SPI_READ 0x00 +#define SPI_READ 0x00 -#define BURST_READ_32 (0x68|SPI_READ) +#define BURST_READ_32 (0x68 | SPI_READ) extern Time64 time64; -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t adis165xx_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE]={0}; -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t adis165xx_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE]={0}; - -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t adis165xx_fifo_rx_buffer[ADIS165XX_FIFO_BUFFERS*sizeof(ImuPacket)]={0}; - - -uint32_t Adis165xx::init -( - // Driver initializers - uint16_t sample_rate_hz, - GPIO_TypeDef *drdy_port, // Reset GPIO Port - uint16_t drdy_pin, // Reset GPIO Pin - // SPI initializers - SPI_HandleTypeDef *hspi, - GPIO_TypeDef *cs_port, // Reset GPIO Port - uint16_t cs_pin, // Reset GPIO Pin - // ADIS165xx initializers - GPIO_TypeDef *reset_port, // Reset GPIO Port - uint16_t reset_pin, // Reset GPIO Pin - TIM_HandleTypeDef *htim, - TIM_TypeDef *htim_instance, - uint32_t htim_channel, - uint32_t htim_period_us -) +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t adis165xx_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE] = {0}; +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t adis165xx_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE] = {0}; + +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t adis165xx_fifo_rx_buffer[ADIS165XX_FIFO_BUFFERS * sizeof(ImuPacket)] = {0}; + +uint32_t Adis165xx::init( + // Driver initializers + uint16_t sample_rate_hz, + GPIO_TypeDef *drdy_port, // Reset GPIO Port + uint16_t drdy_pin, // Reset GPIO Pin + // SPI initializers + SPI_HandleTypeDef *hspi, + GPIO_TypeDef *cs_port, // Reset GPIO Port + uint16_t cs_pin, // Reset GPIO Pin + // ADIS165xx initializers + GPIO_TypeDef *reset_port, // Reset GPIO Port + uint16_t reset_pin, // Reset GPIO Pin + TIM_HandleTypeDef *htim, + TIM_TypeDef *htim_instance, + uint32_t htim_channel, + uint32_t htim_period_us) { - uint32_t status = DRIVER_OK; - sampleRateHz_ = sample_rate_hz; - drdyPort_ = drdy_port; - drdyPin_ = drdy_pin; + uint32_t status = DRIVER_OK; + sampleRateHz_ = sample_rate_hz; + drdyPort_ = drdy_port; + drdyPin_ = drdy_pin; - spi_.init(hspi,adis165xx_dma_txbuf,adis165xx_dma_rxbuf,cs_port,cs_pin); + spi_.init(hspi, adis165xx_dma_txbuf, adis165xx_dma_rxbuf, cs_port, cs_pin); - timeoutMs_ = 100; + timeoutMs_ = 100; - resetPort_ = reset_port; - resetPin_ = reset_pin; - htim_ = htim; - htimChannel_ = htim_channel; + resetPort_ = reset_port; + resetPin_ = reset_pin; + htim_ = htim; + htimChannel_ = htim_channel; - groupDelay_ = (uint64_t)1510 + (uint64_t)500000/sampleRateHz_; // us, Approximate, Accel is 1.57ms, Gyro x&y are 1.51ms, and Gyro z is 1.29ms. + groupDelay_ = (uint64_t)1510 + + (uint64_t)500000 + / sampleRateHz_; // us, Approximate, Accel is 1.57ms, Gyro x&y are 1.51ms, and Gyro z is 1.29ms. - HAL_GPIO_WritePin(spi_.port_, spi_.pin_, GPIO_PIN_SET); - HAL_GPIO_WritePin(resetPort_, resetPin_, GPIO_PIN_SET); + HAL_GPIO_WritePin(spi_.port_, spi_.pin_, GPIO_PIN_SET); + HAL_GPIO_WritePin(resetPort_, resetPin_, GPIO_PIN_SET); - rxFifo_.init(ADIS165XX_FIFO_BUFFERS, sizeof(ImuPacket), adis165xx_fifo_rx_buffer); + rxFifo_.init(ADIS165XX_FIFO_BUFFERS, sizeof(ImuPacket), adis165xx_fifo_rx_buffer); - // Startup the external clock + // Startup the external clock TIM_MasterConfigTypeDef sMasterConfig = {0}; TIM_OC_InitTypeDef sConfigOC = {0}; @@ -112,174 +113,179 @@ uint32_t Adis165xx::init htim_->Init.Period = htim_period_us; htim_->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim_->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; - if (HAL_TIM_PWM_Init(&htim12) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_Init(&htim12) != HAL_OK) + return DRIVER_HAL_ERROR; sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(htim_, &sMasterConfig) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIMEx_MasterConfigSynchronization(htim_, &sMasterConfig) != HAL_OK) + return DRIVER_HAL_ERROR; sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 250; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - if (HAL_TIM_PWM_ConfigChannel(htim_, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(htim_, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) + return DRIVER_HAL_ERROR; HAL_TIM_MspPostInit(htim_); - HAL_TIM_PWM_Start( htim_, htimChannel_); //(2kHz) clock source for ADIS165xx - time64.dUs(100); - - // Reset - HAL_GPIO_WritePin(resetPort_, resetPin_, GPIO_PIN_RESET); - time64.dUs(100); // was 16 - HAL_GPIO_WritePin(resetPort_, resetPin_, GPIO_PIN_SET); - time64.dMs(350); // Data sheet specifies 255ms for power-on startup empirically 300 is required - - #define ADIS16500_PROD_ID_ADDR 0x72 - #define ADIS16500_PROD_ID 0x4074 - uint16_t prod_id = readRegister(ADIS16500_PROD_ID_ADDR); - - misc_printf("ADIS165xx Product ID = 0x%04X (0x4074) - ",prod_id); - - if(prod_id == ADIS16500_PROD_ID) - { - misc_printf("OK\n\r"); - } - else - { - misc_printf("ERROR\n\r"); - status |= DRIVER_ID_MISMATCH; - return status; - } - - #define ADIS16500_FILT_CTRL 0x5C // shift so we can or the data into the first 16 bit packet - // [15:3] not used - // [2:0] 0 no digital filter default) - writeRegister(ADIS16500_FILT_CTRL,0); - - #define ADIS16500_DEC_RATE 0x64 // decimation - // [15:11] don't care - // [10:0] decimation rate minus 1, e.g., use 5-1 = 4 - - uint16_t dec_rate = 2000/sampleRateHz_-1; - writeRegister(ADIS16500_DEC_RATE, dec_rate); //00 for 2000 Hz, 2000/400-1 = 4 for 400 Hz. - - // Miscellaneous Control Register (MSC_CTRL) - #define ADIS16500_MSC_CTRL 0x60 - // [15:10] 0's unused - // [9] 1 32-bit burst data (default = 0) - // [8] 0 burst data has gyro and accel data (default = 0) - - // [7] 1 enable linear acceleration compensation for gyros (default 0) - // [6] 0 point of percussion alignment - // [5] 0 always zero - // [4] 0 wide sensor bandwidth (default) - - // [3:2] 01 Direct Input Sync Mode - // [1] 0 falling edge sync (default =0) - // [0] 1 active high when data is valid (default is 0, low) - // 0b0000 0010 1000 0101 = 0x0285 - writeRegister(ADIS16500_MSC_CTRL, 0x0285); // values 0b0000 0010 1000 0101 = 0x0285 - - #define ADIS16500_DIAG_STAT 0x02 - uint16_t diag_stat = readRegister(ADIS16500_DIAG_STAT); - misc_printf("ADIS165xx DIAG_STAT = 0x%04X (0x0000) - ",diag_stat); - if(diag_stat == 0) - { - misc_printf("OK\n\r"); - } - else - { - misc_printf("ERROR\n\r"); - status |= DRIVER_SELF_DIAG_ERROR; - } - - return status; + HAL_TIM_PWM_Start(htim_, htimChannel_); //(2kHz) clock source for ADIS165xx + time64.dUs(100); + + // Reset + HAL_GPIO_WritePin(resetPort_, resetPin_, GPIO_PIN_RESET); + time64.dUs(100); // was 16 + HAL_GPIO_WritePin(resetPort_, resetPin_, GPIO_PIN_SET); + time64.dMs(350); // Data sheet specifies 255ms for power-on startup empirically 300 is required + +#define ADIS16500_PROD_ID_ADDR 0x72 +#define ADIS16500_PROD_ID 0x4074 + uint16_t prod_id = readRegister(ADIS16500_PROD_ID_ADDR); + + misc_printf("ADIS165xx Product ID = 0x%04X (0x4074) - ", prod_id); + + if (prod_id == ADIS16500_PROD_ID) + { + misc_printf("OK\n\r"); + } + else + { + misc_printf("ERROR\n\r"); + status |= DRIVER_ID_MISMATCH; + return status; + } + +#define ADIS16500_FILT_CTRL 0x5C // shift so we can or the data into the first 16 bit packet + // [15:3] not used + // [2:0] 0 no digital filter default) + writeRegister(ADIS16500_FILT_CTRL, 0); + +#define ADIS16500_DEC_RATE 0x64 // decimation + // [15:11] don't care + // [10:0] decimation rate minus 1, e.g., use 5-1 = 4 + + uint16_t dec_rate = 2000 / sampleRateHz_ - 1; + writeRegister(ADIS16500_DEC_RATE, dec_rate); // 00 for 2000 Hz, 2000/400-1 = 4 for 400 Hz. + +// Miscellaneous Control Register (MSC_CTRL) +#define ADIS16500_MSC_CTRL 0x60 + // [15:10] 0's unused + // [9] 1 32-bit burst data (default = 0) + // [8] 0 burst data has gyro and accel data (default = 0) + + // [7] 1 enable linear acceleration compensation for gyros (default 0) + // [6] 0 point of percussion alignment + // [5] 0 always zero + // [4] 0 wide sensor bandwidth (default) + + // [3:2] 01 Direct Input Sync Mode + // [1] 0 falling edge sync (default =0) + // [0] 1 active high when data is valid (default is 0, low) + // 0b0000 0010 1000 0101 = 0x0285 + writeRegister(ADIS16500_MSC_CTRL, 0x0285); // values 0b0000 0010 1000 0101 = 0x0285 + +#define ADIS16500_DIAG_STAT 0x02 + uint16_t diag_stat = readRegister(ADIS16500_DIAG_STAT); + misc_printf("ADIS165xx DIAG_STAT = 0x%04X (0x0000) - ", diag_stat); + if (diag_stat == 0) + { + misc_printf("OK\n\r"); + } + else + { + misc_printf("ERROR\n\r"); + status |= DRIVER_SELF_DIAG_ERROR; + } + + return status; } inline double val(uint8_t *x) { - return (double)((int32_t)x[0]<<8| (int32_t)x[1]<<0 | (int32_t)x[2]<<24 | (int32_t)x[3]<<16)/((double)(1<<16)); + return (double)((int32_t)x[0] << 8 | (int32_t)x[1] << 0 | (int32_t)x[2] << 24 | (int32_t)x[3] << 16) + / ((double)(1 << 16)); } -bool Adis165xx::startDma(void) //called to start dma read +bool Adis165xx::startDma(void) // called to start dma read { - HAL_StatusTypeDef hal_Status = HAL_OK; - drdy_ = time64.Us(); - hal_Status = spi_.startDma(BURST_READ_32, ADIS_BUFFBYTES32); - return hal_Status == HAL_OK; + HAL_StatusTypeDef hal_Status = HAL_OK; + drdy_ = time64.Us(); + hal_Status = spi_.startDma(BURST_READ_32, ADIS_BUFFBYTES32); + return hal_Status == HAL_OK; } void Adis165xx::endDma(void) // called when DMA data is ready { - uint8_t *rx = spi_.endDma(); - // compute checksum - uint16_t sum=0; for(int n=2;n<(ADIS_BUFFBYTES32-2);n++) sum += (uint16_t)rx[n]; - - int16_t data[ADIS_BUFFBYTES32/2]; - for(int i=0;i>8)&0x00FF; - spi_.tx(tx,2,timeoutMs_); - time64.dUs(ADIS_SPI_PAUSE_US); + uint8_t tx[2] = {0}; + tx[0] = (address) | SPI_WRITE; + tx[1] = value & 0x00FF; + + spi_.tx(tx, 2, timeoutMs_); + time64.dUs(ADIS_SPI_PAUSE_US); + tx[0] = (++address) | SPI_WRITE; + tx[1] = (value >> 8) & 0x00FF; + spi_.tx(tx, 2, timeoutMs_); + time64.dUs(ADIS_SPI_PAUSE_US); } uint16_t Adis165xx::readRegister(uint8_t address) { - uint8_t tx[2]={0}; - uint8_t rx[2]={0}; - tx[0] = (address) | SPI_READ; - spi_.rx(tx,rx,2,timeoutMs_); - time64.dUs(ADIS_SPI_PAUSE_US); - tx[0] = (++address) | SPI_READ; - spi_.rx(tx,rx,2,timeoutMs_); - time64.dUs(ADIS_SPI_PAUSE_US); - return (uint16_t)rx[1] | (uint16_t)rx[0]<<8; + uint8_t tx[2] = {0}; + uint8_t rx[2] = {0}; + tx[0] = (address) | SPI_READ; + spi_.rx(tx, rx, 2, timeoutMs_); + time64.dUs(ADIS_SPI_PAUSE_US); + tx[0] = (++address) | SPI_READ; + spi_.rx(tx, rx, 2, timeoutMs_); + time64.dUs(ADIS_SPI_PAUSE_US); + return (uint16_t)rx[1] | (uint16_t)rx[0] << 8; } bool Adis165xx::display(void) { - ImuPacket p; - char name[] = "Adis165xx (imu0)"; - if(rxFifo_.readMostRecent((uint8_t*)&p,sizeof(p))) - { - misc_header(name,p.drdy, p.timestamp,p.groupDelay); - misc_printf("%10.3f %10.3f %10.3f g ", p.accel[0]/9.80665,p.accel[1]/9.80665,p.accel[2]/9.80665); - misc_printf(" | %10.3f %10.3f %10.3f deg/s", p.gyro[0]*57.2958,p.gyro[1]*57.2958,p.gyro[2]*57.2958); - misc_printf(" | %7.1f C", p.temperature-273.15); - misc_printf(" | %10.3f s | 0x%04X \n\r", p.dataTime, p.status); - return 1; - } - else - { - misc_printf("%s\n\r",name); - } - return true; + ImuPacket p; + char name[] = "Adis165xx (imu0)"; + if (rxFifo_.readMostRecent((uint8_t *)&p, sizeof(p))) + { + misc_header(name, p.drdy, p.timestamp, p.groupDelay); + misc_printf("%10.3f %10.3f %10.3f g ", p.accel[0] / 9.80665, p.accel[1] / 9.80665, p.accel[2] / 9.80665); + misc_printf(" | %10.3f %10.3f %10.3f deg/s", p.gyro[0] * 57.2958, p.gyro[1] * 57.2958, p.gyro[2] * 57.2958); + misc_printf(" | %7.1f C", p.temperature - 273.15); + misc_printf(" | %10.3f s | 0x%04X \n\r", p.dataTime, p.status); + return 1; + } + else + { + misc_printf("%s\n\r", name); + } + return true; } diff --git a/boards/varmint/src/board/Bmi088.cpp b/boards/varmint/src/board/Bmi088.cpp index 9ea9ced3..b0effe4e 100644 --- a/boards/varmint/src/board/Bmi088.cpp +++ b/boards/varmint/src/board/Bmi088.cpp @@ -36,319 +36,370 @@ **/ #include -#include -#include #include +#include +#include #include -#define SPI_READ (uint8_t)0x80 +#define SPI_READ (uint8_t)0x80 #define SPI_WRITE (uint8_t)0x00 -#define BMI_ACCEL_BYTES 20 // read bytes + 2 -#define BMI_ACCEL_SYNC_BYTES 4 // read bytes + 2 -#define BMI_GYRO_BYTES 7 // read bytes + 1 -#define BMI_PAUSE_US 2 // us between spi transactions +#define BMI_ACCEL_BYTES 20 // read bytes + 2 +#define BMI_ACCEL_SYNC_BYTES 4 // read bytes + 2 +#define BMI_GYRO_BYTES 7 // read bytes + 1 +#define BMI_PAUSE_US 2 // us between spi transactions -#define BMI_ACCEL_CMD (0x12|SPI_READ) -#define BMI_ACCEL_SYNC_CMD (0x27|SPI_READ) -#define BMI_GYRO_CMD (0x02|SPI_READ) +#define BMI_ACCEL_CMD (0x12 | SPI_READ) +#define BMI_ACCEL_SYNC_CMD (0x27 | SPI_READ) +#define BMI_GYRO_CMD (0x02 | SPI_READ) extern Time64 time64; -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t bmi088_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE]={0}; -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t bmi088_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE]={0}; - -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t bmi088_fifo_rx_buffer[BMI088_FIFO_BUFFERS*sizeof(ImuPacket)]={0}; - -uint32_t Bmi088::init -( - // Driver initializers - uint16_t sample_rate_hz, - GPIO_TypeDef *drdy_port, // DRDY GPIO Port - uint16_t drdy_pin, // DRDY GPIO Pin - // SPI initializers - SPI_HandleTypeDef *hspi, - GPIO_TypeDef *cs_port_a, // Chip Select GPIO Port - uint16_t cs_pin_a, // Chip Select GPIO Pin - GPIO_TypeDef *cs_port_g, // Chip Select GPIO Port - uint16_t cs_pin_g, // Chip Select GPIO Pin - // Sensor Specific - uint8_t range_a, // 0,1,2,3 --> 3,6,12,24g - uint8_t range_g // 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t bmi088_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE] = {0}; +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t bmi088_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE] = {0}; + +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t bmi088_fifo_rx_buffer[BMI088_FIFO_BUFFERS * sizeof(ImuPacket)] = {0}; + +uint32_t Bmi088::init( + // Driver initializers + uint16_t sample_rate_hz, + GPIO_TypeDef *drdy_port, // DRDY GPIO Port + uint16_t drdy_pin, // DRDY GPIO Pin + // SPI initializers + SPI_HandleTypeDef *hspi, + GPIO_TypeDef *cs_port_a, // Chip Select GPIO Port + uint16_t cs_pin_a, // Chip Select GPIO Pin + GPIO_TypeDef *cs_port_g, // Chip Select GPIO Port + uint16_t cs_pin_g, // Chip Select GPIO Pin + // Sensor Specific + uint8_t range_a, // 0,1,2,3 --> 3,6,12,24g + uint8_t range_g // 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s ) { - uint32_t status = DRIVER_OK; - sampleRateHz_ = sample_rate_hz; - drdyPort_ = drdy_port; - drdyPin_ = drdy_pin; - - spiA_.init(hspi,bmi088_dma_txbuf,bmi088_dma_rxbuf,cs_port_a,cs_pin_a); - spiG_.init(hspi,bmi088_dma_txbuf,bmi088_dma_rxbuf,cs_port_g,cs_pin_g); - - seqCount_ = 0; - timeoutMs_ = 1000; - rangeA_ = range_a; - rangeG_ = range_g; - - rxFifo_.init(BMI088_FIFO_BUFFERS, sizeof(ImuPacket), bmi088_fifo_rx_buffer); - - if( sampleRateHz_ <= 400) {sampleRateHz_ = 400; syncCfgMode_ = BMI08_ACCEL_DATA_SYNC_MODE_400HZ; groupDelay_ = 7000;} - else if( sampleRateHz_ <= 1000) {sampleRateHz_ = 1000; syncCfgMode_ = BMI08_ACCEL_DATA_SYNC_MODE_1000HZ; groupDelay_ = 2500;} - else if( sampleRateHz_ <= 2000) {sampleRateHz_ = 2000; syncCfgMode_ = BMI08_ACCEL_DATA_SYNC_MODE_2000HZ; groupDelay_ = 1500;} - else {sampleRateHz_ = 400; syncCfgMode_ = BMI08_ACCEL_DATA_SYNC_MODE_400HZ; groupDelay_ = 7000;} - - HAL_GPIO_WritePin(spiG_.port_, spiG_.pin_, GPIO_PIN_SET); - HAL_GPIO_WritePin(spiA_.port_, spiA_.pin_, GPIO_PIN_SET); - time64.dUs(100); - - // Lock into SPI Mode - HAL_GPIO_WritePin(spiA_.port_, spiA_.pin_, GPIO_PIN_RESET); - time64.dUs(25); - HAL_GPIO_WritePin(spiA_.port_, spiA_.pin_, GPIO_PIN_SET); - time64.dUs(4); - - // Check Accel ID (0x80) - uint8_t accel_id = readRegisterA(BMI08_REG_ACCEL_CHIP_ID); - misc_printf("BMI088 Accel ID = 0x%02X (0x1E) - ",accel_id); - if( accel_id==0x1E) misc_printf("OK\n\r"); else {misc_printf("FAIL\n\r"); status |= DRIVER_ID_MISMATCH;} - - // Check Gyro ID (0x80) - uint8_t gyro_id = readRegisterG(BMI08_REG_GYRO_CHIP_ID); - misc_printf("BMI088 Gyro ID = 0x%02X (0x0F) - ",gyro_id); - if(gyro_id == 0x0F) misc_printf("OK\n\r"); else {misc_printf("FAIL\n\r");status |= DRIVER_ID_MISMATCH;} - - // Accel Soft Reset (0x7E, 0xB6) - writeRegisterA(BMI08_REG_ACCEL_SOFTRESET, 0xB6); - time64.dUs(1000); // required - - // Lock into SPI Mode (again after rest) - HAL_GPIO_WritePin(spiA_.port_, spiA_.pin_, GPIO_PIN_RESET); - time64.dUs(25); - HAL_GPIO_WritePin(spiA_.port_, spiA_.pin_, GPIO_PIN_SET); - - // Why is this here in the Bosh examples? - // time64.dUs(4900); - - // Activate Accel (0x7C,0x00) - writeRegisterA(BMI08_REG_ACCEL_PWR_CONF, 0x00); // 0x00 active, 0x03 suspend. - time64.dUs(450); // do we need this delay?? - - // Disable "Config Loading" (0x59, 0x00) - writeRegisterA(BMI08_REG_ACCEL_INIT_CTRL,0X00); - - #define BLOCK_SIZE 32 - for(uint8_t block = 0;block<0xC0;block++) - { - writeRegisterA(BMI08_REG_ACCEL_RESERVED_5B,0X00); // (0x5B) - writeRegisterA(BMI08_REG_ACCEL_RESERVED_5C,block); // (0x5C) - - uint8_t tx[BLOCK_SIZE+1]; - tx[0] = BMI08_REG_ACCEL_FEATURE_CFG|SPI_WRITE; // (0x5E, data) - memcpy(tx+1,bmi_config + block*BLOCK_SIZE,BLOCK_SIZE); - spiA_.tx(tx,sizeof(tx),1000); - } - // Re-enable "Config Loading" - writeRegisterA(BMI08_REG_ACCEL_INIT_CTRL,0X01); // (0x59, 0x01) - time64.dUs(150000); - - // Unknown status read (0xAA) -// uint8_t stat_a = readRegisterA(BMI08_REG_ACCEL_INTERNAL_STAT); - readRegisterA(BMI08_REG_ACCEL_INTERNAL_STAT); - // check something here?? - - // Activate Accel, again (0x7C,0x00) - writeRegisterA(BMI08_REG_ACCEL_PWR_CONF,0X00); - //time64.dUs(5000); // could this be 450us like above??? - time64.dUs(450); // do we need this delay?? - - // Accelerometer ON (0x7D,0x04) - writeRegisterA(BMI08_REG_ACCEL_PWR_CTRL,0X04); - time64.dUs(100); - - // Read/Write Low Power Mode - readRegisterG(BMI08_REG_GYRO_LPM1); // (0x91) - writeRegisterG(BMI08_REG_GYRO_LPM1,0x00); // (0x11,0x00) Set normal mode - time64.dUs(30000); - - // Read/Write Gyro Bandwidth - uint8_t GyroBW; - if(sampleRateHz_>=2000) GyroBW = BMI08_GYRO_BW_230_ODR_2000_HZ; - else if(sampleRateHz_>=1000) GyroBW = BMI08_GYRO_BW_116_ODR_1000_HZ; - else GyroBW = BMI08_GYRO_BW_47_ODR_400_HZ; - - readRegisterG(BMI08_REG_GYRO_BANDWIDTH); //(0x90) - writeRegisterG(BMI08_REG_GYRO_BANDWIDTH,GyroBW); //0x10,0x83) - - // Read/Write Gyro Range - readRegisterG(BMI08_REG_GYRO_RANGE); //(0x8F) - writeRegisterG(BMI08_REG_GYRO_RANGE,rangeG_); // (0x0F,0x02) - time64.dUs(10000); // do we need this delay?? - - // Read/Write BMI08_REG_ACCEL_FEATURE_CFG 7+1 bytes - // This is undocumented, mimic Bosh code - { - uint8_t tx[8]={0}; - uint8_t sync_cfg[8]={0}; - tx[0] = BMI08_REG_ACCEL_FEATURE_CFG|SPI_READ; // (0xDE) - spiA_.rx(tx,sync_cfg,8,1000); - sync_cfg[1] = BMI08_REG_ACCEL_FEATURE_CFG|SPI_WRITE; // (0x5E) - uint16_t reg_data = (syncCfgMode_ & BMI08_ACCEL_DATA_SYNC_MODE_MASK); - sync_cfg[6] = reg_data & 0xFF; // low byte - sync_cfg[7] = reg_data >> 8; // high byte - spiA_.tx(sync_cfg+1,7,1000); - } - time64.dUs(100000); // delay of 100ms for data sync to take effect. - - readRegisterA(BMI08_REG_ACCEL_INT2_IO_CONF); //(0xD4) - writeRegisterA(BMI08_REG_ACCEL_INT2_IO_CONF,0x13); //(0x54, 0x13) Int2 as input, active high, reserved[0] = 1 - writeRegisterA(BMI08_REG_ACCEL_INT1_MAP,0x01); // (0x56,0x01) undocumented. - - readRegisterA(BMI08_REG_ACCEL_INT1_IO_CONF); // (0xD3) - writeRegisterA(BMI08_REG_ACCEL_INT1_IO_CONF, 0x0A);// (0x53,0x0A) Int1 as output, active high - -// readRegisterA(BMI08_REG_ACCEL_CONF); // (0x40) -// writeRegisterA(BMI08_REG_ACCEL_CONF, rangeA_);// (0x53,0x0A) Int1 as output, active high - - // why is this sequence here twice?? - readRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_MAP); // (0x98) - writeRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_MAP, 0x81); //(0x18,0x80) 0x81 to map drdy to goth int3 and int4 - readRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_CONF); // (0x96) - writeRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_CONF, 0x05); //(0x16,0x05), both push-pull and active high - writeRegisterG(BMI08_REG_GYRO_INT_CTRL, 0x80); //(0x15,0x80) Enable drdy interrupt on new data - - // why is this sequence here twice?? - readRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_MAP); // (0x98) - writeRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_MAP, 0x81); //(0x18,0x80) - readRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_CONF); // (0x96) - writeRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_CONF, 0x05); //(0x16,0x05) - writeRegisterG(BMI08_REG_GYRO_INT_CTRL, 0x80); //(0x15,0x80) - - return status; + uint32_t status = DRIVER_OK; + sampleRateHz_ = sample_rate_hz; + drdyPort_ = drdy_port; + drdyPin_ = drdy_pin; + + spiA_.init(hspi, bmi088_dma_txbuf, bmi088_dma_rxbuf, cs_port_a, cs_pin_a); + spiG_.init(hspi, bmi088_dma_txbuf, bmi088_dma_rxbuf, cs_port_g, cs_pin_g); + + seqCount_ = 0; + timeoutMs_ = 1000; + rangeA_ = range_a; + rangeG_ = range_g; + + rxFifo_.init(BMI088_FIFO_BUFFERS, sizeof(ImuPacket), bmi088_fifo_rx_buffer); + + if (sampleRateHz_ <= 400) + { + sampleRateHz_ = 400; + syncCfgMode_ = BMI08_ACCEL_DATA_SYNC_MODE_400HZ; + groupDelay_ = 7000; + } + else if (sampleRateHz_ <= 1000) + { + sampleRateHz_ = 1000; + syncCfgMode_ = BMI08_ACCEL_DATA_SYNC_MODE_1000HZ; + groupDelay_ = 2500; + } + else if (sampleRateHz_ <= 2000) + { + sampleRateHz_ = 2000; + syncCfgMode_ = BMI08_ACCEL_DATA_SYNC_MODE_2000HZ; + groupDelay_ = 1500; + } + else + { + sampleRateHz_ = 400; + syncCfgMode_ = BMI08_ACCEL_DATA_SYNC_MODE_400HZ; + groupDelay_ = 7000; + } + + HAL_GPIO_WritePin(spiG_.port_, spiG_.pin_, GPIO_PIN_SET); + HAL_GPIO_WritePin(spiA_.port_, spiA_.pin_, GPIO_PIN_SET); + time64.dUs(100); + + // Lock into SPI Mode + HAL_GPIO_WritePin(spiA_.port_, spiA_.pin_, GPIO_PIN_RESET); + time64.dUs(25); + HAL_GPIO_WritePin(spiA_.port_, spiA_.pin_, GPIO_PIN_SET); + time64.dUs(4); + + // Check Accel ID (0x80) + uint8_t accel_id = readRegisterA(BMI08_REG_ACCEL_CHIP_ID); + misc_printf("BMI088 Accel ID = 0x%02X (0x1E) - ", accel_id); + if (accel_id == 0x1E) + misc_printf("OK\n\r"); + else + { + misc_printf("FAIL\n\r"); + status |= DRIVER_ID_MISMATCH; + } + + // Check Gyro ID (0x80) + uint8_t gyro_id = readRegisterG(BMI08_REG_GYRO_CHIP_ID); + misc_printf("BMI088 Gyro ID = 0x%02X (0x0F) - ", gyro_id); + if (gyro_id == 0x0F) + misc_printf("OK\n\r"); + else + { + misc_printf("FAIL\n\r"); + status |= DRIVER_ID_MISMATCH; + } + + // Accel Soft Reset (0x7E, 0xB6) + writeRegisterA(BMI08_REG_ACCEL_SOFTRESET, 0xB6); + time64.dUs(1000); // required + + // Lock into SPI Mode (again after rest) + HAL_GPIO_WritePin(spiA_.port_, spiA_.pin_, GPIO_PIN_RESET); + time64.dUs(25); + HAL_GPIO_WritePin(spiA_.port_, spiA_.pin_, GPIO_PIN_SET); + + // Why is this here in the Bosh examples? + // time64.dUs(4900); + + // Activate Accel (0x7C,0x00) + writeRegisterA(BMI08_REG_ACCEL_PWR_CONF, 0x00); // 0x00 active, 0x03 suspend. + time64.dUs(450); // do we need this delay?? + + // Disable "Config Loading" (0x59, 0x00) + writeRegisterA(BMI08_REG_ACCEL_INIT_CTRL, 0X00); + +#define BLOCK_SIZE 32 + for (uint8_t block = 0; block < 0xC0; block++) + { + writeRegisterA(BMI08_REG_ACCEL_RESERVED_5B, 0X00); // (0x5B) + writeRegisterA(BMI08_REG_ACCEL_RESERVED_5C, block); // (0x5C) + + uint8_t tx[BLOCK_SIZE + 1]; + tx[0] = BMI08_REG_ACCEL_FEATURE_CFG | SPI_WRITE; // (0x5E, data) + memcpy(tx + 1, bmi_config + block * BLOCK_SIZE, BLOCK_SIZE); + spiA_.tx(tx, sizeof(tx), 1000); + } + // Re-enable "Config Loading" + writeRegisterA(BMI08_REG_ACCEL_INIT_CTRL, 0X01); // (0x59, 0x01) + time64.dUs(150000); + + // Unknown status read (0xAA) + // uint8_t stat_a = readRegisterA(BMI08_REG_ACCEL_INTERNAL_STAT); + readRegisterA(BMI08_REG_ACCEL_INTERNAL_STAT); + // check something here?? + + // Activate Accel, again (0x7C,0x00) + writeRegisterA(BMI08_REG_ACCEL_PWR_CONF, 0X00); + // time64.dUs(5000); // could this be 450us like above??? + time64.dUs(450); // do we need this delay?? + + // Accelerometer ON (0x7D,0x04) + writeRegisterA(BMI08_REG_ACCEL_PWR_CTRL, 0X04); + time64.dUs(100); + + // Read/Write Low Power Mode + readRegisterG(BMI08_REG_GYRO_LPM1); // (0x91) + writeRegisterG(BMI08_REG_GYRO_LPM1, 0x00); // (0x11,0x00) Set normal mode + time64.dUs(30000); + + // Read/Write Gyro Bandwidth + uint8_t GyroBW; + if (sampleRateHz_ >= 2000) + GyroBW = BMI08_GYRO_BW_230_ODR_2000_HZ; + else if (sampleRateHz_ >= 1000) + GyroBW = BMI08_GYRO_BW_116_ODR_1000_HZ; + else + GyroBW = BMI08_GYRO_BW_47_ODR_400_HZ; + + readRegisterG(BMI08_REG_GYRO_BANDWIDTH); //(0x90) + writeRegisterG(BMI08_REG_GYRO_BANDWIDTH, GyroBW); // 0x10,0x83) + + // Read/Write Gyro Range + readRegisterG(BMI08_REG_GYRO_RANGE); //(0x8F) + writeRegisterG(BMI08_REG_GYRO_RANGE, rangeG_); // (0x0F,0x02) + time64.dUs(10000); // do we need this delay?? + + // Read/Write BMI08_REG_ACCEL_FEATURE_CFG 7+1 bytes + // This is undocumented, mimic Bosh code + { + uint8_t tx[8] = {0}; + uint8_t sync_cfg[8] = {0}; + tx[0] = BMI08_REG_ACCEL_FEATURE_CFG | SPI_READ; // (0xDE) + spiA_.rx(tx, sync_cfg, 8, 1000); + sync_cfg[1] = BMI08_REG_ACCEL_FEATURE_CFG | SPI_WRITE; // (0x5E) + uint16_t reg_data = (syncCfgMode_ & BMI08_ACCEL_DATA_SYNC_MODE_MASK); + sync_cfg[6] = reg_data & 0xFF; // low byte + sync_cfg[7] = reg_data >> 8; // high byte + spiA_.tx(sync_cfg + 1, 7, 1000); + } + time64.dUs(100000); // delay of 100ms for data sync to take effect. + + readRegisterA(BMI08_REG_ACCEL_INT2_IO_CONF); //(0xD4) + writeRegisterA(BMI08_REG_ACCEL_INT2_IO_CONF, 0x13); //(0x54, 0x13) Int2 as input, active high, reserved[0] = 1 + writeRegisterA(BMI08_REG_ACCEL_INT1_MAP, 0x01); // (0x56,0x01) undocumented. + + readRegisterA(BMI08_REG_ACCEL_INT1_IO_CONF); // (0xD3) + writeRegisterA(BMI08_REG_ACCEL_INT1_IO_CONF, 0x0A); // (0x53,0x0A) Int1 as output, active high + + // readRegisterA(BMI08_REG_ACCEL_CONF); // (0x40) + // writeRegisterA(BMI08_REG_ACCEL_CONF, rangeA_);// (0x53,0x0A) Int1 as output, active high + + // why is this sequence here twice?? + readRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_MAP); // (0x98) + writeRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_MAP, 0x81); //(0x18,0x80) 0x81 to map drdy to goth int3 and int4 + readRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_CONF); // (0x96) + writeRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_CONF, 0x05); //(0x16,0x05), both push-pull and active high + writeRegisterG(BMI08_REG_GYRO_INT_CTRL, 0x80); //(0x15,0x80) Enable drdy interrupt on new data + + // why is this sequence here twice?? + readRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_MAP); // (0x98) + writeRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_MAP, 0x81); //(0x18,0x80) + readRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_CONF); // (0x96) + writeRegisterG(BMI08_REG_GYRO_INT3_INT4_IO_CONF, 0x05); //(0x16,0x05) + writeRegisterG(BMI08_REG_GYRO_INT_CTRL, 0x80); //(0x15,0x80) + + return status; } bool Bmi088::startDma(void) { - drdy_ = time64.Us(); - HAL_StatusTypeDef hal_status = spiA_.startDma(BMI_ACCEL_CMD, BMI_ACCEL_BYTES); - if(hal_status==HAL_OK) seqCount_ = 1; - return hal_status == HAL_OK; + drdy_ = time64.Us(); + HAL_StatusTypeDef hal_status = spiA_.startDma(BMI_ACCEL_CMD, BMI_ACCEL_BYTES); + if (hal_status == HAL_OK) + seqCount_ = 1; + return hal_status == HAL_OK; } void Bmi088::endDma(void) { - static ImuPacket p; - - double scale_factor; - if (seqCount_ == 1) - { - memset(&p,0,sizeof(p)); - uint8_t *rx = spiA_.endDma(); - double accel_range = 3.0*(double)((0x0001)<<(rangeA_)); // G's - scale_factor = (double)9.80665*accel_range/(double)(32768L)/4.; // m/s^2 - - - p.dataTime = (double)(39.0625e-6*(double)((uint32_t)rx[8] | (uint32_t)rx[9]<<8 | (uint32_t)rx[10]<<16)); - p.temperature = (double)((int16_t)rx[18]<<3 | (((int16_t)rx[19]>>6)&0x0003)); - if (p.temperature>1023) p.temperature-=2048.0; - p.temperature *= 0.125; - p.temperature += 23+273.15; // K - - int16_t data; - data = (int16_t)rx[15]<<8 |(int16_t)rx[14]; p.accel[0] = -scale_factor*(double)data; - data = (int16_t)rx[17]<<8 |(int16_t)rx[16]; p.accel[1] = -scale_factor*(double)data; - - // Launch the Accel read for az - HAL_StatusTypeDef hal_status = spiA_.startDma(BMI_ACCEL_SYNC_CMD, BMI_ACCEL_SYNC_BYTES); - if(hal_status == HAL_OK) seqCount_= 2; else seqCount_ = 0; - } - else if (seqCount_ == 2) - { - uint8_t *rx = spiA_.endDma(); - - double accel_range = 3.0*(double)((0x0001)<<(rangeA_)); // G's - scale_factor = (double)9.80665*accel_range/(double)(32768L)/4.; // m/s^2 - - int16_t az = (int16_t)rx[3]<<8 |(int16_t)rx[2]; - p.accel[2] = az*scale_factor; - - seqCount_=3; - HAL_StatusTypeDef hal_status = spiG_.startDma(BMI_GYRO_CMD, BMI_GYRO_BYTES); - if(hal_status == HAL_OK) seqCount_= 3; else seqCount_ = 0; - } - else if (seqCount_ == 3) - { - uint8_t *rx = spiG_.endDma(); - // _gyro_range = 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s - scale_factor = (double)1.0/8.192/(double)( 0x0001<<(rangeG_+1))*0.01745329252; // to rad/s - - int16_t data; - data = (int16_t)rx[2]<<8 |(int16_t)rx[1]; p.gyro[0] = -scale_factor*(double)data; - data = (int16_t)rx[4]<<8 |(int16_t)rx[3]; p.gyro[1] = -scale_factor*(double)data; - data = (int16_t)rx[6]<<8 |(int16_t)rx[5]; p.gyro[2] = scale_factor*(double)data; - - p.drdy = drdy_; - p.groupDelay = groupDelay_; - - p.timestamp = time64.Us(); - - rxFifo_.write((uint8_t*)&p,sizeof(p)); - - seqCount_ = 0; - } - else - { - seqCount_ = 0; - } + static ImuPacket p; + + double scale_factor; + if (seqCount_ == 1) + { + memset(&p, 0, sizeof(p)); + uint8_t *rx = spiA_.endDma(); + double accel_range = 3.0 * (double)((0x0001) << (rangeA_)); // G's + scale_factor = (double)9.80665 * accel_range / (double)(32768L) / 4.; // m/s^2 + + p.dataTime = (double)(39.0625e-6 * (double)((uint32_t)rx[8] | (uint32_t)rx[9] << 8 | (uint32_t)rx[10] << 16)); + p.temperature = (double)((int16_t)rx[18] << 3 | (((int16_t)rx[19] >> 6) & 0x0003)); + if (p.temperature > 1023) + p.temperature -= 2048.0; + p.temperature *= 0.125; + p.temperature += 23 + 273.15; // K + + int16_t data; + data = (int16_t)rx[15] << 8 | (int16_t)rx[14]; + p.accel[0] = -scale_factor * (double)data; + data = (int16_t)rx[17] << 8 | (int16_t)rx[16]; + p.accel[1] = -scale_factor * (double)data; + + // Launch the Accel read for az + HAL_StatusTypeDef hal_status = spiA_.startDma(BMI_ACCEL_SYNC_CMD, BMI_ACCEL_SYNC_BYTES); + if (hal_status == HAL_OK) + seqCount_ = 2; + else + seqCount_ = 0; + } + else if (seqCount_ == 2) + { + uint8_t *rx = spiA_.endDma(); + + double accel_range = 3.0 * (double)((0x0001) << (rangeA_)); // G's + scale_factor = (double)9.80665 * accel_range / (double)(32768L) / 4.; // m/s^2 + + int16_t az = (int16_t)rx[3] << 8 | (int16_t)rx[2]; + p.accel[2] = az * scale_factor; + + seqCount_ = 3; + HAL_StatusTypeDef hal_status = spiG_.startDma(BMI_GYRO_CMD, BMI_GYRO_BYTES); + if (hal_status == HAL_OK) + seqCount_ = 3; + else + seqCount_ = 0; + } + else if (seqCount_ == 3) + { + uint8_t *rx = spiG_.endDma(); + // _gyro_range = 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s + scale_factor = (double)1.0 / 8.192 / (double)(0x0001 << (rangeG_ + 1)) * 0.01745329252; // to rad/s + + int16_t data; + data = (int16_t)rx[2] << 8 | (int16_t)rx[1]; + p.gyro[0] = -scale_factor * (double)data; + data = (int16_t)rx[4] << 8 | (int16_t)rx[3]; + p.gyro[1] = -scale_factor * (double)data; + data = (int16_t)rx[6] << 8 | (int16_t)rx[5]; + p.gyro[2] = scale_factor * (double)data; + + p.drdy = drdy_; + p.groupDelay = groupDelay_; + + p.timestamp = time64.Us(); + + rxFifo_.write((uint8_t *)&p, sizeof(p)); + + seqCount_ = 0; + } + else + { + seqCount_ = 0; + } } -uint8_t Bmi088::readRegisterA(uint8_t reg) +uint8_t Bmi088::readRegisterA(uint8_t reg) { - uint8_t tx[3]={0}; tx[0] = reg|SPI_READ; - uint8_t rx[3]={0}; - spiA_.rx(tx, rx, 3, timeoutMs_); // Ignore status - return rx[2]; + uint8_t tx[3] = {0}; + tx[0] = reg | SPI_READ; + uint8_t rx[3] = {0}; + spiA_.rx(tx, rx, 3, timeoutMs_); // Ignore status + return rx[2]; } -uint8_t Bmi088::readRegisterG(uint8_t reg) +uint8_t Bmi088::readRegisterG(uint8_t reg) { - uint8_t tx[2]= {0,0}; tx[0] = reg|SPI_READ; - uint8_t rx[2]={0}; - spiG_.rx(tx, rx, 2, timeoutMs_); // Ignore status - return rx[1]; + uint8_t tx[2] = {0, 0}; + tx[0] = reg | SPI_READ; + uint8_t rx[2] = {0}; + spiG_.rx(tx, rx, 2, timeoutMs_); // Ignore status + return rx[1]; } void Bmi088::writeRegisterA(uint8_t reg, uint8_t data) { - uint8_t tx[2]; tx[0] = reg|SPI_WRITE; tx[1] = data; - spiA_.tx(tx, 2, timeoutMs_); + uint8_t tx[2]; + tx[0] = reg | SPI_WRITE; + tx[1] = data; + spiA_.tx(tx, 2, timeoutMs_); } void Bmi088::writeRegisterG(uint8_t reg, uint8_t data) { - uint8_t tx[2]; tx[0] = reg|SPI_WRITE; tx[1] = data; - spiG_.tx(tx, 2, timeoutMs_); + uint8_t tx[2]; + tx[0] = reg | SPI_WRITE; + tx[1] = data; + spiG_.tx(tx, 2, timeoutMs_); } bool Bmi088::display(void) { - ImuPacket p; - char name[] = "Bmi088 (imu1)"; - if(rxFifo_.readMostRecent((uint8_t*)&p,sizeof(p))) - { - misc_header(name,p.drdy, p.timestamp,p.groupDelay); - misc_printf("%10.3f %10.3f %10.3f g ", p.accel[0]/9.80665,p.accel[1]/9.80665,p.accel[2]/9.80665); - misc_printf(" | %10.3f %10.3f %10.3f deg/s", p.gyro[0]*57.2958,p.gyro[1]*57.2958,p.gyro[2]*57.2958); - misc_printf(" | %7.1f C", p.temperature-273.15); - misc_printf(" | %10.3f s\n\r", p.dataTime); - return 1; - } - else - { - misc_printf("%s\n\r",name); - } - return true; + ImuPacket p; + char name[] = "Bmi088 (imu1)"; + if (rxFifo_.readMostRecent((uint8_t *)&p, sizeof(p))) + { + misc_header(name, p.drdy, p.timestamp, p.groupDelay); + misc_printf("%10.3f %10.3f %10.3f g ", p.accel[0] / 9.80665, p.accel[1] / 9.80665, p.accel[2] / 9.80665); + misc_printf(" | %10.3f %10.3f %10.3f deg/s", p.gyro[0] * 57.2958, p.gyro[1] * 57.2958, p.gyro[2] * 57.2958); + misc_printf(" | %7.1f C", p.temperature - 273.15); + misc_printf(" | %10.3f s\n\r", p.dataTime); + return 1; + } + else + { + misc_printf("%s\n\r", name); + } + return true; } - - - - diff --git a/boards/varmint/src/board/Callbacks.cpp b/boards/varmint/src/board/Callbacks.cpp index 51d3d705..0bcafbe8 100644 --- a/boards/varmint/src/board/Callbacks.cpp +++ b/boards/varmint/src/board/Callbacks.cpp @@ -43,81 +43,96 @@ extern Time64 time64; #include - // High Rate Periodic Timer Interrupt Routine for Polling void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { - static uint64_t poll_counter=0; - poll_counter++; - - if (htim->Instance == POLL_HTIM_INSTANCE) // Filter out other timer interrupts. - { - varmint.baro_.poll(); // Check for data ready and start dma read - varmint.mag_.poll(); // Check for data ready and start dma read - varmint.pitot_.poll((uint16_t)(poll_counter%(POLLING_FREQ_HZ/PITOT_HZ))); // Periodic start, check for data ready, and start dma read - varmint.rc_.poll(); // Restart if dead - varmint.gps_.poll(); // Restart if dead - varmint.telem_.poll(); // Check for new data packet to tx - varmint.adc_.poll((uint16_t)(poll_counter%(POLLING_FREQ_HZ/ADC_HZ))); // Start dma read - varmint.vcp_.poll(); // Timeout - - // Blink Green LED at 1 Hz. - if(0== poll_counter%(POLLING_FREQ_HZ/2)) GRN_TOG; - } + static uint64_t poll_counter = 0; + poll_counter++; + + if (htim->Instance == POLL_HTIM_INSTANCE) // Filter out other timer interrupts. + { + varmint.baro_.poll(); // Check for data ready and start dma read + varmint.mag_.poll(); // Check for data ready and start dma read + varmint.pitot_.poll( + (uint16_t)(poll_counter + % (POLLING_FREQ_HZ / PITOT_HZ))); // Periodic start, check for data ready, and start dma read + varmint.rc_.poll(); // Restart if dead + varmint.gps_.poll(); // Restart if dead + varmint.telem_.poll(); // Check for new data packet to tx + varmint.adc_.poll((uint16_t)(poll_counter % (POLLING_FREQ_HZ / ADC_HZ))); // Start dma read + varmint.vcp_.poll(); // Timeout + + // Blink Green LED at 1 Hz. + if (0 == poll_counter % (POLLING_FREQ_HZ / 2)) + GRN_TOG; + } } // EXTI (Data Ready) Interrupts void HAL_GPIO_EXTI_Callback(uint16_t exti_pin) { - if( varmint.imu0_.isMy(exti_pin) ) varmint.imu0_.startDma(); - if( varmint.imu1_.isMy(exti_pin) ) varmint.imu1_.startDma(); - if( varmint.gps_.isMy(exti_pin) ) varmint.gps_.pps(time64.Us()); + if (varmint.imu0_.isMy(exti_pin)) + varmint.imu0_.startDma(); + if (varmint.imu1_.isMy(exti_pin)) + varmint.imu1_.startDma(); + if (varmint.gps_.isMy(exti_pin)) + varmint.gps_.pps(time64.Us()); } // SPI Rx complete callback -void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) // All spi dma rx interrupts are handled here. +void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) // All spi dma rx interrupts are handled here. { - if(varmint.imu0_.isMy(hspi)) varmint.imu0_.endDma(); - else if(varmint.imu1_.isMy(hspi)) varmint.imu1_.endDma(); - else if(varmint.baro_.isMy(hspi)) varmint.baro_.endDma(); - else if(varmint.mag_.isMy(hspi)) varmint.mag_.endDma(); + if (varmint.imu0_.isMy(hspi)) + varmint.imu0_.endDma(); + else if (varmint.imu1_.isMy(hspi)) + varmint.imu1_.endDma(); + else if (varmint.baro_.isMy(hspi)) + varmint.baro_.endDma(); + else if (varmint.mag_.isMy(hspi)) + varmint.mag_.endDma(); } // I2C Rx complete callback void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *hi2c) { - if(varmint.pitot_.isMy(hi2c)) varmint.pitot_.endDma(); + if (varmint.pitot_.isMy(hi2c)) + varmint.pitot_.endDma(); } // UART Rx complete callbacks void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { - if(varmint.rc_.isMy(huart)) varmint.rc_.endDma(); - if(varmint.gps_.isMy(huart)) varmint.gps_.endDma(); + if (varmint.rc_.isMy(huart)) + varmint.rc_.endDma(); + if (varmint.gps_.isMy(huart)) + varmint.gps_.endDma(); } void RxIsrCallback(UART_HandleTypeDef *huart) { - if(varmint.telem_.isMy(huart)) varmint.telem_.rxIsrCallback(huart); + if (varmint.telem_.isMy(huart)) + varmint.telem_.rxIsrCallback(huart); } void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) { - if(varmint.telem_.isMy(huart)) varmint.telem_.txStart(); + if (varmint.telem_.isMy(huart)) + varmint.telem_.txStart(); } // ADC Rx complete callback void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc) { - if(varmint.adc_.isMy(hadc)) varmint.adc_.endDma(); + if (varmint.adc_.isMy(hadc)) + varmint.adc_.endDma(); } -void CDC_Receive_FS_Callback(uint8_t* buffer, uint16_t size) +void CDC_Receive_FS_Callback(uint8_t *buffer, uint16_t size) { - varmint.vcp_.rxCdcCallback(buffer, size); + varmint.vcp_.rxCdcCallback(buffer, size); } -void CDC_TransmitCplt_FS_Callback(uint8_t* buffer, uint16_t size) +void CDC_TransmitCplt_FS_Callback(uint8_t *buffer, uint16_t size) { - varmint.vcp_.txCdcCallback(); + varmint.vcp_.txCdcCallback(); } diff --git a/boards/varmint/src/board/DlhrL20G.cpp b/boards/varmint/src/board/DlhrL20G.cpp index 03c87850..79ea5180 100644 --- a/boards/varmint/src/board/DlhrL20G.cpp +++ b/boards/varmint/src/board/DlhrL20G.cpp @@ -34,141 +34,156 @@ * ****************************************************************************** **/ -#include #include +#include extern Time64 time64; -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t dlhr_i2c_dma_buf[I2C_DMA_MAX_BUFFER_SIZE]={0}; +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t dlhr_i2c_dma_buf[I2C_DMA_MAX_BUFFER_SIZE] = {0}; -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t dlhr_fifo_rx_buffer[DLHRL20G_FIFO_BUFFERS*sizeof(PitotPacket)]={0}; +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t dlhr_fifo_rx_buffer[DLHRL20G_FIFO_BUFFERS * sizeof(PitotPacket)] = {0}; #define DLHR_I2C_STATUS_SIZE 1 #define DLHR_I2C_DMA_SIZE 7 -uint32_t DlhrL20G::init -( - // Driver initializers - uint16_t sample_rate_hz, - GPIO_TypeDef *drdy_port, // Reset GPIO Port - uint16_t drdy_pin, // Reset GPIO Pin - // I2C initializers - I2C_HandleTypeDef *hi2c, // The SPI handle - uint16_t i2c_address // Chip select Port +uint32_t DlhrL20G::init( + // Driver initializers + uint16_t sample_rate_hz, + GPIO_TypeDef *drdy_port, // Reset GPIO Port + uint16_t drdy_pin, // Reset GPIO Pin + // I2C initializers + I2C_HandleTypeDef *hi2c, // The SPI handle + uint16_t i2c_address // Chip select Port ) { - uint32_t status = DRIVER_OK; - sampleRateHz_ = sample_rate_hz; - drdyPort_ = drdy_port; - drdyPin_ = drdy_pin; - - hi2c_ = hi2c; - address_ = i2c_address<<1; - launchUs_ = 0; - // groupDelay_ = 0; //Computed later based on launchUs_ and drdy_ timestamps. - - rxFifo_.init(DLHRL20G_FIFO_BUFFERS, sizeof(PitotPacket), dlhr_fifo_rx_buffer); - - dtMs_ = 1000./(double)sampleRateHz_; - - if(dtMs_<=8.0) cmdByte_ = 0xAA; - else if(dtMs_<= 15.7) cmdByte_ = 0xAC; - else if(dtMs_<= 31.1) cmdByte_ = 0xAD; - else if(dtMs_<= 61.9) cmdByte_ = 0xAE; - else cmdByte_ = 0xAF; - - // Read the status register - dlhr_i2c_dma_buf[0] = cmdByte_; - dlhr_i2c_dma_buf[1] = 0x00; - uint8_t sensor_status; - - HAL_I2C_Master_Receive(hi2c_, address_, &sensor_status, 1,1000); //Receive 7 bytes of data over I2C - - misc_printf("DLHRL20G Status = 0x%02X (0x40) - ", sensor_status); - if(sensor_status==0x40) misc_printf("OK\n\r"); - else - { misc_printf("ERROR\n\r"); - status |= DRIVER_SELF_DIAG_ERROR;} - - return status; + uint32_t status = DRIVER_OK; + sampleRateHz_ = sample_rate_hz; + drdyPort_ = drdy_port; + drdyPin_ = drdy_pin; + + hi2c_ = hi2c; + address_ = i2c_address << 1; + launchUs_ = 0; + // groupDelay_ = 0; //Computed later based on launchUs_ and drdy_ timestamps. + + rxFifo_.init(DLHRL20G_FIFO_BUFFERS, sizeof(PitotPacket), dlhr_fifo_rx_buffer); + + dtMs_ = 1000. / (double)sampleRateHz_; + + if (dtMs_ <= 8.0) + cmdByte_ = 0xAA; + else if (dtMs_ <= 15.7) + cmdByte_ = 0xAC; + else if (dtMs_ <= 31.1) + cmdByte_ = 0xAD; + else if (dtMs_ <= 61.9) + cmdByte_ = 0xAE; + else + cmdByte_ = 0xAF; + + // Read the status register + dlhr_i2c_dma_buf[0] = cmdByte_; + dlhr_i2c_dma_buf[1] = 0x00; + uint8_t sensor_status; + + HAL_I2C_Master_Receive(hi2c_, address_, &sensor_status, 1, 1000); // Receive 7 bytes of data over I2C + + misc_printf("DLHRL20G Status = 0x%02X (0x40) - ", sensor_status); + if (sensor_status == 0x40) + misc_printf("OK\n\r"); + else + { + misc_printf("ERROR\n\r"); + status |= DRIVER_SELF_DIAG_ERROR; + } + + return status; } bool DlhrL20G::poll(uint16_t poll_offset) { - bool status = false; - static bool previous_drdy=0; - bool current_drdy = HAL_GPIO_ReadPin(drdyPort_, drdyPin_); - - if(poll_offset==0) // polled sensor measurement start - { - dlhr_i2c_dma_buf[0] = cmdByte_; - dlhr_i2c_dma_buf[1] = 0x00; - #if USE_D_CACHE_MANAGEMENT_FUNCTIONS - SCB_CleanDCache_by_Addr((uint32_t *)dlhr_i2c_dma_buf, 1) ; // push data from cache to SRAM - #endif - launchUs_ = time64.Us(); - status = (HAL_OK==HAL_I2C_Master_Transmit_DMA(hi2c_, address_, dlhr_i2c_dma_buf, 1)); - } - else if(!previous_drdy && current_drdy) // drdy triggers a read. - { - drdy_ = time64.Us(); - status = startDma(); - } - previous_drdy=current_drdy; - return status; + bool status = false; + static bool previous_drdy = 0; + bool current_drdy = HAL_GPIO_ReadPin(drdyPort_, drdyPin_); + + if (poll_offset == 0) // polled sensor measurement start + { + dlhr_i2c_dma_buf[0] = cmdByte_; + dlhr_i2c_dma_buf[1] = 0x00; +#if USE_D_CACHE_MANAGEMENT_FUNCTIONS + SCB_CleanDCache_by_Addr((uint32_t *)dlhr_i2c_dma_buf, 1); // push data from cache to SRAM +#endif + launchUs_ = time64.Us(); + status = (HAL_OK == HAL_I2C_Master_Transmit_DMA(hi2c_, address_, dlhr_i2c_dma_buf, 1)); + } + else if (!previous_drdy && current_drdy) // drdy triggers a read. + { + drdy_ = time64.Us(); + status = startDma(); + } + previous_drdy = current_drdy; + return status; } bool DlhrL20G::startDma(void) { - #if USE_D_CACHE_MANAGEMENT_FUNCTIONS - SCB_CleanDCache_by_Addr((uint32_t *)dlhr_i2c_dma_buf, DLHR_I2C_DMA_SIZE) ; // push data from cache to SRAM, unclear if this is needed. - #endif - return HAL_OK == HAL_I2C_Master_Receive_DMA(hi2c_, address_, dlhr_i2c_dma_buf, DLHR_I2C_DMA_SIZE); //Receive 7 bytes of data over I2C +#if USE_D_CACHE_MANAGEMENT_FUNCTIONS + SCB_CleanDCache_by_Addr((uint32_t *)dlhr_i2c_dma_buf, + DLHR_I2C_DMA_SIZE); // push data from cache to SRAM, unclear if this is needed. +#endif + return HAL_OK + == HAL_I2C_Master_Receive_DMA(hi2c_, address_, dlhr_i2c_dma_buf, + DLHR_I2C_DMA_SIZE); // Receive 7 bytes of data over I2C } void DlhrL20G::endDma(void) { - #if USE_D_CACHE_MANAGEMENT_FUNCTIONS - SCB_InvalidateDCache_by_Addr((uint32_t *)dlhr_i2c_dma_buf, DLHR_I2C_DMA_SIZE) ; // force fetch from SRAM - #endif - - PitotPacket p; - p.status = dlhr_i2c_dma_buf[0]; - if(p.status & 0x0040) - { - p.timestamp = time64.Us(); - p.drdy = drdy_; - p.groupDelay = (p.drdy-launchUs_)/2; - int32_t i_pressure = ( (uint32_t)dlhr_i2c_dma_buf[1]<<24 | (uint32_t)dlhr_i2c_dma_buf[2]<<16 | (uint32_t)dlhr_i2c_dma_buf[3]<<8 )>>8; - int32_t i_temperature = ( (uint32_t)dlhr_i2c_dma_buf[4]<<24 | (uint32_t)dlhr_i2c_dma_buf[5]<<16 | (uint32_t)dlhr_i2c_dma_buf[6]<<8 )>>8; - - double FS = 5000; //Pa - double OSdig = 0.1; // Offset percent of full scale. - - p.pressure = 1.25*FS*( (double)i_pressure/16777216.0 - OSdig ); // Pa - p.temperature = 125.0*(double)i_temperature/16777216.0 - 40.0+273.15; // K - - rxFifo_.write((uint8_t*)&p,sizeof(p)); - } +#if USE_D_CACHE_MANAGEMENT_FUNCTIONS + SCB_InvalidateDCache_by_Addr((uint32_t *)dlhr_i2c_dma_buf, DLHR_I2C_DMA_SIZE); // force fetch from SRAM +#endif + + PitotPacket p; + p.status = dlhr_i2c_dma_buf[0]; + if (p.status & 0x0040) + { + p.timestamp = time64.Us(); + p.drdy = drdy_; + p.groupDelay = (p.drdy - launchUs_) / 2; + int32_t i_pressure = + ((uint32_t)dlhr_i2c_dma_buf[1] << 24 | (uint32_t)dlhr_i2c_dma_buf[2] << 16 | (uint32_t)dlhr_i2c_dma_buf[3] << 8) + >> 8; + int32_t i_temperature = + ((uint32_t)dlhr_i2c_dma_buf[4] << 24 | (uint32_t)dlhr_i2c_dma_buf[5] << 16 | (uint32_t)dlhr_i2c_dma_buf[6] << 8) + >> 8; + + double FS = 5000; // Pa + double OSdig = 0.1; // Offset percent of full scale. + + p.pressure = 1.25 * FS * ((double)i_pressure / 16777216.0 - OSdig); // Pa + p.temperature = 125.0 * (double)i_temperature / 16777216.0 - 40.0 + 273.15; // K + + rxFifo_.write((uint8_t *)&p, sizeof(p)); + } } bool DlhrL20G::display(void) { - PitotPacket p; - char name[] = "DlhrL20G (pitot)"; - if(rxFifo_.readMostRecent((uint8_t*)&p,sizeof(p))) - { - misc_header(name,p.drdy, p.timestamp, p.groupDelay); - misc_printf("%10.3f Pa | | %7.1f C | | 0x%04X\n\r", - p.pressure, p.temperature-273.15, p.status); - return 1; - } - else - { - misc_printf("%s\n\r",name); - } return true; + PitotPacket p; + char name[] = "DlhrL20G (pitot)"; + if (rxFifo_.readMostRecent((uint8_t *)&p, sizeof(p))) + { + misc_header(name, p.drdy, p.timestamp, p.groupDelay); + misc_printf( + "%10.3f Pa | | %7.1f C | | " + "0x%04X\n\r", + p.pressure, p.temperature - 273.15, p.status); + return 1; + } + else + { + misc_printf("%s\n\r", name); + } + return true; } - - - - diff --git a/boards/varmint/src/board/Dps310.cpp b/boards/varmint/src/board/Dps310.cpp index b7903d41..d9f30bee 100644 --- a/boards/varmint/src/board/Dps310.cpp +++ b/boards/varmint/src/board/Dps310.cpp @@ -38,11 +38,11 @@ #include #include -#define SPI_WRITE ((uint8_t)0x00) -#define SPI_READ ((uint8_t)0x80) +#define SPI_WRITE ((uint8_t)0x00) +#define SPI_READ ((uint8_t)0x80) -#define DPS310_READ_CMD (0x00|SPI_READ) -#define DPS310_BUFFBYTES (12) +#define DPS310_READ_CMD (0x00 | SPI_READ) +#define DPS310_BUFFBYTES (12) #define KP 7864320.0 // 8x oversample #define KT 524288.0 // No oversampling @@ -54,249 +54,261 @@ extern Time64 time64; -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t dps310_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE]={0}; -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t dps310_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE]={0}; - -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t dps310_fifo_rx_buffer[DPS310_FIFO_BUFFERS*sizeof(BaroPacket)]={0}; +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t dps310_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE] = {0}; +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t dps310_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE] = {0}; +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t dps310_fifo_rx_buffer[DPS310_FIFO_BUFFERS * sizeof(BaroPacket)] = {0}; static int32_t Compliment(int32_t x, int16_t bits) { - if (x & ((int32_t)1 << (bits - 1))) - { - x -= (int32_t)1 << bits; - } - return x; + if (x & ((int32_t)1 << (bits - 1))) + { + x -= (int32_t)1 << bits; + } + return x; } -uint32_t Dps310::init -( - - // Driver initializers - uint16_t sample_rate_hz, - GPIO_TypeDef *drdy_port, // Reset GPIO Port - uint16_t drdy_pin, // Reset GPIO Pin - // SPI initializers - SPI_HandleTypeDef *hspi, - GPIO_TypeDef *cs_port, // Chip Select GPIO Port - uint16_t cs_pin // Chip Select GPIO Pin +uint32_t Dps310::init( + + // Driver initializers + uint16_t sample_rate_hz, + GPIO_TypeDef *drdy_port, // Reset GPIO Port + uint16_t drdy_pin, // Reset GPIO Pin + // SPI initializers + SPI_HandleTypeDef *hspi, + GPIO_TypeDef *cs_port, // Chip Select GPIO Port + uint16_t cs_pin // Chip Select GPIO Pin ) { - uint32_t status=DRIVER_OK; - - sampleRateHz_ = sample_rate_hz; - drdyPort_ = drdy_port; - drdyPin_ = drdy_pin; - - spi_.init(hspi,dps310_dma_txbuf,dps310_dma_rxbuf,cs_port,cs_pin); - - timeoutMs_ = 100; - groupDelay_ = 1000000/sampleRateHz_; - HAL_GPIO_WritePin(spi_.port_, spi_.pin_, GPIO_PIN_SET); - - rxFifo_.init(DPS310_FIFO_BUFFERS, sizeof(BaroPacket), dps310_fifo_rx_buffer); - - #define RESET 0x0C - writeRegister(RESET, 0x09); - HAL_Delay(40); - - // Set to 3-wire SPI mode so we can read registers. - // Interrupt and FIFO Config 0x09 - // 7 - 1, DRDY active high - // 6 - 0, Disable FIFO full interrupt - // 5 - 0, Int on temp - // 4 - 1, Int on pressure - // 3 - 0, no Temp data shift - // 2 - 0, no Press data shift - // 1 - 0, Disable FIFO - // 0 - 1, 3-wire SPI interface - #define CFG_REG 0x09 - writeRegister(CFG_REG,0x01); - - // Product ID 0x0D - #define PRODUCT_ID 0x0D - uint8_t product_id = readRegister(PRODUCT_ID); - misc_printf("DPS310: PRODUCT ID = 0x%02X (0x10) -",product_id); - if (product_id==0x10) misc_printf(" OK\n\r"); - else { status |= DRIVER_ID_MISMATCH; misc_printf(" Not OK\n\r");} - - // Calibration constants - #define MEAS_CFG 0x08 - uint8_t coef_rdy=readRegister(MEAS_CFG)&0x80; - - for (int n=0;n<10;n++) // Wait 10 times for Coefficients to be ready - { - coef_rdy = readRegister(MEAS_CFG)&0x80; -// misc_printf("DPS310: COEF_RDY = 0x%02X\n\r",coef_rdy); - if((coef_rdy&0x80) == 0x80 ) break; - time64.dUs(1000); - } - misc_printf("DPS310: COEF_RDY = 0x%02X (0x80) ",coef_rdy); - if ((coef_rdy&0x80)==0x80) misc_printf("- READY\n\r"); - else { misc_printf("- NOT READYn\n\r"); status|=DRIVER_SELF_DIAG_ERROR;} - - misc_printf("DPS310: Reading Coefficients\n\r"); - - // Read Calibration Constants - #define COEF_REG 0x10 - uint8_t tx[19] = {COEF_REG|SPI_READ, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; - uint8_t rx[19]; - - spi_.rx(tx, rx, 19, timeoutMs_); - - int32_t buf[18]; - for(int n=0;n<18;n++) buf[n] = rx[n+1]; - int32_t C0,C1,C01,C11,C20,C21,C30; // Calibration Constants - int32_t C00, C10; - - C0 = (buf[0] << 4) | ((buf[1] >> 4) & 0x0F); - C1 = ((buf[1] & 0x0F) << 8) | buf[2]; - C00 = (buf[3] << 12) | (buf[4] << 4) | ((buf[5] >> 4) & 0x0F); - C10 = ((buf[5] & 0x0F) << 16) | (buf[6] << 8) | buf[7]; - - C01 = (buf[8] << 8) | buf[9]; - C11 = (buf[10] << 8) | buf[11]; - C20 = (buf[12] << 8) | buf[13]; - C21 = (buf[14] << 8) | buf[15]; - C30 = (buf[16] << 8) | buf[17]; - - C0_ = Compliment(C0,12); - C1_ = Compliment(C1,12); - C00_ = Compliment(C00,20); - C10_ = Compliment(C10,20); - C01_ = Compliment(C01,16); - C11_ = Compliment(C11,16); - C20_ = Compliment(C20,16); - C21_ = Compliment(C21,16); - C30_ = Compliment(C30,16); - - misc_printf("DPS310: C0, C1 = %10.0f %10.0f\n\r", C0_, C1_); - misc_printf("DPS310: C00, C10 = %10.0f %10.0f\n\r", C00_, C10_); - misc_printf("DPS310: C01, C11 = %10.0f %10.0f\n\r", C01_, C11_); - misc_printf("DPS310: C20, C21 = %10.0f %10.0f\n\r", C20_, C21_); - misc_printf("DPS310: C30 = %10.0f\n\r", C30_); - - #define COEF_SRCE 0x28 - uint8_t temp_source = readRegister(COEF_SRCE)&0x80; - misc_printf("DPS310: temp source = 0x%02X\n\r",temp_source); - - #define PRS_CFG 0x06 // Pressure Configuration - // 64 measurements per second, 8x oversampling - writeRegister(PRS_CFG, 0x63); // write_reg(PRS_CFG, 0); - - #define TMP_CFG 0x07 // Temperature Configuration - // 64 measurements per second, no oversampling - writeRegister(TMP_CFG,temp_source|0x60); - - // Interrupt and FIFO Config 0x09 - // 7 - 1, DRDY active high - // 6 - 0, Disable FIFO full interrupt - // 5 - 0, Int on temp - // 4 - 1, Int on pressure - // 3 - 0, no Temp data shift - // 2 - 0, no Press data shift - // 1 - 0, Disable FIFO - // 0 - 1, 3-wire SPI interface - // 1011 0001 = 0xB1 active high - // 0011 0001 = 0x31 active low - // 1001 0001 = 0x91 - #define CFG_REG 0x09 - writeRegister(CFG_REG,0x91); - - // Measurement Configuration - // 7 - 0, read only - // 6 - 0, read only - // 5 - 0, read only - // 4 - 0, read only - // 3 - 0, reserved - // 2:0 - 111, pressure and temperature continuous mode - // 0000 0111 = 0x07 - #define MEAS_CFG 0x08 - writeRegister(MEAS_CFG,0x07); - - return status; + uint32_t status = DRIVER_OK; + + sampleRateHz_ = sample_rate_hz; + drdyPort_ = drdy_port; + drdyPin_ = drdy_pin; + + spi_.init(hspi, dps310_dma_txbuf, dps310_dma_rxbuf, cs_port, cs_pin); + + timeoutMs_ = 100; + groupDelay_ = 1000000 / sampleRateHz_; + HAL_GPIO_WritePin(spi_.port_, spi_.pin_, GPIO_PIN_SET); + + rxFifo_.init(DPS310_FIFO_BUFFERS, sizeof(BaroPacket), dps310_fifo_rx_buffer); + +#define RESET 0x0C + writeRegister(RESET, 0x09); + HAL_Delay(40); + +// Set to 3-wire SPI mode so we can read registers. +// Interrupt and FIFO Config 0x09 +// 7 - 1, DRDY active high +// 6 - 0, Disable FIFO full interrupt +// 5 - 0, Int on temp +// 4 - 1, Int on pressure +// 3 - 0, no Temp data shift +// 2 - 0, no Press data shift +// 1 - 0, Disable FIFO +// 0 - 1, 3-wire SPI interface +#define CFG_REG 0x09 + writeRegister(CFG_REG, 0x01); + +// Product ID 0x0D +#define PRODUCT_ID 0x0D + uint8_t product_id = readRegister(PRODUCT_ID); + misc_printf("DPS310: PRODUCT ID = 0x%02X (0x10) -", product_id); + if (product_id == 0x10) + misc_printf(" OK\n\r"); + else + { + status |= DRIVER_ID_MISMATCH; + misc_printf(" Not OK\n\r"); + } + +// Calibration constants +#define MEAS_CFG 0x08 + uint8_t coef_rdy = readRegister(MEAS_CFG) & 0x80; + + for (int n = 0; n < 10; n++) // Wait 10 times for Coefficients to be ready + { + coef_rdy = readRegister(MEAS_CFG) & 0x80; + // misc_printf("DPS310: COEF_RDY = 0x%02X\n\r",coef_rdy); + if ((coef_rdy & 0x80) == 0x80) + break; + time64.dUs(1000); + } + misc_printf("DPS310: COEF_RDY = 0x%02X (0x80) ", coef_rdy); + if ((coef_rdy & 0x80) == 0x80) + misc_printf("- READY\n\r"); + else + { + misc_printf("- NOT READYn\n\r"); + status |= DRIVER_SELF_DIAG_ERROR; + } + + misc_printf("DPS310: Reading Coefficients\n\r"); + +// Read Calibration Constants +#define COEF_REG 0x10 + uint8_t tx[19] = {COEF_REG | SPI_READ, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + uint8_t rx[19]; + + spi_.rx(tx, rx, 19, timeoutMs_); + + int32_t buf[18]; + for (int n = 0; n < 18; n++) buf[n] = rx[n + 1]; + int32_t C0, C1, C01, C11, C20, C21, C30; // Calibration Constants + int32_t C00, C10; + + C0 = (buf[0] << 4) | ((buf[1] >> 4) & 0x0F); + C1 = ((buf[1] & 0x0F) << 8) | buf[2]; + C00 = (buf[3] << 12) | (buf[4] << 4) | ((buf[5] >> 4) & 0x0F); + C10 = ((buf[5] & 0x0F) << 16) | (buf[6] << 8) | buf[7]; + + C01 = (buf[8] << 8) | buf[9]; + C11 = (buf[10] << 8) | buf[11]; + C20 = (buf[12] << 8) | buf[13]; + C21 = (buf[14] << 8) | buf[15]; + C30 = (buf[16] << 8) | buf[17]; + + C0_ = Compliment(C0, 12); + C1_ = Compliment(C1, 12); + C00_ = Compliment(C00, 20); + C10_ = Compliment(C10, 20); + C01_ = Compliment(C01, 16); + C11_ = Compliment(C11, 16); + C20_ = Compliment(C20, 16); + C21_ = Compliment(C21, 16); + C30_ = Compliment(C30, 16); + + misc_printf("DPS310: C0, C1 = %10.0f %10.0f\n\r", C0_, C1_); + misc_printf("DPS310: C00, C10 = %10.0f %10.0f\n\r", C00_, C10_); + misc_printf("DPS310: C01, C11 = %10.0f %10.0f\n\r", C01_, C11_); + misc_printf("DPS310: C20, C21 = %10.0f %10.0f\n\r", C20_, C21_); + misc_printf("DPS310: C30 = %10.0f\n\r", C30_); + +#define COEF_SRCE 0x28 + uint8_t temp_source = readRegister(COEF_SRCE) & 0x80; + misc_printf("DPS310: temp source = 0x%02X\n\r", temp_source); + +#define PRS_CFG 0x06 // Pressure Configuration + // 64 measurements per second, 8x oversampling + writeRegister(PRS_CFG, 0x63); // write_reg(PRS_CFG, 0); + +#define TMP_CFG 0x07 // Temperature Configuration + // 64 measurements per second, no oversampling + writeRegister(TMP_CFG, temp_source | 0x60); + +// Interrupt and FIFO Config 0x09 +// 7 - 1, DRDY active high +// 6 - 0, Disable FIFO full interrupt +// 5 - 0, Int on temp +// 4 - 1, Int on pressure +// 3 - 0, no Temp data shift +// 2 - 0, no Press data shift +// 1 - 0, Disable FIFO +// 0 - 1, 3-wire SPI interface +// 1011 0001 = 0xB1 active high +// 0011 0001 = 0x31 active low +// 1001 0001 = 0x91 +#define CFG_REG 0x09 + writeRegister(CFG_REG, 0x91); + +// Measurement Configuration +// 7 - 0, read only +// 6 - 0, read only +// 5 - 0, read only +// 4 - 0, read only +// 3 - 0, reserved +// 2:0 - 111, pressure and temperature continuous mode +// 0000 0111 = 0x07 +#define MEAS_CFG 0x08 + writeRegister(MEAS_CFG, 0x07); + + return status; } bool Dps310::poll(void) { - static bool previous_drdy=0; - bool status=true; - bool current_drdy = HAL_GPIO_ReadPin(drdyPort_, drdyPin_); - - // This sensor runs in continuous mode, so no action needed to instigate. - - if(!previous_drdy && current_drdy) - { - status = startDma(); - } - previous_drdy=current_drdy; - return status; + static bool previous_drdy = 0; + bool status = true; + bool current_drdy = HAL_GPIO_ReadPin(drdyPort_, drdyPin_); + + // This sensor runs in continuous mode, so no action needed to instigate. + + if (!previous_drdy && current_drdy) + { + status = startDma(); + } + previous_drdy = current_drdy; + return status; } -bool Dps310::startDma(void) //called to start dma read +bool Dps310::startDma(void) // called to start dma read { - drdy_ = time64.Us(); - HAL_StatusTypeDef hal_Status = HAL_OK; - hal_Status = spi_.startDma(DPS310_READ_CMD, DPS310_BUFFBYTES); - return hal_Status == HAL_OK; + drdy_ = time64.Us(); + HAL_StatusTypeDef hal_Status = HAL_OK; + hal_Status = spi_.startDma(DPS310_READ_CMD, DPS310_BUFFBYTES); + return hal_Status == HAL_OK; } void Dps310::endDma(void) { - uint8_t *rx = spi_.endDma(); + uint8_t *rx = spi_.endDma(); - BaroPacket p; + BaroPacket p; - p.drdy = drdy_; - p.groupDelay = groupDelay_; - p.status = (uint16_t)rx[9]; + p.drdy = drdy_; + p.groupDelay = groupDelay_; + p.status = (uint16_t)rx[9]; - int32_t traw = ((int32_t)rx[4]<<24 | (int32_t)rx[5]<<16 | (int32_t)rx[6]<<8)>>8; + int32_t traw = ((int32_t)rx[4] << 24 | (int32_t)rx[5] << 16 | (int32_t)rx[6] << 8) >> 8; - double Traw = (double)traw/KT; - p.temperature = C0_*0.5 + C1_*Traw + 273.15; //K + double Traw = (double)traw / KT; + p.temperature = C0_ * 0.5 + C1_ * Traw + 273.15; // K - int32_t praw = ((int32_t)rx[1]<<24 | (int32_t)rx[2]<<16 | (int32_t)rx[3]<<8)>>8; - double Praw = (double)praw/KP; - p.pressure = C00_ - + Praw * (C10_ + Praw * (C20_ + Praw *C30_)) - + Traw * (C01_ + Praw *(C11_ + Praw *C21_)); // Pa + int32_t praw = ((int32_t)rx[1] << 24 | (int32_t)rx[2] << 16 | (int32_t)rx[3] << 8) >> 8; + double Praw = (double)praw / KP; + p.pressure = C00_ + Praw * (C10_ + Praw * (C20_ + Praw * C30_)) + Traw * (C01_ + Praw * (C11_ + Praw * C21_)); // Pa - p.timestamp = time64.Us(); - rxFifo_.write((uint8_t*)&p,sizeof(p)); + p.timestamp = time64.Us(); + rxFifo_.write((uint8_t *)&p, sizeof(p)); } void Dps310::writeRegister(uint8_t address, uint8_t value) { - uint8_t tx[2]={0}; - tx[0] = (address) | SPI_WRITE; - tx[1] = value; - spi_.tx(tx,2,timeoutMs_); + uint8_t tx[2] = {0}; + tx[0] = (address) | SPI_WRITE; + tx[1] = value; + spi_.tx(tx, 2, timeoutMs_); } uint8_t Dps310::readRegister(uint8_t address) { - uint8_t tx[2]={0}; - uint8_t rx[2]={0}; - tx[0] = (address) | SPI_READ; - tx[1] = 0; - spi_.rx(tx,rx,2,timeoutMs_); - return rx[1]; + uint8_t tx[2] = {0}; + uint8_t rx[2] = {0}; + tx[0] = (address) | SPI_READ; + tx[1] = 0; + spi_.rx(tx, rx, 2, timeoutMs_); + return rx[1]; } bool Dps310::display(void) { - BaroPacket p; - char name[] = "Dps310 (baro)"; - if(rxFifo_.readMostRecent((uint8_t*)&p,sizeof(p))) - { - misc_header(name,p.drdy, p.timestamp, p.groupDelay); - misc_printf("%10.3f kPa | | %7.1f C | | 0x%04X\n\r", - p.pressure/1000., p.temperature-273.15, p.status); - return 1; - } - else - { - misc_printf("%s\n\r",name); - } - return 0; + BaroPacket p; + char name[] = "Dps310 (baro)"; + if (rxFifo_.readMostRecent((uint8_t *)&p, sizeof(p))) + { + misc_header(name, p.drdy, p.timestamp, p.groupDelay); + misc_printf( + "%10.3f kPa | | %7.1f C | | " + "0x%04X\n\r", + p.pressure / 1000., p.temperature - 273.15, p.status); + return 1; + } + else + { + misc_printf("%s\n\r", name); + } + return 0; } diff --git a/boards/varmint/src/board/Iis2mdc.cpp b/boards/varmint/src/board/Iis2mdc.cpp index 00e096a4..62eeef5a 100644 --- a/boards/varmint/src/board/Iis2mdc.cpp +++ b/boards/varmint/src/board/Iis2mdc.cpp @@ -38,234 +38,265 @@ #include #include -#define WHO_AM_I 0x4F -#define STATUS_REG 0x67 +#define WHO_AM_I 0x4F +#define STATUS_REG 0x67 -#define SPI_WRITE 0x00 -#define SPI_READ 0x80 +#define SPI_WRITE 0x00 +#define SPI_READ 0x80 -#define IIS_FLUX_CMD (0x67|SPI_READ) +#define IIS_FLUX_CMD (0x67 | SPI_READ) #define IIS_FLUX_BYTES 8 -#define IIS_TEMP_CMD (0x6E|SPI_READ) +#define IIS_TEMP_CMD (0x6E | SPI_READ) #define IIS_TEMP_BYTES 3 -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t iis2mdc_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE]={0}; -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t iis2mdc_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE]={0}; - -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t iis2mdc_fifo_rx_buffer[IIS2MDC_FIFO_BUFFERS*sizeof(MagPacket)]={0}; - -uint32_t Iis2mdc::init -( - // Driver initializers - uint16_t sample_rate_hz, - GPIO_TypeDef *drdy_port, // Reset GPIO Port - uint16_t drdy_pin, // Reset GPIO Pin - // SPI initializers - SPI_HandleTypeDef *hspi, - GPIO_TypeDef *cs_port, // Chip Select GPIO Port - uint16_t cs_pin // Chip Select GPIO Pin +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t iis2mdc_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE] = {0}; +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t iis2mdc_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE] = {0}; + +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t iis2mdc_fifo_rx_buffer[IIS2MDC_FIFO_BUFFERS * sizeof(MagPacket)] = {0}; + +uint32_t Iis2mdc::init( + // Driver initializers + uint16_t sample_rate_hz, + GPIO_TypeDef *drdy_port, // Reset GPIO Port + uint16_t drdy_pin, // Reset GPIO Pin + // SPI initializers + SPI_HandleTypeDef *hspi, + GPIO_TypeDef *cs_port, // Chip Select GPIO Port + uint16_t cs_pin // Chip Select GPIO Pin ) { - uint32_t status = DRIVER_OK; - sampleRateHz_ = sample_rate_hz; - drdyPort_ = drdy_port; - drdyPin_ = drdy_pin; - - spi_.init(hspi,iis2mdc_dma_txbuf,iis2mdc_dma_rxbuf,cs_port,cs_pin); - seqCount_ = 0; - - HAL_GPIO_WritePin(spi_.port_, spi_.pin_, GPIO_PIN_SET); - - rxFifo_.init(IIS2MDC_FIFO_BUFFERS, sizeof(MagPacket), iis2mdc_fifo_rx_buffer); - - uint8_t odr_mode=3; - if( sampleRateHz_ <= 10) {sampleRateHz_ = 10; odr_mode = 0; } - else if( sampleRateHz_ <= 20) {sampleRateHz_ = 20; odr_mode = 1; } - else if( sampleRateHz_ <= 50) {sampleRateHz_ = 50; odr_mode = 2; } - else if( sampleRateHz_ <= 100) {sampleRateHz_ = 100; odr_mode = 3; } - else {sampleRateHz_ = 100; odr_mode = 3; } - - groupDelay_ = 500000/sampleRateHz_; // Do something better if anyone cares. - - uint8_t id = readRegister(WHO_AM_I); - - misc_printf("Iis2mdc: WHO_AM_I = 0x%02X (0x40) - ",id); - if (id==0x40) misc_printf(" Matches\n\r"); - else { misc_printf(" Does not match\n\r"); status |= DRIVER_ID_MISMATCH; } - - // Reboot the sensor - // Register A (0x60) - // 7: = 0 COMP_TEMP_EN Temp comp enable - // 6: = X REBOOT - // 5: = X SOFT_RST - // 4: = 0 High resolution Mode (LP=0) - // 3:2 = 00 10 Hz Data Rate (ODR) - // 1:0 = 00 Continuous Mode - writeRegister(0x60,0x20); // soft reset - time64.dUs(10); // Wait at least 5 us - writeRegister(0x60,0x40); // reboot - time64.dMs(21); // wait at least 20 ms for reboot - - // Register A (0x60) - // 7: = 1 COMP_TEMP_EN Temp comp enable - // 6: = 0 REBOOT - // 5: = 0 SOFT_RST - // 4: = 0 High resolution Mode (High resolution = 0, Low power =1) - // - // 3:2 = 11 = 100 Hz Data Rate (ODR) - // 1:0 = 00 Continuous Mode, 01 = single mode - // write_register(0x60,0x81); // 1000 0001 = 0x81 For Single Acq -// writeRegister(0x60,0x8C); // 1000 1100 = 0x8C For 100 Hz. - writeRegister(0x60,0x80|(odr_mode<<2)); - -// Register B (0x61) - // [7:5] 000 - // [4] 1 OFF_CANC_ONE_SHOT 1=Offset Cancellation in single mode - // [3] 0 - // [2] 0 Set Freq of Set pulse to 63 ODR - // [1] 1, OFF_CANC 1= enable offset cancellation in single mode - // [0] 0 LPF disable offset filter (1- enabled) - // write_register(0x61,0x12); // 0001 0010 For Single - writeRegister(0x61,0x00); // 0000 0000 = 0x00 - - // Register C (0x62) - // 7: =0 Unused - // 6: =0 INT_on_PIN Enable event interrupts - // 5: =1 I2C_DIS (Disable I2C interface use only SPI) - // 4: =1 BDU - // - // 3: =0 BLE do not swap data bytes - // 2: =0 Unused - // 1: =0 SELF_TEST - // 0: =1 DRDY_on_PIN Enable DRDY - writeRegister(0x62,0x31); // 0011 0001 = 0x31 // 0011 1001 = 0x39 - - // INT_CTRL_REG (0x63) - // Disable Interrupts (this is not DRDY) - writeRegister(0x63,0x00); - writeRegister(0x64,0x00); - writeRegister(0x65,0x00); - writeRegister(0x66,0x00); - - // Read Status Register (0x67) - uint8_t sensor_status = readRegister(0x67); - misc_printf("IIS2MDC: Mag status register = 0x%02X (0x00)\n\r",sensor_status); - if(sensor_status!=0x00) status |= DRIVER_SELF_DIAG_ERROR; - - // Read Offset Registers (6 bytes starting 0x45) - uint8_t tx[7],h[7]; - memset(tx,0,sizeof(tx)); - tx[0] = 0x45 | SPI_READ; - spi_.rx(tx, h, 7, 100); - misc_printf("H Offsets should be zero %8d %8d %8d mGauss\n\r",((int16_t)h[1] | (int16_t)h[2]<<8)*3/2, ((int16_t)h[3] | (int16_t)h[4]<<8)*3/2, ((int16_t)h[5] | (int16_t)h[6]<<8)*3/2); - - return status; + uint32_t status = DRIVER_OK; + sampleRateHz_ = sample_rate_hz; + drdyPort_ = drdy_port; + drdyPin_ = drdy_pin; + + spi_.init(hspi, iis2mdc_dma_txbuf, iis2mdc_dma_rxbuf, cs_port, cs_pin); + seqCount_ = 0; + + HAL_GPIO_WritePin(spi_.port_, spi_.pin_, GPIO_PIN_SET); + + rxFifo_.init(IIS2MDC_FIFO_BUFFERS, sizeof(MagPacket), iis2mdc_fifo_rx_buffer); + + uint8_t odr_mode = 3; + if (sampleRateHz_ <= 10) + { + sampleRateHz_ = 10; + odr_mode = 0; + } + else if (sampleRateHz_ <= 20) + { + sampleRateHz_ = 20; + odr_mode = 1; + } + else if (sampleRateHz_ <= 50) + { + sampleRateHz_ = 50; + odr_mode = 2; + } + else if (sampleRateHz_ <= 100) + { + sampleRateHz_ = 100; + odr_mode = 3; + } + else + { + sampleRateHz_ = 100; + odr_mode = 3; + } + + groupDelay_ = 500000 / sampleRateHz_; // Do something better if anyone cares. + + uint8_t id = readRegister(WHO_AM_I); + + misc_printf("Iis2mdc: WHO_AM_I = 0x%02X (0x40) - ", id); + if (id == 0x40) + misc_printf(" Matches\n\r"); + else + { + misc_printf(" Does not match\n\r"); + status |= DRIVER_ID_MISMATCH; + } + + // Reboot the sensor + // Register A (0x60) + // 7: = 0 COMP_TEMP_EN Temp comp enable + // 6: = X REBOOT + // 5: = X SOFT_RST + // 4: = 0 High resolution Mode (LP=0) + // 3:2 = 00 10 Hz Data Rate (ODR) + // 1:0 = 00 Continuous Mode + writeRegister(0x60, 0x20); // soft reset + time64.dUs(10); // Wait at least 5 us + writeRegister(0x60, 0x40); // reboot + time64.dMs(21); // wait at least 20 ms for reboot + + // Register A (0x60) + // 7: = 1 COMP_TEMP_EN Temp comp enable + // 6: = 0 REBOOT + // 5: = 0 SOFT_RST + // 4: = 0 High resolution Mode (High resolution = 0, Low power =1) + // + // 3:2 = 11 = 100 Hz Data Rate (ODR) + // 1:0 = 00 Continuous Mode, 01 = single mode + // write_register(0x60,0x81); // 1000 0001 = 0x81 For Single Acq + // writeRegister(0x60,0x8C); // 1000 1100 = 0x8C For 100 Hz. + writeRegister(0x60, 0x80 | (odr_mode << 2)); + + // Register B (0x61) + // [7:5] 000 + // [4] 1 OFF_CANC_ONE_SHOT 1=Offset Cancellation in single mode + // [3] 0 + // [2] 0 Set Freq of Set pulse to 63 ODR + // [1] 1, OFF_CANC 1= enable offset cancellation in single mode + // [0] 0 LPF disable offset filter (1- enabled) + // write_register(0x61,0x12); // 0001 0010 For Single + writeRegister(0x61, 0x00); // 0000 0000 = 0x00 + + // Register C (0x62) + // 7: =0 Unused + // 6: =0 INT_on_PIN Enable event interrupts + // 5: =1 I2C_DIS (Disable I2C interface use only SPI) + // 4: =1 BDU + // + // 3: =0 BLE do not swap data bytes + // 2: =0 Unused + // 1: =0 SELF_TEST + // 0: =1 DRDY_on_PIN Enable DRDY + writeRegister(0x62, 0x31); // 0011 0001 = 0x31 // 0011 1001 = 0x39 + + // INT_CTRL_REG (0x63) + // Disable Interrupts (this is not DRDY) + writeRegister(0x63, 0x00); + writeRegister(0x64, 0x00); + writeRegister(0x65, 0x00); + writeRegister(0x66, 0x00); + + // Read Status Register (0x67) + uint8_t sensor_status = readRegister(0x67); + misc_printf("IIS2MDC: Mag status register = 0x%02X (0x00)\n\r", sensor_status); + if (sensor_status != 0x00) + status |= DRIVER_SELF_DIAG_ERROR; + + // Read Offset Registers (6 bytes starting 0x45) + uint8_t tx[7], h[7]; + memset(tx, 0, sizeof(tx)); + tx[0] = 0x45 | SPI_READ; + spi_.rx(tx, h, 7, 100); + misc_printf("H Offsets should be zero %8d %8d %8d mGauss\n\r", ((int16_t)h[1] | (int16_t)h[2] << 8) * 3 / 2, + ((int16_t)h[3] | (int16_t)h[4] << 8) * 3 / 2, ((int16_t)h[5] | (int16_t)h[6] << 8) * 3 / 2); + + return status; } bool Iis2mdc::poll(void) { - static bool previous_drdy=0; - bool status=true; - bool current_drdy = HAL_GPIO_ReadPin(drdyPort_, drdyPin_); - - // This sensor runs in continuous mode, so no action needed to instigate. - - if(!previous_drdy && current_drdy) - { - status = startDma(); - } - previous_drdy=current_drdy; - return status; + static bool previous_drdy = 0; + bool status = true; + bool current_drdy = HAL_GPIO_ReadPin(drdyPort_, drdyPin_); + + // This sensor runs in continuous mode, so no action needed to instigate. + + if (!previous_drdy && current_drdy) + { + status = startDma(); + } + previous_drdy = current_drdy; + return status; } -bool Iis2mdc::startDma(void) //called to start dma read +bool Iis2mdc::startDma(void) // called to start dma read { - drdy_ = time64.Us(); - HAL_StatusTypeDef hal_status = HAL_OK; - hal_status = spi_.startDma(IIS_FLUX_CMD,IIS_FLUX_BYTES); - if(hal_status==HAL_OK) seqCount_ = 1; else seqCount_ = 0; - return hal_status == HAL_OK; + drdy_ = time64.Us(); + HAL_StatusTypeDef hal_status = HAL_OK; + hal_status = spi_.startDma(IIS_FLUX_CMD, IIS_FLUX_BYTES); + if (hal_status == HAL_OK) + seqCount_ = 1; + else + seqCount_ = 0; + return hal_status == HAL_OK; } void Iis2mdc::endDma(void) { - static MagPacket p; - if (seqCount_ == 1) - { - memset(&p,0,sizeof(p)); - uint8_t *rx = spi_.endDma(); - p.status = rx[1]; - - int16_t data; - data = (int16_t)rx[3]<<8 | (int16_t)rx[2]; p.flux[0] = -(double)data*1.5e-7; // T, 1.5e-7 T/LSB, 1mG = 1e-7 T. - data = (int16_t)rx[5]<<8 | (int16_t)rx[4]; p.flux[1] = (double)data*1.5e-7; // T, 1.5e-7 T/LSB - data = (int16_t)rx[7]<<8 | (int16_t)rx[6]; p.flux[2] = (double)data*1.5e-7; // T, 1.5e-7 T/LSB - - HAL_StatusTypeDef hal_status = spi_.startDma(IIS_TEMP_CMD,IIS_TEMP_BYTES); - if(hal_status == HAL_OK) seqCount_ = 2; else seqCount_ = 0; - } - else if(seqCount_ == 2) - { - uint8_t *rx = spi_.endDma(); - - uint16_t data = (int16_t)rx[2]<<8 | (int16_t)rx[1]; - p.temperature = (double)data/8.0+25.0+273.15; // K - - p.drdy = drdy_; - p.groupDelay = groupDelay_; - p.timestamp = time64.Us(); - rxFifo_.write((uint8_t*)&p,sizeof(p)); - - seqCount_ = 0; - } - else - { - seqCount_ = 0; - } - + static MagPacket p; + if (seqCount_ == 1) + { + memset(&p, 0, sizeof(p)); + uint8_t *rx = spi_.endDma(); + p.status = rx[1]; + + int16_t data; + data = (int16_t)rx[3] << 8 | (int16_t)rx[2]; + p.flux[0] = -(double)data * 1.5e-7; // T, 1.5e-7 T/LSB, 1mG = 1e-7 T. + data = (int16_t)rx[5] << 8 | (int16_t)rx[4]; + p.flux[1] = (double)data * 1.5e-7; // T, 1.5e-7 T/LSB + data = (int16_t)rx[7] << 8 | (int16_t)rx[6]; + p.flux[2] = (double)data * 1.5e-7; // T, 1.5e-7 T/LSB + + HAL_StatusTypeDef hal_status = spi_.startDma(IIS_TEMP_CMD, IIS_TEMP_BYTES); + if (hal_status == HAL_OK) + seqCount_ = 2; + else + seqCount_ = 0; + } + else if (seqCount_ == 2) + { + uint8_t *rx = spi_.endDma(); + + uint16_t data = (int16_t)rx[2] << 8 | (int16_t)rx[1]; + p.temperature = (double)data / 8.0 + 25.0 + 273.15; // K + + p.drdy = drdy_; + p.groupDelay = groupDelay_; + p.timestamp = time64.Us(); + rxFifo_.write((uint8_t *)&p, sizeof(p)); + + seqCount_ = 0; + } + else + { + seqCount_ = 0; + } } bool Iis2mdc::display() { - MagPacket p; - char name[] = "Iis2mdc (mag)"; - if(rxFifo_.readMostRecent((uint8_t*)&p,sizeof(p))) - { - misc_header(name, p.drdy, p.timestamp, p.groupDelay); - - misc_printf("%10.3f %10.3f %10.3f uT ", p.flux[0]*1e6+10.9, p.flux[1]*1e6+45.0, p.flux[2]*1e6-37.5); - misc_printf(" | "); - misc_printf(" | %7.1f C \n\r", p.temperature-273.15); - return 1; - } - else - { - misc_printf("%s\n\r",name); - } - - return 0; + MagPacket p; + char name[] = "Iis2mdc (mag)"; + if (rxFifo_.readMostRecent((uint8_t *)&p, sizeof(p))) + { + misc_header(name, p.drdy, p.timestamp, p.groupDelay); + + misc_printf("%10.3f %10.3f %10.3f uT ", p.flux[0] * 1e6 + 10.9, p.flux[1] * 1e6 + 45.0, p.flux[2] * 1e6 - 37.5); + misc_printf(" | "); + misc_printf(" | %7.1f C \n\r", p.temperature - 273.15); + return 1; + } + else + { + misc_printf("%s\n\r", name); + } + + return 0; } - void Iis2mdc::writeRegister(uint8_t address, uint8_t value) { - uint8_t tx[2]={0}; - tx[0] = (address) | SPI_WRITE; - tx[1] = value; - spi_.tx(tx,2,100); + uint8_t tx[2] = {0}; + tx[0] = (address) | SPI_WRITE; + tx[1] = value; + spi_.tx(tx, 2, 100); } uint8_t Iis2mdc::readRegister(uint8_t address) { - uint8_t tx[2]={0}; - uint8_t rx[2]={0}; - tx[0] = (address) | SPI_READ; - tx[1] = 0; - HAL_StatusTypeDef hal_status = spi_.rx(tx,rx,2,100); - return rx[1]|hal_status; + uint8_t tx[2] = {0}; + uint8_t rx[2] = {0}; + tx[0] = (address) | SPI_READ; + tx[1] = 0; + HAL_StatusTypeDef hal_status = spi_.rx(tx, rx, 2, 100); + return rx[1] | hal_status; } - - - - - diff --git a/boards/varmint/src/board/Pwm.cpp b/boards/varmint/src/board/Pwm.cpp index 25645600..73a67d78 100644 --- a/boards/varmint/src/board/Pwm.cpp +++ b/boards/varmint/src/board/Pwm.cpp @@ -35,54 +35,61 @@ ****************************************************************************** **/ -#include #include +#include uint32_t init_pwm_timers(uint32_t servo_pwm_period_us) { - { - TIM_MasterConfigTypeDef sMasterConfig = {0}; - TIM_OC_InitTypeDef sConfigOC = {0}; - TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; + { + TIM_MasterConfigTypeDef sMasterConfig = {0}; + TIM_OC_InitTypeDef sConfigOC = {0}; + TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; - htim1.Instance = TIM1; - htim1.Init.Prescaler = (SERVO_PWM_CLK_DIV); - htim1.Init.CounterMode = TIM_COUNTERMODE_UP; - htim1.Init.Period = servo_pwm_period_us; - htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim1.Init.RepetitionCounter = 0; - htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; - if (HAL_TIM_PWM_Init(&htim1) != HAL_OK) return DRIVER_HAL_ERROR; - sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; - sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK) return DRIVER_HAL_ERROR; - sConfigOC.OCMode = TIM_OCMODE_PWM1; - sConfigOC.Pulse = (SERVO_PWM_CENTER); - sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; - sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; - sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; - sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) return DRIVER_HAL_ERROR; - sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; - sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; - sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; - sBreakDeadTimeConfig.DeadTime = 0; - sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; - sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; - sBreakDeadTimeConfig.BreakFilter = 0; - sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE; - sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH; - sBreakDeadTimeConfig.Break2Filter = 0; - sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; - if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK) return DRIVER_HAL_ERROR; - HAL_TIM_MspPostInit(&htim1); - } - { + htim1.Instance = TIM1; + htim1.Init.Prescaler = (SERVO_PWM_CLK_DIV); + htim1.Init.CounterMode = TIM_COUNTERMODE_UP; + htim1.Init.Period = servo_pwm_period_us; + htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim1.Init.RepetitionCounter = 0; + htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; + if (HAL_TIM_PWM_Init(&htim1) != HAL_OK) + return DRIVER_HAL_ERROR; + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + sConfigOC.OCMode = TIM_OCMODE_PWM1; + sConfigOC.Pulse = (SERVO_PWM_CENTER); + sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) + return DRIVER_HAL_ERROR; + sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; + sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; + sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; + sBreakDeadTimeConfig.DeadTime = 0; + sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; + sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; + sBreakDeadTimeConfig.BreakFilter = 0; + sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE; + sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH; + sBreakDeadTimeConfig.Break2Filter = 0; + sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; + if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK) + return DRIVER_HAL_ERROR; + HAL_TIM_MspPostInit(&htim1); + } + { TIM_MasterConfigTypeDef sMasterConfig = {0}; TIM_OC_InitTypeDef sConfigOC = {0}; htim3.Instance = TIM3; @@ -91,16 +98,20 @@ uint32_t init_pwm_timers(uint32_t servo_pwm_period_us) htim3.Init.Period = servo_pwm_period_us; htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; - if (HAL_TIM_PWM_Init(&htim3) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_Init(&htim3) != HAL_OK) + return DRIVER_HAL_ERROR; sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) + return DRIVER_HAL_ERROR; sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = (SERVO_PWM_CENTER); sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) + return DRIVER_HAL_ERROR; HAL_TIM_MspPostInit(&htim3); } { @@ -112,18 +123,24 @@ uint32_t init_pwm_timers(uint32_t servo_pwm_period_us) htim4.Init.Period = servo_pwm_period_us; htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; - if (HAL_TIM_PWM_Init(&htim4) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_Init(&htim4) != HAL_OK) + return DRIVER_HAL_ERROR; sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) + return DRIVER_HAL_ERROR; sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = (SERVO_PWM_CENTER); sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) + return DRIVER_HAL_ERROR; HAL_TIM_MspPostInit(&htim4); } return DRIVER_OK; diff --git a/boards/varmint/src/board/Sbus.cpp b/boards/varmint/src/board/Sbus.cpp index b77cae3a..1ecd491d 100644 --- a/boards/varmint/src/board/Sbus.cpp +++ b/boards/varmint/src/board/Sbus.cpp @@ -43,63 +43,60 @@ extern Time64 time64; typedef struct __attribute__((__packed__)) { - uint8_t header; // 0x0F - unsigned int chan0 : 11; - unsigned int chan1 : 11; - unsigned int chan2 : 11; - unsigned int chan3 : 11; - unsigned int chan4 : 11; - unsigned int chan5 : 11; - unsigned int chan6 : 11; - unsigned int chan7 : 11; - unsigned int chan8 : 11; - unsigned int chan9 : 11; - unsigned int chan10 : 11; - unsigned int chan11 : 11; - unsigned int chan12 : 11; - unsigned int chan13 : 11; - unsigned int chan14 : 11; - unsigned int chan15 : 11; - uint8_t dig_chan0 :1; - uint8_t dig_chan1 :1; - uint8_t dig_chan2 :1; - uint8_t dig_chan3 :1; - uint8_t dig_chan4 :1; - uint8_t dig_chan5 :1; - uint8_t dig_chan6 :1; - uint8_t dig_chan7 :1; - uint8_t footer; // 0x00 + uint8_t header; // 0x0F + unsigned int chan0 : 11; + unsigned int chan1 : 11; + unsigned int chan2 : 11; + unsigned int chan3 : 11; + unsigned int chan4 : 11; + unsigned int chan5 : 11; + unsigned int chan6 : 11; + unsigned int chan7 : 11; + unsigned int chan8 : 11; + unsigned int chan9 : 11; + unsigned int chan10 : 11; + unsigned int chan11 : 11; + unsigned int chan12 : 11; + unsigned int chan13 : 11; + unsigned int chan14 : 11; + unsigned int chan15 : 11; + uint8_t dig_chan0 : 1; + uint8_t dig_chan1 : 1; + uint8_t dig_chan2 : 1; + uint8_t dig_chan3 : 1; + uint8_t dig_chan4 : 1; + uint8_t dig_chan5 : 1; + uint8_t dig_chan6 : 1; + uint8_t dig_chan7 : 1; + uint8_t footer; // 0x00 } SbusPacket; -#define SBUS_DMA_BUFFER_SIZE (sizeof(SbusPacket)*4) -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t sbus_dma_rxbuf[SBUS_DMA_BUFFER_SIZE]={0}; - -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) uint8_t sbus_fifo_rx_buffer[SBUS_FIFO_BUFFERS*sizeof(RcPacket)]={0}; - - - - -uint32_t Sbus::init -( - // Driver initializers - uint16_t sample_rate_hz, - // UART initializers - UART_HandleTypeDef *huart, - USART_TypeDef *huart_instance, - DMA_HandleTypeDef *hdma_uart_rx, - uint32_t baud -) +#define SBUS_DMA_BUFFER_SIZE (sizeof(SbusPacket) * 4) +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t sbus_dma_rxbuf[SBUS_DMA_BUFFER_SIZE] = {0}; + +__attribute__((section("my_buffers"))) __attribute__((aligned(32))) +uint8_t sbus_fifo_rx_buffer[SBUS_FIFO_BUFFERS * sizeof(RcPacket)] = {0}; + +uint32_t Sbus::init( + // Driver initializers + uint16_t sample_rate_hz, + // UART initializers + UART_HandleTypeDef *huart, + USART_TypeDef *huart_instance, + DMA_HandleTypeDef *hdma_uart_rx, + uint32_t baud) { - sampleRateHz_ = sample_rate_hz; - drdyPort_ = 0; // do not use - drdyPin_ = 0; // do not use - dtimeout_ = 100000; // 0.1 seconds - timeout_ = 0; + sampleRateHz_ = sample_rate_hz; + drdyPort_ = 0; // do not use + drdyPin_ = 0; // do not use + dtimeout_ = 100000; // 0.1 seconds + timeout_ = 0; - huart_ = huart; - hdmaUartRx_ = hdma_uart_rx; + huart_ = huart; + hdmaUartRx_ = hdma_uart_rx; - // USART initialization begin (taken from STM32Cube Codegen) + // USART initialization begin (taken from STM32Cube Codegen) huart_->Instance = huart_instance; huart_->Init.BaudRate = baud; huart_->Init.WordLength = UART_WORDLENGTH_9B; // Including Parity @@ -110,135 +107,161 @@ uint32_t Sbus::init huart_->Init.OverSampling = UART_OVERSAMPLING_16; huart_->Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; huart_->Init.ClockPrescaler = UART_PRESCALER_DIV1; - huart_->AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_TXINVERT_INIT|UART_ADVFEATURE_RXINVERT_INIT; + huart_->AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_TXINVERT_INIT | UART_ADVFEATURE_RXINVERT_INIT; huart_->AdvancedInit.TxPinLevelInvert = UART_ADVFEATURE_TXINV_ENABLE; huart_->AdvancedInit.RxPinLevelInvert = UART_ADVFEATURE_RXINV_ENABLE; - if (HAL_UART_Init(huart_) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_UARTEx_SetTxFifoThreshold(huart_, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_UARTEx_SetRxFifoThreshold(huart_, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_UARTEx_DisableFifoMode(huart_) != HAL_OK) return DRIVER_HAL_ERROR; + if (HAL_UART_Init(huart_) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_UARTEx_SetTxFifoThreshold(huart_, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_UARTEx_SetRxFifoThreshold(huart_, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_UARTEx_DisableFifoMode(huart_) != HAL_OK) + return DRIVER_HAL_ERROR; // USART initialization end - rxFifo_.init(SBUS_FIFO_BUFFERS, sizeof(RcPacket), sbus_fifo_rx_buffer); + rxFifo_.init(SBUS_FIFO_BUFFERS, sizeof(RcPacket), sbus_fifo_rx_buffer); - __HAL_UART_CLEAR_IDLEFLAG(huart_); - __HAL_UART_DISABLE_IT(huart_, UART_IT_IDLE); + __HAL_UART_CLEAR_IDLEFLAG(huart_); + __HAL_UART_DISABLE_IT(huart_, UART_IT_IDLE); - return DRIVER_OK; + return DRIVER_OK; } bool Sbus::poll(void) { - // Check if we are timed-out - if (time64.Us()>timeout_) - { - if( (((DMA_Stream_TypeDef *)(hdmaUartRx_)->Instance)->CR & DMA_SxCR_EN) != DMA_SxCR_EN ) - { - __HAL_UART_CLEAR_IDLEFLAG(huart_); // this may be redundant with call to HAL_UART_Abort() - __HAL_UART_ENABLE_IT(huart_, UART_IT_IDLE); - HAL_UART_Abort(huart_); // flush any leftover crumbs. - startDma(); - } - } - return 0; + // Check if we are timed-out + if (time64.Us() > timeout_) + { + if ((((DMA_Stream_TypeDef *)(hdmaUartRx_)->Instance)->CR & DMA_SxCR_EN) != DMA_SxCR_EN) + { + __HAL_UART_CLEAR_IDLEFLAG(huart_); // this may be redundant with call to HAL_UART_Abort() + __HAL_UART_ENABLE_IT(huart_, UART_IT_IDLE); + HAL_UART_Abort(huart_); // flush any leftover crumbs. + startDma(); + } + } + return 0; } bool Sbus::startDma(void) { - timeout_ = time64.Us() + dtimeout_; // 0.1 second timeout + timeout_ = time64.Us() + dtimeout_; // 0.1 second timeout -// #if USE_D_CACHE_MANAGEMENT_FUNCTIONS -// SCB_CleanDCache_by_Addr((uint32_t *)sbus_dma_rxbuf, SBUS_DMA_BUFFER_SIZE) ; // not needed -// #endif + // #if USE_D_CACHE_MANAGEMENT_FUNCTIONS + // SCB_CleanDCache_by_Addr((uint32_t *)sbus_dma_rxbuf, SBUS_DMA_BUFFER_SIZE) ; // not needed + // #endif - HAL_StatusTypeDef hal_status = HAL_UART_Receive_DMA(huart_,sbus_dma_rxbuf,SBUS_DMA_BUFFER_SIZE); // start next read - return HAL_OK == hal_status; + HAL_StatusTypeDef hal_status = HAL_UART_Receive_DMA(huart_, sbus_dma_rxbuf, SBUS_DMA_BUFFER_SIZE); // start next read + return HAL_OK == hal_status; } void Sbus::endDma(void) { - - drdy_ = time64.Us(); - - #if USE_D_CACHE_MANAGEMENT_FUNCTIONS - SCB_InvalidateDCache_by_Addr ((uint32_t *)sbus_dma_rxbuf, SBUS_DMA_BUFFER_LEN); // force read from SRAM vs cache - #endif - - RcPacket p; - SbusPacket *sbus = (SbusPacket *)sbus_dma_rxbuf; - - if((sbus->header == 0x0F) && (sbus->footer == 0x00)) - { - p.chan[0] = sbus->chan0; - p.chan[1] = sbus->chan1; - p.chan[2] = sbus->chan2; - p.chan[3] = sbus->chan3; - p.chan[4] = sbus->chan4; - p.chan[5] = sbus->chan5; - p.chan[6] = sbus->chan6; - p.chan[7] = sbus->chan7; - p.chan[8] = sbus->chan8; - p.chan[9] = sbus->chan9; - p.chan[10] = sbus->chan10; - p.chan[11] = sbus->chan11; - p.chan[12] = sbus->chan12; - p.chan[13] = sbus->chan13; - p.chan[14] = sbus->chan14; - p.chan[15] = sbus->chan15; - - if(sbus->dig_chan0) p.chan[16] = 172; else p.chan[16] = 1811; - if(sbus->dig_chan1) p.chan[17] = 172; else p.chan[17] = 1811; - if(sbus->dig_chan2) p.chan[18] = 172; else p.chan[18] = 1811; - if(sbus->dig_chan3) p.chan[19] = 172; else p.chan[19] = 1811; - if(sbus->dig_chan4) p.chan[20] = 172; else p.chan[20] = 1811; - if(sbus->dig_chan5) p.chan[21] = 172; else p.chan[21] = 1811; - if(sbus->dig_chan6) p.chan[22] = 172; else p.chan[22] = 1811; - if(sbus->dig_chan7) p.chan[23] = 172; else p.chan[23] = 1811; - - p.frameLost = sbus->dig_chan2; - p.failsafeActivated = sbus->dig_chan3; - for(int n=0;nheader == 0x0F) && (sbus->footer == 0x00)) + { + p.chan[0] = sbus->chan0; + p.chan[1] = sbus->chan1; + p.chan[2] = sbus->chan2; + p.chan[3] = sbus->chan3; + p.chan[4] = sbus->chan4; + p.chan[5] = sbus->chan5; + p.chan[6] = sbus->chan6; + p.chan[7] = sbus->chan7; + p.chan[8] = sbus->chan8; + p.chan[9] = sbus->chan9; + p.chan[10] = sbus->chan10; + p.chan[11] = sbus->chan11; + p.chan[12] = sbus->chan12; + p.chan[13] = sbus->chan13; + p.chan[14] = sbus->chan14; + p.chan[15] = sbus->chan15; + + if (sbus->dig_chan0) + p.chan[16] = 172; + else + p.chan[16] = 1811; + if (sbus->dig_chan1) + p.chan[17] = 172; + else + p.chan[17] = 1811; + if (sbus->dig_chan2) + p.chan[18] = 172; + else + p.chan[18] = 1811; + if (sbus->dig_chan3) + p.chan[19] = 172; + else + p.chan[19] = 1811; + if (sbus->dig_chan4) + p.chan[20] = 172; + else + p.chan[20] = 1811; + if (sbus->dig_chan5) + p.chan[21] = 172; + else + p.chan[21] = 1811; + if (sbus->dig_chan6) + p.chan[22] = 172; + else + p.chan[22] = 1811; + if (sbus->dig_chan7) + p.chan[23] = 172; + else + p.chan[23] = 1811; + + p.frameLost = sbus->dig_chan2; + p.failsafeActivated = sbus->dig_chan3; + for (int n = 0; n < RC_PACKET_CHANNELS; n++) p.chan[n] = (p.chan[n] - 172) / 1639.0; + + p.timestamp = time64.Us(); // usTime(); + p.drdy = drdy_; p.groupDelay = 9000; // Use one packet time latency for now. - p.status = !(p.frameLost|p.failsafeActivated); + p.status = !(p.frameLost | p.failsafeActivated); lol_ = p.frameLost | p.failsafeActivated; - rxFifo_.write((uint8_t*)&p,sizeof(p)); - timeout_ = drdy_ + dtimeout_; // Give it a second before we say it's lost - } - startDma(); - + rxFifo_.write((uint8_t *)&p, sizeof(p)); + timeout_ = drdy_ + dtimeout_; // Give it a second before we say it's lost + } + startDma(); } bool Sbus::display(void) { - RcPacket p; - char name[] = "Sbus (rc)"; - if(rxFifo_.readMostRecent((uint8_t*)&p,sizeof(p))) - { - misc_header(name,p.drdy, p.timestamp, p.groupDelay); for(int i=0;i<8;i++) misc_printf("[%2u]:%4.0f, ", i+1, (p.chan[i]*100.0)); misc_printf("%\n\r"); - misc_header(name,p.drdy, p.timestamp, p.groupDelay); for(int i=8;i<16;i++) misc_printf("[%2u]:%4.0f, ", i+1, (p.chan[i]*100.0)); misc_printf("%\n\r"); - misc_header(name,p.drdy, p.timestamp, p.groupDelay); for(int i=16;i<24;i++) misc_printf("[%2u]:%4.0f, ", i+1, (p.chan[i]*100.0)); misc_printf("%\n\r"); - misc_header(name,p.drdy, p.timestamp, p.groupDelay); - misc_printf("Frame Lost: %1u, Failsafe Activated: %1u, Status: %1u\n\r",p.frameLost, p.failsafeActivated, p.status); - return 1; - } - else - { - misc_printf("%s\n\r",name); - misc_printf("%s\n\r",name); - misc_printf("%s\n\r",name); - misc_printf("%s\n\r",name); - } - return 0; + RcPacket p; + char name[] = "Sbus (rc)"; + if (rxFifo_.readMostRecent((uint8_t *)&p, sizeof(p))) + { + misc_header(name, p.drdy, p.timestamp, p.groupDelay); + for (int i = 0; i < 8; i++) misc_printf("[%2u]:%4.0f, ", i + 1, (p.chan[i] * 100.0)); + misc_printf("%\n\r"); + misc_header(name, p.drdy, p.timestamp, p.groupDelay); + for (int i = 8; i < 16; i++) misc_printf("[%2u]:%4.0f, ", i + 1, (p.chan[i] * 100.0)); + misc_printf("%\n\r"); + misc_header(name, p.drdy, p.timestamp, p.groupDelay); + for (int i = 16; i < 24; i++) misc_printf("[%2u]:%4.0f, ", i + 1, (p.chan[i] * 100.0)); + misc_printf("%\n\r"); + misc_header(name, p.drdy, p.timestamp, p.groupDelay); + misc_printf("Frame Lost: %1u, Failsafe Activated: %1u, Status: %1u\n\r", p.frameLost, p.failsafeActivated, + p.status); + return 1; + } + else + { + misc_printf("%s\n\r", name); + misc_printf("%s\n\r", name); + misc_printf("%s\n\r", name); + misc_printf("%s\n\r", name); + } + return 0; } - - - - - - - diff --git a/boards/varmint/src/board/Sd.cpp b/boards/varmint/src/board/Sd.cpp index c74c5cae..af0e0524 100644 --- a/boards/varmint/src/board/Sd.cpp +++ b/boards/varmint/src/board/Sd.cpp @@ -35,91 +35,95 @@ ****************************************************************************** **/ -#include #include - -#include +#include #include +#include #include extern Time64 time64; #define SD_MAXBLKS (5L) #define SD_BLKSIZE (512L) -#define SD_BUFF_SIZE (SD_MAXBLKS*SD_BLKSIZE) -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t sd_rx_buf[SD_BUFF_SIZE]={0}; -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t sd_tx_buf[SD_BUFF_SIZE]={0}; +#define SD_BUFF_SIZE (SD_MAXBLKS * SD_BLKSIZE) +__attribute__((section("my_dma_buffers"))) __attribute__((aligned(32))) static uint8_t sd_rx_buf[SD_BUFF_SIZE] = {0}; +__attribute__((section("my_dma_buffers"))) __attribute__((aligned(32))) static uint8_t sd_tx_buf[SD_BUFF_SIZE] = {0}; uint32_t Sd::init(SD_HandleTypeDef *hsd, SD_TypeDef *hsd_instance) { - hsd_ = hsd; - hsd_->Instance = hsd_instance; - hsd_->Init.ClockEdge = SDMMC_CLOCK_EDGE_RISING; - hsd_->Init.ClockPowerSave = SDMMC_CLOCK_POWER_SAVE_DISABLE; - hsd_->Init.BusWide = SDMMC_BUS_WIDE_4B; - hsd_->Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_DISABLE; - hsd_->Init.ClockDiv = 10; - HAL_StatusTypeDef hal_status=HAL_SD_Init(hsd_); - if (hal_status != HAL_OK) - { - misc_printf("SD Card Initialization failure 0x%02X\n\r",hal_status); - return DRIVER_HAL_ERROR; - } - HAL_SD_CardInfoTypeDef sd_info; - hal_status = HAL_SD_GetCardInfo(hsd_, &sd_info); - if(hal_status != HAL_OK) - { - misc_printf("SD Card Info Read failure 0x%02X\n\r",hal_status); - return DRIVER_HAL_ERROR; - } - else - { - misc_printf("SD Card Capacity= %.0f GB\n\r", - (double)sd_info.BlockNbr*(double)sd_info.BlockSize/1024./1024./1024.); - } - return DRIVER_OK; + hsd_ = hsd; + hsd_->Instance = hsd_instance; + hsd_->Init.ClockEdge = SDMMC_CLOCK_EDGE_RISING; + hsd_->Init.ClockPowerSave = SDMMC_CLOCK_POWER_SAVE_DISABLE; + hsd_->Init.BusWide = SDMMC_BUS_WIDE_4B; + hsd_->Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_DISABLE; + hsd_->Init.ClockDiv = 10; + HAL_StatusTypeDef hal_status = HAL_SD_Init(hsd_); + if (hal_status != HAL_OK) + { + misc_printf("SD Card Initialization failure 0x%02X\n\r", hal_status); + return DRIVER_HAL_ERROR; + } + HAL_SD_CardInfoTypeDef sd_info; + hal_status = HAL_SD_GetCardInfo(hsd_, &sd_info); + if (hal_status != HAL_OK) + { + misc_printf("SD Card Info Read failure 0x%02X\n\r", hal_status); + return DRIVER_HAL_ERROR; + } + else + { + misc_printf("SD Card Capacity= %.0f GB\n\r", + (double)sd_info.BlockNbr * (double)sd_info.BlockSize / 1024. / 1024. / 1024.); + } + return DRIVER_OK; } bool Sd::read(uint8_t *dest, size_t len) { - uint16_t Nblocks = (len+SD_BLKSIZE-1)/SD_BLKSIZE; -// printf("Nblocks = %u\n\r",Nblocks); -// if(Nblocks > sd_info.BlockNbr) Nblocks = sd_info.BlockNbr; - if(Nblocks > SD_MAXBLKS) return 0; // too large and don't want to be here forever, throw an error - HAL_StatusTypeDef hal_status; - HAL_SD_CardStateTypeDef sd_state; + uint16_t Nblocks = (len + SD_BLKSIZE - 1) / SD_BLKSIZE; + // printf("Nblocks = %u\n\r",Nblocks); + // if(Nblocks > sd_info.BlockNbr) Nblocks = sd_info.BlockNbr; + if (Nblocks > SD_MAXBLKS) + return 0; // too large and don't want to be here forever, throw an error + HAL_StatusTypeDef hal_status; + HAL_SD_CardStateTypeDef sd_state; - uint64_t timeout = time64.Us()+250000; - while( (HAL_SD_CARD_TRANSFER != (sd_state=HAL_SD_GetCardState(hsd_))) && (timeout > time64.Us()) ); - if(HAL_SD_CARD_TRANSFER != sd_state) return 0; + uint64_t timeout = time64.Us() + 250000; + while ((HAL_SD_CARD_TRANSFER != (sd_state = HAL_SD_GetCardState(hsd_))) && (timeout > time64.Us())) + ; + if (HAL_SD_CARD_TRANSFER != sd_state) + return 0; - hal_status = HAL_SD_ReadBlocks(hsd_,sd_rx_buf,0,Nblocks,250); - HAL_SD_GetCardState(hsd_); - if(hal_status!=HAL_OK) return 0; - memcpy(dest,sd_rx_buf,len); + hal_status = HAL_SD_ReadBlocks(hsd_, sd_rx_buf, 0, Nblocks, 250); + HAL_SD_GetCardState(hsd_); + if (hal_status != HAL_OK) + return 0; + memcpy(dest, sd_rx_buf, len); - return 1; + return 1; } bool Sd::write(uint8_t *src, size_t len) { - uint16_t Nblocks = (len+511)/512; -// printf("Nblocks = %u\n\r",Nblocks); -// if(Nblocks > sd_info.BlockNbr) Nblocks = sd_info.BlockNbr; - if(Nblocks > SD_MAXBLKS) return 0; // too large and don't want to be here forever, throw an error - HAL_StatusTypeDef hal_status; - HAL_SD_CardStateTypeDef sd_state; + uint16_t Nblocks = (len + 511) / 512; + // printf("Nblocks = %u\n\r",Nblocks); + // if(Nblocks > sd_info.BlockNbr) Nblocks = sd_info.BlockNbr; + if (Nblocks > SD_MAXBLKS) + return 0; // too large and don't want to be here forever, throw an error + HAL_StatusTypeDef hal_status; + HAL_SD_CardStateTypeDef sd_state; - uint64_t timeout = time64.Us()+250000; - while( (HAL_SD_CARD_TRANSFER != (sd_state=HAL_SD_GetCardState(hsd_))) && (timeout > time64.Us()) ); - if(HAL_SD_CARD_TRANSFER != sd_state) return 0; + uint64_t timeout = time64.Us() + 250000; + while ((HAL_SD_CARD_TRANSFER != (sd_state = HAL_SD_GetCardState(hsd_))) && (timeout > time64.Us())) + ; + if (HAL_SD_CARD_TRANSFER != sd_state) + return 0; - memcpy(sd_tx_buf, src, len); - hal_status = HAL_SD_WriteBlocks(hsd_,sd_tx_buf,0,Nblocks,250); - HAL_SD_GetCardState(hsd_); - if(hal_status!=HAL_OK) return 0; + memcpy(sd_tx_buf, src, len); + hal_status = HAL_SD_WriteBlocks(hsd_, sd_tx_buf, 0, Nblocks, 250); + HAL_SD_GetCardState(hsd_); + if (hal_status != HAL_OK) + return 0; - return 1; + return 1; } - - - diff --git a/boards/varmint/src/board/Telem.cpp b/boards/varmint/src/board/Telem.cpp index 4b43a4c1..681d3765 100644 --- a/boards/varmint/src/board/Telem.cpp +++ b/boards/varmint/src/board/Telem.cpp @@ -35,183 +35,193 @@ ****************************************************************************** **/ +#include #include - -#include #include - +#include #include -#include - extern Time64 time64; #define TELEM_RX_BUFFER_SIZE (4096) // Use a multiple of 32! -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t telem_fifo_rx_buffer[TELEM_RX_BUFFER_SIZE]={0}; +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t telem_fifo_rx_buffer[TELEM_RX_BUFFER_SIZE] = {0}; #define TELEM_DMA_TX_BUFFER_SIZE (SERIAL_MAX_PAYLOAD_SIZE) -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t telem_dma_txbuf[TELEM_DMA_TX_BUFFER_SIZE]={0}; +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t telem_dma_txbuf[TELEM_DMA_TX_BUFFER_SIZE] = {0}; #define SERIAL_TX_FIFO_BUFFERS0 (4) #define SERIAL_TX_FIFO_BUFFERS1 (10) #define SERIAL_TX_FIFO_BUFFERS2 (SERIAL_TX_FIFO_BUFFERS) -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t telem_fifo_tx_buffer0[SERIAL_TX_FIFO_BUFFERS0*sizeof(SerialTxPacket)]; -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t telem_fifo_tx_buffer1[SERIAL_TX_FIFO_BUFFERS1*sizeof(SerialTxPacket)]; -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t telem_fifo_tx_buffer2[SERIAL_TX_FIFO_BUFFERS2*sizeof(SerialTxPacket)]; -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t *telem_fifo_tx_buffer[SERIAL_QOS_FIFOS] = {telem_fifo_tx_buffer0,telem_fifo_tx_buffer1,telem_fifo_tx_buffer2}; -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static PacketFifo telem_tx_fifos[SERIAL_QOS_FIFOS]; - -uint32_t Telem::init -( - // Driver initializers - uint16_t sample_rate_hz, - // UART initializers - UART_HandleTypeDef *huart, - USART_TypeDef *huart_instance, - DMA_HandleTypeDef *hdma_uart_rx, - uint32_t baud, - void (*RxISR)(struct __UART_HandleTypeDef *huart) -) +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t telem_fifo_tx_buffer0[SERIAL_TX_FIFO_BUFFERS0 * sizeof(SerialTxPacket)]; +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t telem_fifo_tx_buffer1[SERIAL_TX_FIFO_BUFFERS1 * sizeof(SerialTxPacket)]; +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t telem_fifo_tx_buffer2[SERIAL_TX_FIFO_BUFFERS2 * sizeof(SerialTxPacket)]; +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t *telem_fifo_tx_buffer[SERIAL_QOS_FIFOS] = { + telem_fifo_tx_buffer0, telem_fifo_tx_buffer1, telem_fifo_tx_buffer2}; +__attribute__((section("my_buffers"))) __attribute__((aligned(32))) static PacketFifo telem_tx_fifos[SERIAL_QOS_FIFOS]; + +uint32_t Telem::init( + // Driver initializers + uint16_t sample_rate_hz, + // UART initializers + UART_HandleTypeDef *huart, + USART_TypeDef *huart_instance, + DMA_HandleTypeDef *hdma_uart_rx, + uint32_t baud, + void (*RxISR)(struct __UART_HandleTypeDef *huart)) { - // Common initializations - sampleRateHz_ = sample_rate_hz; - txFrameSizeUs_ = (uint64_t)1000000/sampleRateHz_*95/100; // one period - 5% for margin. - txFrameEndUs_ = 0 + txFrameSizeUs_; - usPerByte_ = (uint64_t)1000000*10/baud; // assume 10 bits/byte. - txFifo_ = telem_tx_fifos; - uint32_t serial_tx_fifo_buffer_size[SERIAL_QOS_FIFOS] = {SERIAL_TX_FIFO_BUFFERS0,SERIAL_TX_FIFO_BUFFERS1,SERIAL_TX_FIFO_BUFFERS2}; - - for(int n=0;nInstance = huart_instance; - huart_->Init.BaudRate = baud; - huart_->Init.WordLength = UART_WORDLENGTH_8B; - huart_->Init.StopBits = UART_STOPBITS_1; - huart_->Init.Parity = UART_PARITY_NONE; - huart_->Init.Mode = UART_MODE_TX_RX; - huart_->Init.HwFlowCtl = UART_HWCONTROL_NONE; - huart_->Init.OverSampling = UART_OVERSAMPLING_16; - huart_->Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; - huart_->Init.ClockPrescaler = UART_PRESCALER_DIV1; - huart_->AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; - - if (HAL_UART_Init(huart_) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_UARTEx_SetTxFifoThreshold(huart_, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_UARTEx_SetRxFifoThreshold(huart_, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_UARTEx_DisableFifoMode(huart_) != HAL_OK) return DRIVER_HAL_ERROR; - - return DRIVER_OK; + // Common initializations + sampleRateHz_ = sample_rate_hz; + txFrameSizeUs_ = (uint64_t)1000000 / sampleRateHz_ * 95 / 100; // one period - 5% for margin. + txFrameEndUs_ = 0 + txFrameSizeUs_; + usPerByte_ = (uint64_t)1000000 * 10 / baud; // assume 10 bits/byte. + txFifo_ = telem_tx_fifos; + uint32_t serial_tx_fifo_buffer_size[SERIAL_QOS_FIFOS] = {SERIAL_TX_FIFO_BUFFERS0, SERIAL_TX_FIFO_BUFFERS1, + SERIAL_TX_FIFO_BUFFERS2}; + + for (int n = 0; n < SERIAL_QOS_FIFOS; n++) + txFifo_[n].init(serial_tx_fifo_buffer_size[n], sizeof(SerialTxPacket), telem_fifo_tx_buffer[n]); // Packet Fifo + rxFifo_.init(TELEM_RX_BUFFER_SIZE, telem_fifo_rx_buffer); // byte Fifo + + txIdle_ = true; + + // Telem-specific + + huart_ = huart; + hdmaUartRx_ = hdma_uart_rx; + + huart_->Instance = huart_instance; + huart_->Init.BaudRate = baud; + huart_->Init.WordLength = UART_WORDLENGTH_8B; + huart_->Init.StopBits = UART_STOPBITS_1; + huart_->Init.Parity = UART_PARITY_NONE; + huart_->Init.Mode = UART_MODE_TX_RX; + huart_->Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart_->Init.OverSampling = UART_OVERSAMPLING_16; + huart_->Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; + huart_->Init.ClockPrescaler = UART_PRESCALER_DIV1; + huart_->AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; + + if (HAL_UART_Init(huart_) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_UARTEx_SetTxFifoThreshold(huart_, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_UARTEx_SetRxFifoThreshold(huart_, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_UARTEx_DisableFifoMode(huart_) != HAL_OK) + return DRIVER_HAL_ERROR; + + return DRIVER_OK; } uint32_t Telem::reset_baud(uint32_t baud) { - if(baud != huart_->Init.BaudRate) - { - huart_->Init.BaudRate = baud; - if (HAL_UART_Init(huart_) != HAL_OK) return DRIVER_HAL_ERROR; - } - return DRIVER_OK; + if (baud != huart_->Init.BaudRate) + { + huart_->Init.BaudRate = baud; + if (HAL_UART_Init(huart_) != HAL_OK) + return DRIVER_HAL_ERROR; + } + return DRIVER_OK; } void Telem::poll(void) { - // TX - if(txIdle_) - { - txStart(); - } + // TX + if (txIdle_) + { + txStart(); + } } bool Telem::rxStart(void) // RX DMA { - // Enable interrupt - ATOMIC_SET_BIT(huart_->Instance->CR1, USART_CR1_RXNEIE_RXFNEIE); + // Enable interrupt + ATOMIC_SET_BIT(huart_->Instance->CR1, USART_CR1_RXNEIE_RXFNEIE); - return true; + return true; } void Telem::rxIsrCallback(UART_HandleTypeDef *huart) { - if (huart->Instance->ISR & UART_FLAG_RXNE ) - { - ATOMIC_SET_BIT(huart->Instance->CR1, USART_CR1_RXNEIE_RXFNEIE); - rxFifo_.write(huart->Instance->RDR); - } + if (huart->Instance->ISR & UART_FLAG_RXNE) + { + ATOMIC_SET_BIT(huart->Instance->CR1, USART_CR1_RXNEIE_RXFNEIE); + rxFifo_.write(huart->Instance->RDR); + } } -uint16_t Telem::writePacket(SerialTxPacket* p) +uint16_t Telem::writePacket(SerialTxPacket *p) { - p->timestamp = time64.Us(); - p->packetSize = sizeof(SerialTxPacket) + p->payloadSize - SERIAL_MAX_PAYLOAD_SIZE; - if(p->qos<0x02) return txFifo_[0].write((uint8_t*)p, p->packetSize); - else if(p->qos<0xFF) return txFifo_[1].write((uint8_t*)p, p->packetSize); - else return txFifo_[2].write((uint8_t*)p, p->packetSize); + p->timestamp = time64.Us(); + p->packetSize = sizeof(SerialTxPacket) + p->payloadSize - SERIAL_MAX_PAYLOAD_SIZE; + if (p->qos < 0x02) + return txFifo_[0].write((uint8_t *)p, p->packetSize); + else if (p->qos < 0xFF) + return txFifo_[1].write((uint8_t *)p, p->packetSize); + else + return txFifo_[2].write((uint8_t *)p, p->packetSize); } -bool Telem::newPacket(SerialTxPacket* p) +bool Telem::newPacket(SerialTxPacket *p) { - uint16_t size=0; - int64_t time_now_us = (int64_t)time64.Us(); - int64_t time_remaining_us = txFrameEndUs_-time_now_us; - - if( txFifo_[0].packetCount()>0 ) - { - size = txFifo_[0].read((uint8_t*)p, sizeof(SerialTxPacket)); - if(p->qos==0x00) - { - txFrameEndUs_ = time_now_us + txFrameSizeUs_; - } - } - else if( (txFifo_[1].packetCount()>0) && ( (int64_t)usPerByte_*txFifo_[1].peek()->size < time_remaining_us)) - { - size = txFifo_[1].read((uint8_t*)p, sizeof(SerialTxPacket)); - } - else if( (txFifo_[2].packetCount()>0) && ( (int64_t)usPerByte_*txFifo_[2].peek()->size < time_remaining_us)) - { - size = txFifo_[2].read((uint8_t*)p, sizeof(SerialTxPacket)); - } - - if((size!=0)&&(size==p->packetSize)) - { - return true; - } - else - { - return false; - } + uint16_t size = 0; + int64_t time_now_us = (int64_t)time64.Us(); + int64_t time_remaining_us = txFrameEndUs_ - time_now_us; + + if (txFifo_[0].packetCount() > 0) + { + size = txFifo_[0].read((uint8_t *)p, sizeof(SerialTxPacket)); + if (p->qos == 0x00) + { + txFrameEndUs_ = time_now_us + txFrameSizeUs_; + } + } + else if ((txFifo_[1].packetCount() > 0) && ((int64_t)usPerByte_ * txFifo_[1].peek()->size < time_remaining_us)) + { + size = txFifo_[1].read((uint8_t *)p, sizeof(SerialTxPacket)); + } + else if ((txFifo_[2].packetCount() > 0) && ((int64_t)usPerByte_ * txFifo_[2].peek()->size < time_remaining_us)) + { + size = txFifo_[2].read((uint8_t *)p, sizeof(SerialTxPacket)); + } + + if ((size != 0) && (size == p->packetSize)) + { + return true; + } + else + { + return false; + } } bool Telem::txStart(void) // Transmit complete callback. { - txIdle_ = false; - SerialTxPacket p={0}; - - if(newPacket(&p)) - { - - memcpy(telem_dma_txbuf,p.payload,p.payloadSize); - - #if USE_D_CACHE_MANAGEMENT_FUNCTIONS - SCB_CleanDCache_by_Addr((uint32_t *)telem_dma_txbuf, p.payloadSize) ; // Push cache to SRAM - #endif - - if(HAL_UART_Transmit_DMA(huart_ ,telem_dma_txbuf, p.payloadSize)!=HAL_OK) txIdle_ = true; - } - else - { - txIdle_ = true; - } - return !txIdle_; + txIdle_ = false; + SerialTxPacket p = {0}; + + if (newPacket(&p)) + { + memcpy(telem_dma_txbuf, p.payload, p.payloadSize); + +#if USE_D_CACHE_MANAGEMENT_FUNCTIONS + SCB_CleanDCache_by_Addr((uint32_t *)telem_dma_txbuf, p.payloadSize); // Push cache to SRAM +#endif + + if (HAL_UART_Transmit_DMA(huart_, telem_dma_txbuf, p.payloadSize) != HAL_OK) + txIdle_ = true; + } + else + { + txIdle_ = true; + } + return !txIdle_; } - - diff --git a/boards/varmint/src/board/Ubx.cpp b/boards/varmint/src/board/Ubx.cpp index 7095cd55..5b8872f2 100644 --- a/boards/varmint/src/board/Ubx.cpp +++ b/boards/varmint/src/board/Ubx.cpp @@ -35,52 +35,49 @@ ****************************************************************************** **/ -#include - #include - +#include #include extern Time64 time64; -#define SET(buf,data,type) *((type*)(buf))=data - - -#define UBX_DMA_BUFFER_SIZE 16// must be multiple of 16 -__attribute__((section("my_dma_buffers"))) __attribute__((aligned (32))) static uint8_t ubx_dma_rxbuf[UBX_DMA_BUFFER_SIZE]={0}; - -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t ubx_fifo_rx_buffer[UBX_FIFO_BUFFERS*sizeof(UbxPacket)]={0}; - -uint32_t Ubx::init -( - // Driver initializers - uint16_t sample_rate_hz, - GPIO_TypeDef *drdy_port, // Reset GPIO Port - uint16_t drdy_pin, // Reset GPIO Pin - // UART initializers - UART_HandleTypeDef *huart, - USART_TypeDef *huart_instance, - DMA_HandleTypeDef *hdma_uart_rx, - uint32_t baud -) +#define SET(buf, data, type) *((type *)(buf)) = data + +#define UBX_DMA_BUFFER_SIZE 16 // must be multiple of 16 +__attribute__((section("my_dma_buffers"))) +__attribute__((aligned(32))) static uint8_t ubx_dma_rxbuf[UBX_DMA_BUFFER_SIZE] = {0}; + +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t ubx_fifo_rx_buffer[UBX_FIFO_BUFFERS * sizeof(UbxPacket)] = {0}; + +uint32_t Ubx::init( + // Driver initializers + uint16_t sample_rate_hz, + GPIO_TypeDef *drdy_port, // Reset GPIO Port + uint16_t drdy_pin, // Reset GPIO Pin + // UART initializers + UART_HandleTypeDef *huart, + USART_TypeDef *huart_instance, + DMA_HandleTypeDef *hdma_uart_rx, + uint32_t baud) { - sampleRateHz_ = sample_rate_hz; - drdyPort_ = drdy_port; - drdyPin_ = drdy_pin; + sampleRateHz_ = sample_rate_hz; + drdyPort_ = drdy_port; + drdyPin_ = drdy_pin; - dtimeout_ = 1000000; // 1 seconds - timeout_ = 0; + dtimeout_ = 1000000; // 1 seconds + timeout_ = 0; - huart_ = huart; - hdmaUartRx_ = hdma_uart_rx; + huart_ = huart; + hdmaUartRx_ = hdma_uart_rx; - groupDelay_ = 0; + groupDelay_ = 0; - gotNav_ = false; - gotPvt_ = false; + gotNav_ = false; + gotPvt_ = false; - // USART initialization - huart_->Instance = USART1; + // USART initialization + huart_->Instance = USART1; huart_->Init.BaudRate = 9600; huart_->Init.WordLength = UART_WORDLENGTH_8B; huart_->Init.StopBits = UART_STOPBITS_1; @@ -92,429 +89,447 @@ uint32_t Ubx::init huart_->Init.ClockPrescaler = UART_PRESCALER_DIV1; huart_->AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; - if (HAL_UART_Init(huart_) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_UARTEx_SetTxFifoThreshold(huart_, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_UARTEx_SetRxFifoThreshold(huart_, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK) return DRIVER_HAL_ERROR; - if (HAL_UARTEx_DisableFifoMode(huart_) != HAL_OK) return DRIVER_HAL_ERROR; - - uint32_t ubx_baud = 0; - uint16_t i; - for(i=0;i<=100;i++) if((ubx_baud=pollCfgPrt())) break; - misc_printf("Baud Before = %lu, after %u retries.\n\r",ubx_baud,i); - - misc_printf("Setting baud to = %lu\n\r",baud); - uint32_t j; - for(j=0;j<10;j++) - { - huart_->Init.BaudRate = 9600; - HAL_UART_Init(huart_); - - cfgPrt(baud); // 0x06 0x00, baud rate change message - - huart_->Init.BaudRate = baud; - HAL_UART_Init(huart_); - - time64.dMs(30); // takes around 30ms to get an ack for the baud rate change. - - for(i=0;i<5;i++) if((ubx_baud=pollCfgPrt())) break; - if(baud==ubx_baud) break; - misc_printf("Failed, retry %u\n\r",j+1); - } - misc_printf("Baud After = %lu, %u retries\n\r",ubx_baud,j); - - if(ubx_baud != baud) return UBX_FAIL_BAUD_CHANGE; - - rxFifo_.init(UBX_FIFO_BUFFERS, sizeof(UbxPacket), ubx_fifo_rx_buffer); - - // Disable these messages to get rid of clutter - uint16_t error = 0; - - // Disable these messages - error |= (uint16_t)cfgMsg(0x0A,0x09,0); // MON-HW - error |= (uint16_t)cfgMsg(0x0A,0x0B,0); // MON-HW2 - error |= (uint16_t)cfgMsg(0x01,0x04,0); // NAV-DOP - error |= (uint16_t)cfgMsg(0x01,0x03,0); // NAV-STATUS - error |= (uint16_t)cfgMsg(0x01,0x20,0); // NAV-TIMEGPS - error |= (uint16_t)cfgMsg(0x01,0x35,0); // NAV-SAT - error |= (uint16_t)cfgMsg(0x01,0x01,0); // NAV-POSECEF (length 20) - error |= (uint16_t)cfgMsg(0x01,0x11,0); // NAV-VELECEF (length 20) - - // Enable only these messages - error |= (uint16_t)cfgMsg(0x01,0x07,1); // NAV-PVT (length 92) - error |= (uint16_t)cfgMsg(0x01,0x06,1); // NAV-SOL - // Set GPS Configuration - error |= (uint16_t)cfgRate(sampleRateHz_); // Nav rate 0x06 0x08 - error |= (uint16_t)cfgTp5(sampleRateHz_); // PPS rate 0x06 0x31 - error |= (uint16_t)cfgNav5(); // airplane mode 0x06 0x24 - - __HAL_UART_CLEAR_IDLEFLAG(huart_); - __HAL_UART_DISABLE_IT(huart_, UART_IT_IDLE); - - return DRIVER_OK; + if (HAL_UART_Init(huart_) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_UARTEx_SetTxFifoThreshold(huart_, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_UARTEx_SetRxFifoThreshold(huart_, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK) + return DRIVER_HAL_ERROR; + if (HAL_UARTEx_DisableFifoMode(huart_) != HAL_OK) + return DRIVER_HAL_ERROR; + + uint32_t ubx_baud = 0; + uint16_t i; + for (i = 0; i <= 100; i++) + if ((ubx_baud = pollCfgPrt())) + break; + misc_printf("Baud Before = %lu, after %u retries.\n\r", ubx_baud, i); + + misc_printf("Setting baud to = %lu\n\r", baud); + uint32_t j; + for (j = 0; j < 10; j++) + { + huart_->Init.BaudRate = 9600; + HAL_UART_Init(huart_); + + cfgPrt(baud); // 0x06 0x00, baud rate change message + + huart_->Init.BaudRate = baud; + HAL_UART_Init(huart_); + + time64.dMs(30); // takes around 30ms to get an ack for the baud rate change. + + for (i = 0; i < 5; i++) + if ((ubx_baud = pollCfgPrt())) + break; + if (baud == ubx_baud) + break; + misc_printf("Failed, retry %u\n\r", j + 1); + } + misc_printf("Baud After = %lu, %u retries\n\r", ubx_baud, j); + + if (ubx_baud != baud) + return UBX_FAIL_BAUD_CHANGE; + + rxFifo_.init(UBX_FIFO_BUFFERS, sizeof(UbxPacket), ubx_fifo_rx_buffer); + + // Disable these messages to get rid of clutter + uint16_t error = 0; + + // Disable these messages + error |= (uint16_t)cfgMsg(0x0A, 0x09, 0); // MON-HW + error |= (uint16_t)cfgMsg(0x0A, 0x0B, 0); // MON-HW2 + error |= (uint16_t)cfgMsg(0x01, 0x04, 0); // NAV-DOP + error |= (uint16_t)cfgMsg(0x01, 0x03, 0); // NAV-STATUS + error |= (uint16_t)cfgMsg(0x01, 0x20, 0); // NAV-TIMEGPS + error |= (uint16_t)cfgMsg(0x01, 0x35, 0); // NAV-SAT + error |= (uint16_t)cfgMsg(0x01, 0x01, 0); // NAV-POSECEF (length 20) + error |= (uint16_t)cfgMsg(0x01, 0x11, 0); // NAV-VELECEF (length 20) + + // Enable only these messages + error |= (uint16_t)cfgMsg(0x01, 0x07, 1); // NAV-PVT (length 92) + error |= (uint16_t)cfgMsg(0x01, 0x06, 1); // NAV-SOL + // Set GPS Configuration + error |= (uint16_t)cfgRate(sampleRateHz_); // Nav rate 0x06 0x08 + error |= (uint16_t)cfgTp5(sampleRateHz_); // PPS rate 0x06 0x31 + error |= (uint16_t)cfgNav5(); // airplane mode 0x06 0x24 + + __HAL_UART_CLEAR_IDLEFLAG(huart_); + __HAL_UART_DISABLE_IT(huart_, UART_IT_IDLE); + + return DRIVER_OK; } bool Ubx::poll(void) { - // Check if we are timed-out - if (time64.Us()>timeout_) - { - if( (((DMA_Stream_TypeDef *)(hdmaUartRx_)->Instance)->CR & DMA_SxCR_EN) != DMA_SxCR_EN ) - { - __HAL_UART_CLEAR_IDLEFLAG(huart_); // this may be redundant with call to HAL_UART_Abort() - __HAL_UART_ENABLE_IT(huart_, UART_IT_IDLE); - HAL_UART_Abort(huart_); // flush any leftover crumbs. - startDma(); - } - } - return 0; + // Check if we are timed-out + if (time64.Us() > timeout_) + { + if ((((DMA_Stream_TypeDef *)(hdmaUartRx_)->Instance)->CR & DMA_SxCR_EN) != DMA_SxCR_EN) + { + __HAL_UART_CLEAR_IDLEFLAG(huart_); // this may be redundant with call to HAL_UART_Abort() + __HAL_UART_ENABLE_IT(huart_, UART_IT_IDLE); + HAL_UART_Abort(huart_); // flush any leftover crumbs. + startDma(); + } + } + return 0; } bool Ubx::startDma(void) { - timeout_ = time64.Us() + dtimeout_; // 1000000 for one second timeout + timeout_ = time64.Us() + dtimeout_; // 1000000 for one second timeout - HAL_StatusTypeDef hal_status = HAL_UART_Receive_DMA(huart_,ubx_dma_rxbuf,UBX_DMA_BUFFER_SIZE); // start next read - return HAL_OK == hal_status; + HAL_StatusTypeDef hal_status = HAL_UART_Receive_DMA(huart_, ubx_dma_rxbuf, UBX_DMA_BUFFER_SIZE); // start next read + return HAL_OK == hal_status; } void Ubx::endDma(void) { - drdy_ = time64.Us(); - - #if USE_D_CACHE_MANAGEMENT_FUNCTIONS - SCB_InvalidateDCache_by_Addr ((uint32_t *)ubx_dma_rxbuf, UBX_DMA_BUFFER_SIZE); // Force read from SRAM - #endif - - uint16_t bytes_in_dma_buffer = misc_bytes_in_dma(hdmaUartRx_,UBX_DMA_BUFFER_SIZE); - - static UbxFrame p; - - for(int i=0;iA = 0; p->B = 0; // Reset the checksum calculation - } - else if(n==1) // header byte 2 - { - if(c==0x62) n++; - else if(c==0xB5) n = 1; // repeated 'mu' - else n = 0; - } - else if(n==2) // Class - { - if((c==0x01) || (c==0x05) || (c==0x06) ) // NAV, CFG, and ACK messages. - { - p->cl = c; - n++; - p->A += c; p->B += p->A; - } - else n = 0; - } - else if(n==3) // ID, allow all - { - p->id = c; - n++; - p->A += c; p->B += p->A; - } - else if(n==4) // length LSB - { - p->length = (uint16_t)c; - n++; - p->A += c; p->B += p->A; - } - else if(n==5) // length MSB - { - p->length |= ((uint16_t)c)<<8; - if( p->length > UBX_MAX_PAYLOAD_BYTES ) n = 0; - else {n++; p->A += c; p->B += p->A; } - } - else if(nlength+6) // Packet Payload bytes and first byte of checksum. - { - p->payload[n-6] = c; - n++; - p->A += c; p->B += p->A; - } - else if(n==p->length+6) // Checksum A - { - if(p->A!=c) n = 0; - else n++; - } - else //if(n==p->length+7) // Checksum B (the end) - { - n = 0; - if(p->B==c) - { - return true; - } - } - return false; + static uint16_t n = 0; + + // special case where we get 0xB5 randomly duplicated at the start (DMA wierdness). + if ((c == 0xB5) & (n == 1)) + n = 0; + + if (n == 0) // header byte 1 "mu" character + { + if (c == 0xB5) + n++; + else + n = 0; + p->A = 0; + p->B = 0; // Reset the checksum calculation + } + else if (n == 1) // header byte 2 + { + if (c == 0x62) + n++; + else if (c == 0xB5) + n = 1; // repeated 'mu' + else + n = 0; + } + else if (n == 2) // Class + { + if ((c == 0x01) || (c == 0x05) || (c == 0x06)) // NAV, CFG, and ACK messages. + { + p->cl = c; + n++; + p->A += c; + p->B += p->A; + } + else + n = 0; + } + else if (n == 3) // ID, allow all + { + p->id = c; + n++; + p->A += c; + p->B += p->A; + } + else if (n == 4) // length LSB + { + p->length = (uint16_t)c; + n++; + p->A += c; + p->B += p->A; + } + else if (n == 5) // length MSB + { + p->length |= ((uint16_t)c) << 8; + if (p->length > UBX_MAX_PAYLOAD_BYTES) + n = 0; + else + { + n++; + p->A += c; + p->B += p->A; + } + } + else if (n < p->length + 6) // Packet Payload bytes and first byte of checksum. + { + p->payload[n - 6] = c; + n++; + p->A += c; + p->B += p->A; + } + else if (n == p->length + 6) // Checksum A + { + if (p->A != c) + n = 0; + else + n++; + } + else // if(n==p->length+7) // Checksum B (the end) + { + n = 0; + if (p->B == c) + { + return true; + } + } + return false; } void Ubx::pps(uint64_t pps_timestamp) { - gotNav_ = false; - gotPvt_ = false; - ubx_.pps = pps_timestamp; + gotNav_ = false; + gotPvt_ = false; + ubx_.pps = pps_timestamp; } bool Ubx::display(void) { - UbxPacket p; - - char name_nav[] = "Ubx (nav)"; - char name_pvt[] = "Ubx (pvt)"; - - if(rxFifo_.read((uint8_t*)&p,sizeof(p))) - { - misc_header(name_pvt, p.drdy, p.timestamp, p.groupDelay); - misc_printf("%10.3f ms | ",(double)(p.timestamp-p.pps)/1000.); - misc_printf(" iTOW %10u | ", p.pvt.iTOW); - misc_printf("%02u/%02u/%04u ", p.pvt.month, p.pvt.day, p.pvt.year); - misc_printf("%02u:%02u:%09.6f", p.pvt.hour, p.pvt.min, (double)p.pvt.sec+(double)p.pvt.nano*1e-9); - misc_printf("%14.8f deg %14.8f deg | ",(double)p.pvt.lat*1e-7,(double)p.pvt.lon*1e-7); - misc_printf("numSV %02d\n\r", p.pvt.numSV); - - misc_header(name_nav, p.drdy, p.timestamp, p.groupDelay); - misc_printf("%10.3f ms | ",(double)(p.timestamp-p.pps)/1000.); - misc_printf(" TOW %14.3f\n\r", (double)p.nav.iTOW + (double)p.nav.fTOW/1000); - - } - else - { - misc_printf("%s\n\r",name_pvt); - misc_printf("%s\n\r",name_nav); - } - - return 1; + UbxPacket p; + + char name_nav[] = "Ubx (nav)"; + char name_pvt[] = "Ubx (pvt)"; + + if (rxFifo_.read((uint8_t *)&p, sizeof(p))) + { + misc_header(name_pvt, p.drdy, p.timestamp, p.groupDelay); + misc_printf("%10.3f ms | ", (double)(p.timestamp - p.pps) / 1000.); + misc_printf(" iTOW %10u | ", p.pvt.iTOW); + misc_printf("%02u/%02u/%04u ", p.pvt.month, p.pvt.day, p.pvt.year); + misc_printf("%02u:%02u:%09.6f", p.pvt.hour, p.pvt.min, (double)p.pvt.sec + (double)p.pvt.nano * 1e-9); + misc_printf("%14.8f deg %14.8f deg | ", (double)p.pvt.lat * 1e-7, (double)p.pvt.lon * 1e-7); + misc_printf("numSV %02d\n\r", p.pvt.numSV); + + misc_header(name_nav, p.drdy, p.timestamp, p.groupDelay); + misc_printf("%10.3f ms | ", (double)(p.timestamp - p.pps) / 1000.); + misc_printf(" TOW %14.3f\n\r", (double)p.nav.iTOW + (double)p.nav.fTOW / 1000); + } + else + { + misc_printf("%s\n\r", name_pvt); + misc_printf("%s\n\r", name_nav); + } + + return 1; } void Ubx::checksum(uint8_t *buffer) { - uint8_t a=0, b=0; - uint16_t n = ((((uint16_t)buffer[5])<<8) | (uint16_t)buffer[4]) + 6; - for(int16_t i=2;i -#include #include +#include +#include #include #include -#include extern Time64 time64; #define VCP_TX_FIFO_BUFFERS SERIAL_TX_FIFO_BUFFERS -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t vcp_fifo_tx_buffer[VCP_TX_FIFO_BUFFERS*sizeof(SerialTxPacket)]={0}; +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t vcp_fifo_tx_buffer[VCP_TX_FIFO_BUFFERS * sizeof(SerialTxPacket)] = {0}; #define VCP_RX_FIFO_BUFFER_BYTES 4096 -__attribute__((section("my_buffers"))) __attribute__((aligned (32))) static uint8_t vcp_fifo_rx_buffer[VCP_RX_FIFO_BUFFER_BYTES]={0}; +__attribute__((section("my_buffers"))) +__attribute__((aligned(32))) static uint8_t vcp_fifo_rx_buffer[VCP_RX_FIFO_BUFFER_BYTES] = {0}; uint32_t Vcp::init(uint16_t sample_rate_hz) { - // Common initializations - sampleRateHz_ = sample_rate_hz; + // Common initializations + sampleRateHz_ = sample_rate_hz; - txTimeout_ = 0; - txDtimeout_ = 250;//1000000/sampleRateHz_; + txTimeout_ = 0; + txDtimeout_ = 250; // 1000000/sampleRateHz_; - txFifo_.init(VCP_TX_FIFO_BUFFERS, sizeof(SerialTxPacket), vcp_fifo_tx_buffer); // Packet Fifo - rxFifo_.init(VCP_RX_FIFO_BUFFER_BYTES, vcp_fifo_rx_buffer); // byte Fifo + txFifo_.init(VCP_TX_FIFO_BUFFERS, sizeof(SerialTxPacket), vcp_fifo_tx_buffer); // Packet Fifo + rxFifo_.init(VCP_RX_FIFO_BUFFER_BYTES, vcp_fifo_rx_buffer); // byte Fifo - txIdle_ = true; - retry_ = 0; + txIdle_ = true; + retry_ = 0; - // VCP-specific - MX_USB_DEVICE_Init(); + // VCP-specific + MX_USB_DEVICE_Init(); - return DRIVER_OK; + return DRIVER_OK; } -//typedef enum +// typedef enum //{ -// USBD_OK = 0U, -// USBD_BUSY, -// USBD_EMEM, -// USBD_FAIL, -//} USBD_StatusTypeDef; +// USBD_OK = 0U, +// USBD_BUSY, +// USBD_EMEM, +// USBD_FAIL, +// } USBD_StatusTypeDef; /** * @fn uint16_t writePacket(SerialTxPacket*) @@ -87,7 +89,7 @@ uint32_t Vcp::init(uint16_t sample_rate_hz) uint16_t Vcp::writePacket(SerialTxPacket* p_new) { - return txFifo_.write((uint8_t*)p_new,sizeof(SerialTxPacket)); + return txFifo_.write((uint8_t*)p_new, sizeof(SerialTxPacket)); } /** @@ -97,9 +99,9 @@ uint16_t Vcp::writePacket(SerialTxPacket* p_new) */ void Vcp::poll(void) { -// // TX - if((txFifo_.packetCountMax()>0)&&((time64.Us() > txTimeout_))) - txStart(); + // // TX + if ((txFifo_.packetCountMax() > 0) && ((time64.Us() > txTimeout_))) + txStart(); } /** * @fn void txCdcCallback(void) @@ -108,39 +110,50 @@ void Vcp::poll(void) */ void Vcp::txCdcCallback(void) { - txStart(); + txStart(); } void Vcp::txStart() { - txTimeout_ = time64.Us() + txDtimeout_; - - static SerialTxPacket p; - uint8_t status= !USBD_OK; - - if(retry_) - { - status = CDC_Transmit_FS((uint8_t*)p.payload, p.payloadSize); - if(status==USBD_OK) {retry_ = 0; txIdle_ = false;} - else {retry_--; txIdle_ = true; } -// return; - } - else if(txFifo_.packetCount()) - { - if(txFifo_.read((uint8_t*)&p,sizeof(SerialTxPacket))) - { - status = CDC_Transmit_FS((uint8_t*)p.payload, p.payloadSize); - if(status==USBD_OK) {retry_ = 0; txIdle_ = false; } - else {retry_ = 2; txIdle_ = true; } // try at most two more times - } - else - { - retry_ = 0; - } - } + txTimeout_ = time64.Us() + txDtimeout_; + + static SerialTxPacket p; + uint8_t status = !USBD_OK; + + if (retry_) + { + status = CDC_Transmit_FS((uint8_t*)p.payload, p.payloadSize); + if (status == USBD_OK) + { + retry_ = 0; + txIdle_ = false; + } + else + { + retry_--; + txIdle_ = true; + } + // return; + } + else if (txFifo_.packetCount()) + { + if (txFifo_.read((uint8_t*)&p, sizeof(SerialTxPacket))) + { + status = CDC_Transmit_FS((uint8_t*)p.payload, p.payloadSize); + if (status == USBD_OK) + { + retry_ = 0; + txIdle_ = false; + } + else + { + retry_ = 2; + txIdle_ = true; + } // try at most two more times + } + else + { + retry_ = 0; + } + } } - - - - - diff --git a/boards/varmint/src/board/misc.cpp b/boards/varmint/src/board/misc.cpp index 65d7b5db..46e68c8a 100644 --- a/boards/varmint/src/board/misc.cpp +++ b/boards/varmint/src/board/misc.cpp @@ -35,27 +35,23 @@ ****************************************************************************** **/ -#include +#include +#include +#include #include #include +#include #include #include -#include - -#include - -#include - - extern Time64 time64; extern bool verbose; extern UART_HandleTypeDef huart1; -#include #include +#include extern rosflight_firmware::ROSflight firmware; extern "C" @@ -77,66 +73,73 @@ extern "C" #define MAX_SPRINTF_CHARS 256 char sprintf_buffer[MAX_SPRINTF_CHARS]; -//typedef enum +// typedef enum //{ -// USBD_OK = 0U, -// USBD_BUSY, -// USBD_EMEM, -// USBD_FAIL, -//} USBD_StatusTypeDef; +// USBD_OK = 0U, +// USBD_BUSY, +// USBD_EMEM, +// USBD_FAIL, +// } USBD_StatusTypeDef; -void misc_printf(const char* format, ...) +void misc_printf(const char *format, ...) { - if(verbose) - { - va_list argp; - va_start(argp, format); - vprintf(format, argp); - va_end(argp); - } - -// uint8_t vcp_status=USBD_OK; -// uint64_t timeout = time64.Us()+1000000; -// va_list argp; -// va_start(argp, format); -// vsnprintf(sprintf_buffer, MAX_SPRINTF_CHARS, format, argp); -// while( (time64.Us()Instance)->NDTR; - return (size>dma_buffer_size)?dma_buffer_size:size; + uint16_t size = dma_buffer_size - ((DMA_Stream_TypeDef *)hdma_uart_rx->Instance)->NDTR; + return (size > dma_buffer_size) ? dma_buffer_size : size; } void misc_exit_status(uint32_t status) { - misc_printf("Exit Status: "); - if(status==DRIVER_OK) misc_printf(" DRIVER_OK"); - if(status & DRIVER_ID_MISMATCH) misc_printf(" DRIVER_ID_MISMATCH"); - if(status & DRIVER_SELF_DIAG_ERROR) misc_printf(" DRIVER_SELF_DIAG_ERROR"); - if(status & DRIVER_HAL_ERROR) misc_printf(" DRIVER_HAL_ERROR"); - if(status & DRIVER_HAL_ERROR2) misc_printf(" DRIVER_HAL_ERROR2"); - if(status & DRIVER_FIFO_INIT_ERROR) misc_printf(" DRIVER_FIFO_INIT_ERROR"); - if(status & UBX_ACK) misc_printf(" UBX_ACK,"); - if(status & UBX_NAK) misc_printf(" UBX_NAK,"); - if(status & UBX_ACKNAK_FAIL) misc_printf(" UBX_ACKNAK_FAIL"); - if(status & UBX_SUCCESS) misc_printf(" UBX_SUCCESS"); - if(status & UBX_FAIL_BAUD_CHANGE) misc_printf(" UBX_FAIL_BAUD_CHANGE"); - misc_printf("\n\r"); + misc_printf("Exit Status: "); + if (status == DRIVER_OK) + misc_printf(" DRIVER_OK"); + if (status & DRIVER_ID_MISMATCH) + misc_printf(" DRIVER_ID_MISMATCH"); + if (status & DRIVER_SELF_DIAG_ERROR) + misc_printf(" DRIVER_SELF_DIAG_ERROR"); + if (status & DRIVER_HAL_ERROR) + misc_printf(" DRIVER_HAL_ERROR"); + if (status & DRIVER_HAL_ERROR2) + misc_printf(" DRIVER_HAL_ERROR2"); + if (status & DRIVER_FIFO_INIT_ERROR) + misc_printf(" DRIVER_FIFO_INIT_ERROR"); + if (status & UBX_ACK) + misc_printf(" UBX_ACK,"); + if (status & UBX_NAK) + misc_printf(" UBX_NAK,"); + if (status & UBX_ACKNAK_FAIL) + misc_printf(" UBX_ACKNAK_FAIL"); + if (status & UBX_SUCCESS) + misc_printf(" UBX_SUCCESS"); + if (status & UBX_FAIL_BAUD_CHANGE) + misc_printf(" UBX_FAIL_BAUD_CHANGE"); + misc_printf("\n\r"); } - - - - diff --git a/boards/varmint/src/board/sandbox.cpp b/boards/varmint/src/board/sandbox.cpp index 41167d4a..619c92de 100644 --- a/boards/varmint/src/board/sandbox.cpp +++ b/boards/varmint/src/board/sandbox.cpp @@ -35,12 +35,10 @@ ****************************************************************************** **/ -#include - #include -#include - #include +#include +#include extern Varmint varmint; extern Time64 time64; @@ -52,75 +50,82 @@ extern bool verbose; void verbose_dashes(void) { - for(int i=0;i - +#include +#include #include +#include #include +#include #include -#include - -#include #include -#include extern Varmint varmint; #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif -int main(void); + int main(void); #ifdef __cplusplus } @@ -67,21 +66,20 @@ int main(void); int main(void) { - // Rosflight board code - varmint.init_board(); -// sandbox(); + // Rosflight board code + varmint.init_board(); + // sandbox(); - // Rosflight base code - rosflight_firmware::Mavlink mavlink(varmint); - rosflight_firmware::ROSflight firmware(varmint, mavlink); + // Rosflight base code + rosflight_firmware::Mavlink mavlink(varmint); + rosflight_firmware::ROSflight firmware(varmint, mavlink); - firmware.init(); + firmware.init(); - while (true) - { - firmware.run(); - } + while (true) + { + firmware.run(); + } - return 0; + return 0; } - diff --git a/scripts/fix-code-style.sh b/scripts/fix-code-style.sh index 85d77f50..9b43c265 100755 --- a/scripts/fix-code-style.sh +++ b/scripts/fix-code-style.sh @@ -7,5 +7,5 @@ echo #SCRIPTPATH cd $SCRIPTPATH/.. find . -iname "*.h" -o -iname "*.cpp" | \ -egrep -v "^(./comms/mavlink/v1.0/|./boards/airbourne/airbourne/|./boards/breezy/breezystm32|./.git|./test/build)" | \ + grep -Ev "^(./comms/mavlink/v1.0/|./.git|./boards/varmint/lib/)" | \ xargs clang-format -i