diff --git a/README.md b/README.md index 949120b..3bce290 100644 --- a/README.md +++ b/README.md @@ -67,6 +67,7 @@ The main features the RS41ng firmware are: * Support for transmitting multiple modes consecutively with custom, rotating comment messages (see `config.c`) * Support for GPS-based scheduling is available for transmission modes that require specific timing for transmissions * Support for custom sensors via the external I²C bus +* Support for counting pulses on expansion header pin 2 (I2C2_SDA (PB11) / UART3 RX) for use with sensors like Geiger counters * GPS NMEA data output via the external serial port pin 3 (see below). This disables use of I²C devices as the serial port pins are shared with the I²C bus pins. * This allows using the RS41 sonde GPS data in external tracker hardware, such as Raspberry Pi or other microcontrollers. * Enhanced support for the internal Si4032 radio transmitter via PWM-based tone generation (and ultimately DMA-based symbol timing, if possible) @@ -200,6 +201,7 @@ ______________________| |______________________ * 1 - GND * 2 - I2C2_SDA (PB11) / UART3 RX * This is the external I²C port data pin for Si5351 and sensors + * This pin can be used as input for the pulse counter. * 3 - I2C2_SCL (PB10) / UART3 TX * This is the external I²C port clock pin for Si5351 and sensors * This pin can alternatively be used to output GPS NMEA data to external tracker hardware (e.g. Raspberry Pi or other microcontrollers) diff --git a/src/codecs/horus/horus_packet_v2.c b/src/codecs/horus/horus_packet_v2.c index b03d099..01c3bda 100644 --- a/src/codecs/horus/horus_packet_v2.c +++ b/src/codecs/horus/horus_packet_v2.c @@ -1,6 +1,7 @@ #include #include "horus_packet_v2.h" #include "horus_common.h" +#include "config.h" volatile uint16_t horus_v2_packet_counter = 0; @@ -15,7 +16,7 @@ size_t horus_packet_v2_create(uint8_t *payload, size_t length, telemetry_data *d float float_lat = (float) gps_data->latitude_degrees_1000000 / 10000000.0f; float float_lon = (float) gps_data->longitude_degrees_1000000 / 10000000.0f; - uint8_t volts_scaled = (uint8_t)(255 * (float) data->battery_voltage_millivolts / 5000.0f); + uint8_t volts_scaled = (uint8_t) (255 * (float) data->battery_voltage_millivolts / 5000.0f); horus_v2_packet_counter++; @@ -29,8 +30,8 @@ size_t horus_packet_v2_create(uint8_t *payload, size_t length, telemetry_data *d horus_packet.Seconds = gps_data->seconds; horus_packet.Latitude = float_lat; horus_packet.Longitude = float_lon; - horus_packet.Altitude = (uint16_t)((gps_data->altitude_mm > 0 ? gps_data->altitude_mm : 0) / 1000); - horus_packet.Speed = (uint8_t)((float) gps_data->ground_speed_cm_per_second * 0.036); + horus_packet.Altitude = (uint16_t) ((gps_data->altitude_mm > 0 ? gps_data->altitude_mm : 0) / 1000); + horus_packet.Speed = (uint8_t) ((float) gps_data->ground_speed_cm_per_second * 0.036); horus_packet.BattVoltage = volts_scaled; horus_packet.Sats = gps_data->satellites_visible; @@ -66,7 +67,13 @@ size_t horus_packet_v2_create(uint8_t *payload, size_t length, telemetry_data *d // Unit: mbar * 10 uint16_t ext_pressure_mbar = (uint16_t) (data->pressure_mbar_100 / 10.0f); memcpy(custom_data_pointer, &ext_pressure_mbar, sizeof(ext_pressure_mbar)); - // custom_data_pointer += sizeof(ext_pressure_mbar); + + if (pulse_counter_enabled) { + // Unit: pulse count + custom_data_pointer += sizeof(ext_pressure_mbar); + uint16_t ext_pulse_count = (uint16_t) data->pulse_count; + memcpy(custom_data_pointer, &ext_pulse_count, sizeof(ext_pulse_count)); + } horus_packet.Checksum = (uint16_t) calculate_crc16_checksum((char *) &horus_packet, sizeof(horus_packet) - sizeof(horus_packet.Checksum)); diff --git a/src/config.c b/src/config.c index 9e0516b..4a95ffe 100644 --- a/src/config.c +++ b/src/config.c @@ -25,6 +25,7 @@ * $gs - Ground speed in km/h (up to 3 chars) * $cl - Climb in m/s (up to 2 chars) * $he - Heading in degrees (up to 3 chars) + * $pc - Pulse counter value (wraps to zero at 65535, 16-bit unsigned value) * * Allowed message lengths: * @@ -44,6 +45,7 @@ bool leds_enabled = LEDS_ENABLE; bool bmp280_enabled = SENSOR_BMP280_ENABLE; bool si5351_enabled = RADIO_SI5351_ENABLE; bool gps_nmea_output_enabled = GPS_NMEA_OUTPUT_VIA_SERIAL_PORT_ENABLE; +bool pulse_counter_enabled = PULSE_COUNTER_ENABLE; volatile bool system_initialized = false; diff --git a/src/config.h b/src/config.h index 5a73ac6..ea16cc0 100644 --- a/src/config.h +++ b/src/config.h @@ -55,6 +55,33 @@ #error GPS NMEA output via serial port cannot be enabled simultaneously with the I2C bus. #endif +// Enable pulse counter via expansion header pin for use with devices like Geiger counters. +// This disables the external I²C bus and the serial port as the expansion header pin 2 (I2C2_SDA (PB11) / UART3 RX) is used for pulse input. +// Also changes the Horus 4FSK V2 data format and adds a custom data field for pulse count. +// The pulse count will wrap to zero at 65535 as it is stored as a 16-bit unsigned integer value. +#define PULSE_COUNTER_ENABLE false + +// Pulse counter pin modes +#define PULSE_COUNTER_PIN_MODE_FLOATING 0 +#define PULSE_COUNTER_PIN_MODE_INTERNAL_PULL_UP 1 +#define PULSE_COUNTER_PIN_MODE_INTERNAL_PULL_DOWN 2 + +// Enable the internal pull-up or pull-down resistor on expansion header pin 2 (I2C2_SDA (PB11) / UART3 RX) +// This is necessary if the pulse counter needs to count pulses where the pin is pulled low (ground) or high (VCC) during the pulse. +// Set to "floating" if the circuit that generates the pulses already has a pull-up or a pull-down resistor. +#define PULSE_COUNTER_PIN_MODE PULSE_COUNTER_PIN_MODE_INTERNAL_PULL_UP + +// Pulse counter interrupt edges +#define PULSE_COUNTER_INTERRUPT_EDGE_FALLING 1 +#define PULSE_COUNTER_INTERRUPT_EDGE_RISING 2 + +// Set the edge of the pulse where the interrupt is triggered: falling or rising. +#define PULSE_COUNTER_INTERRUPT_EDGE PULSE_COUNTER_INTERRUPT_EDGE_FALLING + +#if (PULSE_COUNTER_ENABLE) && ((GPS_NMEA_OUTPUT_VIA_SERIAL_PORT_ENABLE) || (RADIO_SI5351_ENABLE) || (SENSOR_BMP280_ENABLE)) +#error Pulse counter cannot be enabled simultaneously with GPS NMEA output or I2C bus sensors. +#endif + /** * Built-in Si4032 radio chip transmission configuration */ diff --git a/src/config_internal.h b/src/config_internal.h index 60c5539..71e8a87 100644 --- a/src/config_internal.h +++ b/src/config_internal.h @@ -19,6 +19,7 @@ extern bool leds_enabled; extern bool gps_nmea_output_enabled; extern bool bmp280_enabled; extern bool si5351_enabled; +extern bool pulse_counter_enabled; extern volatile bool system_initialized; diff --git a/src/drivers/pulse_counter/pulse_counter.c b/src/drivers/pulse_counter/pulse_counter.c new file mode 100644 index 0000000..1019305 --- /dev/null +++ b/src/drivers/pulse_counter/pulse_counter.c @@ -0,0 +1,55 @@ +#include "pulse_counter.h" +#include "stm32f10x_exti.h" +#include "stm32f10x_gpio.h" +#include "stm32f10x.h" +#include "misc.h" +#include "config.h" + +// The pulse count will wrap to zero at 65535 as it is stored as a 16-bit unsigned integer value +uint16_t pulse_count = 0; + +void pulse_counter_init(int pin_mode, int edge) +{ + // Initialize pin PB11 with optional internal pull-up resistor + GPIO_InitTypeDef gpio_init; + gpio_init.GPIO_Pin = GPIO_Pin_11; + gpio_init.GPIO_Mode = (pin_mode == PULSE_COUNTER_PIN_MODE_INTERNAL_PULL_UP) + ? GPIO_Mode_IPU : + ((pin_mode == PULSE_COUNTER_PIN_MODE_INTERNAL_PULL_DOWN) + ? GPIO_Mode_IPD + : GPIO_Mode_IN_FLOATING); + gpio_init.GPIO_Speed = GPIO_Speed_10MHz; + GPIO_Init(GPIOB, &gpio_init); + + // PB11 is connected to interrupt line 11, set trigger on the configured edge and enable the interrupt + EXTI_InitTypeDef exti_init; + exti_init.EXTI_Line = EXTI_Line11; + exti_init.EXTI_Mode = EXTI_Mode_Interrupt; + exti_init.EXTI_Trigger = (edge == PULSE_COUNTER_INTERRUPT_EDGE_FALLING) + ? EXTI_Trigger_Falling + : EXTI_Trigger_Rising; + exti_init.EXTI_LineCmd = ENABLE; + EXTI_Init(&exti_init); + + // Attach interrupt line to port B + GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource11); + + // PB11 is connected to EXTI_Line11, which has EXTI15_10_IRQn vector. Use priority 0 for now. + NVIC_InitTypeDef NVIC_InitStruct; + NVIC_InitStruct.NVIC_IRQChannel = EXTI15_10_IRQn; + NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00; + NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00; + NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStruct); +} + +uint16_t pulse_counter_get_count() +{ + return pulse_count; +} + +void EXTI15_10_IRQHandler(void) +{ + pulse_count = pulse_count + 1; + EXTI_ClearITPendingBit(EXTI_Line11); +} diff --git a/src/drivers/pulse_counter/pulse_counter.h b/src/drivers/pulse_counter/pulse_counter.h new file mode 100644 index 0000000..92a7578 --- /dev/null +++ b/src/drivers/pulse_counter/pulse_counter.h @@ -0,0 +1,5 @@ +#include +#include + +void pulse_counter_init(int pin_mode, int edge); +uint16_t pulse_counter_get_count(); diff --git a/src/hal/stm_lib/inc/stm32f10x_exti.h b/src/hal/stm_lib/inc/stm32f10x_exti.h new file mode 100644 index 0000000..2e6a9f9 --- /dev/null +++ b/src/hal/stm_lib/inc/stm32f10x_exti.h @@ -0,0 +1,199 @@ +/** + ****************************************************************************** + * @file stm32f10x_exti.h + * @author MCD Application Team + * @version V3.6.1 + * @date 05-March-2012 + * @brief This file contains all the functions prototypes for the EXTI firmware + * library. + ******************************************************************************* + * Copyright (c) 2014, STMicroelectronics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************* + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F10x_EXTI_H +#define __STM32F10x_EXTI_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f10x.h" + +/** @addtogroup STM32F10x_StdPeriph_Driver + * @{ + */ + +/** @addtogroup EXTI + * @{ + */ + +/** @defgroup EXTI_Exported_Types + * @{ + */ + +/** + * @brief EXTI mode enumeration + */ + +typedef enum +{ + EXTI_Mode_Interrupt = 0x00, + EXTI_Mode_Event = 0x04 +}EXTIMode_TypeDef; + +#define IS_EXTI_MODE(MODE) (((MODE) == EXTI_Mode_Interrupt) || ((MODE) == EXTI_Mode_Event)) + +/** + * @brief EXTI Trigger enumeration + */ + +typedef enum +{ + EXTI_Trigger_Rising = 0x08, + EXTI_Trigger_Falling = 0x0C, + EXTI_Trigger_Rising_Falling = 0x10 +}EXTITrigger_TypeDef; + +#define IS_EXTI_TRIGGER(TRIGGER) (((TRIGGER) == EXTI_Trigger_Rising) || \ + ((TRIGGER) == EXTI_Trigger_Falling) || \ + ((TRIGGER) == EXTI_Trigger_Rising_Falling)) +/** + * @brief EXTI Init Structure definition + */ + +typedef struct +{ + uint32_t EXTI_Line; /*!< Specifies the EXTI lines to be enabled or disabled. + This parameter can be any combination of @ref EXTI_Lines */ + + EXTIMode_TypeDef EXTI_Mode; /*!< Specifies the mode for the EXTI lines. + This parameter can be a value of @ref EXTIMode_TypeDef */ + + EXTITrigger_TypeDef EXTI_Trigger; /*!< Specifies the trigger signal active edge for the EXTI lines. + This parameter can be a value of @ref EXTITrigger_TypeDef */ + + FunctionalState EXTI_LineCmd; /*!< Specifies the new state of the selected EXTI lines. + This parameter can be set either to ENABLE or DISABLE */ +}EXTI_InitTypeDef; + +/** + * @} + */ + +/** @defgroup EXTI_Exported_Constants + * @{ + */ + +/** @defgroup EXTI_Lines + * @{ + */ + +#define EXTI_Line0 ((uint32_t)0x00001) /*!< External interrupt line 0 */ +#define EXTI_Line1 ((uint32_t)0x00002) /*!< External interrupt line 1 */ +#define EXTI_Line2 ((uint32_t)0x00004) /*!< External interrupt line 2 */ +#define EXTI_Line3 ((uint32_t)0x00008) /*!< External interrupt line 3 */ +#define EXTI_Line4 ((uint32_t)0x00010) /*!< External interrupt line 4 */ +#define EXTI_Line5 ((uint32_t)0x00020) /*!< External interrupt line 5 */ +#define EXTI_Line6 ((uint32_t)0x00040) /*!< External interrupt line 6 */ +#define EXTI_Line7 ((uint32_t)0x00080) /*!< External interrupt line 7 */ +#define EXTI_Line8 ((uint32_t)0x00100) /*!< External interrupt line 8 */ +#define EXTI_Line9 ((uint32_t)0x00200) /*!< External interrupt line 9 */ +#define EXTI_Line10 ((uint32_t)0x00400) /*!< External interrupt line 10 */ +#define EXTI_Line11 ((uint32_t)0x00800) /*!< External interrupt line 11 */ +#define EXTI_Line12 ((uint32_t)0x01000) /*!< External interrupt line 12 */ +#define EXTI_Line13 ((uint32_t)0x02000) /*!< External interrupt line 13 */ +#define EXTI_Line14 ((uint32_t)0x04000) /*!< External interrupt line 14 */ +#define EXTI_Line15 ((uint32_t)0x08000) /*!< External interrupt line 15 */ +#define EXTI_Line16 ((uint32_t)0x10000) /*!< External interrupt line 16 Connected to the PVD Output */ +#define EXTI_Line17 ((uint32_t)0x20000) /*!< External interrupt line 17 Connected to the RTC Alarm event */ +#define EXTI_Line18 ((uint32_t)0x40000) /*!< External interrupt line 18 Connected to the USB Device/USB OTG FS + Wakeup from suspend event */ +#define EXTI_Line19 ((uint32_t)0x80000) /*!< External interrupt line 19 Connected to the Ethernet Wakeup event */ + +#define IS_EXTI_LINE(LINE) ((((LINE) & (uint32_t)0xFFF00000) == 0x00) && ((LINE) != (uint16_t)0x00)) +#define IS_GET_EXTI_LINE(LINE) (((LINE) == EXTI_Line0) || ((LINE) == EXTI_Line1) || \ + ((LINE) == EXTI_Line2) || ((LINE) == EXTI_Line3) || \ + ((LINE) == EXTI_Line4) || ((LINE) == EXTI_Line5) || \ + ((LINE) == EXTI_Line6) || ((LINE) == EXTI_Line7) || \ + ((LINE) == EXTI_Line8) || ((LINE) == EXTI_Line9) || \ + ((LINE) == EXTI_Line10) || ((LINE) == EXTI_Line11) || \ + ((LINE) == EXTI_Line12) || ((LINE) == EXTI_Line13) || \ + ((LINE) == EXTI_Line14) || ((LINE) == EXTI_Line15) || \ + ((LINE) == EXTI_Line16) || ((LINE) == EXTI_Line17) || \ + ((LINE) == EXTI_Line18) || ((LINE) == EXTI_Line19)) + + +/** + * @} + */ + +/** + * @} + */ + +/** @defgroup EXTI_Exported_Macros + * @{ + */ + +/** + * @} + */ + +/** @defgroup EXTI_Exported_Functions + * @{ + */ + +void EXTI_DeInit(void); +void EXTI_Init(EXTI_InitTypeDef* EXTI_InitStruct); +void EXTI_StructInit(EXTI_InitTypeDef* EXTI_InitStruct); +void EXTI_GenerateSWInterrupt(uint32_t EXTI_Line); +FlagStatus EXTI_GetFlagStatus(uint32_t EXTI_Line); +void EXTI_ClearFlag(uint32_t EXTI_Line); +ITStatus EXTI_GetITStatus(uint32_t EXTI_Line); +void EXTI_ClearITPendingBit(uint32_t EXTI_Line); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F10x_EXTI_H */ +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/hal/stm_lib/src/stm32f10x_exti.c b/src/hal/stm_lib/src/stm32f10x_exti.c new file mode 100644 index 0000000..d7094ef --- /dev/null +++ b/src/hal/stm_lib/src/stm32f10x_exti.c @@ -0,0 +1,284 @@ +/** + ****************************************************************************** + * @file stm32f10x_exti.c + * @author MCD Application Team + * @version V3.6.1 + * @date 05-March-2012 + * @brief This file provides all the EXTI firmware functions. + ******************************************************************************* + * Copyright (c) 2014, STMicroelectronics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************* + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f10x_exti.h" + +/** @addtogroup STM32F10x_StdPeriph_Driver + * @{ + */ + +/** @defgroup EXTI + * @brief EXTI driver modules + * @{ + */ + +/** @defgroup EXTI_Private_TypesDefinitions + * @{ + */ + +/** + * @} + */ + +/** @defgroup EXTI_Private_Defines + * @{ + */ + +#define EXTI_LINENONE ((uint32_t)0x00000) /* No interrupt selected */ + +/** + * @} + */ + +/** @defgroup EXTI_Private_Macros + * @{ + */ + +/** + * @} + */ + +/** @defgroup EXTI_Private_Variables + * @{ + */ + +/** + * @} + */ + +/** @defgroup EXTI_Private_FunctionPrototypes + * @{ + */ + +/** + * @} + */ + +/** @defgroup EXTI_Private_Functions + * @{ + */ + +/** + * @brief Deinitializes the EXTI peripheral registers to their default reset values. + * @param None + * @retval None + */ +void EXTI_DeInit(void) +{ + EXTI->IMR = 0x00000000; + EXTI->EMR = 0x00000000; + EXTI->RTSR = 0x00000000; + EXTI->FTSR = 0x00000000; + EXTI->PR = 0x000FFFFF; +} + +/** + * @brief Initializes the EXTI peripheral according to the specified + * parameters in the EXTI_InitStruct. + * @param EXTI_InitStruct: pointer to a EXTI_InitTypeDef structure + * that contains the configuration information for the EXTI peripheral. + * @retval None + */ +void EXTI_Init(EXTI_InitTypeDef* EXTI_InitStruct) +{ + uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_EXTI_MODE(EXTI_InitStruct->EXTI_Mode)); + assert_param(IS_EXTI_TRIGGER(EXTI_InitStruct->EXTI_Trigger)); + assert_param(IS_EXTI_LINE(EXTI_InitStruct->EXTI_Line)); + assert_param(IS_FUNCTIONAL_STATE(EXTI_InitStruct->EXTI_LineCmd)); + + tmp = (uint32_t)EXTI_BASE; + + if (EXTI_InitStruct->EXTI_LineCmd != DISABLE) + { + /* Clear EXTI line configuration */ + EXTI->IMR &= ~EXTI_InitStruct->EXTI_Line; + EXTI->EMR &= ~EXTI_InitStruct->EXTI_Line; + + tmp += EXTI_InitStruct->EXTI_Mode; + + *(__IO uint32_t *) tmp |= EXTI_InitStruct->EXTI_Line; + + /* Clear Rising Falling edge configuration */ + EXTI->RTSR &= ~EXTI_InitStruct->EXTI_Line; + EXTI->FTSR &= ~EXTI_InitStruct->EXTI_Line; + + /* Select the trigger for the selected external interrupts */ + if (EXTI_InitStruct->EXTI_Trigger == EXTI_Trigger_Rising_Falling) + { + /* Rising Falling edge */ + EXTI->RTSR |= EXTI_InitStruct->EXTI_Line; + EXTI->FTSR |= EXTI_InitStruct->EXTI_Line; + } + else + { + tmp = (uint32_t)EXTI_BASE; + tmp += EXTI_InitStruct->EXTI_Trigger; + + *(__IO uint32_t *) tmp |= EXTI_InitStruct->EXTI_Line; + } + } + else + { + tmp += EXTI_InitStruct->EXTI_Mode; + + /* Disable the selected external lines */ + *(__IO uint32_t *) tmp &= ~EXTI_InitStruct->EXTI_Line; + } +} + +/** + * @brief Fills each EXTI_InitStruct member with its reset value. + * @param EXTI_InitStruct: pointer to a EXTI_InitTypeDef structure which will + * be initialized. + * @retval None + */ +void EXTI_StructInit(EXTI_InitTypeDef* EXTI_InitStruct) +{ + EXTI_InitStruct->EXTI_Line = EXTI_LINENONE; + EXTI_InitStruct->EXTI_Mode = EXTI_Mode_Interrupt; + EXTI_InitStruct->EXTI_Trigger = EXTI_Trigger_Falling; + EXTI_InitStruct->EXTI_LineCmd = DISABLE; +} + +/** + * @brief Generates a Software interrupt. + * @param EXTI_Line: specifies the EXTI lines to be enabled or disabled. + * This parameter can be any combination of EXTI_Linex where x can be (0..19). + * @retval None + */ +void EXTI_GenerateSWInterrupt(uint32_t EXTI_Line) +{ + /* Check the parameters */ + assert_param(IS_EXTI_LINE(EXTI_Line)); + + EXTI->SWIER |= EXTI_Line; +} + +/** + * @brief Checks whether the specified EXTI line flag is set or not. + * @param EXTI_Line: specifies the EXTI line flag to check. + * This parameter can be: + * @arg EXTI_Linex: External interrupt line x where x(0..19) + * @retval The new state of EXTI_Line (SET or RESET). + */ +FlagStatus EXTI_GetFlagStatus(uint32_t EXTI_Line) +{ + FlagStatus bitstatus = RESET; + /* Check the parameters */ + assert_param(IS_GET_EXTI_LINE(EXTI_Line)); + + if ((EXTI->PR & EXTI_Line) != (uint32_t)RESET) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Clears the EXTI's line pending flags. + * @param EXTI_Line: specifies the EXTI lines flags to clear. + * This parameter can be any combination of EXTI_Linex where x can be (0..19). + * @retval None + */ +void EXTI_ClearFlag(uint32_t EXTI_Line) +{ + /* Check the parameters */ + assert_param(IS_EXTI_LINE(EXTI_Line)); + + EXTI->PR = EXTI_Line; +} + +/** + * @brief Checks whether the specified EXTI line is asserted or not. + * @param EXTI_Line: specifies the EXTI line to check. + * This parameter can be: + * @arg EXTI_Linex: External interrupt line x where x(0..19) + * @retval The new state of EXTI_Line (SET or RESET). + */ +ITStatus EXTI_GetITStatus(uint32_t EXTI_Line) +{ + ITStatus bitstatus = RESET; + uint32_t enablestatus = 0; + /* Check the parameters */ + assert_param(IS_GET_EXTI_LINE(EXTI_Line)); + + enablestatus = EXTI->IMR & EXTI_Line; + if (((EXTI->PR & EXTI_Line) != (uint32_t)RESET) && (enablestatus != (uint32_t)RESET)) + { + bitstatus = SET; + } + else + { + bitstatus = RESET; + } + return bitstatus; +} + +/** + * @brief Clears the EXTI's line pending bits. + * @param EXTI_Line: specifies the EXTI lines to clear. + * This parameter can be any combination of EXTI_Linex where x can be (0..19). + * @retval None + */ +void EXTI_ClearITPendingBit(uint32_t EXTI_Line) +{ + /* Check the parameters */ + assert_param(IS_EXTI_LINE(EXTI_Line)); + + EXTI->PR = EXTI_Line; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main.c b/src/main.c index 9469e2d..6ec4d67 100644 --- a/src/main.c +++ b/src/main.c @@ -7,6 +7,7 @@ #include "hal/datatimer.h" #include "drivers/ubxg6010/ubxg6010.h" #include "drivers/si4032/si4032.h" +#include "drivers/pulse_counter/pulse_counter.h" #include "bmp280_handler.h" #include "si5351_handler.h" #include "radio.h" @@ -33,7 +34,7 @@ void handle_timer_tick() if (leds_enabled) { // Blink fast until GPS fix is acquired - if (counter % (SYSTEM_SCHEDULER_TIMER_TICKS_PER_SECOND / 4) == 0) { + if (counter % (SYSTEM_SCHEDULER_TIMER_TICKS_PER_SECOND / 4) == 0) { if (GPS_HAS_FIX(current_gps_data)) { if (counter == 0) { led_state = !led_state; @@ -83,14 +84,18 @@ int main(void) if (gps_nmea_output_enabled) { log_info("External USART init\n"); usart_ext_init(EXTERNAL_SERIAL_PORT_BAUD_RATE); + } else if (pulse_counter_enabled) { + log_info("Pulse counter init\n"); + pulse_counter_init(PULSE_COUNTER_PIN_MODE, PULSE_COUNTER_INTERRUPT_EDGE); } else { log_info("I2C init: clock speed %d kHz\n", I2C_BUS_CLOCK_SPEED / 1000); i2c_init(I2C_BUS_CLOCK_SPEED); } + log_info("SPI init\n"); spi_init(); -gps_init: + gps_init: log_info("GPS init\n"); success = ubxg6010_init(); if (!success) { diff --git a/src/telemetry.c b/src/telemetry.c index 14588c4..1179edb 100644 --- a/src/telemetry.c +++ b/src/telemetry.c @@ -2,6 +2,7 @@ #include "hal/system.h" #include "drivers/si4032/si4032.h" #include "drivers/ubxg6010/ubxg6010.h" +#include "drivers/pulse_counter/pulse_counter.h" #include "bmp280_handler.h" #include "locator.h" #include "config.h" @@ -19,6 +20,10 @@ void telemetry_collect(telemetry_data *data) bmp280_read_telemetry(data); } + if (pulse_counter_enabled) { + data->pulse_count = pulse_counter_get_count(); + } + ubxg6010_get_current_gps_data(&data->gps); // Zero out position data if we don't have a valid GPS fix. diff --git a/src/telemetry.h b/src/telemetry.h index be9d5b2..4a59f54 100644 --- a/src/telemetry.h +++ b/src/telemetry.h @@ -15,6 +15,7 @@ typedef struct _telemetry_data { int32_t temperature_celsius_100; uint32_t pressure_mbar_100; uint32_t humidity_percentage_100; + uint16_t pulse_count; gps_data gps; diff --git a/src/template.c b/src/template.c index 9cd89f2..93f4c92 100644 --- a/src/template.c +++ b/src/template.c @@ -89,7 +89,8 @@ size_t template_replace(char *dest, size_t dest_len, char *src, telemetry_data * strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$alt", replacement); - snprintf(replacement, sizeof(replacement), "%d", (int) ((float) data->gps.ground_speed_cm_per_second * 3.6f / 100.0f)); + snprintf(replacement, sizeof(replacement), "%d", + (int) ((float) data->gps.ground_speed_cm_per_second * 3.6f / 100.0f)); strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$gs", replacement); @@ -101,6 +102,10 @@ size_t template_replace(char *dest, size_t dest_len, char *src, telemetry_data * strlcpy(temp, dest, dest_len); size_t len = str_replace(dest, dest_len, temp, "$he", replacement); + snprintf(replacement, sizeof(replacement), "%d", (int) data->pulse_count); + strlcpy(temp, dest, dest_len); + str_replace(dest, dest_len, temp, "$pc", replacement); + free(temp); return len; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index ac94aec..4b882c2 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -7,7 +7,7 @@ project(RS41ng_test C CXX) set(BINARY ${CMAKE_PROJECT_NAME}) -file(GLOB_RECURSE USER_SOURCES "../src/codecs/*.c" "../src/template.c" "../src/utils.c") +file(GLOB_RECURSE USER_SOURCES "../src/config.c" "../src/codecs/*.c" "../src/template.c" "../src/utils.c") file(GLOB_RECURSE USER_SOURCES_CXX "../src/codecs/*.cpp") file(GLOB_RECURSE USER_HEADERS "../src/codecs/*.h" "../src/template.h" "../src/utils.h" "../src/config.h")