From 50dd22ed0184d61c1a980fc95de52606641500ab Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 29 Oct 2025 15:14:21 -0700 Subject: [PATCH 001/101] still compiles --- FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp | 2 ++ FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp | 1 + 2 files changed, 3 insertions(+) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index e18b143f..83b7e32d 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -84,4 +84,6 @@ module ReferenceDeployment { instance comSplitterTelemetry: Svc.ComSplitter base id 0x10028000 instance antennaDeployer: Components.AntennaDeployer base id 0x10029000 + + instance spiDriver: Zephyr.ZephyrSpiDriver base id 0x10030000 } diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 6a9d2480..146fe21d 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -43,6 +43,7 @@ module ReferenceDeployment { instance comSplitterTelemetry # For UART sideband communication instance comDriver + instance spiDriver # ---------------------------------------------------------------------- # Pattern graph specifiers From d58b38a1d58e8909550e55f5ec0f2efa029e5056 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 5 Nov 2025 16:37:54 -0800 Subject: [PATCH 002/101] add FOO command --- .../Components/CMakeLists.txt | 1 + .../Components/MyComponent/CMakeLists.txt | 36 ++++++++++ .../Components/MyComponent/MyComponent.cpp | 36 ++++++++++ .../Components/MyComponent/MyComponent.fpp | 48 ++++++++++++++ .../Components/MyComponent/MyComponent.hpp | 54 +++++++++++++++ .../Components/MyComponent/docs/sdd.md | 66 +++++++++++++++++++ .../ReferenceDeployment/Top/topology.fpp | 4 ++ 7 files changed, 245 insertions(+) create mode 100644 FprimeZephyrReference/Components/MyComponent/CMakeLists.txt create mode 100644 FprimeZephyrReference/Components/MyComponent/MyComponent.cpp create mode 100644 FprimeZephyrReference/Components/MyComponent/MyComponent.fpp create mode 100644 FprimeZephyrReference/Components/MyComponent/MyComponent.hpp create mode 100644 FprimeZephyrReference/Components/MyComponent/docs/sdd.md diff --git a/FprimeZephyrReference/Components/CMakeLists.txt b/FprimeZephyrReference/Components/CMakeLists.txt index 5245d97d..c6c4df16 100644 --- a/FprimeZephyrReference/Components/CMakeLists.txt +++ b/FprimeZephyrReference/Components/CMakeLists.txt @@ -9,3 +9,4 @@ add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Watchdog") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Burnwire/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/BootloaderTrigger/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/AntennaDeployer/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/MyComponent/") diff --git a/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt b/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt new file mode 100644 index 00000000..2667f799 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt @@ -0,0 +1,36 @@ +#### +# F Prime CMakeLists.txt: +# +# SOURCES: list of source files (to be compiled) +# AUTOCODER_INPUTS: list of files to be passed to the autocoders +# DEPENDS: list of libraries that this module depends on +# +# More information in the F´ CMake API documentation: +# https://fprime.jpl.nasa.gov/latest/docs/reference/api/cmake/API/ +# +#### + +# Module names are derived from the path from the nearest project/library/framework +# root when not specifically overridden by the developer. i.e. The module defined by +# `Ref/SignalGen/CMakeLists.txt` will be named `Ref_SignalGen`. + +register_fprime_library( + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/MyComponent.fpp" + SOURCES + "${CMAKE_CURRENT_LIST_DIR}/MyComponent.cpp" +# DEPENDS +# MyPackage_MyOtherModule +) + +### Unit Tests ### +# register_fprime_ut( +# AUTOCODER_INPUTS +# "${CMAKE_CURRENT_LIST_DIR}/MyComponent.fpp" +# SOURCES +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/MyComponentTestMain.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/MyComponentTester.cpp" +# DEPENDS +# STest # For rules-based testing +# UT_AUTO_HELPERS +# ) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp new file mode 100644 index 00000000..c815939f --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -0,0 +1,36 @@ +// ====================================================================== +// \title MyComponent.cpp +// \author jrpear +// \brief cpp file for MyComponent component implementation class +// ====================================================================== + +#include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" + +namespace Components { + +// ---------------------------------------------------------------------- +// Component construction and destruction +// ---------------------------------------------------------------------- + +MyComponent ::MyComponent(const char* const compName) : MyComponentComponentBase(compName) {} + +MyComponent ::~MyComponent() {} + +// ---------------------------------------------------------------------- +// Handler implementations for typed input ports +// ---------------------------------------------------------------------- + +void MyComponent ::run_handler(FwIndexType portNum, U32 context) { + // TODO +} + +// ---------------------------------------------------------------------- +// Handler implementations for commands +// ---------------------------------------------------------------------- + +void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { + // TODO + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); +} + +} // namespace Components diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp new file mode 100644 index 00000000..8a9350be --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -0,0 +1,48 @@ + + +module Components { + @ Component for F Prime FSW framework. + passive component MyComponent { + + ############################################################################## + #### Uncomment the following examples to start customizing your component #### + ############################################################################## + + @ Port receiving calls from the rate group + sync input port run: Svc.Sched + + @ Command for testing + sync command FOO() + + ############################################################################### + # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # + ############################################################################### + @ Port for requesting the current time + time get port timeCaller + + @ Port for sending command registrations + command reg port cmdRegOut + + @ Port for receiving commands + command recv port cmdIn + + @ Port for sending command responses + command resp port cmdResponseOut + + @ Port for sending textual representation of events + text event port logTextOut + + @ Port for sending events to downlink + event port logOut + + @ Port for sending telemetry channels to downlink + telemetry port tlmOut + + @ Port to return the value of a parameter + param get port prmGetOut + + @Port to set the value of a parameter + param set port prmSetOut + + } +} diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp new file mode 100644 index 00000000..db609a4e --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -0,0 +1,54 @@ +// ====================================================================== +// \title MyComponent.hpp +// \author jrpear +// \brief hpp file for MyComponent component implementation class +// ====================================================================== + +#ifndef Components_MyComponent_HPP +#define Components_MyComponent_HPP + +#include "FprimeZephyrReference/Components/MyComponent/MyComponentComponentAc.hpp" + +namespace Components { + +class MyComponent final : public MyComponentComponentBase { + public: + // ---------------------------------------------------------------------- + // Component construction and destruction + // ---------------------------------------------------------------------- + + //! Construct MyComponent object + MyComponent(const char* const compName //!< The component name + ); + + //! Destroy MyComponent object + ~MyComponent(); + + private: + // ---------------------------------------------------------------------- + // Handler implementations for typed input ports + // ---------------------------------------------------------------------- + + //! Handler implementation for run + //! + //! Port receiving calls from the rate group + void run_handler(FwIndexType portNum, //!< The port number + U32 context //!< The call order + ) override; + + private: + // ---------------------------------------------------------------------- + // Handler implementations for commands + // ---------------------------------------------------------------------- + + //! Handler implementation for command FOO + //! + //! Command for testing + void FOO_cmdHandler(FwOpcodeType opCode, //!< The opcode + U32 cmdSeq //!< The command sequence number + ) override; +}; + +} // namespace Components + +#endif diff --git a/FprimeZephyrReference/Components/MyComponent/docs/sdd.md b/FprimeZephyrReference/Components/MyComponent/docs/sdd.md new file mode 100644 index 00000000..2cc9562c --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/docs/sdd.md @@ -0,0 +1,66 @@ +# FprimeZephyrReference::MyComponent + +Component for F Prime FSW framework. + +## Usage Examples +Add usage examples here + +### Diagrams +Add diagrams here + +### Typical Usage +And the typical usage of the component here + +## Class Diagram +Add a class diagram here + +## Port Descriptions +| Name | Description | +|---|---| +|---|---| + +## Component States +Add component states in the chart below +| Name | Description | +|---|---| +|---|---| + +## Sequence Diagrams +Add sequence diagrams here + +## Parameters +| Name | Description | +|---|---| +|---|---| + +## Commands +| Name | Description | +|---|---| +|---|---| + +## Events +| Name | Description | +|---|---| +|---|---| + +## Telemetry +| Name | Description | +|---|---| +|---|---| + +## Unit Tests +Add unit test descriptions in the chart below +| Name | Description | Output | Coverage | +|---|---|---|---| +|---|---|---|---| + +## Requirements +Add requirements in the chart below +| Name | Description | Validation | +|---|---|---| +|---|---|---| + +## Change Log +| Date | Description | +|---|---| +|---| Initial Draft | diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 146fe21d..31b3440e 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -159,5 +159,9 @@ module ReferenceDeployment { imuManager.magneticFieldGet -> lis2mdlManager.magneticFieldGet imuManager.temperatureGet -> lsm6dsoManager.temperatureGet } + + connections MyConnectionGraph { + + } } } From 5acc25e43acf602b9226d5ca9b35122b3ed5f62c Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 5 Nov 2025 16:43:30 -0800 Subject: [PATCH 003/101] add MyComponent instance to Reference topology --- FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 83b7e32d..9cf7d92b 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -86,4 +86,6 @@ module ReferenceDeployment { instance antennaDeployer: Components.AntennaDeployer base id 0x10029000 instance spiDriver: Zephyr.ZephyrSpiDriver base id 0x10030000 + + instance mycomp: Components.MyComponent base id 0x10031000 } From ae42f73a5509ad4c4d41f20e89ca3836511c6497 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 5 Nov 2025 16:57:59 -0800 Subject: [PATCH 004/101] add spiSend output port to MyComponent --- FprimeZephyrReference/Components/MyComponent/MyComponent.fpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp index 8a9350be..55f935ce 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -14,6 +14,8 @@ module Components { @ Command for testing sync command FOO() + output port spiSend: Drv.SpiReadWrite + ############################################################################### # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # ############################################################################### From e1f0bbd7f9c33b9109cdd5995f2b74070570a770 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 5 Nov 2025 17:05:05 -0800 Subject: [PATCH 005/101] add mycomp to topology and connect to spiDriver --- FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 31b3440e..e8d8fd55 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -44,6 +44,7 @@ module ReferenceDeployment { # For UART sideband communication instance comDriver instance spiDriver + instance mycomp # ---------------------------------------------------------------------- # Pattern graph specifiers @@ -161,7 +162,7 @@ module ReferenceDeployment { } connections MyConnectionGraph { - + mycomp.spiSend -> spiDriver.SpiReadWrite } } } From c2b287fa824d0eae5b32549278d5b167789e8a7d Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 7 Nov 2025 18:33:30 -0800 Subject: [PATCH 006/101] not compiling---ZephyrSpiDriver has `configure` as private method --- .../Components/MyComponent/MyComponent.cpp | 6 ++++++ .../Components/MyComponent/MyComponent.fpp | 1 + FprimeZephyrReference/ReferenceDeployment/Main.cpp | 2 ++ .../Top/ReferenceDeploymentTopology.cpp | 6 ++++++ .../Top/ReferenceDeploymentTopologyDefs.hpp | 1 + .../proves_flight_control_board_v5-pinctrl.dtsi | 10 ++++++++++ .../proves_flight_control_board_v5.dtsi | 8 ++++++++ 7 files changed, 34 insertions(+) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index c815939f..a4d5aeaa 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -30,6 +30,12 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { // TODO + FwIndexType out_num = getNum_spiSend_OutputPorts(); + U8 write_data[] = {1, 2, 3, 4, 5, 6, 7, 8}; + U8 read_data[8]; + Fw::Buffer writeBuffer(write_data, sizeof(write_data)); + Fw::Buffer readBuffer(read_data, sizeof(read_data)); + this->spiSend_out(out_num, writeBuffer, readBuffer); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp index 55f935ce..a2c1af8e 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -14,6 +14,7 @@ module Components { @ Command for testing sync command FOO() + @ SPI Output Port output port spiSend: Drv.SpiReadWrite ############################################################################### diff --git a/FprimeZephyrReference/ReferenceDeployment/Main.cpp b/FprimeZephyrReference/ReferenceDeployment/Main.cpp index 17479569..067c777a 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Main.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Main.cpp @@ -12,6 +12,7 @@ const struct device* serial = DEVICE_DT_GET(DT_NODELABEL(cdc_acm_uart0)); const struct device* lora = DEVICE_DT_GET(DT_NODELABEL(lora0)); +const struct device* spi0 = DEVICE_DT_GET(DT_NODELABEL(spi0)); int main(int argc, char* argv[]) { // ** DO NOT REMOVE **// @@ -23,6 +24,7 @@ int main(int argc, char* argv[]) { Os::init(); // Object for communicating state to the topology ReferenceDeployment::TopologyState inputs; + inputs.spi0Device = spi0; inputs.loraDevice = lora; inputs.uartDevice = serial; inputs.baudRate = 115200; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index 21306966..01419058 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -12,6 +12,7 @@ #include #include +#include static const struct gpio_dt_spec ledGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(led0), gpios); static const struct gpio_dt_spec burnwire0Gpio = GPIO_DT_SPEC_GET(DT_NODELABEL(burnwire0), gpios); @@ -85,6 +86,11 @@ void setupTopology(const TopologyState& state) { // for over-the-air communications. lora.start(state.loraDevice, Zephyr::TransmitState::DISABLED); comDriver.configure(state.uartDevice, state.baudRate); + + struct spi_config cfg = { + .frequency = 10000000 // 10 MHz -- sx1280 has maximum 18.18 MHz + }; + spiDriver.configure(state.spi0Device, cfg) } void startRateGroups() { diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp index 81064f65..96fb4614 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp @@ -71,6 +71,7 @@ namespace ReferenceDeployment { struct TopologyState { const device* uartDevice; //!< UART device path for communication const device* loraDevice; //!< LoRa device path for communication + const device* spi0Device; //!< Spi device path for s-band LoRa module U32 baudRate; //!< Baud rate for UART communication CdhCore::SubtopologyState cdhCore; //!< Subtopology state for CdhCore ComCcsds::SubtopologyState comCcsds; //!< Subtopology state for ComCcsds diff --git a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5-pinctrl.dtsi b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5-pinctrl.dtsi index 0ae79cf6..ba96defd 100644 --- a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5-pinctrl.dtsi +++ b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5-pinctrl.dtsi @@ -11,6 +11,16 @@ input-enable; }; }; + spi0_default: spi0_default { + group1 { + pinmux = , ; + }; + + group2 { + pinmux = ; + input-enable; + }; + }; i2c1_default: i2c1_default { group1 { pinmux = , ; diff --git a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi index 397708c7..10bd8563 100644 --- a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi +++ b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi @@ -106,6 +106,14 @@ zephyr_udc0: &usbd { }; }; +&spi0 { + status = "okay"; + cs-gpios = <&gpio0 7 GPIO_ACTIVE_LOW>; + pinctrl-0 = <&spi0_default>; + pinctrl-names = "default"; +}; + + &i2c1 { status = "okay"; pinctrl-0 = <&i2c1_default>; From 8734b6be168bdc46acd13494e34bfd83abf07e72 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 8 Nov 2025 13:36:07 -0800 Subject: [PATCH 007/101] fix MyComponent out index --- FprimeZephyrReference/Components/MyComponent/MyComponent.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index a4d5aeaa..2c138227 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -30,12 +30,12 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { // TODO - FwIndexType out_num = getNum_spiSend_OutputPorts(); + FwIndexType out_idx = 0; U8 write_data[] = {1, 2, 3, 4, 5, 6, 7, 8}; U8 read_data[8]; Fw::Buffer writeBuffer(write_data, sizeof(write_data)); Fw::Buffer readBuffer(read_data, sizeof(read_data)); - this->spiSend_out(out_num, writeBuffer, readBuffer); + this->spiSend_out(out_idx, writeBuffer, readBuffer); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } From 11fcbc757bf0878878d13b683b0d0b2579c96b9f Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 8 Nov 2025 13:37:51 -0800 Subject: [PATCH 008/101] add missing semicolon --- .../ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index 01419058..c1e605e6 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -90,7 +90,7 @@ void setupTopology(const TopologyState& state) { struct spi_config cfg = { .frequency = 10000000 // 10 MHz -- sx1280 has maximum 18.18 MHz }; - spiDriver.configure(state.spi0Device, cfg) + spiDriver.configure(state.spi0Device, cfg); } void startRateGroups() { From 94dfc366ed6c09e5ab0a2bc1b693ff30eef8e7da Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Mon, 10 Nov 2025 23:11:18 -0800 Subject: [PATCH 009/101] add complete SPI config --- .../ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index c1e605e6..82ab7dbc 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -88,7 +88,11 @@ void setupTopology(const TopologyState& state) { comDriver.configure(state.uartDevice, state.baudRate); struct spi_config cfg = { - .frequency = 10000000 // 10 MHz -- sx1280 has maximum 18.18 MHz + .frequency = 1000000, // 1 MHz -- sx1280 has maximum 18.18 MHz -- there is a 12MHz oscillator on-board + .operation = SPI_WORD_SET(8), + .slave = 0, + .cs = 0, + }; spiDriver.configure(state.spi0Device, cfg); } From a6e919766944f83e123e9b6940693bd41c243ad3 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 13 Nov 2025 11:44:38 -0800 Subject: [PATCH 010/101] add chipselect functionality --- .../Top/ReferenceDeploymentTopology.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index 82ab7dbc..8544f60e 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -87,11 +87,16 @@ void setupTopology(const TopologyState& state) { lora.start(state.loraDevice, Zephyr::TransmitState::DISABLED); comDriver.configure(state.uartDevice, state.baudRate); + static struct spi_cs_control cs_ctrl = { + .gpio = GPIO_DT_SPEC_GET_BY_IDX(DT_NODELABEL(spi0), cs_gpios, 0), + .delay = 0U, /* us to wait after asserting CS before transfer */ + }; + struct spi_config cfg = { - .frequency = 1000000, // 1 MHz -- sx1280 has maximum 18.18 MHz -- there is a 12MHz oscillator on-board + .frequency = 100000, // 100 KHz -- sx1280 has maximum 18.18 MHz -- there is a 12MHz oscillator on-board .operation = SPI_WORD_SET(8), .slave = 0, - .cs = 0, + .cs = cs_ctrl, }; spiDriver.configure(state.spi0Device, cfg); From 187888732de889ee2334e399137b6fd38bc144fc Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 13 Nov 2025 11:45:59 -0800 Subject: [PATCH 011/101] change command to GetStatus and log recvd bytes --- .../Components/MyComponent/MyComponent.cpp | 11 +++++++++-- .../Components/MyComponent/MyComponent.fpp | 3 +++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 2c138227..1571d322 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -5,6 +5,7 @@ // ====================================================================== #include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" +#include namespace Components { @@ -31,11 +32,17 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { // TODO FwIndexType out_idx = 0; - U8 write_data[] = {1, 2, 3, 4, 5, 6, 7, 8}; - U8 read_data[8]; + // Firmware version is at 0x153 -- expect 0xB7A9 or 0xB5A9 + // OP ADDR_HI ADDR_LOW NOP NOP NOP + /*U8 write_data[] = {0x19, 0x1, 0x53, 0, 0, 0};*/ + U8 write_data[] = {0xC0, 0}; + U8 read_data[sizeof(write_data)]; Fw::Buffer writeBuffer(write_data, sizeof(write_data)); Fw::Buffer readBuffer(read_data, sizeof(read_data)); this->spiSend_out(out_idx, writeBuffer, readBuffer); + for (int i = 0; i < sizeof(read_data); i++) { + Fw::Logger::log("Byte %i: %" PRI_U8 "\n", i, read_data[i]); + } this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp index a2c1af8e..264b2170 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -11,6 +11,9 @@ module Components { @ Port receiving calls from the rate group sync input port run: Svc.Sched + # @ Import Communication Interface + # import Svc.Com + @ Command for testing sync command FOO() From bff957a71b93d8d50ff3245d30571fe7966976c9 Mon Sep 17 00:00:00 2001 From: Michael Pham <61564344+Mikefly123@users.noreply.github.com> Date: Thu, 13 Nov 2025 11:56:15 -0800 Subject: [PATCH 012/101] Update MyComponent.cpp --- FprimeZephyrReference/Components/MyComponent/MyComponent.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 1571d322..2521fbce 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -5,6 +5,7 @@ // ====================================================================== #include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" + #include namespace Components { From a6ba2c9bcff5a3768be0d0eff77e6082eea234ad Mon Sep 17 00:00:00 2001 From: Michael Pham <61564344+Mikefly123@users.noreply.github.com> Date: Thu, 13 Nov 2025 12:01:17 -0800 Subject: [PATCH 013/101] Update instances.fpp --- FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 3b84966a..102a5f65 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -95,7 +95,7 @@ module ReferenceDeployment { instance spiDriver: Zephyr.ZephyrSpiDriver base id 0x10030000 - instance mycomp: Components.MyComponent base id 0x10031000 + instance mycomp: Components.MyComponent base id 0x10041000 instance gpioface4LS: Zephyr.ZephyrGpioDriver base id 0x1002A000 From 43516136d955f4b2bd8cb6c9e2dae0081b7c1e6a Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 13 Nov 2025 13:46:31 -0800 Subject: [PATCH 014/101] add reset command --- .../Components/MyComponent/MyComponent.cpp | 9 +++++++++ .../Components/MyComponent/MyComponent.fpp | 6 ++++++ .../Components/MyComponent/MyComponent.hpp | 7 +++++++ .../Top/ReferenceDeploymentTopology.cpp | 2 ++ .../ReferenceDeployment/Top/instances.fpp | 4 +++- .../ReferenceDeployment/Top/topology.fpp | 4 +++- .../proves_flight_control_board_v5.dtsi | 7 ++++++- 7 files changed, 36 insertions(+), 3 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 2521fbce..da4899b2 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -47,4 +47,13 @@ void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } +void MyComponent ::RESET_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { + // UNTESTED + this->resetSend_out(0, Fw::Logic::LOW); + /*this->gpioSet_out(0, (Fw::On::ON == this->m_state) ? Fw::Logic::HIGH : Fw::Logic::LOW);*/ + Os::Task::delay(Fw::TimeInterval(0, 1000)); + this->resetSend_out(0, Fw::Logic::HIGH); + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); +} + } // namespace Components diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp index 264b2170..b8201289 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -17,9 +17,15 @@ module Components { @ Command for testing sync command FOO() + @ Reset Radio Module + sync command RESET() + @ SPI Output Port output port spiSend: Drv.SpiReadWrite + @ Radio Module Reset GPIO + output port resetSend: Drv.GpioWrite + ############################################################################### # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # ############################################################################### diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index db609a4e..b5a579ad 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -47,6 +47,13 @@ class MyComponent final : public MyComponentComponentBase { void FOO_cmdHandler(FwOpcodeType opCode, //!< The opcode U32 cmdSeq //!< The command sequence number ) override; + + //! Handler implementation for command RESET + //! + //! Reset Radio Module + void RESET_cmdHandler(FwOpcodeType opCode, //!< The opcode + U32 cmdSeq //!< The command sequence number + ) override; }; } // namespace Components diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index d63f54f9..c85c5291 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -26,6 +26,7 @@ static const struct gpio_dt_spec face5LoadSwitchGpio = GPIO_DT_SPEC_GET(DT_NODEL static const struct gpio_dt_spec payloadPowerLoadSwitchGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(payload_pwr_enable), gpios); static const struct gpio_dt_spec payloadBatteryLoadSwitchGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(payload_batt_enable), gpios); +static const struct gpio_dt_spec sbandNrstGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(sband_nrst), gpios); // Allows easy reference to objects in FPP/autocoder required namespaces using namespace ReferenceDeployment; @@ -80,6 +81,7 @@ void configureTopology() { gpioface5LS.open(face5LoadSwitchGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); gpioPayloadPowerLS.open(payloadPowerLoadSwitchGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); gpioPayloadBatteryLS.open(payloadBatteryLoadSwitchGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); + gpioSbandNrst.open(sbandNrstGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); } // Public functions for use in main program are namespaced with deployment name ReferenceDeployment diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 102a5f65..99ad639c 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -93,10 +93,12 @@ module ReferenceDeployment { instance antennaDeployer: Components.AntennaDeployer base id 0x10029000 - instance spiDriver: Zephyr.ZephyrSpiDriver base id 0x10030000 + instance spiDriver: Zephyr.ZephyrSpiDriver base id 0x10040000 instance mycomp: Components.MyComponent base id 0x10041000 + instance gpioSbandNrst: Zephyr.ZephyrGpioDriver base id 0x10042000 + instance gpioface4LS: Zephyr.ZephyrGpioDriver base id 0x1002A000 instance gpioface0LS: Zephyr.ZephyrGpioDriver base id 0x1002B000 diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 1acf4bf0..52bbe239 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -53,7 +53,7 @@ module ReferenceDeployment { instance comDriver instance spiDriver instance mycomp - + instance gpioSbandNrst instance face4LoadSwitch instance face0LoadSwitch instance face1LoadSwitch @@ -211,7 +211,9 @@ module ReferenceDeployment { connections MyConnectionGraph { mycomp.spiSend -> spiDriver.SpiReadWrite + mycomp.resetSend -> gpioSbandNrst.gpioWrite } + connections ComCcsds_FileHandling { # File Downlink <-> ComQueue FileHandling.fileDownlink.bufferSendOut -> ComCcsdsUart.comQueue.bufferQueueIn[ComCcsds.Ports_ComBufferQueue.FILE] diff --git a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi index 2406a991..d82218b7 100644 --- a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi +++ b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi @@ -235,7 +235,7 @@ zephyr_udc0: &usbd { }; }; -// GPIO Expander IO +// GPIO Names / { // Define the GPIO outputs based on your schematic @@ -301,6 +301,11 @@ zephyr_udc0: &usbd { gpios = <&mcp23017 3 GPIO_ACTIVE_HIGH>; // GPA3 label = "PAYLOAD_BATT_ENABLE"; }; + + sband_nrst: sband-nrst { + gpios = <&gpio0 17 (GPIO_PULL_UP | GPIO_ACTIVE_LOW)>; + label = "SBAND_NRST"; + }; }; // Define GPIO inputs From a49554b8171282e56e40d69aadb0be0dd4f04c40 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 13 Nov 2025 15:05:41 -0800 Subject: [PATCH 015/101] add SetTx and GetStatus commands --- .../Components/MyComponent/MyComponent.cpp | 40 ++++++++++++------- .../Components/MyComponent/MyComponent.hpp | 5 +++ 2 files changed, 31 insertions(+), 14 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index da4899b2..c72b2882 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -8,6 +8,9 @@ #include +#define OP_SET_TX 0x83 +#define OP_GET_STATUS 0xC0 + namespace Components { // ---------------------------------------------------------------------- @@ -31,29 +34,38 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { // ---------------------------------------------------------------------- void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - // TODO - FwIndexType out_idx = 0; - // Firmware version is at 0x153 -- expect 0xB7A9 or 0xB5A9 - // OP ADDR_HI ADDR_LOW NOP NOP NOP - /*U8 write_data[] = {0x19, 0x1, 0x53, 0, 0, 0};*/ - U8 write_data[] = {0xC0, 0}; - U8 read_data[sizeof(write_data)]; - Fw::Buffer writeBuffer(write_data, sizeof(write_data)); - Fw::Buffer readBuffer(read_data, sizeof(read_data)); - this->spiSend_out(out_idx, writeBuffer, readBuffer); - for (int i = 0; i < sizeof(read_data); i++) { - Fw::Logger::log("Byte %i: %" PRI_U8 "\n", i, read_data[i]); - } + this->spiSetTx(); + U8 status = this->spiGetStatus(); + Fw::Logger::log("status: %" PRI_U8 "\n", status); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } void MyComponent ::RESET_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { // UNTESTED this->resetSend_out(0, Fw::Logic::LOW); - /*this->gpioSet_out(0, (Fw::On::ON == this->m_state) ? Fw::Logic::HIGH : Fw::Logic::LOW);*/ Os::Task::delay(Fw::TimeInterval(0, 1000)); this->resetSend_out(0, Fw::Logic::HIGH); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } +// SPI Commands + +void MyComponent ::spiSetTx() { + // periodBase (1ms), periodBaseCount[15:8], periodBaseCount[7:0] + U8 write_data[] = {OP_SET_TX, 0x02, 0x4, 0x00}; + U8 read_data[sizeof(write_data)]; + Fw::Buffer writeBuffer(write_data, sizeof(write_data)); + Fw::Buffer readBuffer(read_data, sizeof(read_data)); + this->spiSend_out(0, writeBuffer, readBuffer); +} + +U8 MyComponent ::spiGetStatus() { + U8 write_data[] = {OP_GET_STATUS, 0x00}; + U8 read_data[sizeof(write_data)]; + Fw::Buffer writeBuffer(write_data, sizeof(write_data)); + Fw::Buffer readBuffer(read_data, sizeof(read_data)); + this->spiSend_out(0, writeBuffer, readBuffer); + return read_data[sizeof(read_data) - 1]; +} + } // namespace Components diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index b5a579ad..fb371175 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -54,6 +54,11 @@ class MyComponent final : public MyComponentComponentBase { void RESET_cmdHandler(FwOpcodeType opCode, //!< The opcode U32 cmdSeq //!< The command sequence number ) override; + + //! SX1280 SPI Commands + + void spiSetTx(); + U8 spiGetStatus(); }; } // namespace Components From 9fdceaec937d7a3f1920f62fa2bd8f8cd235adf0 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 13 Nov 2025 16:11:02 -0800 Subject: [PATCH 016/101] add setRfFrequency --- .../Components/MyComponent/MyComponent.cpp | 24 ++++++++++++++++--- .../Components/MyComponent/MyComponent.hpp | 1 + 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index c72b2882..f35fc8fc 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -8,6 +8,7 @@ #include +#define OP_SET_RF_FREQUENCY 0x86 #define OP_SET_TX 0x83 #define OP_GET_STATUS 0xC0 @@ -34,6 +35,7 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { // ---------------------------------------------------------------------- void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { + this->spiSetRfFrequency(); this->spiSetTx(); U8 status = this->spiGetStatus(); Fw::Logger::log("status: %" PRI_U8 "\n", status); @@ -41,15 +43,31 @@ void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { } void MyComponent ::RESET_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - // UNTESTED - this->resetSend_out(0, Fw::Logic::LOW); - Os::Task::delay(Fw::TimeInterval(0, 1000)); + // BROKEN this->resetSend_out(0, Fw::Logic::HIGH); + Os::Task::delay(Fw::TimeInterval(0, 1000)); + this->resetSend_out(0, Fw::Logic::LOW); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } // SPI Commands +void MyComponent ::spiSetRfFrequency() { + constexpr long double target_freq = 2405000000; // 2.405 GHz + constexpr long double step = 52000000.0L / static_cast(1ULL << 18); // per datasheet + constexpr U32 steps = static_cast(target_freq / step); + static_assert(steps <= 0xFFFFFF, "Result must fit in 24 bits (<= 0xFFFFFF)."); + uint8_t b0 = (uint8_t)(steps & 0xFFu); // least significant byte + uint8_t b1 = (uint8_t)((steps >> 8) & 0xFFu); + uint8_t b2 = (uint8_t)((steps >> 16) & 0xFFu); + + U8 write_data[] = {OP_SET_RF_FREQUENCY, b2, b1, b0}; + U8 read_data[sizeof(write_data)]; + Fw::Buffer writeBuffer(write_data, sizeof(write_data)); + Fw::Buffer readBuffer(read_data, sizeof(read_data)); + this->spiSend_out(0, writeBuffer, readBuffer); +} + void MyComponent ::spiSetTx() { // periodBase (1ms), periodBaseCount[15:8], periodBaseCount[7:0] U8 write_data[] = {OP_SET_TX, 0x02, 0x4, 0x00}; diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index fb371175..f0a3b07b 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -57,6 +57,7 @@ class MyComponent final : public MyComponentComponentBase { //! SX1280 SPI Commands + void spiSetRfFrequency(); void spiSetTx(); U8 spiGetStatus(); }; From 8be9cd070a9eee3b955604cdef097024fb8b5177 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 13 Nov 2025 16:32:27 -0800 Subject: [PATCH 017/101] add TxEn and RxEn pins --- .../Top/ReferenceDeploymentTopology.cpp | 4 ++++ .../ReferenceDeployment/Top/instances.fpp | 4 ++++ .../ReferenceDeployment/Top/topology.fpp | 2 ++ .../proves_flight_control_board_v5.dtsi | 10 ++++++++++ 4 files changed, 20 insertions(+) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index c85c5291..c15bcf35 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -27,6 +27,8 @@ static const struct gpio_dt_spec payloadPowerLoadSwitchGpio = GPIO_DT_SPEC_GET(D static const struct gpio_dt_spec payloadBatteryLoadSwitchGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(payload_batt_enable), gpios); static const struct gpio_dt_spec sbandNrstGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(sband_nrst), gpios); +static const struct gpio_dt_spec sbandRxEnGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(sband_rx_en), gpios); +static const struct gpio_dt_spec sbandTxEnGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(sband_tx_en), gpios); // Allows easy reference to objects in FPP/autocoder required namespaces using namespace ReferenceDeployment; @@ -82,6 +84,8 @@ void configureTopology() { gpioPayloadPowerLS.open(payloadPowerLoadSwitchGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); gpioPayloadBatteryLS.open(payloadBatteryLoadSwitchGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); gpioSbandNrst.open(sbandNrstGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); + gpioSbandRxEn.open(sbandRxEnGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); + gpioSbandTxEn.open(sbandTxEnGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); } // Public functions for use in main program are namespaced with deployment name ReferenceDeployment diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 99ad639c..01c64a8e 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -99,6 +99,10 @@ module ReferenceDeployment { instance gpioSbandNrst: Zephyr.ZephyrGpioDriver base id 0x10042000 + instance gpioSbandRxEn: Zephyr.ZephyrGpioDriver base id 0x10043000 + + instance gpioSbandTxEn: Zephyr.ZephyrGpioDriver base id 0x10044000 + instance gpioface4LS: Zephyr.ZephyrGpioDriver base id 0x1002A000 instance gpioface0LS: Zephyr.ZephyrGpioDriver base id 0x1002B000 diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 52bbe239..7cdf62af 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -54,6 +54,8 @@ module ReferenceDeployment { instance spiDriver instance mycomp instance gpioSbandNrst + instance gpioSbandRxEn + instance gpioSbandTxEn instance face4LoadSwitch instance face0LoadSwitch instance face1LoadSwitch diff --git a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi index d82218b7..6388b248 100644 --- a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi +++ b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi @@ -306,6 +306,16 @@ zephyr_udc0: &usbd { gpios = <&gpio0 17 (GPIO_PULL_UP | GPIO_ACTIVE_LOW)>; label = "SBAND_NRST"; }; + + sband_rx_en: sband-rx-en { + gpios = <&gpio0 21 (GPIO_PULL_UP | GPIO_ACTIVE_LOW)>; + label = "SBAND_RX_EN"; + }; + + sband_tx_en: sband-tx-en { + gpios = <&gpio0 22 (GPIO_PULL_UP | GPIO_ACTIVE_LOW)>; + label = "SBAND_TX_EN"; + }; }; // Define GPIO inputs From 59676532c75a0cb7e99568fceb1930454e63903b Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 13 Nov 2025 16:45:30 -0800 Subject: [PATCH 018/101] add SetTxContinuousWave --- .../Components/MyComponent/MyComponent.cpp | 11 ++++++++++- .../Components/MyComponent/MyComponent.hpp | 1 + 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index f35fc8fc..c15d18a6 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -8,6 +8,7 @@ #include +#define OP_SET_CONTINUOUS_WAVE 0xD1 #define OP_SET_RF_FREQUENCY 0x86 #define OP_SET_TX 0x83 #define OP_GET_STATUS 0xC0 @@ -36,7 +37,7 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->spiSetRfFrequency(); - this->spiSetTx(); + this->spiSetTxContinuousWave(); U8 status = this->spiGetStatus(); Fw::Logger::log("status: %" PRI_U8 "\n", status); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); @@ -52,6 +53,14 @@ void MyComponent ::RESET_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { // SPI Commands +void MyComponent ::spiSetTxContinuousWave() { + U8 write_data[] = {OP_SET_CONTINUOUS_WAVE}; + U8 read_data[sizeof(write_data)]; + Fw::Buffer writeBuffer(write_data, sizeof(write_data)); + Fw::Buffer readBuffer(read_data, sizeof(read_data)); + this->spiSend_out(0, writeBuffer, readBuffer); +} + void MyComponent ::spiSetRfFrequency() { constexpr long double target_freq = 2405000000; // 2.405 GHz constexpr long double step = 52000000.0L / static_cast(1ULL << 18); // per datasheet diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index f0a3b07b..36dab4db 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -57,6 +57,7 @@ class MyComponent final : public MyComponentComponentBase { //! SX1280 SPI Commands + void spiSetTxContinuousWave(); void spiSetRfFrequency(); void spiSetTx(); U8 spiGetStatus(); From 7b24172f18b9c9f074e6bae6df2fefe2a0e36d3a Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 13 Nov 2025 19:46:33 -0800 Subject: [PATCH 019/101] set only tx_en --- .../proves_flight_control_board_v5.dtsi | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi index 6388b248..75f678d8 100644 --- a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi +++ b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi @@ -308,7 +308,7 @@ zephyr_udc0: &usbd { }; sband_rx_en: sband-rx-en { - gpios = <&gpio0 21 (GPIO_PULL_UP | GPIO_ACTIVE_LOW)>; + gpios = <&gpio0 21 (GPIO_PULL_UP | GPIO_ACTIVE_HIGH)>; label = "SBAND_RX_EN"; }; From f2ac67798f6955c74500e233b8547357504fc555 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 13 Nov 2025 19:46:48 -0800 Subject: [PATCH 020/101] add SetTxParams --- .../Components/MyComponent/MyComponent.cpp | 11 +++++++++++ .../Components/MyComponent/MyComponent.hpp | 1 + 2 files changed, 12 insertions(+) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index c15d18a6..cba57557 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -8,6 +8,7 @@ #include +#define OP_SET_TX_PARAMS 0x8E #define OP_SET_CONTINUOUS_WAVE 0xD1 #define OP_SET_RF_FREQUENCY 0x86 #define OP_SET_TX 0x83 @@ -37,6 +38,7 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->spiSetRfFrequency(); + this->spiSetTxParams(); this->spiSetTxContinuousWave(); U8 status = this->spiGetStatus(); Fw::Logger::log("status: %" PRI_U8 "\n", status); @@ -53,6 +55,15 @@ void MyComponent ::RESET_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { // SPI Commands +void MyComponent ::spiSetTxParams() { + // power, rampTime + U8 write_data[] = {OP_SET_TX_PARAMS, 0x1F, 0x80}; + U8 read_data[sizeof(write_data)]; + Fw::Buffer writeBuffer(write_data, sizeof(write_data)); + Fw::Buffer readBuffer(read_data, sizeof(read_data)); + this->spiSend_out(0, writeBuffer, readBuffer); +} + void MyComponent ::spiSetTxContinuousWave() { U8 write_data[] = {OP_SET_CONTINUOUS_WAVE}; U8 read_data[sizeof(write_data)]; diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 36dab4db..5fbe663a 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -57,6 +57,7 @@ class MyComponent final : public MyComponentComponentBase { //! SX1280 SPI Commands + void spiSetTxParams(); void spiSetTxContinuousWave(); void spiSetRfFrequency(); void spiSetTx(); From 79b423c077a5c3ac2bd0ac763867f032df908ecd Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 13 Nov 2025 20:11:31 -0800 Subject: [PATCH 021/101] add spiSetTxContinuousPreamble --- .../Components/MyComponent/MyComponent.cpp | 11 ++++++++++- .../Components/MyComponent/MyComponent.hpp | 1 + 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index cba57557..8487abc6 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -9,6 +9,7 @@ #include #define OP_SET_TX_PARAMS 0x8E +#define OP_SET_CONTINUOUS_PREAMBLE 0xD2 #define OP_SET_CONTINUOUS_WAVE 0xD1 #define OP_SET_RF_FREQUENCY 0x86 #define OP_SET_TX 0x83 @@ -39,7 +40,7 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->spiSetRfFrequency(); this->spiSetTxParams(); - this->spiSetTxContinuousWave(); + this->spiSetTxContinuousPreamble(); U8 status = this->spiGetStatus(); Fw::Logger::log("status: %" PRI_U8 "\n", status); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); @@ -64,6 +65,14 @@ void MyComponent ::spiSetTxParams() { this->spiSend_out(0, writeBuffer, readBuffer); } +void MyComponent ::spiSetTxContinuousPreamble() { + U8 write_data[] = {OP_SET_CONTINUOUS_PREAMBLE}; + U8 read_data[sizeof(write_data)]; + Fw::Buffer writeBuffer(write_data, sizeof(write_data)); + Fw::Buffer readBuffer(read_data, sizeof(read_data)); + this->spiSend_out(0, writeBuffer, readBuffer); +} + void MyComponent ::spiSetTxContinuousWave() { U8 write_data[] = {OP_SET_CONTINUOUS_WAVE}; U8 read_data[sizeof(write_data)]; diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 5fbe663a..683b42df 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -58,6 +58,7 @@ class MyComponent final : public MyComponentComponentBase { //! SX1280 SPI Commands void spiSetTxParams(); + void spiSetTxContinuousPreamble(); void spiSetTxContinuousWave(); void spiSetRfFrequency(); void spiSetTx(); From 389623e38c6d96c145d3371db7d7a30f5dbe35dc Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 13 Nov 2025 20:40:51 -0800 Subject: [PATCH 022/101] add SetModulationParams --- .../Components/MyComponent/MyComponent.cpp | 11 +++++++++++ .../Components/MyComponent/MyComponent.hpp | 1 + 2 files changed, 12 insertions(+) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 8487abc6..efefd1c7 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -8,6 +8,7 @@ #include +#define OP_SET_MODULATION_PARAMS 0x8B #define OP_SET_TX_PARAMS 0x8E #define OP_SET_CONTINUOUS_PREAMBLE 0xD2 #define OP_SET_CONTINUOUS_WAVE 0xD1 @@ -40,6 +41,7 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->spiSetRfFrequency(); this->spiSetTxParams(); + this->spiSetModulationParams(); this->spiSetTxContinuousPreamble(); U8 status = this->spiGetStatus(); Fw::Logger::log("status: %" PRI_U8 "\n", status); @@ -56,6 +58,15 @@ void MyComponent ::RESET_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { // SPI Commands +void MyComponent ::spiSetModulationParams() { + // Bitrate+Bandwidth, Modulation Index, Shaping + U8 write_data[] = {OP_SET_MODULATION_PARAMS, 0x4C, 0x0F, 0x00}; + U8 read_data[sizeof(write_data)]; + Fw::Buffer writeBuffer(write_data, sizeof(write_data)); + Fw::Buffer readBuffer(read_data, sizeof(read_data)); + this->spiSend_out(0, writeBuffer, readBuffer); +} + void MyComponent ::spiSetTxParams() { // power, rampTime U8 write_data[] = {OP_SET_TX_PARAMS, 0x1F, 0x80}; diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 683b42df..f7be16a2 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -57,6 +57,7 @@ class MyComponent final : public MyComponentComponentBase { //! SX1280 SPI Commands + void spiSetModulationParams(); void spiSetTxParams(); void spiSetTxContinuousPreamble(); void spiSetTxContinuousWave(); From a14029f632200bf98d57d2a1f2c1b67b051fd60e Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 11:00:10 -0800 Subject: [PATCH 023/101] add SetStandby --- .../Components/MyComponent/MyComponent.cpp | 11 +++++++++++ .../Components/MyComponent/MyComponent.hpp | 1 + 2 files changed, 12 insertions(+) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index efefd1c7..e9fcf9e2 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -14,6 +14,7 @@ #define OP_SET_CONTINUOUS_WAVE 0xD1 #define OP_SET_RF_FREQUENCY 0x86 #define OP_SET_TX 0x83 +#define OP_SET_STANDBY 0x80 #define OP_GET_STATUS 0xC0 namespace Components { @@ -39,6 +40,7 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { // ---------------------------------------------------------------------- void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { + this->spiSetStandby(); this->spiSetRfFrequency(); this->spiSetTxParams(); this->spiSetModulationParams(); @@ -117,6 +119,15 @@ void MyComponent ::spiSetTx() { this->spiSend_out(0, writeBuffer, readBuffer); } +void MyComponent ::spiSetStandby() { + // config + U8 write_data[] = {OP_SET_STANDBY, 0x00}; + U8 read_data[sizeof(write_data)]; + Fw::Buffer writeBuffer(write_data, sizeof(write_data)); + Fw::Buffer readBuffer(read_data, sizeof(read_data)); + this->spiSend_out(0, writeBuffer, readBuffer); +} + U8 MyComponent ::spiGetStatus() { U8 write_data[] = {OP_GET_STATUS, 0x00}; U8 read_data[sizeof(write_data)]; diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index f7be16a2..0ed2a455 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -63,6 +63,7 @@ class MyComponent final : public MyComponentComponentBase { void spiSetTxContinuousWave(); void spiSetRfFrequency(); void spiSetTx(); + void spiSetStandby(); U8 spiGetStatus(); }; From 6862c92937fdd15a6b22cac2308f9eba1d66e074 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 16:07:31 -0800 Subject: [PATCH 024/101] initial import of external radio driver code --- .../Components/MyComponent/CMakeLists.txt | 2 + .../Components/MyComponent/MyComponent.cpp | 6 + .../Components/MyComponent/radio.h | 302 +++ .../Components/MyComponent/sx1280-hal.cpp | 457 +++++ .../Components/MyComponent/sx1280-hal.h | 192 ++ .../Components/MyComponent/sx1280.cpp | 1063 +++++++++++ .../Components/MyComponent/sx1280.h | 1627 +++++++++++++++++ 7 files changed, 3649 insertions(+) create mode 100644 FprimeZephyrReference/Components/MyComponent/radio.h create mode 100644 FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp create mode 100644 FprimeZephyrReference/Components/MyComponent/sx1280-hal.h create mode 100644 FprimeZephyrReference/Components/MyComponent/sx1280.cpp create mode 100644 FprimeZephyrReference/Components/MyComponent/sx1280.h diff --git a/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt b/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt index 2667f799..e018e1d5 100644 --- a/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt +++ b/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt @@ -19,6 +19,8 @@ register_fprime_library( "${CMAKE_CURRENT_LIST_DIR}/MyComponent.fpp" SOURCES "${CMAKE_CURRENT_LIST_DIR}/MyComponent.cpp" + "${CMAKE_CURRENT_LIST_DIR}/sx1280.cpp" + "${CMAKE_CURRENT_LIST_DIR}/sx1280-hal.cpp" # DEPENDS # MyPackage_MyOtherModule ) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index e9fcf9e2..10789350 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -6,6 +6,12 @@ #include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" +/*#include "FprimeZephyrReference/Components/MyComponent/radio.h"*/ +/*#include "FprimeZephyrReference/Components/MyComponent/sx1280.h"*/ +/*#include "FprimeZephyrReference/Components/MyComponent/sx1280-hal.h"*/ +/*#include "FprimeZephyrReference/Components/MyComponent/sx1280.cpp"*/ +/*#include "FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp"*/ + #include #define OP_SET_MODULATION_PARAMS 0x8B diff --git a/FprimeZephyrReference/Components/MyComponent/radio.h b/FprimeZephyrReference/Components/MyComponent/radio.h new file mode 100644 index 00000000..bcdf1a82 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/radio.h @@ -0,0 +1,302 @@ +/* + ______ _ + / _____) _ | | +( (____ _____ ____ _| |_ _____ ____| |__ + \____ \| ___ | (_ _) ___ |/ ___) _ \ + _____) ) ____| | | || |_| ____( (___| | | | +(______/|_____)_|_|_| \__)_____)\____)_| |_| + (C)2016 Semtech + +Description: Handling of the node configuration protocol + +License: Revised BSD License, see LICENSE.TXT file include in the project + +Maintainer: Miguel Luis, Gregory Cristian and Matthieu Verdy +*/ +#ifndef __RADIO_H__ +#define __RADIO_H__ + +/*#include "mbed.h"*/ +#include + +/*! + * \brief Structure describing the radio status + */ +typedef union { + /*! + * \brief Structure of the radio status + */ + struct { + uint8_t CpuBusy : 1; //!< Flag for CPU radio busy + uint8_t DmaBusy : 1; //!< Flag for DMA busy + uint8_t CmdStatus : 3; //!< Command status + uint8_t ChipMode : 3; //!< Chip mode + } Fields; + + /*! + * \brief Serialized radio status + */ + uint8_t Value; +} RadioStatus_t; + +/*! + * \brief Structure describing the ranging codes for callback functions + */ +typedef enum { + IRQ_RANGING_SLAVE_ERROR_CODE = 0x00, + IRQ_RANGING_SLAVE_VALID_CODE, + IRQ_RANGING_MASTER_ERROR_CODE, + IRQ_RANGING_MASTER_VALID_CODE, +} IrqRangingCode_t; + +/*! + * \brief Structure describing the error codes for callback functions + */ +typedef enum { + IRQ_HEADER_ERROR_CODE = 0x00, + IRQ_SYNCWORD_ERROR_CODE, + IRQ_CRC_ERROR_CODE, + IRQ_RANGING_ON_LORA_ERROR_CODE, +} IrqErrorCode_t; + +/*! + * \brief Structure describing the validity codes for callback function rxValid + */ +typedef enum { + IRQ_HEADER_VALID_CODE = 0x00, + IRQ_SYNCWORD_VALID_CODE, +} IrqValidCode_t; + +/*! + * \brief Represents all possible opcode understood by the radio + */ +typedef enum RadioCommands_u { + RADIO_GET_STATUS = 0xC0, + RADIO_WRITE_REGISTER = 0x18, + RADIO_READ_REGISTER = 0x19, + RADIO_WRITE_BUFFER = 0x1A, + RADIO_READ_BUFFER = 0x1B, + RADIO_SET_SLEEP = 0x84, + RADIO_SET_STANDBY = 0x80, + RADIO_SET_FS = 0xC1, + RADIO_SET_TX = 0x83, + RADIO_SET_RX = 0x82, + RADIO_SET_RXDUTYCYCLE = 0x94, + RADIO_SET_CAD = 0xC5, + RADIO_SET_TXCONTINUOUSWAVE = 0xD1, + RADIO_SET_TXCONTINUOUSPREAMBLE = 0xD2, + RADIO_SET_PACKETTYPE = 0x8A, + RADIO_GET_PACKETTYPE = 0x03, + RADIO_SET_RFFREQUENCY = 0x86, + RADIO_SET_TXPARAMS = 0x8E, + RADIO_SET_CADPARAMS = 0x88, + RADIO_SET_BUFFERBASEADDRESS = 0x8F, + RADIO_SET_MODULATIONPARAMS = 0x8B, + RADIO_SET_PACKETPARAMS = 0x8C, + RADIO_GET_RXBUFFERSTATUS = 0x17, + RADIO_GET_PACKETSTATUS = 0x1D, + RADIO_GET_RSSIINST = 0x1F, + RADIO_SET_DIOIRQPARAMS = 0x8D, + RADIO_GET_IRQSTATUS = 0x15, + RADIO_CLR_IRQSTATUS = 0x97, + RADIO_CALIBRATE = 0x89, + RADIO_SET_REGULATORMODE = 0x96, + RADIO_SET_SAVECONTEXT = 0xD5, + RADIO_SET_AUTOTX = 0x98, + RADIO_SET_AUTOFS = 0x9E, + RADIO_SET_LONGPREAMBLE = 0x9B, + RADIO_SET_UARTSPEED = 0x9D, + RADIO_SET_RANGING_ROLE = 0xA3, +} RadioCommands_t; + +/*! + * \brief The radio callbacks structure + * Holds function pointers to be called on radio interrupts + */ +typedef struct { + void (*txDone)(void); //!< Pointer to a function run on successful transmission + void (*rxDone)(void); //!< Pointer to a function run on successful reception + void (*rxSyncWordDone)(void); //!< Pointer to a function run on successful SyncWord reception + void (*rxHeaderDone)(void); //!< Pointer to a function run on successful Header reception + void (*txTimeout)(void); //!< Pointer to a function run on transmission timeout + void (*rxTimeout)(void); //!< Pointer to a function run on reception timeout + void (*rxError)(IrqErrorCode_t errCode); //!< Pointer to a function run on reception error + void (*rangingDone)(IrqRangingCode_t val); //!< Pointer to a function run on ranging terminated + void (*cadDone)(bool cadFlag); //!< Pointer to a function run on channel activity detected +} RadioCallbacks_t; + +/*! + * \brief Class holding the basic communications with a radio + * + * It sets the functions to read/write registers, send commands and read/write + * payload. + * It also provides functions to run callback functions depending on the + * interrupts generated from the radio. + */ +class Radio { + protected: + /*! + * \brief Callback on Tx done interrupt + */ + void (*txDone)(void); + + /*! + * \brief Callback on Rx done interrupt + */ + void (*rxDone)(void); + + /*! + * \brief Callback on Rx SyncWord interrupt + */ + void (*rxSyncWordDone)(void); + + /*! + * \brief Callback on Rx header received interrupt + */ + void (*rxHeaderDone)(void); + + /*! + * \brief Callback on Tx timeout interrupt + */ + void (*txTimeout)(void); + + /*! + * \brief Callback on Rx timeout interrupt + */ + void (*rxTimeout)(void); + + /*! + * \brief Callback on Rx error interrupt + * + * \param [out] errCode A code indicating the type of interrupt (SyncWord error or CRC error) + */ + void (*rxError)(IrqErrorCode_t errCode); + + /*! + * \brief Callback on ranging done interrupt + * + * \param [out] val A flag indicating the type of interrupt (Master/Slave and Valid/Error) + */ + void (*rangingDone)(IrqRangingCode_t val); + + /*! + * \brief Callback on Channel Activity Detection done interrupt + * + * \param [out] cadFlag Flag for channel activity detected or not + */ + void (*cadDone)(bool cadFlag); + + public: + /*! + * \brief Constructor for radio class + * Sets the callbacks functions pointers + * + * \param [in] callbacks The structure of callbacks function pointers + * to be called on radio interrupts + * + */ + explicit Radio(RadioCallbacks_t* callbacks) { + this->txDone = callbacks->txDone; + this->rxDone = callbacks->rxDone; + this->rxSyncWordDone = callbacks->rxSyncWordDone; + this->rxHeaderDone = callbacks->rxHeaderDone; + this->txTimeout = callbacks->txTimeout; + this->rxTimeout = callbacks->rxTimeout; + this->rxError = callbacks->rxError; + this->rangingDone = callbacks->rangingDone; + this->cadDone = callbacks->cadDone; + } + virtual ~Radio(void) {}; + + /*! + * \brief Resets the radio + */ + virtual void Reset(void) = 0; + + /*! + * \brief Gets the current radio status + * + * \retval status Radio status + */ + virtual RadioStatus_t GetStatus(void) = 0; + + /*! + * \brief Writes the given command to the radio + * + * \param [in] opcode Command opcode + * \param [in] buffer Command parameters byte array + * \param [in] size Command parameters byte array size + */ + virtual void WriteCommand(RadioCommands_t opcode, uint8_t* buffer, uint16_t size) = 0; + + /*! + * \brief Reads the given command from the radio + * + * \param [in] opcode Command opcode + * \param [in] buffer Command parameters byte array + * \param [in] size Command parameters byte array size + */ + virtual void ReadCommand(RadioCommands_t opcode, uint8_t* buffer, uint16_t size) = 0; + + /*! + * \brief Writes multiple radio registers starting at address + * + * \param [in] address First Radio register address + * \param [in] buffer Buffer containing the new register's values + * \param [in] size Number of registers to be written + */ + virtual void WriteRegister(uint16_t address, uint8_t* buffer, uint16_t size) = 0; + + /*! + * \brief Writes the radio register at the specified address + * + * \param [in] address Register address + * \param [in] value New register value + */ + virtual void WriteRegister(uint16_t address, uint8_t value) = 0; + + /*! + * \brief Reads multiple radio registers starting at address + * + * \param [in] address First Radio register address + * \param [out] buffer Buffer where to copy the registers data + * \param [in] size Number of registers to be read + */ + virtual void ReadRegister(uint16_t address, uint8_t* buffer, uint16_t size) = 0; + + /*! + * \brief Reads the radio register at the specified address + * + * \param [in] address Register address + * + * \retval value The register value + */ + virtual uint8_t ReadRegister(uint16_t address) = 0; + + /*! + * \brief Writes Radio Data Buffer with buffer of size starting at offset. + * + * \param [in] offset Offset where to start writing + * \param [in] buffer Buffer pointer + * \param [in] size Buffer size + */ + virtual void WriteBuffer(uint8_t offset, uint8_t* buffer, uint8_t size) = 0; + + /*! + * \brief Reads Radio Data Buffer at offset to buffer of size + * + * \param [in] offset Offset where to start reading + * \param [out] buffer Buffer pointer + * \param [in] size Buffer size + */ + virtual void ReadBuffer(uint8_t offset, uint8_t* buffer, uint8_t size) = 0; + + /*! + * \brief Return firmware version + * + * \retval firmware The firmware version + */ + virtual uint16_t GetFirmwareVersion(void) = 0; +}; + +#endif // __RADIO_H__ diff --git a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp new file mode 100644 index 00000000..ccb6965b --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp @@ -0,0 +1,457 @@ +/* + ______ _ + / _____) _ | | +( (____ _____ ____ _| |_ _____ ____| |__ + \____ \| ___ | (_ _) ___ |/ ___) _ \ + _____) ) ____| | | || |_| ____( (___| | | | +(______/|_____)_|_|_| \__)_____)\____)_| |_| + (C)2016 Semtech + +Description: Handling of the node configuration protocol + +License: Revised BSD License, see LICENSE.TXT file include in the project + +Maintainer: Miguel Luis, Gregory Cristian and Matthieu Verdy +*/ +#include "sx1280-hal.h" + +/*! + * \brief Helper macro to create Interrupt objects only if the pin name is + * different from NC + */ +/*#define CreateDioPin(pinName, dio) \*/ +/* if (pinName == NC) { \*/ +/* dio = NULL; \*/ +/* } else { \*/ +/* dio = new Interrupt(pinName); \*/ +/* }*/ + +/*! + * \brief Helper macro to avoid duplicating code for setting dio pins parameters + */ +/*#if defined( TARGET_NUCLEO_L476RG )*/ +/*#define DioAssignCallback( dio, pinMode, callback ) \*/ +/* if( dio != NULL ) \*/ +/* { \*/ +/* dio->mode( pinMode ); \*/ +/* dio->rise( this, static_cast ( callback ) ); \*/ +/* }*/ +/*#else*/ +#define DioAssignCallback(dio, pinMode, callback) \ + if (dio != NULL) { \ + dio->rise(this, static_cast(callback)); \ + } +/*#endif*/ +/*! + * \brief Used to block execution waiting for low state on radio busy pin. + * Essentially used in SPI communications + */ +#define WaitOnBusy() \ + while (BUSY == 1) { \ + } + +/*! + * \brief Blocking routine for waiting the UART to be writeable + * + */ +#define WaitUartWritable() \ + while (RadioUart->writeable() == false) { \ + } + +/*! + * \brief Blocking routine for waiting the UART to be readable + * + */ +#define WaitUartReadable() \ + while (RadioUart->readable() == false) { \ + } + +// This code handles cases where assert_param is undefined +#ifndef assert_param +#define assert_param(...) +#endif + +/*SX1280Hal::SX1280Hal( PinName mosi, PinName miso, PinName sclk, PinName nss,*/ +/* PinName busy, PinName dio1, PinName dio2, PinName dio3, PinName rst,*/ +/* RadioCallbacks_t *callbacks )*/ +/* : SX1280( callbacks ),*/ +/* RadioNss( nss ),*/ +/* RadioReset( rst ),*/ +/* RadioCtsn( NC ),*/ +/* BUSY( busy )*/ +/*{*/ +/* CreateDioPin( dio1, DIO1 );*/ +/* CreateDioPin( dio2, DIO2 );*/ +/* CreateDioPin( dio3, DIO3 );*/ +/* RadioSpi = new SPI( mosi, miso, sclk );*/ +/* RadioUart = NULL;*/ +/**/ +/* RadioNss = 1;*/ +/* RadioReset = 1;*/ +/*}*/ +/**/ +/*SX1280Hal::SX1280Hal( PinName tx, PinName rx, PinName ctsn,*/ +/* PinName busy, PinName dio1, PinName dio2, PinName dio3, PinName rst,*/ +/* RadioCallbacks_t *callbacks )*/ +/* : SX1280( callbacks ),*/ +/* RadioNss( NC ),*/ +/* RadioReset( rst ),*/ +/* RadioCtsn( ctsn ),*/ +/* BUSY( busy )*/ +/*{*/ +/* CreateDioPin( dio1, DIO1 );*/ +/* CreateDioPin( dio2, DIO2 );*/ +/* CreateDioPin( dio3, DIO3 );*/ +/* RadioSpi = NULL;*/ +/* RadioUart = new Serial( tx, rx );*/ +/* RadioCtsn = 0;*/ +/* RadioReset = 1;*/ +/*}*/ + +SX1280Hal::~SX1280Hal(void) { + /*if( this->RadioSpi != NULL )*/ + /*{*/ + /* delete RadioSpi;*/ + /*}*/ + /*if( this->RadioUart != NULL )*/ + /*{*/ + /* delete RadioUart;*/ + /*}*/ + /*if( DIO1 != NULL )*/ + /*{*/ + /* delete DIO1;*/ + /*}*/ + /*if( DIO2 != NULL )*/ + /*{*/ + /* delete DIO2;*/ + /*}*/ + /*if( DIO3 != NULL )*/ + /*{*/ + /* delete DIO3;*/ + /*}*/ +}; + +void SX1280Hal::SpiInit(void) { + /* RadioNss = 1;*/ + /* RadioSpi->format( 8, 0 );*/ + /*#if defined( TARGET_KL25Z )*/ + /* this->SetSpiSpeed( 4000000 );*/ + /*#elif defined( TARGET_NUCLEO_L476RG )*/ + /* this->SetSpiSpeed( 8000000 );*/ + /*#else*/ + /* this->SetSpiSpeed( 8000000 );*/ + /*#endif*/ + /* wait( 0.1 );*/ +} + +void SX1280Hal::SetSpiSpeed(uint32_t spiSpeed) { + /*RadioSpi->frequency( spiSpeed );*/ +} + +void SX1280Hal::UartInit(void) { + // RadioUart->format( 9, SerialBase::Even, 1 ); // 8 data bits + 1 even parity bit + 1 stop bit + // RadioUart->baud( 115200 ); + // After this point, the UART is running standard mode: 8 data bit, 1 even + // parity bit, 1 stop bit, 115200 baud, LSB first + // wait_us( 10 ); +} + +void SX1280Hal::IoIrqInit(DioIrqHandler irqHandler) { + /*assert_param( RadioSpi != NULL || RadioUart != NULL );*/ + /*if( RadioSpi != NULL )*/ + /*{*/ + /* SpiInit( );*/ + /*}*/ + /*if( RadioUart != NULL )*/ + /*{*/ + /* UartInit( );*/ + /*}*/ + /**/ + /*BUSY.mode( PullNone );*/ + /**/ + /*DioAssignCallback( DIO1, PullNone, irqHandler );*/ + /*DioAssignCallback( DIO2, PullNone, irqHandler );*/ + /*DioAssignCallback( DIO3, PullNone, irqHandler );*/ +} + +void SX1280Hal::Reset(void) { + /*__disable_irq( );*/ + /*wait_ms( 20 );*/ + /*RadioReset.output( );*/ + /*RadioReset = 0;*/ + /*wait_ms( 50 );*/ + /*RadioReset = 1;*/ + // RadioReset.input( ); // Using the internal pull-up + /*wait_ms( 20 );*/ + /*__enable_irq( );*/ +} + +void SX1280Hal::Wakeup(void) { + /*__disable_irq( );*/ + /**/ + // Don't wait for BUSY here + /**/ + /*if( RadioSpi != NULL )*/ + /*{*/ + /* RadioNss = 0;*/ + /* RadioSpi->write( RADIO_GET_STATUS );*/ + /* RadioSpi->write( 0 );*/ + /* RadioNss = 1;*/ + /*}*/ + /*if( RadioUart != NULL )*/ + /*{*/ + /* RadioUart->putc( RADIO_GET_STATUS );*/ + /* WaitUartReadable( );*/ + /* RadioUart->getc( );*/ + /*}*/ + /**/ + // Wait for chip to be ready. + /*WaitOnBusy( );*/ + /**/ + /*__enable_irq( );*/ +} + +void SX1280Hal::WriteCommand(RadioCommands_t command, uint8_t* buffer, uint16_t size) { + /*WaitOnBusy( );*/ + /**/ + /*if( RadioSpi != NULL )*/ + /*{*/ + /* RadioNss = 0;*/ + /* RadioSpi->write( ( uint8_t )command );*/ + /* for( uint16_t i = 0; i < size; i++ )*/ + /* {*/ + /* RadioSpi->write( buffer[i] );*/ + /* }*/ + /* RadioNss = 1;*/ + /*}*/ + /*if( RadioUart != NULL )*/ + /*{*/ + /* RadioUart->putc( command );*/ + /* if( size > 0 )*/ + /* {*/ + /* RadioUart->putc( size );*/ + /* for( uint16_t i = 0; i < size; i++ )*/ + /* {*/ + /* RadioUart->putc( buffer[i] );*/ + /* }*/ + /* }*/ + /*}*/ + /**/ + /*if( command != RADIO_SET_SLEEP )*/ + /*{*/ + /* WaitOnBusy( );*/ + /*}*/ +} + +void SX1280Hal::ReadCommand(RadioCommands_t command, uint8_t* buffer, uint16_t size) { + /*WaitOnBusy( );*/ + /**/ + /*if( RadioSpi != NULL )*/ + /*{*/ + /* RadioNss = 0;*/ + /* if( command == RADIO_GET_STATUS )*/ + /* {*/ + /* buffer[0] = RadioSpi->write( ( uint8_t )command );*/ + /* RadioSpi->write( 0 );*/ + /* RadioSpi->write( 0 );*/ + /* }*/ + /* else*/ + /* {*/ + /* RadioSpi->write( ( uint8_t )command );*/ + /* RadioSpi->write( 0 );*/ + /* for( uint16_t i = 0; i < size; i++ )*/ + /* {*/ + /* buffer[i] = RadioSpi->write( 0 );*/ + /* }*/ + /* }*/ + /* RadioNss = 1;*/ + /*}*/ + /*if( RadioUart != NULL )*/ + /*{*/ + /* RadioUart->putc( command );*/ + /**/ + // Behavior on the UART is different depending of the opcode command + /* if( ( command == RADIO_GET_PACKETTYPE ) ||*/ + /* ( command == RADIO_GET_RXBUFFERSTATUS ) ||*/ + /* ( command == RADIO_GET_RSSIINST ) ||*/ + /* ( command == RADIO_GET_PACKETSTATUS ) ||*/ + /* ( command == RADIO_GET_IRQSTATUS ) )*/ + /* {*/ + /* + * TODO : Check size size in UART (uint8_t in putc) + */ + /* RadioUart->putc( size );*/ + /* }*/ + /**/ + /* WaitUartReadable( );*/ + /* for( uint16_t i = 0; i < size; i++ )*/ + /* {*/ + /* buffer[i] = RadioUart->getc( );*/ + /* }*/ + /*}*/ + /**/ + /*WaitOnBusy( );*/ +} + +void SX1280Hal::WriteRegister(uint16_t address, uint8_t* buffer, uint16_t size) { + /*WaitOnBusy( );*/ + /**/ + /*if( RadioSpi != NULL )*/ + /*{*/ + /* RadioNss = 0;*/ + /* RadioSpi->write( RADIO_WRITE_REGISTER );*/ + /* RadioSpi->write( ( address & 0xFF00 ) >> 8 );*/ + /* RadioSpi->write( address & 0x00FF );*/ + /* for( uint16_t i = 0; i < size; i++ )*/ + /* {*/ + /* RadioSpi->write( buffer[i] );*/ + /* }*/ + /* RadioNss = 1;*/ + /*}*/ + /*if( RadioUart != NULL )*/ + /*{*/ + /* uint16_t addr = address;*/ + /* uint16_t i = 0;*/ + /* for( addr = address; ( addr + 255 ) < ( address + size ); )*/ + /* {*/ + /* RadioUart->putc( RADIO_WRITE_REGISTER );*/ + /* RadioUart->putc( ( addr & 0xFF00 ) >> 8 );*/ + /* RadioUart->putc( addr & 0x00FF );*/ + /* RadioUart->putc( 255 );*/ + /* for( uint16_t lastAddr = addr + 255 ; addr < lastAddr; i++, addr++ )*/ + /* {*/ + /* RadioUart->putc( buffer[i] );*/ + /* }*/ + /* }*/ + /* RadioUart->putc( RADIO_WRITE_REGISTER );*/ + /* RadioUart->putc( ( addr & 0xFF00 ) >> 8 );*/ + /* RadioUart->putc( addr & 0x00FF );*/ + /* RadioUart->putc( address + size - addr );*/ + /**/ + /* for( ; addr < ( address + size ); addr++, i++ )*/ + /* {*/ + /* RadioUart->putc( buffer[i] );*/ + /* }*/ + /*}*/ + /**/ + /*WaitOnBusy( );*/ +} + +void SX1280Hal::WriteRegister(uint16_t address, uint8_t value) { + /*WriteRegister( address, &value, 1 );*/ +} + +void SX1280Hal::ReadRegister(uint16_t address, uint8_t* buffer, uint16_t size) { + /*WaitOnBusy( );*/ + /**/ + /*if( RadioSpi != NULL )*/ + /*{*/ + /* RadioNss = 0;*/ + /* RadioSpi->write( RADIO_READ_REGISTER );*/ + /* RadioSpi->write( ( address & 0xFF00 ) >> 8 );*/ + /* RadioSpi->write( address & 0x00FF );*/ + /* RadioSpi->write( 0 );*/ + /* for( uint16_t i = 0; i < size; i++ )*/ + /* {*/ + /* buffer[i] = RadioSpi->write( 0 );*/ + /* }*/ + /* RadioNss = 1;*/ + /*}*/ + /*if( RadioUart != NULL )*/ + /*{*/ + /* uint16_t addr = address;*/ + /* uint16_t i = 0;*/ + /* for( addr = address; ( addr + 255 ) < ( address + size ); )*/ + /* {*/ + /* RadioUart->putc( RADIO_READ_REGISTER );*/ + /* RadioUart->putc( ( addr & 0xFF00 ) >> 8 );*/ + /* RadioUart->putc( addr & 0x00FF );*/ + /* RadioUart->putc( 255 );*/ + /* WaitUartReadable( );*/ + /* for( uint16_t lastAddr = addr + 255 ; addr < lastAddr; i++, addr++ )*/ + /* {*/ + /* buffer[i] = RadioUart->getc( );*/ + /* }*/ + /* }*/ + /* RadioUart->putc( RADIO_READ_REGISTER );*/ + /* RadioUart->putc( ( addr & 0xFF00 ) >> 8 );*/ + /* RadioUart->putc( addr & 0x00FF );*/ + /* RadioUart->putc( address + size - addr );*/ + /* WaitUartReadable( );*/ + /* for( ; addr < ( address + size ); addr++, i++ )*/ + /* {*/ + /* buffer[i] = RadioUart->getc( );*/ + /* }*/ + /*}*/ + /**/ + /*WaitOnBusy( );*/ +} + +uint8_t SX1280Hal::ReadRegister(uint16_t address) { + /*uint8_t data;*/ + /**/ + /*ReadRegister( address, &data, 1 );*/ + /*return data;*/ +} + +void SX1280Hal::WriteBuffer(uint8_t offset, uint8_t* buffer, uint8_t size) { + /*WaitOnBusy( );*/ + /**/ + /*if( RadioSpi != NULL )*/ + /*{*/ + /* RadioNss = 0;*/ + /* RadioSpi->write( RADIO_WRITE_BUFFER );*/ + /* RadioSpi->write( offset );*/ + /* for( uint16_t i = 0; i < size; i++ )*/ + /* {*/ + /* RadioSpi->write( buffer[i] );*/ + /* }*/ + /* RadioNss = 1;*/ + /*}*/ + /*if( RadioUart != NULL )*/ + /*{*/ + /* RadioUart->putc( RADIO_WRITE_BUFFER );*/ + /* RadioUart->putc( offset );*/ + /* RadioUart->putc( size );*/ + /* for( uint16_t i = 0; i < size; i++ )*/ + /* {*/ + /* RadioUart->putc( buffer[i] );*/ + /* }*/ + /*}*/ + /**/ + /*WaitOnBusy( );*/ +} + +void SX1280Hal::ReadBuffer(uint8_t offset, uint8_t* buffer, uint8_t size) { + /*WaitOnBusy( );*/ + /**/ + /*if( RadioSpi != NULL )*/ + /*{*/ + /* RadioNss = 0;*/ + /* RadioSpi->write( RADIO_READ_BUFFER );*/ + /* RadioSpi->write( offset );*/ + /* RadioSpi->write( 0 );*/ + /* for( uint16_t i = 0; i < size; i++ )*/ + /* {*/ + /* buffer[i] = RadioSpi->write( 0 );*/ + /* }*/ + /* RadioNss = 1;*/ + /*}*/ + /*if( RadioUart != NULL )*/ + /*{*/ + /* RadioUart->putc( RADIO_READ_BUFFER );*/ + /* RadioUart->putc( offset );*/ + /* RadioUart->putc( size );*/ + /* WaitUartReadable( );*/ + /* for( uint16_t i = 0; i < size; i++ )*/ + /* {*/ + /* buffer[i] = RadioUart->getc( );*/ + /* }*/ + /*}*/ + /**/ + /*WaitOnBusy( );*/ +} + +uint8_t SX1280Hal::GetDioStatus(void) { + /*return ( *DIO3 << 3 ) | ( *DIO2 << 2 ) | ( *DIO1 << 1 ) | ( BUSY << 0 );*/ +} diff --git a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h new file mode 100644 index 00000000..95e3d6a2 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h @@ -0,0 +1,192 @@ +/* + ______ _ + / _____) _ | | +( (____ _____ ____ _| |_ _____ ____| |__ + \____ \| ___ | (_ _) ___ |/ ___) _ \ + _____) ) ____| | | || |_| ____( (___| | | | +(______/|_____)_|_|_| \__)_____)\____)_| |_| + (C)2015 Semtech + +Description: Handling of the node configuration protocol + +License: Revised BSD License, see LICENSE.TXT file include in the project + +Maintainer: Miguel Luis and Gregory Cristian +*/ +#ifndef __SX1280_HAL_H__ +#define __SX1280_HAL_H__ + +#include "sx1280.h" + +/*! + * \brief Actual implementation of a SX1280 radio + */ +class SX1280Hal : public SX1280 { + public: + /*! + * \brief Constructor for SX1280Hal with SPI support + * + * Represents the physical connectivity with the radio and set callback functions on radio interrupts + */ + /*SX1280Hal(PinName mosi,*/ + /* PinName miso,*/ + /* PinName sclk,*/ + /* PinName nss,*/ + /* PinName busy,*/ + /* PinName dio1,*/ + /* PinName dio2,*/ + /* PinName dio3,*/ + /* PinName rst,*/ + /* RadioCallbacks_t* callbacks);*/ + + /*! + * \brief Constructor for SX1280Hal with UART support + * + * Represents the physical connectivity with the radio and set callback functions on radio interrupts + */ + /*SX1280Hal(PinName tx,*/ + /* PinName rx,*/ + /* PinName ctsn,*/ + /* PinName busy,*/ + /* PinName dio1,*/ + /* PinName dio2,*/ + /* PinName dio3,*/ + /* PinName rst,*/ + /* RadioCallbacks_t* callbacks);*/ + + /*! + * \brief Destructor for SX1280Hal with UART support + * + * Take care of the correct destruction of the communication objects + */ + virtual ~SX1280Hal(void); + + /*! + * \brief Soft resets the radio + */ + virtual void Reset(void); + + /*! + * \brief Wakes up the radio + */ + virtual void Wakeup(void); + + /*! + * \brief Set the SPI Speed + * + * \param [in] spiSpeed Speed of the SPI in Hz + */ + void SetSpiSpeed(uint32_t spiSpeed); + + /*! + * \brief Send a command that write data to the radio + * + * \param [in] opcode Opcode of the command + * \param [in] buffer Buffer to be send to the radio + * \param [in] size Size of the buffer to send + */ + virtual void WriteCommand(RadioCommands_t opcode, uint8_t* buffer, uint16_t size); + + /*! + * \brief Send a command that read data from the radio + * + * \param [in] opcode Opcode of the command + * \param [out] buffer Buffer holding data from the radio + * \param [in] size Size of the buffer + */ + virtual void ReadCommand(RadioCommands_t opcode, uint8_t* buffer, uint16_t size); + + /*! + * \brief Write data to the radio memory + * + * \param [in] address The address of the first byte to write in the radio + * \param [in] buffer The data to be written in radio's memory + * \param [in] size The number of bytes to write in radio's memory + */ + virtual void WriteRegister(uint16_t address, uint8_t* buffer, uint16_t size); + + /*! + * \brief Write a single byte of data to the radio memory + * + * \param [in] address The address of the first byte to write in the radio + * \param [in] value The data to be written in radio's memory + */ + virtual void WriteRegister(uint16_t address, uint8_t value); + + /*! + * \brief Read data from the radio memory + * + * \param [in] address The address of the first byte to read from the radio + * \param [out] buffer The buffer that holds data read from radio + * \param [in] size The number of bytes to read from radio's memory + */ + virtual void ReadRegister(uint16_t address, uint8_t* buffer, uint16_t size); + + /*! + * \brief Read a single byte of data from the radio memory + * + * \param [in] address The address of the first byte to write in the + * radio + * + * \retval value The value of the byte at the given address in + * radio's memory + */ + virtual uint8_t ReadRegister(uint16_t address); + + /*! + * \brief Write data to the buffer holding the payload in the radio + * + * \param [in] offset The offset to start writing the payload + * \param [in] buffer The data to be written (the payload) + * \param [in] size The number of byte to be written + */ + virtual void WriteBuffer(uint8_t offset, uint8_t* buffer, uint8_t size); + + /*! + * \brief Read data from the buffer holding the payload in the radio + * + * \param [in] offset The offset to start reading the payload + * \param [out] buffer A pointer to a buffer holding the data from the radio + * \param [in] size The number of byte to be read + */ + virtual void ReadBuffer(uint8_t offset, uint8_t* buffer, uint8_t size); + + /*! + * \brief Returns the status of DIOs pins + * + * \retval dioStatus A byte where each bit represents a DIO state: + * [ DIO3 | DIO2 | DIO1 | BUSY ] + */ + virtual uint8_t GetDioStatus(void); + + protected: + // SPI* RadioSpi; //!< The SPI object used to communicate with the radio + // Serial* RadioUart; //!< The UART object used to communicate with the radio + // DigitalOut RadioNss; //!< The pin connected to Radio chip select (active low) + // DigitalInOut RadioReset; //!< The reset pin connected to the radio + // DigitalOut RadioCtsn; //!< The Clear To Send radio pin (active low) + /**/ + // DigitalIn BUSY; //!< The pin connected to BUSY + // Interrupt* DIO1; //!< The pin connected to DIO1 + // Interrupt* DIO2; //!< The pin connected to DIO2 + // Interrupt* DIO3; //!< The pin connected to DIO3 + + /*! + * \brief Initializes SPI object used to communicate with the radio + */ + virtual void SpiInit(void); + + /*! + * \brief Initializes UART object used to communicate with the radio + */ + virtual void UartInit(void); + + /*! + * \brief Sets the callback functions to be run on DIO1..3 interrupt + * + * \param [in] irqHandler A function pointer of the function to be run on every DIO interrupt + */ + virtual void IoIrqInit(DioIrqHandler irqHandler); +}; + +#endif // __SX1280_HAL_H__ diff --git a/FprimeZephyrReference/Components/MyComponent/sx1280.cpp b/FprimeZephyrReference/Components/MyComponent/sx1280.cpp new file mode 100644 index 00000000..654dfcb3 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/sx1280.cpp @@ -0,0 +1,1063 @@ +/* + ______ _ + / _____) _ | | +( (____ _____ ____ _| |_ _____ ____| |__ + \____ \| ___ | (_ _) ___ |/ ___) _ \ + _____) ) ____| | | || |_| ____( (___| | | | +(______/|_____)_|_|_| \__)_____)\____)_| |_| + (C)2016 Semtech + +Description: Driver for SX1280 devices + +License: Revised BSD License, see LICENSE.TXT file include in the project + +Maintainer: Miguel Luis, Gregory Cristian and Matthieu Verdy +*/ +/*#include "mbed.h"*/ +#include "sx1280.h" + +#include +#include + +#include "sx1280-hal.h" + +/*! + * \brief Radio registers definition + * + */ +typedef struct { + uint16_t Addr; //!< The address of the register + uint8_t Value; //!< The value of the register +} RadioRegisters_t; + +/*! + * \brief Radio hardware registers initialization definition + */ +#define RADIO_INIT_REGISTERS_VALUE \ + { \ + } + +/*! + * \brief Radio hardware registers initialization + */ +const RadioRegisters_t RadioRegsInit[] = RADIO_INIT_REGISTERS_VALUE; + +void SX1280::Init(void) { + Reset(); + IoIrqInit(dioIrq); + Wakeup(); + SetRegistersDefault(); +} + +void SX1280::SetRegistersDefault(void) { + for (int16_t i = 0; i < sizeof(RadioRegsInit) / sizeof(RadioRegisters_t); i++) { + WriteRegister(RadioRegsInit[i].Addr, RadioRegsInit[i].Value); + } +} + +uint16_t SX1280::GetFirmwareVersion(void) { + return (((ReadRegister(REG_LR_FIRMWARE_VERSION_MSB)) << 8) | (ReadRegister(REG_LR_FIRMWARE_VERSION_MSB + 1))); +} + +RadioStatus_t SX1280::GetStatus(void) { + uint8_t stat = 0; + RadioStatus_t status; + + ReadCommand(RADIO_GET_STATUS, (uint8_t*)&stat, 1); + status.Value = stat; + return (status); +} + +RadioOperatingModes_t SX1280::GetOpMode(void) { + return (OperatingMode); +} + +void SX1280::SetSleep(SleepParams_t sleepConfig) { + uint8_t sleep = (sleepConfig.WakeUpRTC << 3) | (sleepConfig.InstructionRamRetention << 2) | + (sleepConfig.DataBufferRetention << 1) | (sleepConfig.DataRamRetention); + + OperatingMode = MODE_SLEEP; + WriteCommand(RADIO_SET_SLEEP, &sleep, 1); +} + +void SX1280::SetStandby(RadioStandbyModes_t standbyConfig) { + WriteCommand(RADIO_SET_STANDBY, (uint8_t*)&standbyConfig, 1); + if (standbyConfig == STDBY_RC) { + OperatingMode = MODE_STDBY_RC; + } else { + OperatingMode = MODE_STDBY_XOSC; + } +} + +void SX1280::SetFs(void) { + WriteCommand(RADIO_SET_FS, 0, 0); + OperatingMode = MODE_FS; +} + +void SX1280::SetTx(TickTime_t timeout) { + uint8_t buf[3]; + buf[0] = timeout.PeriodBase; + buf[1] = (uint8_t)((timeout.PeriodBaseCount >> 8) & 0x00FF); + buf[2] = (uint8_t)(timeout.PeriodBaseCount & 0x00FF); + + ClearIrqStatus(IRQ_RADIO_ALL); + + // If the radio is doing ranging operations, then apply the specific calls + // prior to SetTx + if (GetPacketType(true) == PACKET_TYPE_RANGING) { + SetRangingRole(RADIO_RANGING_ROLE_MASTER); + } + WriteCommand(RADIO_SET_TX, buf, 3); + OperatingMode = MODE_TX; +} + +void SX1280::SetRx(TickTime_t timeout) { + uint8_t buf[3]; + buf[0] = timeout.PeriodBase; + buf[1] = (uint8_t)((timeout.PeriodBaseCount >> 8) & 0x00FF); + buf[2] = (uint8_t)(timeout.PeriodBaseCount & 0x00FF); + + ClearIrqStatus(IRQ_RADIO_ALL); + + // If the radio is doing ranging operations, then apply the specific calls + // prior to SetRx + if (GetPacketType(true) == PACKET_TYPE_RANGING) { + SetRangingRole(RADIO_RANGING_ROLE_SLAVE); + } + WriteCommand(RADIO_SET_RX, buf, 3); + OperatingMode = MODE_RX; +} + +void SX1280::SetRxDutyCycle(RadioTickSizes_t periodBase, uint16_t periodBaseCountRx, uint16_t periodBaseCountSleep) { + uint8_t buf[5]; + + buf[0] = periodBase; + buf[1] = (uint8_t)((periodBaseCountRx >> 8) & 0x00FF); + buf[2] = (uint8_t)(periodBaseCountRx & 0x00FF); + buf[3] = (uint8_t)((periodBaseCountSleep >> 8) & 0x00FF); + buf[4] = (uint8_t)(periodBaseCountSleep & 0x00FF); + WriteCommand(RADIO_SET_RXDUTYCYCLE, buf, 5); + OperatingMode = MODE_RX; +} + +void SX1280::SetCad(void) { + WriteCommand(RADIO_SET_CAD, 0, 0); + OperatingMode = MODE_CAD; +} + +void SX1280::SetTxContinuousWave(void) { + WriteCommand(RADIO_SET_TXCONTINUOUSWAVE, 0, 0); +} + +void SX1280::SetTxContinuousPreamble(void) { + WriteCommand(RADIO_SET_TXCONTINUOUSPREAMBLE, 0, 0); +} + +void SX1280::SetPacketType(RadioPacketTypes_t packetType) { + // Save packet type internally to avoid questioning the radio + this->PacketType = packetType; + + WriteCommand(RADIO_SET_PACKETTYPE, (uint8_t*)&packetType, 1); +} + +RadioPacketTypes_t SX1280::GetPacketType(bool returnLocalCopy) { + RadioPacketTypes_t packetType = PACKET_TYPE_NONE; + if (returnLocalCopy == false) { + ReadCommand(RADIO_GET_PACKETTYPE, (uint8_t*)&packetType, 1); + if (this->PacketType != packetType) { + this->PacketType = packetType; + } + } else { + packetType = this->PacketType; + } + return packetType; +} + +void SX1280::SetRfFrequency(uint32_t rfFrequency) { + uint8_t buf[3]; + uint32_t freq = 0; + + freq = (uint32_t)((double)rfFrequency / (double)FREQ_STEP); + buf[0] = (uint8_t)((freq >> 16) & 0xFF); + buf[1] = (uint8_t)((freq >> 8) & 0xFF); + buf[2] = (uint8_t)(freq & 0xFF); + WriteCommand(RADIO_SET_RFFREQUENCY, buf, 3); +} + +void SX1280::SetTxParams(int8_t power, RadioRampTimes_t rampTime) { + uint8_t buf[2]; + + // The power value to send on SPI/UART is in the range [0..31] and the + // physical output power is in the range [-18..13]dBm + buf[0] = power + 18; + buf[1] = (uint8_t)rampTime; + WriteCommand(RADIO_SET_TXPARAMS, buf, 2); +} + +void SX1280::SetCadParams(RadioLoRaCadSymbols_t cadSymbolNum) { + WriteCommand(RADIO_SET_CADPARAMS, (uint8_t*)&cadSymbolNum, 1); + OperatingMode = MODE_CAD; +} + +void SX1280::SetBufferBaseAddresses(uint8_t txBaseAddress, uint8_t rxBaseAddress) { + uint8_t buf[2]; + + buf[0] = txBaseAddress; + buf[1] = rxBaseAddress; + WriteCommand(RADIO_SET_BUFFERBASEADDRESS, buf, 2); +} + +void SX1280::SetModulationParams(ModulationParams_t* modParams) { + uint8_t buf[3]; + + // Check if required configuration corresponds to the stored packet type + // If not, silently update radio packet type + if (this->PacketType != modParams->PacketType) { + this->SetPacketType(modParams->PacketType); + } + + switch (modParams->PacketType) { + case PACKET_TYPE_GFSK: + buf[0] = modParams->Params.Gfsk.BitrateBandwidth; + buf[1] = modParams->Params.Gfsk.ModulationIndex; + buf[2] = modParams->Params.Gfsk.ModulationShaping; + break; + case PACKET_TYPE_LORA: + case PACKET_TYPE_RANGING: + buf[0] = modParams->Params.LoRa.SpreadingFactor; + buf[1] = modParams->Params.LoRa.Bandwidth; + buf[2] = modParams->Params.LoRa.CodingRate; + this->LoRaBandwidth = modParams->Params.LoRa.Bandwidth; + break; + case PACKET_TYPE_FLRC: + buf[0] = modParams->Params.Flrc.BitrateBandwidth; + buf[1] = modParams->Params.Flrc.CodingRate; + buf[2] = modParams->Params.Flrc.ModulationShaping; + break; + case PACKET_TYPE_BLE: + buf[0] = modParams->Params.Ble.BitrateBandwidth; + buf[1] = modParams->Params.Ble.ModulationIndex; + buf[2] = modParams->Params.Ble.ModulationShaping; + break; + case PACKET_TYPE_NONE: + buf[0] = NULL; + buf[1] = NULL; + buf[2] = NULL; + break; + } + WriteCommand(RADIO_SET_MODULATIONPARAMS, buf, 3); +} + +void SX1280::SetPacketParams(PacketParams_t* packetParams) { + uint8_t buf[7]; + // Check if required configuration corresponds to the stored packet type + // If not, silently update radio packet type + if (this->PacketType != packetParams->PacketType) { + this->SetPacketType(packetParams->PacketType); + } + + switch (packetParams->PacketType) { + case PACKET_TYPE_GFSK: + buf[0] = packetParams->Params.Gfsk.PreambleLength; + buf[1] = packetParams->Params.Gfsk.SyncWordLength; + buf[2] = packetParams->Params.Gfsk.SyncWordMatch; + buf[3] = packetParams->Params.Gfsk.HeaderType; + buf[4] = packetParams->Params.Gfsk.PayloadLength; + buf[5] = packetParams->Params.Gfsk.CrcLength; + buf[6] = packetParams->Params.Gfsk.Whitening; + break; + case PACKET_TYPE_LORA: + case PACKET_TYPE_RANGING: + buf[0] = packetParams->Params.LoRa.PreambleLength; + buf[1] = packetParams->Params.LoRa.HeaderType; + buf[2] = packetParams->Params.LoRa.PayloadLength; + buf[3] = packetParams->Params.LoRa.Crc; + buf[4] = packetParams->Params.LoRa.InvertIQ; + buf[5] = NULL; + buf[6] = NULL; + break; + case PACKET_TYPE_FLRC: + buf[0] = packetParams->Params.Flrc.PreambleLength; + buf[1] = packetParams->Params.Flrc.SyncWordLength; + buf[2] = packetParams->Params.Flrc.SyncWordMatch; + buf[3] = packetParams->Params.Flrc.HeaderType; + buf[4] = packetParams->Params.Flrc.PayloadLength; + buf[5] = packetParams->Params.Flrc.CrcLength; + buf[6] = packetParams->Params.Flrc.Whitening; + break; + case PACKET_TYPE_BLE: + buf[0] = packetParams->Params.Ble.ConnectionState; + buf[1] = packetParams->Params.Ble.CrcLength; + buf[2] = packetParams->Params.Ble.BleTestPayload; + buf[3] = packetParams->Params.Ble.Whitening; + buf[4] = NULL; + buf[5] = NULL; + buf[6] = NULL; + break; + case PACKET_TYPE_NONE: + buf[0] = NULL; + buf[1] = NULL; + buf[2] = NULL; + buf[3] = NULL; + buf[4] = NULL; + buf[5] = NULL; + buf[6] = NULL; + break; + } + WriteCommand(RADIO_SET_PACKETPARAMS, buf, 7); +} + +void SX1280::ForcePreambleLength(RadioPreambleLengths_t preambleLength) { + this->WriteRegister(REG_LR_PREAMBLELENGTH, + (this->ReadRegister(REG_LR_PREAMBLELENGTH) & MASK_FORCE_PREAMBLELENGTH) | preambleLength); +} + +void SX1280::GetRxBufferStatus(uint8_t* rxPayloadLength, uint8_t* rxStartBufferPointer) { + uint8_t status[2]; + + ReadCommand(RADIO_GET_RXBUFFERSTATUS, status, 2); + + // In case of LORA fixed header, the rxPayloadLength is obtained by reading + // the register REG_LR_PAYLOADLENGTH + if ((this->GetPacketType(true) == PACKET_TYPE_LORA) && (ReadRegister(REG_LR_PACKETPARAMS) >> 7 == 1)) { + *rxPayloadLength = ReadRegister(REG_LR_PAYLOADLENGTH); + } else if (this->GetPacketType(true) == PACKET_TYPE_BLE) { + // In the case of BLE, the size returned in status[0] do not include the 2-byte length PDU header + // so it is added there + *rxPayloadLength = status[0] + 2; + } else { + *rxPayloadLength = status[0]; + } + + *rxStartBufferPointer = status[1]; +} + +void SX1280::GetPacketStatus(PacketStatus_t* packetStatus) { + uint8_t status[5]; + + ReadCommand(RADIO_GET_PACKETSTATUS, status, 5); + + packetStatus->packetType = this->GetPacketType(true); + switch (packetStatus->packetType) { + case PACKET_TYPE_GFSK: + packetStatus->Gfsk.RssiSync = -(status[1] / 2); + + packetStatus->Gfsk.ErrorStatus.SyncError = (status[2] >> 6) & 0x01; + packetStatus->Gfsk.ErrorStatus.LengthError = (status[2] >> 5) & 0x01; + packetStatus->Gfsk.ErrorStatus.CrcError = (status[2] >> 4) & 0x01; + packetStatus->Gfsk.ErrorStatus.AbortError = (status[2] >> 3) & 0x01; + packetStatus->Gfsk.ErrorStatus.HeaderReceived = (status[2] >> 2) & 0x01; + packetStatus->Gfsk.ErrorStatus.PacketReceived = (status[2] >> 1) & 0x01; + packetStatus->Gfsk.ErrorStatus.PacketControlerBusy = status[2] & 0x01; + + packetStatus->Gfsk.TxRxStatus.RxNoAck = (status[3] >> 5) & 0x01; + packetStatus->Gfsk.TxRxStatus.PacketSent = status[3] & 0x01; + + packetStatus->Gfsk.SyncAddrStatus = status[4] & 0x07; + break; + + case PACKET_TYPE_LORA: + case PACKET_TYPE_RANGING: + packetStatus->LoRa.RssiPkt = -(status[0] / 2); + (status[1] < 128) ? (packetStatus->LoRa.SnrPkt = status[1] / 4) + : (packetStatus->LoRa.SnrPkt = ((status[1] - 256) / 4)); + break; + + case PACKET_TYPE_FLRC: + packetStatus->Flrc.RssiSync = -(status[1] / 2); + + packetStatus->Flrc.ErrorStatus.SyncError = (status[2] >> 6) & 0x01; + packetStatus->Flrc.ErrorStatus.LengthError = (status[2] >> 5) & 0x01; + packetStatus->Flrc.ErrorStatus.CrcError = (status[2] >> 4) & 0x01; + packetStatus->Flrc.ErrorStatus.AbortError = (status[2] >> 3) & 0x01; + packetStatus->Flrc.ErrorStatus.HeaderReceived = (status[2] >> 2) & 0x01; + packetStatus->Flrc.ErrorStatus.PacketReceived = (status[2] >> 1) & 0x01; + packetStatus->Flrc.ErrorStatus.PacketControlerBusy = status[2] & 0x01; + + packetStatus->Flrc.TxRxStatus.RxPid = (status[3] >> 6) & 0x03; + packetStatus->Flrc.TxRxStatus.RxNoAck = (status[3] >> 5) & 0x01; + packetStatus->Flrc.TxRxStatus.RxPidErr = (status[3] >> 4) & 0x01; + packetStatus->Flrc.TxRxStatus.PacketSent = status[3] & 0x01; + + packetStatus->Flrc.SyncAddrStatus = status[4] & 0x07; + break; + + case PACKET_TYPE_BLE: + packetStatus->Ble.RssiSync = -(status[1] / 2); + + packetStatus->Ble.ErrorStatus.SyncError = (status[2] >> 6) & 0x01; + packetStatus->Ble.ErrorStatus.LengthError = (status[2] >> 5) & 0x01; + packetStatus->Ble.ErrorStatus.CrcError = (status[2] >> 4) & 0x01; + packetStatus->Ble.ErrorStatus.AbortError = (status[2] >> 3) & 0x01; + packetStatus->Ble.ErrorStatus.HeaderReceived = (status[2] >> 2) & 0x01; + packetStatus->Ble.ErrorStatus.PacketReceived = (status[2] >> 1) & 0x01; + packetStatus->Ble.ErrorStatus.PacketControlerBusy = status[2] & 0x01; + + packetStatus->Ble.TxRxStatus.PacketSent = status[3] & 0x01; + + packetStatus->Ble.SyncAddrStatus = status[4] & 0x07; + break; + + case PACKET_TYPE_NONE: + // In that specific case, we set everything in the packetStatus to zeros + // and reset the packet type accordingly + memset(packetStatus, 0, sizeof(PacketStatus_t)); + packetStatus->packetType = PACKET_TYPE_NONE; + break; + } +} + +int8_t SX1280::GetRssiInst(void) { + uint8_t raw = 0; + + ReadCommand(RADIO_GET_RSSIINST, &raw, 1); + + return (int8_t)(-raw / 2); +} + +void SX1280::SetDioIrqParams(uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask) { + uint8_t buf[8]; + + buf[0] = (uint8_t)((irqMask >> 8) & 0x00FF); + buf[1] = (uint8_t)(irqMask & 0x00FF); + buf[2] = (uint8_t)((dio1Mask >> 8) & 0x00FF); + buf[3] = (uint8_t)(dio1Mask & 0x00FF); + buf[4] = (uint8_t)((dio2Mask >> 8) & 0x00FF); + buf[5] = (uint8_t)(dio2Mask & 0x00FF); + buf[6] = (uint8_t)((dio3Mask >> 8) & 0x00FF); + buf[7] = (uint8_t)(dio3Mask & 0x00FF); + WriteCommand(RADIO_SET_DIOIRQPARAMS, buf, 8); +} + +uint16_t SX1280::GetIrqStatus(void) { + uint8_t irqStatus[2]; + ReadCommand(RADIO_GET_IRQSTATUS, irqStatus, 2); + return (irqStatus[0] << 8) | irqStatus[1]; +} + +void SX1280::ClearIrqStatus(uint16_t irqMask) { + uint8_t buf[2]; + + buf[0] = (uint8_t)(((uint16_t)irqMask >> 8) & 0x00FF); + buf[1] = (uint8_t)((uint16_t)irqMask & 0x00FF); + WriteCommand(RADIO_CLR_IRQSTATUS, buf, 2); +} + +void SX1280::Calibrate(CalibrationParams_t calibParam) { + uint8_t cal = (calibParam.ADCBulkPEnable << 5) | (calibParam.ADCBulkNEnable << 4) | + (calibParam.ADCPulseEnable << 3) | (calibParam.PLLEnable << 2) | (calibParam.RC13MEnable << 1) | + (calibParam.RC64KEnable); + WriteCommand(RADIO_CALIBRATE, &cal, 1); +} + +void SX1280::SetRegulatorMode(RadioRegulatorModes_t mode) { + WriteCommand(RADIO_SET_REGULATORMODE, (uint8_t*)&mode, 1); +} + +void SX1280::SetSaveContext(void) { + WriteCommand(RADIO_SET_SAVECONTEXT, 0, 0); +} + +void SX1280::SetAutoTx(uint16_t time) { + uint16_t compensatedTime = time - (uint16_t)AUTO_TX_OFFSET; + uint8_t buf[2]; + + buf[0] = (uint8_t)((compensatedTime >> 8) & 0x00FF); + buf[1] = (uint8_t)(compensatedTime & 0x00FF); + WriteCommand(RADIO_SET_AUTOTX, buf, 2); +} + +void SX1280::StopAutoTx(void) { + uint8_t buf[2] = {0x00, 0x00}; + WriteCommand(RADIO_SET_AUTOTX, buf, 2); +} + +void SX1280::SetAutoFs(bool enableAutoFs) { + WriteCommand(RADIO_SET_AUTOFS, (uint8_t*)&enableAutoFs, 1); +} + +void SX1280::SetLongPreamble(bool enable) { + WriteCommand(RADIO_SET_LONGPREAMBLE, (uint8_t*)&enable, 1); +} + +void SX1280::SetPayload(uint8_t* buffer, uint8_t size, uint8_t offset) { + WriteBuffer(offset, buffer, size); +} + +uint8_t SX1280::GetPayload(uint8_t* buffer, uint8_t* size, uint8_t maxSize) { + uint8_t offset; + + GetRxBufferStatus(size, &offset); + if (*size > maxSize) { + return 1; + } + ReadBuffer(offset, buffer, *size); + return 0; +} + +void SX1280::SendPayload(uint8_t* payload, uint8_t size, TickTime_t timeout, uint8_t offset) { + SetPayload(payload, size, offset); + SetTx(timeout); +} + +uint8_t SX1280::SetSyncWord(uint8_t syncWordIdx, uint8_t* syncWord) { + uint16_t addr; + uint8_t syncwordSize = 0; + + switch (GetPacketType(true)) { + case PACKET_TYPE_GFSK: + syncwordSize = 5; + switch (syncWordIdx) { + case 1: + addr = REG_LR_SYNCWORDBASEADDRESS1; + break; + case 2: + addr = REG_LR_SYNCWORDBASEADDRESS2; + break; + case 3: + addr = REG_LR_SYNCWORDBASEADDRESS3; + break; + default: + return 1; + } + break; + case PACKET_TYPE_FLRC: + // For FLRC packet type, the SyncWord is one byte shorter and + // the base address is shifted by one byte + syncwordSize = 4; + switch (syncWordIdx) { + case 1: + addr = REG_LR_SYNCWORDBASEADDRESS1 + 1; + break; + case 2: + addr = REG_LR_SYNCWORDBASEADDRESS2 + 1; + break; + case 3: + addr = REG_LR_SYNCWORDBASEADDRESS3 + 1; + break; + default: + return 1; + } + break; + case PACKET_TYPE_BLE: + // For Ble packet type, only the first SyncWord is used and its + // address is shifted by one byte + syncwordSize = 4; + switch (syncWordIdx) { + case 1: + addr = REG_LR_SYNCWORDBASEADDRESS1 + 1; + break; + default: + return 1; + } + break; + default: + return 1; + } + WriteRegister(addr, syncWord, syncwordSize); + return 0; +} + +void SX1280::SetSyncWordErrorTolerance(uint8_t ErrorBits) { + ErrorBits = (ReadRegister(REG_LR_SYNCWORDTOLERANCE) & 0xF0) | (ErrorBits & 0x0F); + WriteRegister(REG_LR_SYNCWORDTOLERANCE, ErrorBits); +} + +uint8_t SX1280::SetCrcSeed(uint8_t* seed) { + uint8_t updated = 0; + switch (GetPacketType(true)) { + case PACKET_TYPE_GFSK: + case PACKET_TYPE_FLRC: + WriteRegister(REG_LR_CRCSEEDBASEADDR, seed, 2); + updated = 1; + break; + case PACKET_TYPE_BLE: + this->WriteRegister(0x9c7, seed[2]); + this->WriteRegister(0x9c8, seed[1]); + this->WriteRegister(0x9c9, seed[0]); + updated = 1; + break; + default: + break; + } + return updated; +} + +void SX1280::SetBleAccessAddress(uint32_t accessAddress) { + this->WriteRegister(REG_LR_BLE_ACCESS_ADDRESS, (accessAddress >> 24) & 0x000000FF); + this->WriteRegister(REG_LR_BLE_ACCESS_ADDRESS + 1, (accessAddress >> 16) & 0x000000FF); + this->WriteRegister(REG_LR_BLE_ACCESS_ADDRESS + 2, (accessAddress >> 8) & 0x000000FF); + this->WriteRegister(REG_LR_BLE_ACCESS_ADDRESS + 3, accessAddress & 0x000000FF); +} + +void SX1280::SetBleAdvertizerAccessAddress(void) { + this->SetBleAccessAddress(BLE_ADVERTIZER_ACCESS_ADDRESS); +} + +void SX1280::SetCrcPolynomial(uint16_t polynomial) { + uint8_t val[2]; + + val[0] = (uint8_t)(polynomial >> 8) & 0xFF; + val[1] = (uint8_t)(polynomial & 0xFF); + + switch (GetPacketType(true)) { + case PACKET_TYPE_GFSK: + case PACKET_TYPE_FLRC: + WriteRegister(REG_LR_CRCPOLYBASEADDR, val, 2); + break; + default: + break; + } +} + +void SX1280::SetWhiteningSeed(uint8_t seed) { + switch (GetPacketType(true)) { + case PACKET_TYPE_GFSK: + case PACKET_TYPE_FLRC: + case PACKET_TYPE_BLE: + WriteRegister(REG_LR_WHITSEEDBASEADDR, seed); + break; + default: + break; + } +} + +void SX1280::EnableManualGain(void) { + this->WriteRegister(REG_ENABLE_MANUAL_GAIN_CONTROL, + this->ReadRegister(REG_ENABLE_MANUAL_GAIN_CONTROL) | MASK_MANUAL_GAIN_CONTROL); + this->WriteRegister(REG_DEMOD_DETECTION, this->ReadRegister(REG_DEMOD_DETECTION) & MASK_DEMOD_DETECTION); +} + +void SX1280::DisableManualGain(void) { + this->WriteRegister(REG_ENABLE_MANUAL_GAIN_CONTROL, + this->ReadRegister(REG_ENABLE_MANUAL_GAIN_CONTROL) & (~MASK_MANUAL_GAIN_CONTROL)); + this->WriteRegister(REG_DEMOD_DETECTION, this->ReadRegister(REG_DEMOD_DETECTION) | (~MASK_DEMOD_DETECTION)); +} + +void SX1280::SetManualGainValue(uint8_t gain) { + this->WriteRegister(REG_MANUAL_GAIN_VALUE, + (this->ReadRegister(REG_MANUAL_GAIN_VALUE) & MASK_MANUAL_GAIN_VALUE) | gain); +} + +void SX1280::SetLNAGainSetting(const RadioLnaSettings_t lnaSetting) { + switch (lnaSetting) { + case LNA_HIGH_SENSITIVITY_MODE: { + this->WriteRegister(REG_LNA_REGIME, this->ReadRegister(REG_LNA_REGIME) | MASK_LNA_REGIME); + break; + } + case LNA_LOW_POWER_MODE: { + this->WriteRegister(REG_LNA_REGIME, this->ReadRegister(REG_LNA_REGIME) & ~MASK_LNA_REGIME); + break; + } + } +} + +void SX1280::SetRangingIdLength(RadioRangingIdCheckLengths_t length) { + switch (GetPacketType(true)) { + case PACKET_TYPE_RANGING: + WriteRegister(REG_LR_RANGINGIDCHECKLENGTH, + ((((uint8_t)length) & 0x03) << 6) | (ReadRegister(REG_LR_RANGINGIDCHECKLENGTH) & 0x3F)); + break; + default: + break; + } +} + +void SX1280::SetDeviceRangingAddress(uint32_t address) { + uint8_t addrArray[] = {address >> 24, address >> 16, address >> 8, address}; + + switch (GetPacketType(true)) { + case PACKET_TYPE_RANGING: + WriteRegister(REG_LR_DEVICERANGINGADDR, addrArray, 4); + break; + default: + break; + } +} + +void SX1280::SetRangingRequestAddress(uint32_t address) { + uint8_t addrArray[] = {address >> 24, address >> 16, address >> 8, address}; + + switch (GetPacketType(true)) { + case PACKET_TYPE_RANGING: + WriteRegister(REG_LR_REQUESTRANGINGADDR, addrArray, 4); + break; + default: + break; + } +} + +double SX1280::GetRangingResult(RadioRangingResultTypes_t resultType) { + uint32_t valLsb = 0; + double val = 0.0; + + switch (GetPacketType(true)) { + case PACKET_TYPE_RANGING: + this->SetStandby(STDBY_XOSC); + this->WriteRegister(0x97F, this->ReadRegister(0x97F) | (1 << 1)); // enable LORA modem clock + WriteRegister(REG_LR_RANGINGRESULTCONFIG, (ReadRegister(REG_LR_RANGINGRESULTCONFIG) & MASK_RANGINGMUXSEL) | + ((((uint8_t)resultType) & 0x03) << 4)); + valLsb = ((ReadRegister(REG_LR_RANGINGRESULTBASEADDR) << 16) | + (ReadRegister(REG_LR_RANGINGRESULTBASEADDR + 1) << 8) | + (ReadRegister(REG_LR_RANGINGRESULTBASEADDR + 2))); + this->SetStandby(STDBY_RC); + + // Conversion from LSB to distance. For explanation on the formula, refer to Datasheet of SX1280 + switch (resultType) { + case RANGING_RESULT_RAW: + // Convert the ranging LSB to distance in meter + // The theoretical conversion from register value to distance [m] is given by: + // distance [m] = ( complement2( register ) * 150 ) / ( 2^12 * bandwidth[MHz] ) ) + // The API provide BW in [Hz] so the implemented formula is complement2( register ) / bandwidth[Hz] + // * A, where A = 150 / (2^12 / 1e6) = 36621.09 + val = (double)complement2(valLsb, 24) / (double)this->GetLoRaBandwidth() * 36621.09375; + break; + + case RANGING_RESULT_AVERAGED: + case RANGING_RESULT_DEBIASED: + case RANGING_RESULT_FILTERED: + val = (double)valLsb * 20.0 / 100.0; + break; + default: + val = 0.0; + } + break; + default: + break; + } + return val; +} + +uint8_t SX1280::GetRangingPowerDeltaThresholdIndicator(void) { + SetStandby(STDBY_XOSC); + WriteRegister(0x97F, ReadRegister(0x97F) | (1 << 1)); // enable LoRa modem clock + WriteRegister(REG_LR_RANGINGRESULTCONFIG, (ReadRegister(REG_LR_RANGINGRESULTCONFIG) & MASK_RANGINGMUXSEL) | + ((((uint8_t)RANGING_RESULT_RAW) & 0x03) << 4)); // Select raw results + return ReadRegister(REG_RANGING_RSSI); +} + +void SX1280::SetRangingCalibration(uint16_t cal) { + switch (GetPacketType(true)) { + case PACKET_TYPE_RANGING: + WriteRegister(REG_LR_RANGINGRERXTXDELAYCAL, (uint8_t)((cal >> 8) & 0xFF)); + WriteRegister(REG_LR_RANGINGRERXTXDELAYCAL + 1, (uint8_t)((cal) & 0xFF)); + break; + default: + break; + } +} + +void SX1280::RangingClearFilterResult(void) { + uint8_t regVal = ReadRegister(REG_LR_RANGINGRESULTCLEARREG); + + // To clear result, set bit 5 to 1 then to 0 + WriteRegister(REG_LR_RANGINGRESULTCLEARREG, regVal | (1 << 5)); + WriteRegister(REG_LR_RANGINGRESULTCLEARREG, regVal & (~(1 << 5))); +} + +void SX1280::RangingSetFilterNumSamples(uint8_t num) { + // Silently set 8 as minimum value + WriteRegister(REG_LR_RANGINGFILTERWINDOWSIZE, + (num < DEFAULT_RANGING_FILTER_SIZE) ? DEFAULT_RANGING_FILTER_SIZE : num); +} + +void SX1280::SetRangingRole(RadioRangingRoles_t role) { + uint8_t buf[1]; + + buf[0] = role; + WriteCommand(RADIO_SET_RANGING_ROLE, &buf[0], 1); +} + +double SX1280::GetFrequencyError() { + uint8_t efeRaw[3] = {0}; + uint32_t efe = 0; + double efeHz = 0.0; + + switch (this->GetPacketType(true)) { + case PACKET_TYPE_LORA: + case PACKET_TYPE_RANGING: + efeRaw[0] = this->ReadRegister(REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB); + efeRaw[1] = this->ReadRegister(REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB + 1); + efeRaw[2] = this->ReadRegister(REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB + 2); + efe = (efeRaw[0] << 16) | (efeRaw[1] << 8) | efeRaw[2]; + efe &= REG_LR_ESTIMATED_FREQUENCY_ERROR_MASK; + + efeHz = 1.55 * (double)complement2(efe, 20) / (1600.0 / (double)this->GetLoRaBandwidth() * 1000.0); + break; + + case PACKET_TYPE_NONE: + case PACKET_TYPE_BLE: + case PACKET_TYPE_FLRC: + case PACKET_TYPE_GFSK: + break; + } + + return efeHz; +} + +void SX1280::SetPollingMode(void) { + this->PollingMode = true; +} + +int32_t SX1280::complement2(const uint32_t num, const uint8_t bitCnt) { + int32_t retVal = (int32_t)num; + if (num >= 2 << (bitCnt - 2)) { + retVal -= 2 << (bitCnt - 1); + } + return retVal; +} + +int32_t SX1280::GetLoRaBandwidth() { + int32_t bwValue = 0; + + switch (this->LoRaBandwidth) { + case LORA_BW_0200: + bwValue = 203125; + break; + case LORA_BW_0400: + bwValue = 406250; + break; + case LORA_BW_0800: + bwValue = 812500; + break; + case LORA_BW_1600: + bwValue = 1625000; + break; + default: + bwValue = 0; + } + return bwValue; +} + +void SX1280::SetInterruptMode(void) { + this->PollingMode = false; +} + +void SX1280::OnDioIrq(void) { + /* + * When polling mode is activated, it is up to the application to call + * ProcessIrqs( ). Otherwise, the driver automatically calls ProcessIrqs( ) + * on radio interrupt. + */ + if (this->PollingMode == true) { + this->IrqState = true; + } else { + this->ProcessIrqs(); + } +} + +void SX1280::ProcessIrqs(void) { + RadioPacketTypes_t packetType = PACKET_TYPE_NONE; + + if (this->PollingMode == true) { + if (this->IrqState == true) { + /*__disable_irq( );*/ + this->IrqState = false; + /*__enable_irq( );*/ + } else { + return; + } + } + + packetType = GetPacketType(true); + uint16_t irqRegs = GetIrqStatus(); + ClearIrqStatus(IRQ_RADIO_ALL); + +#if (SX1280_DEBUG == 1) + DigitalOut TEST_PIN_1(D14); + DigitalOut TEST_PIN_2(D15); + for (int i = 0x8000; i != 0; i >>= 1) { + TEST_PIN_2 = 0; + TEST_PIN_1 = ((irqRegs & i) != 0) ? 1 : 0; + TEST_PIN_2 = 1; + } + TEST_PIN_1 = 0; + TEST_PIN_2 = 0; +#endif + + switch (packetType) { + case PACKET_TYPE_GFSK: + case PACKET_TYPE_FLRC: + case PACKET_TYPE_BLE: + switch (OperatingMode) { + case MODE_RX: + if ((irqRegs & IRQ_RX_DONE) == IRQ_RX_DONE) { + if ((irqRegs & IRQ_CRC_ERROR) == IRQ_CRC_ERROR) { + if (rxError != NULL) { + rxError(IRQ_CRC_ERROR_CODE); + } + } else if ((irqRegs & IRQ_SYNCWORD_ERROR) == IRQ_SYNCWORD_ERROR) { + if (rxError != NULL) { + rxError(IRQ_SYNCWORD_ERROR_CODE); + } + } else { + if (rxDone != NULL) { + rxDone(); + } + } + } + if ((irqRegs & IRQ_SYNCWORD_VALID) == IRQ_SYNCWORD_VALID) { + if (rxSyncWordDone != NULL) { + rxSyncWordDone(); + } + } + if ((irqRegs & IRQ_SYNCWORD_ERROR) == IRQ_SYNCWORD_ERROR) { + if (rxError != NULL) { + rxError(IRQ_SYNCWORD_ERROR_CODE); + } + } + if ((irqRegs & IRQ_RX_TX_TIMEOUT) == IRQ_RX_TX_TIMEOUT) { + if (rxTimeout != NULL) { + rxTimeout(); + } + } + if ((irqRegs & IRQ_TX_DONE) == IRQ_TX_DONE) { + if (txDone != NULL) { + txDone(); + } + } + break; + case MODE_TX: + if ((irqRegs & IRQ_TX_DONE) == IRQ_TX_DONE) { + if (txDone != NULL) { + txDone(); + } + } + if ((irqRegs & IRQ_RX_TX_TIMEOUT) == IRQ_RX_TX_TIMEOUT) { + if (txTimeout != NULL) { + txTimeout(); + } + } + break; + default: + // Unexpected IRQ: silently returns + break; + } + break; + case PACKET_TYPE_LORA: + switch (OperatingMode) { + case MODE_RX: + if ((irqRegs & IRQ_RX_DONE) == IRQ_RX_DONE) { + if ((irqRegs & IRQ_CRC_ERROR) == IRQ_CRC_ERROR) { + if (rxError != NULL) { + rxError(IRQ_CRC_ERROR_CODE); + } + } else { + if (rxDone != NULL) { + rxDone(); + } + } + } + if ((irqRegs & IRQ_HEADER_VALID) == IRQ_HEADER_VALID) { + if (rxHeaderDone != NULL) { + rxHeaderDone(); + } + } + if ((irqRegs & IRQ_HEADER_ERROR) == IRQ_HEADER_ERROR) { + if (rxError != NULL) { + rxError(IRQ_HEADER_ERROR_CODE); + } + } + if ((irqRegs & IRQ_RX_TX_TIMEOUT) == IRQ_RX_TX_TIMEOUT) { + if (rxTimeout != NULL) { + rxTimeout(); + } + } + if ((irqRegs & IRQ_RANGING_SLAVE_REQUEST_DISCARDED) == IRQ_RANGING_SLAVE_REQUEST_DISCARDED) { + if (rxError != NULL) { + rxError(IRQ_RANGING_ON_LORA_ERROR_CODE); + } + } + break; + case MODE_TX: + if ((irqRegs & IRQ_TX_DONE) == IRQ_TX_DONE) { + if (txDone != NULL) { + txDone(); + } + } + if ((irqRegs & IRQ_RX_TX_TIMEOUT) == IRQ_RX_TX_TIMEOUT) { + if (txTimeout != NULL) { + txTimeout(); + } + } + break; + case MODE_CAD: + if ((irqRegs & IRQ_CAD_DONE) == IRQ_CAD_DONE) { + if ((irqRegs & IRQ_CAD_DETECTED) == IRQ_CAD_DETECTED) { + if (cadDone != NULL) { + cadDone(true); + } + } else { + if (cadDone != NULL) { + cadDone(false); + } + } + } else if ((irqRegs & IRQ_RX_TX_TIMEOUT) == IRQ_RX_TX_TIMEOUT) { + if (rxTimeout != NULL) { + rxTimeout(); + } + } + break; + default: + // Unexpected IRQ: silently returns + break; + } + break; + case PACKET_TYPE_RANGING: + switch (OperatingMode) { + // MODE_RX indicates an IRQ on the Slave side + case MODE_RX: + if ((irqRegs & IRQ_RANGING_SLAVE_REQUEST_DISCARDED) == IRQ_RANGING_SLAVE_REQUEST_DISCARDED) { + if (rangingDone != NULL) { + rangingDone(IRQ_RANGING_SLAVE_ERROR_CODE); + } + } + if ((irqRegs & IRQ_RANGING_SLAVE_REQUEST_VALID) == IRQ_RANGING_SLAVE_REQUEST_VALID) { + if (rangingDone != NULL) { + rangingDone(IRQ_RANGING_SLAVE_VALID_CODE); + } + } + if ((irqRegs & IRQ_RANGING_SLAVE_RESPONSE_DONE) == IRQ_RANGING_SLAVE_RESPONSE_DONE) { + if (rangingDone != NULL) { + rangingDone(IRQ_RANGING_SLAVE_VALID_CODE); + } + } + if ((irqRegs & IRQ_RX_TX_TIMEOUT) == IRQ_RX_TX_TIMEOUT) { + if (rangingDone != NULL) { + rangingDone(IRQ_RANGING_SLAVE_ERROR_CODE); + } + } + if ((irqRegs & IRQ_HEADER_VALID) == IRQ_HEADER_VALID) { + if (rxHeaderDone != NULL) { + rxHeaderDone(); + } + } + if ((irqRegs & IRQ_HEADER_ERROR) == IRQ_HEADER_ERROR) { + if (rxError != NULL) { + rxError(IRQ_HEADER_ERROR_CODE); + } + } + break; + // MODE_TX indicates an IRQ on the Master side + case MODE_TX: + if ((irqRegs & IRQ_RANGING_MASTER_TIMEOUT) == IRQ_RANGING_MASTER_TIMEOUT) { + if (rangingDone != NULL) { + rangingDone(IRQ_RANGING_MASTER_ERROR_CODE); + } + } + if ((irqRegs & IRQ_RANGING_MASTER_RESULT_VALID) == IRQ_RANGING_MASTER_RESULT_VALID) { + if (rangingDone != NULL) { + rangingDone(IRQ_RANGING_MASTER_VALID_CODE); + } + } + break; + default: + // Unexpected IRQ: silently returns + break; + } + break; + default: + // Unexpected IRQ: silently returns + break; + } +} diff --git a/FprimeZephyrReference/Components/MyComponent/sx1280.h b/FprimeZephyrReference/Components/MyComponent/sx1280.h new file mode 100644 index 00000000..d29a5796 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/sx1280.h @@ -0,0 +1,1627 @@ +/* + ______ _ + / _____) _ | | +( (____ _____ ____ _| |_ _____ ____| |__ + \____ \| ___ | (_ _) ___ |/ ___) _ \ + _____) ) ____| | | || |_| ____( (___| | | | +(______/|_____)_|_|_| \__)_____)\____)_| |_| + (C)2016 Semtech + +Description: Driver for SX1280 devices + +License: Revised BSD License, see LICENSE.TXT file include in the project + +Maintainer: Miguel Luis, Gregory Cristian and Matthieu Verdy +*/ +#ifndef __SX1280_H__ +#define __SX1280_H__ + +#include "radio.h" + +/*! + * \brief Enables/disables driver debug features + */ +#define SX1280_DEBUG 0 + +/*! + * \brief Hardware IO IRQ callback function definition + */ +class SX1280; +typedef void (SX1280::*DioIrqHandler)(void); + +/*! + * \brief IRQ triggers callback function definition + */ +class SX1280Hal; +typedef void (SX1280Hal::*Trigger)(void); + +/*! + * \brief Provides the frequency of the chip running on the radio and the frequency step + * + * \remark These defines are used for computing the frequency divider to set the RF frequency + */ +#define XTAL_FREQ 52000000 +#define FREQ_STEP ((double)(XTAL_FREQ / (double)(1 << 18))) + +/*! + * \brief Compensation delay for SetAutoTx method in microseconds + */ +#define AUTO_TX_OFFSET 33 + +/*! + * \brief The address of the register holding the firmware version MSB + */ +#define REG_LR_FIRMWARE_VERSION_MSB 0x0153 + +/*! + * \brief The address of the register holding the first byte defining the CRC seed + * + * \remark Only used for packet types GFSK and Flrc + */ +#define REG_LR_CRCSEEDBASEADDR 0x09C8 + +/*! + * \brief The address of the register holding the first byte defining the CRC polynomial + * + * \remark Only used for packet types GFSK and Flrc + */ +#define REG_LR_CRCPOLYBASEADDR 0x09C6 + +/*! + * \brief The address of the register holding the first byte defining the whitening seed + * + * \remark Only used for packet types GFSK, FLRC and BLE + */ +#define REG_LR_WHITSEEDBASEADDR 0x09C5 + +/*! + * \brief The address of the register holding the ranging id check length + * + * \remark Only used for packet type Ranging + */ +#define REG_LR_RANGINGIDCHECKLENGTH 0x0931 + +/*! + * \brief The address of the register holding the device ranging id + * + * \remark Only used for packet type Ranging + */ +#define REG_LR_DEVICERANGINGADDR 0x0916 + +/*! + * \brief The address of the register holding the device ranging id + * + * \remark Only used for packet type Ranging + */ +#define REG_LR_REQUESTRANGINGADDR 0x0912 + +/*! + * \brief The address of the register holding ranging results configuration + * and the corresponding mask + * + * \remark Only used for packet type Ranging + */ +#define REG_LR_RANGINGRESULTCONFIG 0x0924 +#define MASK_RANGINGMUXSEL 0xCF + +/*! + * \brief The address of the register holding the first byte of ranging results + * Only used for packet type Ranging + */ +#define REG_LR_RANGINGRESULTBASEADDR 0x0961 + +/*! + * \brief The address of the register allowing to read ranging results + * + * \remark Only used for packet type Ranging + */ +#define REG_LR_RANGINGRESULTSFREEZE 0x097F + +/*! + * \brief The address of the register holding the first byte of ranging calibration + * + * \remark Only used for packet type Ranging + */ +#define REG_LR_RANGINGRERXTXDELAYCAL 0x092C + +/*! + *\brief The address of the register holding the ranging filter window size + * + * \remark Only used for packet type Ranging + */ +#define REG_LR_RANGINGFILTERWINDOWSIZE 0x091E + +/*! + *\brief The address of the register to reset for clearing ranging filter + * + * \remark Only used for packet type Ranging + */ +#define REG_LR_RANGINGRESULTCLEARREG 0x0923 + +#define REG_RANGING_RSSI 0x0964 + +/*! + * \brief The default number of samples considered in built-in ranging filter + */ +#define DEFAULT_RANGING_FILTER_SIZE 127 + +/*! + * \brief The address of the register holding LORA packet parameters + */ +#define REG_LR_PACKETPARAMS 0x903 + +/*! + * \brief The address of the register holding payload length + * + * \remark Do NOT try to read it directly. Use GetRxBuffer( ) instead. + */ +#define REG_LR_PAYLOADLENGTH 0x901 + +/*! + * \brief The addresses of the registers holding SyncWords values + * + * \remark The addresses depends on the Packet Type in use, and not all + * SyncWords are available for every Packet Type + */ +#define REG_LR_SYNCWORDBASEADDRESS1 0x09CE +#define REG_LR_SYNCWORDBASEADDRESS2 0x09D3 +#define REG_LR_SYNCWORDBASEADDRESS3 0x09D8 + +/*! + * \brief The MSB address and mask used to read the estimated frequency + * error + */ +#define REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB 0x0954 +#define REG_LR_ESTIMATED_FREQUENCY_ERROR_MASK 0x0FFFFF + +/*! + * \brief Defines how many bit errors are tolerated in sync word detection + */ +#define REG_LR_SYNCWORDTOLERANCE 0x09CD + +/*! + * \brief Register and mask for GFSK and BLE preamble length forcing + */ +#define REG_LR_PREAMBLELENGTH 0x09C1 +#define MASK_FORCE_PREAMBLELENGTH 0x8F + +/*! + * \brief Register for MSB Access Address (BLE) + */ +#define REG_LR_BLE_ACCESS_ADDRESS 0x09CF +#define BLE_ADVERTIZER_ACCESS_ADDRESS 0x8E89BED6 + +/*! + * \brief Select high sensitivity versus power consumption + */ +#define REG_LNA_REGIME 0x0891 +#define MASK_LNA_REGIME 0xC0 + +/* + * \brief Register and mask controlling the enabling of manual gain control + */ +#define REG_ENABLE_MANUAL_GAIN_CONTROL 0x089F +#define MASK_MANUAL_GAIN_CONTROL 0x80 + +/*! + * \brief Register and mask controlling the demodulation detection + */ +#define REG_DEMOD_DETECTION 0x0895 +#define MASK_DEMOD_DETECTION 0xFE + +/*! + * Register and mask to set the manual gain parameter + */ +#define REG_MANUAL_GAIN_VALUE 0x089E +#define MASK_MANUAL_GAIN_VALUE 0xF0 + +/*! + * \brief Represents the states of the radio + */ +typedef enum { + RF_IDLE = 0x00, //!< The radio is idle + RF_RX_RUNNING, //!< The radio is in reception state + RF_TX_RUNNING, //!< The radio is in transmission state + RF_CAD, //!< The radio is doing channel activity detection +} RadioStates_t; + +/*! + * \brief Represents the operating mode the radio is actually running + */ +typedef enum { + MODE_SLEEP = 0x00, //! The radio is in sleep mode + MODE_CALIBRATION, //! The radio is in calibration mode + MODE_STDBY_RC, //! The radio is in standby mode with RC oscillator + MODE_STDBY_XOSC, //! The radio is in standby mode with XOSC oscillator + MODE_FS, //! The radio is in frequency synthesis mode + MODE_RX, //! The radio is in receive mode + MODE_TX, //! The radio is in transmit mode + MODE_CAD //! The radio is in channel activity detection mode +} RadioOperatingModes_t; + +/*! + * \brief Declares the oscillator in use while in standby mode + * + * Using the STDBY_RC standby mode allow to reduce the energy consumption + * STDBY_XOSC should be used for time critical applications + */ +typedef enum { + STDBY_RC = 0x00, + STDBY_XOSC = 0x01, +} RadioStandbyModes_t; + +/*! + * \brief Declares the power regulation used to power the device + * + * This command allows the user to specify if DC-DC or LDO is used for power regulation. + * Using only LDO implies that the Rx or Tx current is doubled + */ +typedef enum { + USE_LDO = 0x00, //! Use LDO (default value) + USE_DCDC = 0x01, //! Use DCDC +} RadioRegulatorModes_t; + +/*! + * \brief Represents the possible packet type (i.e. modem) used + */ +typedef enum { + PACKET_TYPE_GFSK = 0x00, + PACKET_TYPE_LORA, + PACKET_TYPE_RANGING, + PACKET_TYPE_FLRC, + PACKET_TYPE_BLE, + PACKET_TYPE_NONE = 0x0F, +} RadioPacketTypes_t; + +/*! + * \brief Represents the ramping time for power amplifier + */ +typedef enum { + RADIO_RAMP_02_US = 0x00, + RADIO_RAMP_04_US = 0x20, + RADIO_RAMP_06_US = 0x40, + RADIO_RAMP_08_US = 0x60, + RADIO_RAMP_10_US = 0x80, + RADIO_RAMP_12_US = 0xA0, + RADIO_RAMP_16_US = 0xC0, + RADIO_RAMP_20_US = 0xE0, +} RadioRampTimes_t; + +/*! + * \brief Represents the number of symbols to be used for channel activity detection operation + */ +typedef enum { + LORA_CAD_01_SYMBOL = 0x00, + LORA_CAD_02_SYMBOLS = 0x20, + LORA_CAD_04_SYMBOLS = 0x40, + LORA_CAD_08_SYMBOLS = 0x60, + LORA_CAD_16_SYMBOLS = 0x80, +} RadioLoRaCadSymbols_t; + +/*! + * \brief Represents the possible combinations of bitrate and bandwidth for + * GFSK and BLE packet types + * + * The bitrate is expressed in Mb/s and the bandwidth in MHz + */ +typedef enum { + GFSK_BLE_BR_2_000_BW_2_4 = 0x04, + GFSK_BLE_BR_1_600_BW_2_4 = 0x28, + GFSK_BLE_BR_1_000_BW_2_4 = 0x4C, + GFSK_BLE_BR_1_000_BW_1_2 = 0x45, + GFSK_BLE_BR_0_800_BW_2_4 = 0x70, + GFSK_BLE_BR_0_800_BW_1_2 = 0x69, + GFSK_BLE_BR_0_500_BW_1_2 = 0x8D, + GFSK_BLE_BR_0_500_BW_0_6 = 0x86, + GFSK_BLE_BR_0_400_BW_1_2 = 0xB1, + GFSK_BLE_BR_0_400_BW_0_6 = 0xAA, + GFSK_BLE_BR_0_250_BW_0_6 = 0xCE, + GFSK_BLE_BR_0_250_BW_0_3 = 0xC7, + GFSK_BLE_BR_0_125_BW_0_3 = 0xEF, +} RadioGfskBleBitrates_t; + +/*! + * \brief Represents the modulation index used in GFSK and BLE packet + * types + */ +typedef enum { + GFSK_BLE_MOD_IND_0_35 = 0, + GFSK_BLE_MOD_IND_0_50 = 1, + GFSK_BLE_MOD_IND_0_75 = 2, + GFSK_BLE_MOD_IND_1_00 = 3, + GFSK_BLE_MOD_IND_1_25 = 4, + GFSK_BLE_MOD_IND_1_50 = 5, + GFSK_BLE_MOD_IND_1_75 = 6, + GFSK_BLE_MOD_IND_2_00 = 7, + GFSK_BLE_MOD_IND_2_25 = 8, + GFSK_BLE_MOD_IND_2_50 = 9, + GFSK_BLE_MOD_IND_2_75 = 10, + GFSK_BLE_MOD_IND_3_00 = 11, + GFSK_BLE_MOD_IND_3_25 = 12, + GFSK_BLE_MOD_IND_3_50 = 13, + GFSK_BLE_MOD_IND_3_75 = 14, + GFSK_BLE_MOD_IND_4_00 = 15, +} RadioGfskBleModIndexes_t; + +/*! + * \brief Represents the possible combination of bitrate and bandwidth for FLRC + * packet type + * + * The bitrate is in Mb/s and the bitrate in MHz + */ +typedef enum { + FLRC_BR_1_300_BW_1_2 = 0x45, + FLRC_BR_1_040_BW_1_2 = 0x69, + FLRC_BR_0_650_BW_0_6 = 0x86, + FLRC_BR_0_520_BW_0_6 = 0xAA, + FLRC_BR_0_325_BW_0_3 = 0xC7, + FLRC_BR_0_260_BW_0_3 = 0xEB, +} RadioFlrcBitrates_t; + +/*! + * \brief Represents the possible values for coding rate parameter in FLRC + * packet type + */ +typedef enum { + FLRC_CR_1_2 = 0x00, + FLRC_CR_3_4 = 0x02, + FLRC_CR_1_0 = 0x04, +} RadioFlrcCodingRates_t; + +/*! + * \brief Represents the modulation shaping parameter for GFSK, FLRC and BLE + * packet types + */ +typedef enum { + RADIO_MOD_SHAPING_BT_OFF = 0x00, //! No filtering + RADIO_MOD_SHAPING_BT_1_0 = 0x10, + RADIO_MOD_SHAPING_BT_0_5 = 0x20, +} RadioModShapings_t; + +/*! + * \brief Represents the possible spreading factor values in LORA packet types + */ +typedef enum { + LORA_SF5 = 0x50, + LORA_SF6 = 0x60, + LORA_SF7 = 0x70, + LORA_SF8 = 0x80, + LORA_SF9 = 0x90, + LORA_SF10 = 0xA0, + LORA_SF11 = 0xB0, + LORA_SF12 = 0xC0, +} RadioLoRaSpreadingFactors_t; + +/*! + * \brief Represents the bandwidth values for LORA packet type + */ +typedef enum { + LORA_BW_0200 = 0x34, + LORA_BW_0400 = 0x26, + LORA_BW_0800 = 0x18, + LORA_BW_1600 = 0x0A, +} RadioLoRaBandwidths_t; + +/*! + * \brief Represents the coding rate values for LORA packet type + */ +typedef enum { + LORA_CR_4_5 = 0x01, + LORA_CR_4_6 = 0x02, + LORA_CR_4_7 = 0x03, + LORA_CR_4_8 = 0x04, + LORA_CR_LI_4_5 = 0x05, + LORA_CR_LI_4_6 = 0x06, + LORA_CR_LI_4_7 = 0x07, +} RadioLoRaCodingRates_t; + +/*! + * \brief Represents the preamble length values for GFSK and FLRC packet + * types + */ +typedef enum { + PREAMBLE_LENGTH_04_BITS = 0x00, //!< Preamble length: 04 bits + PREAMBLE_LENGTH_08_BITS = 0x10, //!< Preamble length: 08 bits + PREAMBLE_LENGTH_12_BITS = 0x20, //!< Preamble length: 12 bits + PREAMBLE_LENGTH_16_BITS = 0x30, //!< Preamble length: 16 bits + PREAMBLE_LENGTH_20_BITS = 0x40, //!< Preamble length: 20 bits + PREAMBLE_LENGTH_24_BITS = 0x50, //!< Preamble length: 24 bits + PREAMBLE_LENGTH_28_BITS = 0x60, //!< Preamble length: 28 bits + PREAMBLE_LENGTH_32_BITS = 0x70, //!< Preamble length: 32 bits +} RadioPreambleLengths_t; + +/*! + * \brief Represents the SyncWord length for FLRC packet type + */ +typedef enum { + FLRC_NO_SYNCWORD = 0x00, + FLRC_SYNCWORD_LENGTH_4_BYTE = 0x04, +} RadioFlrcSyncWordLengths_t; + +/*! + * \brief The length of sync words for GFSK packet type + */ +typedef enum { + GFSK_SYNCWORD_LENGTH_1_BYTE = 0x00, //!< Sync word length: 1 byte + GFSK_SYNCWORD_LENGTH_2_BYTE = 0x02, //!< Sync word length: 2 bytes + GFSK_SYNCWORD_LENGTH_3_BYTE = 0x04, //!< Sync word length: 3 bytes + GFSK_SYNCWORD_LENGTH_4_BYTE = 0x06, //!< Sync word length: 4 bytes + GFSK_SYNCWORD_LENGTH_5_BYTE = 0x08, //!< Sync word length: 5 bytes +} RadioSyncWordLengths_t; + +/*! + * \brief Represents the possible combinations of SyncWord correlators + * activated for GFSK and FLRC packet types + */ +typedef enum { + RADIO_RX_MATCH_SYNCWORD_OFF = 0x00, //!< No correlator turned on, i.e. do not search for SyncWord + RADIO_RX_MATCH_SYNCWORD_1 = 0x10, + RADIO_RX_MATCH_SYNCWORD_2 = 0x20, + RADIO_RX_MATCH_SYNCWORD_1_2 = 0x30, + RADIO_RX_MATCH_SYNCWORD_3 = 0x40, + RADIO_RX_MATCH_SYNCWORD_1_3 = 0x50, + RADIO_RX_MATCH_SYNCWORD_2_3 = 0x60, + RADIO_RX_MATCH_SYNCWORD_1_2_3 = 0x70, +} RadioSyncWordRxMatchs_t; + +/*! + * \brief Radio packet length mode for GFSK and FLRC packet types + */ +typedef enum { + RADIO_PACKET_FIXED_LENGTH = 0x00, //!< The packet is known on both sides, no header included in the packet + RADIO_PACKET_VARIABLE_LENGTH = 0x20, //!< The packet is on variable size, header included +} RadioPacketLengthModes_t; + +/*! + * \brief Represents the CRC length for GFSK and FLRC packet types + * + * \warning Not all configurations are available for both GFSK and FLRC + * packet type. Refer to the datasheet for possible configuration. + */ +typedef enum { + RADIO_CRC_OFF = 0x00, //!< No CRC in use + RADIO_CRC_1_BYTES = 0x10, + RADIO_CRC_2_BYTES = 0x20, + RADIO_CRC_3_BYTES = 0x30, +} RadioCrcTypes_t; + +/*! + * \brief Radio whitening mode activated or deactivated for GFSK, FLRC and + * BLE packet types + */ +typedef enum { + RADIO_WHITENING_ON = 0x00, + RADIO_WHITENING_OFF = 0x08, +} RadioWhiteningModes_t; + +/*! + * \brief Holds the packet length mode of a LORA packet type + */ +typedef enum { + LORA_PACKET_VARIABLE_LENGTH = 0x00, //!< The packet is on variable size, header included + LORA_PACKET_FIXED_LENGTH = 0x80, //!< The packet is known on both sides, no header included in the packet + LORA_PACKET_EXPLICIT = LORA_PACKET_VARIABLE_LENGTH, + LORA_PACKET_IMPLICIT = LORA_PACKET_FIXED_LENGTH, +} RadioLoRaPacketLengthsModes_t; + +/*! + * \brief Represents the CRC mode for LORA packet type + */ +typedef enum { + LORA_CRC_ON = 0x20, //!< CRC activated + LORA_CRC_OFF = 0x00, //!< CRC not used +} RadioLoRaCrcModes_t; + +/*! + * \brief Represents the IQ mode for LORA packet type + */ +typedef enum { + LORA_IQ_NORMAL = 0x40, + LORA_IQ_INVERTED = 0x00, +} RadioLoRaIQModes_t; + +/*! + * \brief Represents the length of the ID to check in ranging operation + */ +typedef enum { + RANGING_IDCHECK_LENGTH_08_BITS = 0x00, + RANGING_IDCHECK_LENGTH_16_BITS, + RANGING_IDCHECK_LENGTH_24_BITS, + RANGING_IDCHECK_LENGTH_32_BITS, +} RadioRangingIdCheckLengths_t; + +/*! + * \brief Represents the result type to be used in ranging operation + */ +typedef enum { + RANGING_RESULT_RAW = 0x00, + RANGING_RESULT_AVERAGED = 0x01, + RANGING_RESULT_DEBIASED = 0x02, + RANGING_RESULT_FILTERED = 0x03, +} RadioRangingResultTypes_t; + +/*! + * \brief Represents the connection state for BLE packet type + */ +typedef enum { + BLE_PAYLOAD_LENGTH_MAX_31_BYTES = 0x00, + BLE_PAYLOAD_LENGTH_MAX_37_BYTES = 0x20, + BLE_TX_TEST_MODE = 0x40, + BLE_PAYLOAD_LENGTH_MAX_255_BYTES = 0x80, +} RadioBleConnectionStates_t; + +/*! + * \brief Represents the CRC field length for BLE packet type + */ +typedef enum { + BLE_CRC_OFF = 0x00, + BLE_CRC_3B = 0x10, +} RadioBleCrcTypes_t; + +/*! + * \brief Represents the specific packets to use in BLE packet type + */ +typedef enum { + BLE_PRBS_9 = 0x00, //!< Pseudo Random Binary Sequence based on 9th degree polynomial + BLE_PRBS_15 = 0x0C, //!< Pseudo Random Binary Sequence based on 15th degree polynomial + BLE_EYELONG_1_0 = 0x04, //!< Repeated '11110000' sequence + BLE_EYELONG_0_1 = 0x18, //!< Repeated '00001111' sequence + BLE_EYESHORT_1_0 = 0x08, //!< Repeated '10101010' sequence + BLE_EYESHORT_0_1 = 0x1C, //!< Repeated '01010101' sequence + BLE_ALL_1 = 0x10, //!< Repeated '11111111' sequence + BLE_ALL_0 = 0x14, //!< Repeated '00000000' sequence +} RadioBleTestPayloads_t; + +/*! + * \brief Represents the interruption masks available for the radio + * + * \remark Note that not all these interruptions are available for all packet types + */ +typedef enum { + IRQ_RADIO_NONE = 0x0000, + IRQ_TX_DONE = 0x0001, + IRQ_RX_DONE = 0x0002, + IRQ_SYNCWORD_VALID = 0x0004, + IRQ_SYNCWORD_ERROR = 0x0008, + IRQ_HEADER_VALID = 0x0010, + IRQ_HEADER_ERROR = 0x0020, + IRQ_CRC_ERROR = 0x0040, + IRQ_RANGING_SLAVE_RESPONSE_DONE = 0x0080, + IRQ_RANGING_SLAVE_REQUEST_DISCARDED = 0x0100, + IRQ_RANGING_MASTER_RESULT_VALID = 0x0200, + IRQ_RANGING_MASTER_TIMEOUT = 0x0400, + IRQ_RANGING_SLAVE_REQUEST_VALID = 0x0800, + IRQ_CAD_DONE = 0x1000, + IRQ_CAD_DETECTED = 0x2000, + IRQ_RX_TX_TIMEOUT = 0x4000, + IRQ_PREAMBLE_DETECTED = 0x8000, + IRQ_RADIO_ALL = 0xFFFF, +} RadioIrqMasks_t; + +/*! + * \brief Represents the digital input/output of the radio + */ +typedef enum { + RADIO_DIO1 = 0x02, + RADIO_DIO2 = 0x04, + RADIO_DIO3 = 0x08, +} RadioDios_t; + +/*! + * \brief Represents the tick size available for Rx/Tx timeout operations + */ +typedef enum { + RADIO_TICK_SIZE_0015_US = 0x00, + RADIO_TICK_SIZE_0062_US = 0x01, + RADIO_TICK_SIZE_1000_US = 0x02, + RADIO_TICK_SIZE_4000_US = 0x03, +} RadioTickSizes_t; + +/*! + * \brief Represents the role of the radio during ranging operations + */ +typedef enum { + RADIO_RANGING_ROLE_SLAVE = 0x00, + RADIO_RANGING_ROLE_MASTER = 0x01, +} RadioRangingRoles_t; + +/*! + * \brief Represents the possible Mask settings for sensitivity selection + */ +typedef enum { + LNA_LOW_POWER_MODE, + LNA_HIGH_SENSITIVITY_MODE, +} RadioLnaSettings_t; + +/*! + * \brief Represents an amount of time measurable by the radio clock + * + * @code + * Time = PeriodBase * PeriodBaseCount + * Example: + * PeriodBase = RADIO_TICK_SIZE_4000_US( 4 ms ) + * PeriodBaseCount = 1000 + * Time = 4e-3 * 1000 = 4 seconds + * @endcode + */ +typedef struct TickTime_s { + RadioTickSizes_t PeriodBase; //!< The base time of ticktime + /*! + * \brief The number of periodBase for ticktime + * Special values are: + * - 0x0000 for single mode + * - 0xFFFF for continuous mode + */ + uint16_t PeriodBaseCount; +} TickTime_t; + +/*! + * \brief RX_TX_CONTINUOUS and RX_TX_SINGLE are two particular values for TickTime. + * The former keep the radio in Rx or Tx mode, even after successful reception + * or transmission. It should never generate Timeout interrupt. + * The later let the radio enough time to make one reception or transmission. + * No Timeout interrupt is generated, and the radio fall in StandBy mode after + * reception or transmission. + */ +#define RX_TX_CONTINUOUS (TickTime_t){RADIO_TICK_SIZE_0015_US, 0xFFFF} +#define RX_TX_SINGLE (TickTime_t){RADIO_TICK_SIZE_0015_US, 0} + +/*! + * \brief The type describing the modulation parameters for every packet types + */ +typedef struct { + RadioPacketTypes_t PacketType; //!< Packet to which the modulation parameters are referring to. + struct { + /*! + * \brief Holds the GFSK modulation parameters + * + * In GFSK modulation, the bit-rate and bandwidth are linked together. In this structure, its values are set + * using the same token. + */ + struct { + RadioGfskBleBitrates_t + BitrateBandwidth; //!< The bandwidth and bit-rate values for BLE and GFSK modulations + RadioGfskBleModIndexes_t ModulationIndex; //!< The coding rate for BLE and GFSK modulations + RadioModShapings_t ModulationShaping; //!< The modulation shaping for BLE and GFSK modulations + } Gfsk; + /*! + * \brief Holds the LORA modulation parameters + * + * LORA modulation is defined by Spreading Factor (SF), Bandwidth and Coding Rate + */ + struct { + RadioLoRaSpreadingFactors_t SpreadingFactor; //!< Spreading Factor for the LORA modulation + RadioLoRaBandwidths_t Bandwidth; //!< Bandwidth for the LORA modulation + RadioLoRaCodingRates_t CodingRate; //!< Coding rate for the LORA modulation + } LoRa; + /*! + * \brief Holds the FLRC modulation parameters + * + * In FLRC modulation, the bit-rate and bandwidth are linked together. In this structure, its values are set + * using the same token. + */ + struct { + RadioFlrcBitrates_t BitrateBandwidth; //!< The bandwidth and bit-rate values for FLRC modulation + RadioFlrcCodingRates_t CodingRate; //!< The coding rate for FLRC modulation + RadioModShapings_t ModulationShaping; //!< The modulation shaping for FLRC modulation + } Flrc; + /*! + * \brief Holds the BLE modulation parameters + * + * In BLE modulation, the bit-rate and bandwidth are linked together. In this structure, its values are set + * using the same token. + */ + struct { + RadioGfskBleBitrates_t + BitrateBandwidth; //!< The bandwidth and bit-rate values for BLE and GFSK modulations + RadioGfskBleModIndexes_t ModulationIndex; //!< The coding rate for BLE and GFSK modulations + RadioModShapings_t ModulationShaping; //!< The modulation shaping for BLE and GFSK modulations + } Ble; + } Params; //!< Holds the modulation parameters structure +} ModulationParams_t; + +/*! + * \brief The type describing the packet parameters for every packet types + */ +typedef struct { + RadioPacketTypes_t PacketType; //!< Packet to which the packet parameters are referring to. + struct { + /*! + * \brief Holds the GFSK packet parameters + */ + struct { + RadioPreambleLengths_t PreambleLength; //!< The preamble length for GFSK packet type + RadioSyncWordLengths_t SyncWordLength; //!< The synchronization word length for GFSK packet type + RadioSyncWordRxMatchs_t + SyncWordMatch; //!< The synchronization correlator to use to check synchronization word + RadioPacketLengthModes_t HeaderType; //!< If the header is explicit, it will be transmitted in the GFSK + //!< packet. If the header is implicit, it will not be transmitted + uint8_t PayloadLength; //!< Size of the payload in the GFSK packet + RadioCrcTypes_t CrcLength; //!< Size of the CRC block in the GFSK packet + RadioWhiteningModes_t Whitening; //!< Usage of whitening on payload and CRC blocks plus header block if + //!< header type is variable + } Gfsk; + /*! + * \brief Holds the LORA packet parameters + */ + struct { + uint8_t PreambleLength; //!< The preamble length is the number of LORA symbols in the preamble. To set it, + //!< use the following formula @code Number of symbols = PreambleLength[3:0] * ( + //!< 2^PreambleLength[7:4] ) @endcode + RadioLoRaPacketLengthsModes_t + HeaderType; //!< If the header is explicit, it will be transmitted in the LORA packet. If the header is + //!< implicit, it will not be transmitted + uint8_t PayloadLength; //!< Size of the payload in the LORA packet + RadioLoRaCrcModes_t Crc; //!< Size of CRC block in LORA packet + RadioLoRaIQModes_t InvertIQ; //!< Allows to swap IQ for LORA packet + } LoRa; + /*! + * \brief Holds the FLRC packet parameters + */ + struct { + RadioPreambleLengths_t PreambleLength; //!< The preamble length for FLRC packet type + RadioFlrcSyncWordLengths_t SyncWordLength; //!< The synchronization word length for FLRC packet type + RadioSyncWordRxMatchs_t + SyncWordMatch; //!< The synchronization correlator to use to check synchronization word + RadioPacketLengthModes_t HeaderType; //!< If the header is explicit, it will be transmitted in the FLRC + //!< packet. If the header is implicit, it will not be transmitted. + uint8_t PayloadLength; //!< Size of the payload in the FLRC packet + RadioCrcTypes_t CrcLength; //!< Size of the CRC block in the FLRC packet + RadioWhiteningModes_t Whitening; //!< Usage of whitening on payload and CRC blocks plus header block if + //!< header type is variable + } Flrc; + /*! + * \brief Holds the BLE packet parameters + */ + struct { + RadioBleConnectionStates_t ConnectionState; //!< The BLE state + RadioBleCrcTypes_t CrcLength; //!< Size of the CRC block in the BLE packet + RadioBleTestPayloads_t BleTestPayload; //!< Special BLE payload for test purpose + RadioWhiteningModes_t Whitening; //!< Usage of whitening on PDU and CRC blocks of BLE packet + } Ble; + } Params; //!< Holds the packet parameters structure +} PacketParams_t; + +/*! + * \brief Represents the packet status for every packet type + */ +typedef struct { + RadioPacketTypes_t packetType; //!< Packet to which the packet status are referring to. + union { + struct { + int8_t RssiSync; //!< The RSSI measured on last packet + struct { + bool SyncError : 1; //!< SyncWord error on last packet + bool LengthError : 1; //!< Length error on last packet + bool CrcError : 1; //!< CRC error on last packet + bool AbortError : 1; //!< Abort error on last packet + bool HeaderReceived : 1; //!< Header received on last packet + bool PacketReceived : 1; //!< Packet received + bool PacketControlerBusy : 1; //!< Packet controller busy + } ErrorStatus; //!< The error status Byte + struct { + bool RxNoAck : 1; //!< No acknowledgment received for Rx with variable length packets + bool PacketSent : 1; //!< Packet sent, only relevant in Tx mode + } TxRxStatus; //!< The Tx/Rx status Byte + uint8_t SyncAddrStatus : 3; //!< The id of the correlator who found the packet + } Gfsk; + struct { + int8_t RssiPkt; //!< The RSSI of the last packet + int8_t SnrPkt; //!< The SNR of the last packet + } LoRa; + struct { + int8_t RssiSync; //!< The RSSI of the last packet + struct { + bool SyncError : 1; //!< SyncWord error on last packet + bool LengthError : 1; //!< Length error on last packet + bool CrcError : 1; //!< CRC error on last packet + bool AbortError : 1; //!< Abort error on last packet + bool HeaderReceived : 1; //!< Header received on last packet + bool PacketReceived : 1; //!< Packet received + bool PacketControlerBusy : 1; //!< Packet controller busy + } ErrorStatus; //!< The error status Byte + struct { + uint8_t RxPid : 2; //!< PID of the Rx + bool RxNoAck : 1; //!< No acknowledgment received for Rx with variable length packets + bool RxPidErr : 1; //!< Received PID error + bool PacketSent : 1; //!< Packet sent, only relevant in Tx mode + } TxRxStatus; //!< The Tx/Rx status Byte + uint8_t SyncAddrStatus : 3; //!< The id of the correlator who found the packet + } Flrc; + struct { + int8_t RssiSync; //!< The RSSI of the last packet + struct { + bool SyncError : 1; //!< SyncWord error on last packet + bool LengthError : 1; //!< Length error on last packet + bool CrcError : 1; //!< CRC error on last packet + bool AbortError : 1; //!< Abort error on last packet + bool HeaderReceived : 1; //!< Header received on last packet + bool PacketReceived : 1; //!< Packet received + bool PacketControlerBusy : 1; //!< Packet controller busy + } ErrorStatus; //!< The error status Byte + struct { + bool PacketSent : 1; //!< Packet sent, only relevant in Tx mode + } TxRxStatus; //!< The Tx/Rx status Byte + uint8_t SyncAddrStatus : 3; //!< The id of the correlator who found the packet + } Ble; + }; +} PacketStatus_t; + +/*! + * \brief Represents the Rx internal counters values when GFSK or LORA packet type is used + */ +typedef struct { + RadioPacketTypes_t packetType; //!< Packet to which the packet status are referring to. + union { + struct { + uint16_t PacketReceived; //!< Number of received packets + uint16_t CrcError; //!< Number of CRC errors + uint16_t LengthError; //!< Number of length errors + uint16_t SyncwordError; //!< Number of sync-word errors + } Gfsk; + struct { + uint16_t PacketReceived; //!< Number of received packets + uint16_t CrcError; //!< Number of CRC errors + uint16_t HeaderValid; //!< Number of valid headers + } LoRa; + }; +} RxCounter_t; + +/*! + * \brief Represents a calibration configuration + */ +typedef struct { + uint8_t RC64KEnable : 1; //!< Calibrate RC64K clock + uint8_t RC13MEnable : 1; //!< Calibrate RC13M clock + uint8_t PLLEnable : 1; //!< Calibrate PLL + uint8_t ADCPulseEnable : 1; //!< Calibrate ADC Pulse + uint8_t ADCBulkNEnable : 1; //!< Calibrate ADC bulkN + uint8_t ADCBulkPEnable : 1; //!< Calibrate ADC bulkP +} CalibrationParams_t; + +/*! + * \brief Represents a sleep mode configuration + */ +typedef struct { + uint8_t WakeUpRTC : 1; //!< Get out of sleep mode if wakeup signal received from RTC + uint8_t InstructionRamRetention : 1; //!< InstructionRam is conserved during sleep + uint8_t DataBufferRetention : 1; //!< Data buffer is conserved during sleep + uint8_t DataRamRetention : 1; //!< Data ram is conserved during sleep +} SleepParams_t; + +/*! + * \brief Represents the SX1280 and its features + * + * It implements the commands the SX1280 can understands + */ +class SX1280 : public Radio { + public: + /*! + * \brief Instantiates a SX1280 object and provides API functions to communicates with the radio + * + * \param [in] callbacks Pointer to the callbacks structure defining + * all callbacks function pointers + */ + explicit SX1280(RadioCallbacks_t* callbacks) + : // The class members are value-initialiazed in member-initilaizer list + Radio(callbacks), + OperatingMode(MODE_STDBY_RC), + PacketType(PACKET_TYPE_NONE), + LoRaBandwidth(LORA_BW_1600), + IrqState(false), + PollingMode(false) { + this->dioIrq = &SX1280::OnDioIrq; + + // Warning: this constructor set the LoRaBandwidth member to a valid + // value, but it is not related to the actual radio configuration! + } + + virtual ~SX1280() {} + + private: + /*! + * \brief Holds the internal operating mode of the radio + */ + RadioOperatingModes_t OperatingMode; + + /*! + * \brief Stores the current packet type set in the radio + */ + RadioPacketTypes_t PacketType; + + /*! + * \brief Stores the current LORA bandwidth set in the radio + */ + RadioLoRaBandwidths_t LoRaBandwidth; + + /*! + * \brief Holds a flag raised on radio interrupt + */ + bool IrqState; + + /*! + * \brief Hardware DIO IRQ functions + */ + DioIrqHandler dioIrq; + + /*! + * \brief Holds the polling state of the driver + */ + bool PollingMode; + + /*! + * \brief Compute the two's complement for a register of size lower than + * 32bits + * + * \param [in] num The register to be two's complemented + * \param [in] bitCnt The position of the sign bit + */ + static int32_t complement2(const uint32_t num, const uint8_t bitCnt); + + /*! + * \brief Returns the value of LoRa bandwidth from driver's value + * + * The value is returned in Hz so that it can be represented as an integer + * type. Most computation should be done as integer to reduce floating + * point related errors. + * + * \retval loRaBw The value of the current bandwidth in Hz + */ + int32_t GetLoRaBandwidth(void); + + protected: + /*! + * \brief Sets a function to be triggered on radio interrupt + * + * \param [in] irqHandler A pointer to a function to be run on interrupt + * from the radio + */ + virtual void IoIrqInit(DioIrqHandler irqHandler) = 0; + + /*! + * \brief DIOs interrupt callback + * + * \remark Called to handle all 3 DIOs pins + */ + void OnDioIrq(void); + + /*! + * \brief Set the role of the radio during ranging operations + * + * \param [in] role Role of the radio + */ + void SetRangingRole(RadioRangingRoles_t role); + + public: + /*! + * \brief Initializes the radio driver + */ + void Init(void); + + /*! + * \brief Set the driver in polling mode. + * + * In polling mode the application is responsible to call ProcessIrqs( ) to + * execute callbacks functions. + * The default mode is Interrupt Mode. + * @code + * // Initializations and callbacks declaration/definition + * radio = SX1280( mosi, miso, sclk, nss, busy, int1, int2, int3, rst, &callbacks ); + * radio.Init( ); + * radio.SetPollingMode( ); + * + * while( true ) + * { + * // IRQ processing is automatically done + * radio.ProcessIrqs( ); // <-- here, as well as callback functions + * // calls + * // Do some applicative work + * } + * @endcode + * + * \see SX1280::SetInterruptMode + */ + void SetPollingMode(void); + + /*! + * \brief Set the driver in interrupt mode. + * + * In interrupt mode, the driver communicate with the radio during the + * interruption by direct calls to ProcessIrqs( ). The main advantage is + * the possibility to have low power application architecture. + * This is the default mode. + * @code + * // Initializations and callbacks declaration/definition + * radio = SX1280( mosi, miso, sclk, nss, busy, int1, int2, int3, rst, &callbacks ); + * radio.Init( ); + * radio.SetInterruptMode( ); // Optional. Driver default behavior + * + * while( true ) + * { + * // Do some applicative work + * } + * @endcode + * + * \see SX1280::SetPollingMode + */ + void SetInterruptMode(void); + + /*! + * \brief Initializes the radio registers to the recommended default values + */ + void SetRegistersDefault(void); + + /*! + * \brief Returns the current device firmware version + * + * \retval version Firmware version + */ + virtual uint16_t GetFirmwareVersion(void); + + /*! + * \brief Resets the radio + */ + virtual void Reset(void) = 0; + + /*! + * \brief Wake-ups the radio from Sleep mode + */ + virtual void Wakeup(void) = 0; + + /*! + * \brief Writes the given command to the radio + * + * \param [in] opcode Command opcode + * \param [in] buffer Command parameters byte array + * \param [in] size Command parameters byte array size + */ + virtual void WriteCommand(RadioCommands_t opcode, uint8_t* buffer, uint16_t size) = 0; + + /*! + * \brief Reads the given command from the radio + * + * \param [in] opcode Command opcode + * \param [in] buffer Command parameters byte array + * \param [in] size Command parameters byte array size + */ + virtual void ReadCommand(RadioCommands_t opcode, uint8_t* buffer, uint16_t size) = 0; + + /*! + * \brief Writes multiple radio registers starting at address + * + * \param [in] address First Radio register address + * \param [in] buffer Buffer containing the new register's values + * \param [in] size Number of registers to be written + */ + virtual void WriteRegister(uint16_t address, uint8_t* buffer, uint16_t size) = 0; + + /*! + * \brief Writes the radio register at the specified address + * + * \param [in] address Register address + * \param [in] value New register value + */ + virtual void WriteRegister(uint16_t address, uint8_t value) = 0; + + /*! + * \brief Reads multiple radio registers starting at address + * + * \param [in] address First Radio register address + * \param [out] buffer Buffer where to copy the registers data + * \param [in] size Number of registers to be read + */ + virtual void ReadRegister(uint16_t address, uint8_t* buffer, uint16_t size) = 0; + + /*! + * \brief Reads the radio register at the specified address + * + * \param [in] address Register address + * + * \retval data Register value + */ + virtual uint8_t ReadRegister(uint16_t address) = 0; + + /*! + * \brief Writes Radio Data Buffer with buffer of size starting at offset. + * + * \param [in] offset Offset where to start writing + * \param [in] buffer Buffer pointer + * \param [in] size Buffer size + */ + virtual void WriteBuffer(uint8_t offset, uint8_t* buffer, uint8_t size) = 0; + + /*! + * \brief Reads Radio Data Buffer at offset to buffer of size + * + * \param [in] offset Offset where to start reading + * \param [out] buffer Buffer pointer + * \param [in] size Buffer size + */ + virtual void ReadBuffer(uint8_t offset, uint8_t* buffer, uint8_t size) = 0; + + /*! + * \brief Gets the current status of the radio DIOs + * + * \retval status [Bit #3: DIO3, Bit #2: DIO2, + * Bit #1: DIO1, Bit #0: BUSY] + */ + virtual uint8_t GetDioStatus(void) = 0; + + /*! + * \brief Gets the current Operation Mode of the Radio + * + * \retval opMode Last operating mode + */ + virtual RadioOperatingModes_t GetOpMode(void); + + /*! + * \brief Gets the current radio status + * + * \retval status Radio status + */ + virtual RadioStatus_t GetStatus(void); + + /*! + * \brief Sets the radio in sleep mode + * + * \param [in] sleepConfig The sleep configuration describing data + * retention and RTC wake-up + */ + void SetSleep(SleepParams_t sleepConfig); + + /*! + * \brief Sets the radio in configuration mode + * + * \param [in] mode The standby mode to put the radio into + */ + void SetStandby(RadioStandbyModes_t mode); + + /*! + * \brief Sets the radio in FS mode + */ + void SetFs(void); + + /*! + * \brief Sets the radio in transmission mode + * + * \param [in] timeout Structure describing the transmission timeout value + */ + void SetTx(TickTime_t timeout); + + /*! + * \brief Sets the radio in reception mode + * + * \param [in] timeout Structure describing the reception timeout value + */ + void SetRx(TickTime_t timeout); + + /*! + * \brief Sets the Rx duty cycle management parameters + * + * \param [in] periodBase Base time for Rx and Sleep TickTime sequences + * \param [in] periodBaseCountRx Number of base time for Rx TickTime sequence + * \param [in] periodBaseCountSleep Number of base time for Sleep TickTime sequence + */ + void SetRxDutyCycle(RadioTickSizes_t periodBase, uint16_t periodBaseCountRx, uint16_t periodBaseCountSleep); + + /*! + * \brief Sets the radio in CAD mode + * + * \see SX1280::SetCadParams + */ + void SetCad(void); + + /*! + * \brief Sets the radio in continuous wave transmission mode + */ + void SetTxContinuousWave(void); + + /*! + * \brief Sets the radio in continuous preamble transmission mode + */ + void SetTxContinuousPreamble(void); + + /*! + * \brief Sets the radio for the given protocol + * + * \param [in] packetType [PACKET_TYPE_GFSK, PACKET_TYPE_LORA, + * PACKET_TYPE_RANGING, PACKET_TYPE_FLRC, + * PACKET_TYPE_BLE] + * + * \remark This method has to be called before SetRfFrequency, + * SetModulationParams and SetPacketParams + */ + void SetPacketType(RadioPacketTypes_t packetType); + + /*! + * \brief Gets the current radio protocol + * + * Default behavior return the packet type returned by the radio. To + * reduce the SPI activity and return the packet type stored by the + * driver, a second optional argument must be provided evaluating as true + * boolean + * + * \param [in] returnLocalCopy If true, the packet type returned is the last + * saved in the driver + * If false, the packet type is obtained from + * the chip + * Default: false + * + * \retval packetType [PACKET_TYPE_GENERIC, PACKET_TYPE_LORA, + * PACKET_TYPE_RANGING, PACKET_TYPE_FLRC, + * PACKET_TYPE_BLE, PACKET_TYPE_NONE] + */ + RadioPacketTypes_t GetPacketType(bool returnLocalCopy = false); + + /*! + * \brief Sets the RF frequency + * + * \param [in] rfFrequency RF frequency [Hz] + */ + void SetRfFrequency(uint32_t rfFrequency); + + /*! + * \brief Sets the transmission parameters + * + * \param [in] power RF output power [-18..13] dBm + * \param [in] rampTime Transmission ramp up time + */ + void SetTxParams(int8_t power, RadioRampTimes_t rampTime); + + /*! + * \brief Sets the number of symbols to be used for Channel Activity + * Detection operation + * + * \param [in] cadSymbolNum The number of symbol to use for Channel Activity + * Detection operations [LORA_CAD_01_SYMBOL, LORA_CAD_02_SYMBOLS, + * LORA_CAD_04_SYMBOLS, LORA_CAD_08_SYMBOLS, LORA_CAD_16_SYMBOLS] + */ + void SetCadParams(RadioLoRaCadSymbols_t cadSymbolNum); + + /*! + * \brief Sets the data buffer base address for transmission and reception + * + * \param [in] txBaseAddress Transmission base address + * \param [in] rxBaseAddress Reception base address + */ + void SetBufferBaseAddresses(uint8_t txBaseAddress, uint8_t rxBaseAddress); + + /*! + * \brief Set the modulation parameters + * + * \param [in] modParams A structure describing the modulation parameters + */ + void SetModulationParams(ModulationParams_t* modParams); + + /*! + * \brief Sets the packet parameters + * + * \param [in] packetParams A structure describing the packet parameters + */ + void SetPacketParams(PacketParams_t* packetParams); + + /*! + * \brief Gets the last received packet buffer status + * + * \param [out] rxPayloadLength Last received packet payload length + * \param [out] rxStartBufferPointer Last received packet buffer address pointer + */ + void GetRxBufferStatus(uint8_t* rxPayloadLength, uint8_t* rxStartBufferPointer); + + /*! + * \brief Gets the last received packet status + * + * The packet status structure returned depends on the modem type selected + * + * \see PacketStatus_t for the description of available information + * + * \param [out] packetStatus A structure of packet status + */ + void GetPacketStatus(PacketStatus_t* packetStatus); + + /*! + * \brief Returns the instantaneous RSSI value for the last packet received + * + * \retval rssiInst Instantaneous RSSI + */ + int8_t GetRssiInst(void); + + /*! + * \brief Sets the IRQ mask and DIO masks + * + * \param [in] irqMask General IRQ mask + * \param [in] dio1Mask DIO1 mask + * \param [in] dio2Mask DIO2 mask + * \param [in] dio3Mask DIO3 mask + */ + void SetDioIrqParams(uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask); + + /*! + * \brief Returns the current IRQ status + * + * \retval irqStatus IRQ status + */ + uint16_t GetIrqStatus(void); + + /*! + * \brief Clears the IRQs + * + * \param [in] irqMask IRQ(s) to be cleared + */ + void ClearIrqStatus(uint16_t irqMask); + + /*! + * \brief Calibrates the given radio block + * + * \param [in] calibParam The description of blocks to be calibrated + */ + void Calibrate(CalibrationParams_t calibParam); + + /*! + * \brief Sets the power regulators operating mode + * + * \param [in] mode [0: LDO, 1:DC_DC] + */ + void SetRegulatorMode(RadioRegulatorModes_t mode); + + /*! + * \brief Saves the current selected modem configuration into data RAM + */ + void SetSaveContext(void); + + /*! + * \brief Sets the chip to automatically send a packet after the end of a packet reception + * + * \remark The offset is automatically compensated inside the function + * + * \param [in] time The delay in us after which a Tx is done + */ + void SetAutoTx(uint16_t time); + + /*! + * \brief Stop the chip to automatically send a packet after the end of a packet reception + * if previously activated with SX1280::SetAutoTx command + */ + void StopAutoTx(void); + + /*! + * \brief Sets the chip to stay in FS mode after sending a packet + * + * \param [in] enableAutoFs Turn on auto FS + */ + void SetAutoFs(bool enableAutoFs); + + /*! + * \brief Enables or disables long preamble detection mode + * + * \param [in] enable Turn on long preamble mode + */ + void SetLongPreamble(bool enable); + + /*! + * \brief Saves the payload to be send in the radio buffer + * + * \param [in] payload A pointer to the payload + * \param [in] size The size of the payload + * \param [in] offset The address in FIFO where writing first byte (default = 0x00) + */ + void SetPayload(uint8_t* payload, uint8_t size, uint8_t offset = 0x00); + + /*! + * \brief Reads the payload received. If the received payload is longer + * than maxSize, then the method returns 1 and do not set size and payload. + * + * \param [out] payload A pointer to a buffer into which the payload will be copied + * \param [out] size A pointer to the size of the payload received + * \param [in] maxSize The maximal size allowed to copy into the buffer + */ + uint8_t GetPayload(uint8_t* payload, uint8_t* size, uint8_t maxSize); + + /*! + * \brief Sends a payload + * + * \param [in] payload A pointer to the payload to send + * \param [in] size The size of the payload to send + * \param [in] timeout The timeout for Tx operation + * \param [in] offset The address in FIFO where writing first byte (default = 0x00) + */ + void SendPayload(uint8_t* payload, uint8_t size, TickTime_t timeout, uint8_t offset = 0x00); + + /*! + * \brief Sets the Sync Word given by index used in GFSK, FLRC and BLE protocols + * + * \remark 5th byte isn't used in FLRC and BLE protocols + * + * \param [in] syncWordIdx Index of SyncWord to be set [1..3] + * \param [in] syncWord SyncWord bytes ( 5 bytes ) + * + * \retval status [0: OK, 1: NOK] + */ + uint8_t SetSyncWord(uint8_t syncWordIdx, uint8_t* syncWord); + + /*! + * \brief Defines how many error bits are tolerated in sync word detection + * + * \param [in] errorBits Number of error bits supported to validate the Sync word detection + * ( default is 4 bit, minimum is 1 bit ) + */ + void SetSyncWordErrorTolerance(uint8_t errorBits); + + /*! + * \brief Sets the Initial value for the LFSR used for the CRC calculation + * + * \param [in] seed Initial LFSR value ( 4 bytes ) + * + * \retval updated [0: failure, 1: success] + * + */ + uint8_t SetCrcSeed(uint8_t* seed); + + /*! + * \brief Set the Access Address field of BLE packet + * + * \param [in] accessAddress The access address to be used for next BLE packet sent + * + * \see SX1280::SetBleAdvertizerAccessAddress + */ + void SetBleAccessAddress(uint32_t accessAddress); + + /*! + * \brief Set the Access Address for Advertizer BLE packets + * + * All advertizer BLE packets must use a particular value for Access + * Address field. This method sets it. + * + * \see SX1280::SetBleAccessAddress + */ + void SetBleAdvertizerAccessAddress(void); + + /*! + * \brief Sets the seed used for the CRC calculation + * + * \param [in] polynomial The seed value + * + */ + void SetCrcPolynomial(uint16_t polynomial); + + /*! + * \brief Sets the Initial value of the LFSR used for the whitening in GFSK, FLRC and BLE protocols + * + * \param [in] seed Initial LFSR value + */ + void SetWhiteningSeed(uint8_t seed); + + /*! + * \brief Enable manual gain control and disable AGC + * + * \see SX1280::SetManualGainValue, SX1280::DisableManualGain + */ + void EnableManualGain(void); + + /*! + * \brief Disable the manual gain control and enable AGC + * + * \see SX1280::EnableManualGain + */ + void DisableManualGain(void); + + /*! + * \brief Set the gain for the AGC + * + * SX1280::EnableManualGain must be called before using this method + * + * \param [in] gain The value of gain to set, refer to datasheet for value meaning + * + * \see SX1280::EnableManualGain, SX1280::DisableManualGain + */ + void SetManualGainValue(uint8_t gain); + + /*! + * \brief Configure the LNA regime of operation + * + * \param [in] lnaSetting The LNA setting. Possible values are + * LNA_LOW_POWER_MODE and + * LNA_HIGH_SENSITIVITY_MODE + */ + void SetLNAGainSetting(const RadioLnaSettings_t lnaSetting); + + /*! + * \brief Sets the number of bits used to check that ranging request match ranging ID + * + * \param [in] length [0: 8 bits, 1: 16 bits, + * 2: 24 bits, 3: 32 bits] + */ + void SetRangingIdLength(RadioRangingIdCheckLengths_t length); + + /*! + * \brief Sets ranging device id + * + * \param [in] address Device address + */ + void SetDeviceRangingAddress(uint32_t address); + + /*! + * \brief Sets the device id to ping in a ranging request + * + * \param [in] address Address of the device to ping + */ + void SetRangingRequestAddress(uint32_t address); + + /*! + * \brief Return the ranging result value + * + * \param [in] resultType Specifies the type of result. + * [0: RAW, 1: Averaged, + * 2: De-biased, 3:Filtered] + * + * \retval ranging The ranging measure filtered according to resultType [m] + */ + double GetRangingResult(RadioRangingResultTypes_t resultType); + + /*! + * \brief Return the last ranging result power indicator + * + * The value returned is not an absolute power measurement. It is + * a relative power measurement. + * + * \retval deltaThreshold A relative power indicator + */ + uint8_t GetRangingPowerDeltaThresholdIndicator(void); + + /*! + * \brief Sets the standard processing delay between Master and Slave + * + * \param [in] cal RxTx delay offset for correcting ranging bias. + * + * The calibration value reflects the group delay of the radio front end and + * must be re-performed for each new SX1280 PCB design. The value is obtained + * empirically by either conducted measurement in a known electrical length + * coaxial RF cable (where the design is connectorised) or by radiated + * measurement, at a known distance, where an antenna is present. + * The result of the calibration process is that the SX1280 ranging result + * accurately reflects the physical range, the calibration procedure therefore + * removes the average timing error from the time-of-flight measurement for a + * given design. + * + * The values are Spreading Factor dependents, and depend also of the board + * design. + */ + void SetRangingCalibration(uint16_t cal); + + /*! + * \brief Clears the ranging filter + */ + void RangingClearFilterResult(void); + + /*! + * \brief Set the number of samples considered in the built-in filter + * + * \param [in] numSample The number of samples to use built-in filter + * [8..255] + * + * \remark Value inferior to 8 will be silently set to 8 + */ + void RangingSetFilterNumSamples(uint8_t numSample); + + /*! + * \brief Return the Estimated Frequency Error in LORA and RANGING operations + * + * \retval efe The estimated frequency error [Hz] + */ + double GetFrequencyError(); + + /*! + * \brief Process the analysis of radio IRQs and calls callback functions + * depending on radio state + */ + void ProcessIrqs(void); + + /*! + * \brief Force the preamble length in GFSK and BLE mode + * + * \param [in] preambleLength The desired preamble length + */ + void ForcePreambleLength(RadioPreambleLengths_t preambleLength); +}; + +#endif // __SX1280_H__ From 6770d7c03daeb393178d157a4c4f8f513f1a6d97 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 16:29:29 -0800 Subject: [PATCH 025/101] instantiate HAL --- .../Components/MyComponent/MyComponent.cpp | 8 +------- .../Components/MyComponent/MyComponent.hpp | 3 +++ .../Components/MyComponent/sx1280-hal.cpp | 19 ------------------- .../Components/MyComponent/sx1280-hal.h | 11 +---------- 4 files changed, 5 insertions(+), 36 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 10789350..bdd15db8 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -6,12 +6,6 @@ #include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" -/*#include "FprimeZephyrReference/Components/MyComponent/radio.h"*/ -/*#include "FprimeZephyrReference/Components/MyComponent/sx1280.h"*/ -/*#include "FprimeZephyrReference/Components/MyComponent/sx1280-hal.h"*/ -/*#include "FprimeZephyrReference/Components/MyComponent/sx1280.cpp"*/ -/*#include "FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp"*/ - #include #define OP_SET_MODULATION_PARAMS 0x8B @@ -29,7 +23,7 @@ namespace Components { // Component construction and destruction // ---------------------------------------------------------------------- -MyComponent ::MyComponent(const char* const compName) : MyComponentComponentBase(compName) {} +MyComponent ::MyComponent(const char* const compName) : MyComponentComponentBase(compName), hal(NULL) {} MyComponent ::~MyComponent() {} diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 0ed2a455..58ca54a8 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -8,6 +8,7 @@ #define Components_MyComponent_HPP #include "FprimeZephyrReference/Components/MyComponent/MyComponentComponentAc.hpp" +#include "FprimeZephyrReference/Components/MyComponent/sx1280-hal.h" namespace Components { @@ -65,6 +66,8 @@ class MyComponent final : public MyComponentComponentBase { void spiSetTx(); void spiSetStandby(); U8 spiGetStatus(); + + SX1280Hal hal; }; } // namespace Components diff --git a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp index ccb6965b..cc0354f4 100644 --- a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp +++ b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp @@ -71,25 +71,6 @@ Maintainer: Miguel Luis, Gregory Cristian and Matthieu Verdy #define assert_param(...) #endif -/*SX1280Hal::SX1280Hal( PinName mosi, PinName miso, PinName sclk, PinName nss,*/ -/* PinName busy, PinName dio1, PinName dio2, PinName dio3, PinName rst,*/ -/* RadioCallbacks_t *callbacks )*/ -/* : SX1280( callbacks ),*/ -/* RadioNss( nss ),*/ -/* RadioReset( rst ),*/ -/* RadioCtsn( NC ),*/ -/* BUSY( busy )*/ -/*{*/ -/* CreateDioPin( dio1, DIO1 );*/ -/* CreateDioPin( dio2, DIO2 );*/ -/* CreateDioPin( dio3, DIO3 );*/ -/* RadioSpi = new SPI( mosi, miso, sclk );*/ -/* RadioUart = NULL;*/ -/**/ -/* RadioNss = 1;*/ -/* RadioReset = 1;*/ -/*}*/ -/**/ /*SX1280Hal::SX1280Hal( PinName tx, PinName rx, PinName ctsn,*/ /* PinName busy, PinName dio1, PinName dio2, PinName dio3, PinName rst,*/ /* RadioCallbacks_t *callbacks )*/ diff --git a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h index 95e3d6a2..99dcbdeb 100644 --- a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h +++ b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h @@ -28,16 +28,7 @@ class SX1280Hal : public SX1280 { * * Represents the physical connectivity with the radio and set callback functions on radio interrupts */ - /*SX1280Hal(PinName mosi,*/ - /* PinName miso,*/ - /* PinName sclk,*/ - /* PinName nss,*/ - /* PinName busy,*/ - /* PinName dio1,*/ - /* PinName dio2,*/ - /* PinName dio3,*/ - /* PinName rst,*/ - /* RadioCallbacks_t* callbacks);*/ + explicit SX1280Hal(RadioCallbacks_t* callbacks) : SX1280(callbacks) {} /*! * \brief Constructor for SX1280Hal with UART support From 05f52b436d618517ac64fd1e88efbde1f27be836 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 16:32:54 -0800 Subject: [PATCH 026/101] remove rate group thing from MyComponent --- .../Components/MyComponent/MyComponent.cpp | 8 -------- .../Components/MyComponent/MyComponent.fpp | 3 --- .../Components/MyComponent/MyComponent.hpp | 12 ------------ 3 files changed, 23 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index bdd15db8..b0c34c18 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -27,14 +27,6 @@ MyComponent ::MyComponent(const char* const compName) : MyComponentComponentBase MyComponent ::~MyComponent() {} -// ---------------------------------------------------------------------- -// Handler implementations for typed input ports -// ---------------------------------------------------------------------- - -void MyComponent ::run_handler(FwIndexType portNum, U32 context) { - // TODO -} - // ---------------------------------------------------------------------- // Handler implementations for commands // ---------------------------------------------------------------------- diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp index b8201289..6a613915 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -8,9 +8,6 @@ module Components { #### Uncomment the following examples to start customizing your component #### ############################################################################## - @ Port receiving calls from the rate group - sync input port run: Svc.Sched - # @ Import Communication Interface # import Svc.Com diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 58ca54a8..9a2db99a 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -25,18 +25,6 @@ class MyComponent final : public MyComponentComponentBase { //! Destroy MyComponent object ~MyComponent(); - private: - // ---------------------------------------------------------------------- - // Handler implementations for typed input ports - // ---------------------------------------------------------------------- - - //! Handler implementation for run - //! - //! Port receiving calls from the rate group - void run_handler(FwIndexType portNum, //!< The port number - U32 context //!< The call order - ) override; - private: // ---------------------------------------------------------------------- // Handler implementations for commands From 67e721a4b9a15b1e18defecc6616ffe7b89b18df Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 18:58:03 -0800 Subject: [PATCH 027/101] Revert "remove rate group thing from MyComponent" This reverts commit 05f52b436d618517ac64fd1e88efbde1f27be836. --- .../Components/MyComponent/MyComponent.cpp | 8 ++++++++ .../Components/MyComponent/MyComponent.fpp | 3 +++ .../Components/MyComponent/MyComponent.hpp | 12 ++++++++++++ 3 files changed, 23 insertions(+) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index b0c34c18..bdd15db8 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -27,6 +27,14 @@ MyComponent ::MyComponent(const char* const compName) : MyComponentComponentBase MyComponent ::~MyComponent() {} +// ---------------------------------------------------------------------- +// Handler implementations for typed input ports +// ---------------------------------------------------------------------- + +void MyComponent ::run_handler(FwIndexType portNum, U32 context) { + // TODO +} + // ---------------------------------------------------------------------- // Handler implementations for commands // ---------------------------------------------------------------------- diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp index 6a613915..b8201289 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -8,6 +8,9 @@ module Components { #### Uncomment the following examples to start customizing your component #### ############################################################################## + @ Port receiving calls from the rate group + sync input port run: Svc.Sched + # @ Import Communication Interface # import Svc.Com diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 9a2db99a..58ca54a8 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -25,6 +25,18 @@ class MyComponent final : public MyComponentComponentBase { //! Destroy MyComponent object ~MyComponent(); + private: + // ---------------------------------------------------------------------- + // Handler implementations for typed input ports + // ---------------------------------------------------------------------- + + //! Handler implementation for run + //! + //! Port receiving calls from the rate group + void run_handler(FwIndexType portNum, //!< The port number + U32 context //!< The call order + ) override; + private: // ---------------------------------------------------------------------- // Handler implementations for commands From dfba4dfd22e8bd7e109e1f6707d98ee1f600d242 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 18:58:06 -0800 Subject: [PATCH 028/101] Revert "instantiate HAL" This reverts commit 6770d7c03daeb393178d157a4c4f8f513f1a6d97. --- .../Components/MyComponent/MyComponent.cpp | 8 +++++++- .../Components/MyComponent/MyComponent.hpp | 3 --- .../Components/MyComponent/sx1280-hal.cpp | 19 +++++++++++++++++++ .../Components/MyComponent/sx1280-hal.h | 11 ++++++++++- 4 files changed, 36 insertions(+), 5 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index bdd15db8..10789350 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -6,6 +6,12 @@ #include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" +/*#include "FprimeZephyrReference/Components/MyComponent/radio.h"*/ +/*#include "FprimeZephyrReference/Components/MyComponent/sx1280.h"*/ +/*#include "FprimeZephyrReference/Components/MyComponent/sx1280-hal.h"*/ +/*#include "FprimeZephyrReference/Components/MyComponent/sx1280.cpp"*/ +/*#include "FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp"*/ + #include #define OP_SET_MODULATION_PARAMS 0x8B @@ -23,7 +29,7 @@ namespace Components { // Component construction and destruction // ---------------------------------------------------------------------- -MyComponent ::MyComponent(const char* const compName) : MyComponentComponentBase(compName), hal(NULL) {} +MyComponent ::MyComponent(const char* const compName) : MyComponentComponentBase(compName) {} MyComponent ::~MyComponent() {} diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 58ca54a8..0ed2a455 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -8,7 +8,6 @@ #define Components_MyComponent_HPP #include "FprimeZephyrReference/Components/MyComponent/MyComponentComponentAc.hpp" -#include "FprimeZephyrReference/Components/MyComponent/sx1280-hal.h" namespace Components { @@ -66,8 +65,6 @@ class MyComponent final : public MyComponentComponentBase { void spiSetTx(); void spiSetStandby(); U8 spiGetStatus(); - - SX1280Hal hal; }; } // namespace Components diff --git a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp index cc0354f4..ccb6965b 100644 --- a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp +++ b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp @@ -71,6 +71,25 @@ Maintainer: Miguel Luis, Gregory Cristian and Matthieu Verdy #define assert_param(...) #endif +/*SX1280Hal::SX1280Hal( PinName mosi, PinName miso, PinName sclk, PinName nss,*/ +/* PinName busy, PinName dio1, PinName dio2, PinName dio3, PinName rst,*/ +/* RadioCallbacks_t *callbacks )*/ +/* : SX1280( callbacks ),*/ +/* RadioNss( nss ),*/ +/* RadioReset( rst ),*/ +/* RadioCtsn( NC ),*/ +/* BUSY( busy )*/ +/*{*/ +/* CreateDioPin( dio1, DIO1 );*/ +/* CreateDioPin( dio2, DIO2 );*/ +/* CreateDioPin( dio3, DIO3 );*/ +/* RadioSpi = new SPI( mosi, miso, sclk );*/ +/* RadioUart = NULL;*/ +/**/ +/* RadioNss = 1;*/ +/* RadioReset = 1;*/ +/*}*/ +/**/ /*SX1280Hal::SX1280Hal( PinName tx, PinName rx, PinName ctsn,*/ /* PinName busy, PinName dio1, PinName dio2, PinName dio3, PinName rst,*/ /* RadioCallbacks_t *callbacks )*/ diff --git a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h index 99dcbdeb..95e3d6a2 100644 --- a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h +++ b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h @@ -28,7 +28,16 @@ class SX1280Hal : public SX1280 { * * Represents the physical connectivity with the radio and set callback functions on radio interrupts */ - explicit SX1280Hal(RadioCallbacks_t* callbacks) : SX1280(callbacks) {} + /*SX1280Hal(PinName mosi,*/ + /* PinName miso,*/ + /* PinName sclk,*/ + /* PinName nss,*/ + /* PinName busy,*/ + /* PinName dio1,*/ + /* PinName dio2,*/ + /* PinName dio3,*/ + /* PinName rst,*/ + /* RadioCallbacks_t* callbacks);*/ /*! * \brief Constructor for SX1280Hal with UART support From 62d881957532d3111ee680816714a702efeb7306 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 18:58:25 -0800 Subject: [PATCH 029/101] Revert "initial import of external radio driver code" This reverts commit 6862c92937fdd15a6b22cac2308f9eba1d66e074. --- .../Components/MyComponent/CMakeLists.txt | 2 - .../Components/MyComponent/MyComponent.cpp | 6 - .../Components/MyComponent/radio.h | 302 --- .../Components/MyComponent/sx1280-hal.cpp | 457 ----- .../Components/MyComponent/sx1280-hal.h | 192 -- .../Components/MyComponent/sx1280.cpp | 1063 ----------- .../Components/MyComponent/sx1280.h | 1627 ----------------- 7 files changed, 3649 deletions(-) delete mode 100644 FprimeZephyrReference/Components/MyComponent/radio.h delete mode 100644 FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp delete mode 100644 FprimeZephyrReference/Components/MyComponent/sx1280-hal.h delete mode 100644 FprimeZephyrReference/Components/MyComponent/sx1280.cpp delete mode 100644 FprimeZephyrReference/Components/MyComponent/sx1280.h diff --git a/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt b/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt index e018e1d5..2667f799 100644 --- a/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt +++ b/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt @@ -19,8 +19,6 @@ register_fprime_library( "${CMAKE_CURRENT_LIST_DIR}/MyComponent.fpp" SOURCES "${CMAKE_CURRENT_LIST_DIR}/MyComponent.cpp" - "${CMAKE_CURRENT_LIST_DIR}/sx1280.cpp" - "${CMAKE_CURRENT_LIST_DIR}/sx1280-hal.cpp" # DEPENDS # MyPackage_MyOtherModule ) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 10789350..e9fcf9e2 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -6,12 +6,6 @@ #include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" -/*#include "FprimeZephyrReference/Components/MyComponent/radio.h"*/ -/*#include "FprimeZephyrReference/Components/MyComponent/sx1280.h"*/ -/*#include "FprimeZephyrReference/Components/MyComponent/sx1280-hal.h"*/ -/*#include "FprimeZephyrReference/Components/MyComponent/sx1280.cpp"*/ -/*#include "FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp"*/ - #include #define OP_SET_MODULATION_PARAMS 0x8B diff --git a/FprimeZephyrReference/Components/MyComponent/radio.h b/FprimeZephyrReference/Components/MyComponent/radio.h deleted file mode 100644 index bcdf1a82..00000000 --- a/FprimeZephyrReference/Components/MyComponent/radio.h +++ /dev/null @@ -1,302 +0,0 @@ -/* - ______ _ - / _____) _ | | -( (____ _____ ____ _| |_ _____ ____| |__ - \____ \| ___ | (_ _) ___ |/ ___) _ \ - _____) ) ____| | | || |_| ____( (___| | | | -(______/|_____)_|_|_| \__)_____)\____)_| |_| - (C)2016 Semtech - -Description: Handling of the node configuration protocol - -License: Revised BSD License, see LICENSE.TXT file include in the project - -Maintainer: Miguel Luis, Gregory Cristian and Matthieu Verdy -*/ -#ifndef __RADIO_H__ -#define __RADIO_H__ - -/*#include "mbed.h"*/ -#include - -/*! - * \brief Structure describing the radio status - */ -typedef union { - /*! - * \brief Structure of the radio status - */ - struct { - uint8_t CpuBusy : 1; //!< Flag for CPU radio busy - uint8_t DmaBusy : 1; //!< Flag for DMA busy - uint8_t CmdStatus : 3; //!< Command status - uint8_t ChipMode : 3; //!< Chip mode - } Fields; - - /*! - * \brief Serialized radio status - */ - uint8_t Value; -} RadioStatus_t; - -/*! - * \brief Structure describing the ranging codes for callback functions - */ -typedef enum { - IRQ_RANGING_SLAVE_ERROR_CODE = 0x00, - IRQ_RANGING_SLAVE_VALID_CODE, - IRQ_RANGING_MASTER_ERROR_CODE, - IRQ_RANGING_MASTER_VALID_CODE, -} IrqRangingCode_t; - -/*! - * \brief Structure describing the error codes for callback functions - */ -typedef enum { - IRQ_HEADER_ERROR_CODE = 0x00, - IRQ_SYNCWORD_ERROR_CODE, - IRQ_CRC_ERROR_CODE, - IRQ_RANGING_ON_LORA_ERROR_CODE, -} IrqErrorCode_t; - -/*! - * \brief Structure describing the validity codes for callback function rxValid - */ -typedef enum { - IRQ_HEADER_VALID_CODE = 0x00, - IRQ_SYNCWORD_VALID_CODE, -} IrqValidCode_t; - -/*! - * \brief Represents all possible opcode understood by the radio - */ -typedef enum RadioCommands_u { - RADIO_GET_STATUS = 0xC0, - RADIO_WRITE_REGISTER = 0x18, - RADIO_READ_REGISTER = 0x19, - RADIO_WRITE_BUFFER = 0x1A, - RADIO_READ_BUFFER = 0x1B, - RADIO_SET_SLEEP = 0x84, - RADIO_SET_STANDBY = 0x80, - RADIO_SET_FS = 0xC1, - RADIO_SET_TX = 0x83, - RADIO_SET_RX = 0x82, - RADIO_SET_RXDUTYCYCLE = 0x94, - RADIO_SET_CAD = 0xC5, - RADIO_SET_TXCONTINUOUSWAVE = 0xD1, - RADIO_SET_TXCONTINUOUSPREAMBLE = 0xD2, - RADIO_SET_PACKETTYPE = 0x8A, - RADIO_GET_PACKETTYPE = 0x03, - RADIO_SET_RFFREQUENCY = 0x86, - RADIO_SET_TXPARAMS = 0x8E, - RADIO_SET_CADPARAMS = 0x88, - RADIO_SET_BUFFERBASEADDRESS = 0x8F, - RADIO_SET_MODULATIONPARAMS = 0x8B, - RADIO_SET_PACKETPARAMS = 0x8C, - RADIO_GET_RXBUFFERSTATUS = 0x17, - RADIO_GET_PACKETSTATUS = 0x1D, - RADIO_GET_RSSIINST = 0x1F, - RADIO_SET_DIOIRQPARAMS = 0x8D, - RADIO_GET_IRQSTATUS = 0x15, - RADIO_CLR_IRQSTATUS = 0x97, - RADIO_CALIBRATE = 0x89, - RADIO_SET_REGULATORMODE = 0x96, - RADIO_SET_SAVECONTEXT = 0xD5, - RADIO_SET_AUTOTX = 0x98, - RADIO_SET_AUTOFS = 0x9E, - RADIO_SET_LONGPREAMBLE = 0x9B, - RADIO_SET_UARTSPEED = 0x9D, - RADIO_SET_RANGING_ROLE = 0xA3, -} RadioCommands_t; - -/*! - * \brief The radio callbacks structure - * Holds function pointers to be called on radio interrupts - */ -typedef struct { - void (*txDone)(void); //!< Pointer to a function run on successful transmission - void (*rxDone)(void); //!< Pointer to a function run on successful reception - void (*rxSyncWordDone)(void); //!< Pointer to a function run on successful SyncWord reception - void (*rxHeaderDone)(void); //!< Pointer to a function run on successful Header reception - void (*txTimeout)(void); //!< Pointer to a function run on transmission timeout - void (*rxTimeout)(void); //!< Pointer to a function run on reception timeout - void (*rxError)(IrqErrorCode_t errCode); //!< Pointer to a function run on reception error - void (*rangingDone)(IrqRangingCode_t val); //!< Pointer to a function run on ranging terminated - void (*cadDone)(bool cadFlag); //!< Pointer to a function run on channel activity detected -} RadioCallbacks_t; - -/*! - * \brief Class holding the basic communications with a radio - * - * It sets the functions to read/write registers, send commands and read/write - * payload. - * It also provides functions to run callback functions depending on the - * interrupts generated from the radio. - */ -class Radio { - protected: - /*! - * \brief Callback on Tx done interrupt - */ - void (*txDone)(void); - - /*! - * \brief Callback on Rx done interrupt - */ - void (*rxDone)(void); - - /*! - * \brief Callback on Rx SyncWord interrupt - */ - void (*rxSyncWordDone)(void); - - /*! - * \brief Callback on Rx header received interrupt - */ - void (*rxHeaderDone)(void); - - /*! - * \brief Callback on Tx timeout interrupt - */ - void (*txTimeout)(void); - - /*! - * \brief Callback on Rx timeout interrupt - */ - void (*rxTimeout)(void); - - /*! - * \brief Callback on Rx error interrupt - * - * \param [out] errCode A code indicating the type of interrupt (SyncWord error or CRC error) - */ - void (*rxError)(IrqErrorCode_t errCode); - - /*! - * \brief Callback on ranging done interrupt - * - * \param [out] val A flag indicating the type of interrupt (Master/Slave and Valid/Error) - */ - void (*rangingDone)(IrqRangingCode_t val); - - /*! - * \brief Callback on Channel Activity Detection done interrupt - * - * \param [out] cadFlag Flag for channel activity detected or not - */ - void (*cadDone)(bool cadFlag); - - public: - /*! - * \brief Constructor for radio class - * Sets the callbacks functions pointers - * - * \param [in] callbacks The structure of callbacks function pointers - * to be called on radio interrupts - * - */ - explicit Radio(RadioCallbacks_t* callbacks) { - this->txDone = callbacks->txDone; - this->rxDone = callbacks->rxDone; - this->rxSyncWordDone = callbacks->rxSyncWordDone; - this->rxHeaderDone = callbacks->rxHeaderDone; - this->txTimeout = callbacks->txTimeout; - this->rxTimeout = callbacks->rxTimeout; - this->rxError = callbacks->rxError; - this->rangingDone = callbacks->rangingDone; - this->cadDone = callbacks->cadDone; - } - virtual ~Radio(void) {}; - - /*! - * \brief Resets the radio - */ - virtual void Reset(void) = 0; - - /*! - * \brief Gets the current radio status - * - * \retval status Radio status - */ - virtual RadioStatus_t GetStatus(void) = 0; - - /*! - * \brief Writes the given command to the radio - * - * \param [in] opcode Command opcode - * \param [in] buffer Command parameters byte array - * \param [in] size Command parameters byte array size - */ - virtual void WriteCommand(RadioCommands_t opcode, uint8_t* buffer, uint16_t size) = 0; - - /*! - * \brief Reads the given command from the radio - * - * \param [in] opcode Command opcode - * \param [in] buffer Command parameters byte array - * \param [in] size Command parameters byte array size - */ - virtual void ReadCommand(RadioCommands_t opcode, uint8_t* buffer, uint16_t size) = 0; - - /*! - * \brief Writes multiple radio registers starting at address - * - * \param [in] address First Radio register address - * \param [in] buffer Buffer containing the new register's values - * \param [in] size Number of registers to be written - */ - virtual void WriteRegister(uint16_t address, uint8_t* buffer, uint16_t size) = 0; - - /*! - * \brief Writes the radio register at the specified address - * - * \param [in] address Register address - * \param [in] value New register value - */ - virtual void WriteRegister(uint16_t address, uint8_t value) = 0; - - /*! - * \brief Reads multiple radio registers starting at address - * - * \param [in] address First Radio register address - * \param [out] buffer Buffer where to copy the registers data - * \param [in] size Number of registers to be read - */ - virtual void ReadRegister(uint16_t address, uint8_t* buffer, uint16_t size) = 0; - - /*! - * \brief Reads the radio register at the specified address - * - * \param [in] address Register address - * - * \retval value The register value - */ - virtual uint8_t ReadRegister(uint16_t address) = 0; - - /*! - * \brief Writes Radio Data Buffer with buffer of size starting at offset. - * - * \param [in] offset Offset where to start writing - * \param [in] buffer Buffer pointer - * \param [in] size Buffer size - */ - virtual void WriteBuffer(uint8_t offset, uint8_t* buffer, uint8_t size) = 0; - - /*! - * \brief Reads Radio Data Buffer at offset to buffer of size - * - * \param [in] offset Offset where to start reading - * \param [out] buffer Buffer pointer - * \param [in] size Buffer size - */ - virtual void ReadBuffer(uint8_t offset, uint8_t* buffer, uint8_t size) = 0; - - /*! - * \brief Return firmware version - * - * \retval firmware The firmware version - */ - virtual uint16_t GetFirmwareVersion(void) = 0; -}; - -#endif // __RADIO_H__ diff --git a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp deleted file mode 100644 index ccb6965b..00000000 --- a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.cpp +++ /dev/null @@ -1,457 +0,0 @@ -/* - ______ _ - / _____) _ | | -( (____ _____ ____ _| |_ _____ ____| |__ - \____ \| ___ | (_ _) ___ |/ ___) _ \ - _____) ) ____| | | || |_| ____( (___| | | | -(______/|_____)_|_|_| \__)_____)\____)_| |_| - (C)2016 Semtech - -Description: Handling of the node configuration protocol - -License: Revised BSD License, see LICENSE.TXT file include in the project - -Maintainer: Miguel Luis, Gregory Cristian and Matthieu Verdy -*/ -#include "sx1280-hal.h" - -/*! - * \brief Helper macro to create Interrupt objects only if the pin name is - * different from NC - */ -/*#define CreateDioPin(pinName, dio) \*/ -/* if (pinName == NC) { \*/ -/* dio = NULL; \*/ -/* } else { \*/ -/* dio = new Interrupt(pinName); \*/ -/* }*/ - -/*! - * \brief Helper macro to avoid duplicating code for setting dio pins parameters - */ -/*#if defined( TARGET_NUCLEO_L476RG )*/ -/*#define DioAssignCallback( dio, pinMode, callback ) \*/ -/* if( dio != NULL ) \*/ -/* { \*/ -/* dio->mode( pinMode ); \*/ -/* dio->rise( this, static_cast ( callback ) ); \*/ -/* }*/ -/*#else*/ -#define DioAssignCallback(dio, pinMode, callback) \ - if (dio != NULL) { \ - dio->rise(this, static_cast(callback)); \ - } -/*#endif*/ -/*! - * \brief Used to block execution waiting for low state on radio busy pin. - * Essentially used in SPI communications - */ -#define WaitOnBusy() \ - while (BUSY == 1) { \ - } - -/*! - * \brief Blocking routine for waiting the UART to be writeable - * - */ -#define WaitUartWritable() \ - while (RadioUart->writeable() == false) { \ - } - -/*! - * \brief Blocking routine for waiting the UART to be readable - * - */ -#define WaitUartReadable() \ - while (RadioUart->readable() == false) { \ - } - -// This code handles cases where assert_param is undefined -#ifndef assert_param -#define assert_param(...) -#endif - -/*SX1280Hal::SX1280Hal( PinName mosi, PinName miso, PinName sclk, PinName nss,*/ -/* PinName busy, PinName dio1, PinName dio2, PinName dio3, PinName rst,*/ -/* RadioCallbacks_t *callbacks )*/ -/* : SX1280( callbacks ),*/ -/* RadioNss( nss ),*/ -/* RadioReset( rst ),*/ -/* RadioCtsn( NC ),*/ -/* BUSY( busy )*/ -/*{*/ -/* CreateDioPin( dio1, DIO1 );*/ -/* CreateDioPin( dio2, DIO2 );*/ -/* CreateDioPin( dio3, DIO3 );*/ -/* RadioSpi = new SPI( mosi, miso, sclk );*/ -/* RadioUart = NULL;*/ -/**/ -/* RadioNss = 1;*/ -/* RadioReset = 1;*/ -/*}*/ -/**/ -/*SX1280Hal::SX1280Hal( PinName tx, PinName rx, PinName ctsn,*/ -/* PinName busy, PinName dio1, PinName dio2, PinName dio3, PinName rst,*/ -/* RadioCallbacks_t *callbacks )*/ -/* : SX1280( callbacks ),*/ -/* RadioNss( NC ),*/ -/* RadioReset( rst ),*/ -/* RadioCtsn( ctsn ),*/ -/* BUSY( busy )*/ -/*{*/ -/* CreateDioPin( dio1, DIO1 );*/ -/* CreateDioPin( dio2, DIO2 );*/ -/* CreateDioPin( dio3, DIO3 );*/ -/* RadioSpi = NULL;*/ -/* RadioUart = new Serial( tx, rx );*/ -/* RadioCtsn = 0;*/ -/* RadioReset = 1;*/ -/*}*/ - -SX1280Hal::~SX1280Hal(void) { - /*if( this->RadioSpi != NULL )*/ - /*{*/ - /* delete RadioSpi;*/ - /*}*/ - /*if( this->RadioUart != NULL )*/ - /*{*/ - /* delete RadioUart;*/ - /*}*/ - /*if( DIO1 != NULL )*/ - /*{*/ - /* delete DIO1;*/ - /*}*/ - /*if( DIO2 != NULL )*/ - /*{*/ - /* delete DIO2;*/ - /*}*/ - /*if( DIO3 != NULL )*/ - /*{*/ - /* delete DIO3;*/ - /*}*/ -}; - -void SX1280Hal::SpiInit(void) { - /* RadioNss = 1;*/ - /* RadioSpi->format( 8, 0 );*/ - /*#if defined( TARGET_KL25Z )*/ - /* this->SetSpiSpeed( 4000000 );*/ - /*#elif defined( TARGET_NUCLEO_L476RG )*/ - /* this->SetSpiSpeed( 8000000 );*/ - /*#else*/ - /* this->SetSpiSpeed( 8000000 );*/ - /*#endif*/ - /* wait( 0.1 );*/ -} - -void SX1280Hal::SetSpiSpeed(uint32_t spiSpeed) { - /*RadioSpi->frequency( spiSpeed );*/ -} - -void SX1280Hal::UartInit(void) { - // RadioUart->format( 9, SerialBase::Even, 1 ); // 8 data bits + 1 even parity bit + 1 stop bit - // RadioUart->baud( 115200 ); - // After this point, the UART is running standard mode: 8 data bit, 1 even - // parity bit, 1 stop bit, 115200 baud, LSB first - // wait_us( 10 ); -} - -void SX1280Hal::IoIrqInit(DioIrqHandler irqHandler) { - /*assert_param( RadioSpi != NULL || RadioUart != NULL );*/ - /*if( RadioSpi != NULL )*/ - /*{*/ - /* SpiInit( );*/ - /*}*/ - /*if( RadioUart != NULL )*/ - /*{*/ - /* UartInit( );*/ - /*}*/ - /**/ - /*BUSY.mode( PullNone );*/ - /**/ - /*DioAssignCallback( DIO1, PullNone, irqHandler );*/ - /*DioAssignCallback( DIO2, PullNone, irqHandler );*/ - /*DioAssignCallback( DIO3, PullNone, irqHandler );*/ -} - -void SX1280Hal::Reset(void) { - /*__disable_irq( );*/ - /*wait_ms( 20 );*/ - /*RadioReset.output( );*/ - /*RadioReset = 0;*/ - /*wait_ms( 50 );*/ - /*RadioReset = 1;*/ - // RadioReset.input( ); // Using the internal pull-up - /*wait_ms( 20 );*/ - /*__enable_irq( );*/ -} - -void SX1280Hal::Wakeup(void) { - /*__disable_irq( );*/ - /**/ - // Don't wait for BUSY here - /**/ - /*if( RadioSpi != NULL )*/ - /*{*/ - /* RadioNss = 0;*/ - /* RadioSpi->write( RADIO_GET_STATUS );*/ - /* RadioSpi->write( 0 );*/ - /* RadioNss = 1;*/ - /*}*/ - /*if( RadioUart != NULL )*/ - /*{*/ - /* RadioUart->putc( RADIO_GET_STATUS );*/ - /* WaitUartReadable( );*/ - /* RadioUart->getc( );*/ - /*}*/ - /**/ - // Wait for chip to be ready. - /*WaitOnBusy( );*/ - /**/ - /*__enable_irq( );*/ -} - -void SX1280Hal::WriteCommand(RadioCommands_t command, uint8_t* buffer, uint16_t size) { - /*WaitOnBusy( );*/ - /**/ - /*if( RadioSpi != NULL )*/ - /*{*/ - /* RadioNss = 0;*/ - /* RadioSpi->write( ( uint8_t )command );*/ - /* for( uint16_t i = 0; i < size; i++ )*/ - /* {*/ - /* RadioSpi->write( buffer[i] );*/ - /* }*/ - /* RadioNss = 1;*/ - /*}*/ - /*if( RadioUart != NULL )*/ - /*{*/ - /* RadioUart->putc( command );*/ - /* if( size > 0 )*/ - /* {*/ - /* RadioUart->putc( size );*/ - /* for( uint16_t i = 0; i < size; i++ )*/ - /* {*/ - /* RadioUart->putc( buffer[i] );*/ - /* }*/ - /* }*/ - /*}*/ - /**/ - /*if( command != RADIO_SET_SLEEP )*/ - /*{*/ - /* WaitOnBusy( );*/ - /*}*/ -} - -void SX1280Hal::ReadCommand(RadioCommands_t command, uint8_t* buffer, uint16_t size) { - /*WaitOnBusy( );*/ - /**/ - /*if( RadioSpi != NULL )*/ - /*{*/ - /* RadioNss = 0;*/ - /* if( command == RADIO_GET_STATUS )*/ - /* {*/ - /* buffer[0] = RadioSpi->write( ( uint8_t )command );*/ - /* RadioSpi->write( 0 );*/ - /* RadioSpi->write( 0 );*/ - /* }*/ - /* else*/ - /* {*/ - /* RadioSpi->write( ( uint8_t )command );*/ - /* RadioSpi->write( 0 );*/ - /* for( uint16_t i = 0; i < size; i++ )*/ - /* {*/ - /* buffer[i] = RadioSpi->write( 0 );*/ - /* }*/ - /* }*/ - /* RadioNss = 1;*/ - /*}*/ - /*if( RadioUart != NULL )*/ - /*{*/ - /* RadioUart->putc( command );*/ - /**/ - // Behavior on the UART is different depending of the opcode command - /* if( ( command == RADIO_GET_PACKETTYPE ) ||*/ - /* ( command == RADIO_GET_RXBUFFERSTATUS ) ||*/ - /* ( command == RADIO_GET_RSSIINST ) ||*/ - /* ( command == RADIO_GET_PACKETSTATUS ) ||*/ - /* ( command == RADIO_GET_IRQSTATUS ) )*/ - /* {*/ - /* - * TODO : Check size size in UART (uint8_t in putc) - */ - /* RadioUart->putc( size );*/ - /* }*/ - /**/ - /* WaitUartReadable( );*/ - /* for( uint16_t i = 0; i < size; i++ )*/ - /* {*/ - /* buffer[i] = RadioUart->getc( );*/ - /* }*/ - /*}*/ - /**/ - /*WaitOnBusy( );*/ -} - -void SX1280Hal::WriteRegister(uint16_t address, uint8_t* buffer, uint16_t size) { - /*WaitOnBusy( );*/ - /**/ - /*if( RadioSpi != NULL )*/ - /*{*/ - /* RadioNss = 0;*/ - /* RadioSpi->write( RADIO_WRITE_REGISTER );*/ - /* RadioSpi->write( ( address & 0xFF00 ) >> 8 );*/ - /* RadioSpi->write( address & 0x00FF );*/ - /* for( uint16_t i = 0; i < size; i++ )*/ - /* {*/ - /* RadioSpi->write( buffer[i] );*/ - /* }*/ - /* RadioNss = 1;*/ - /*}*/ - /*if( RadioUart != NULL )*/ - /*{*/ - /* uint16_t addr = address;*/ - /* uint16_t i = 0;*/ - /* for( addr = address; ( addr + 255 ) < ( address + size ); )*/ - /* {*/ - /* RadioUart->putc( RADIO_WRITE_REGISTER );*/ - /* RadioUart->putc( ( addr & 0xFF00 ) >> 8 );*/ - /* RadioUart->putc( addr & 0x00FF );*/ - /* RadioUart->putc( 255 );*/ - /* for( uint16_t lastAddr = addr + 255 ; addr < lastAddr; i++, addr++ )*/ - /* {*/ - /* RadioUart->putc( buffer[i] );*/ - /* }*/ - /* }*/ - /* RadioUart->putc( RADIO_WRITE_REGISTER );*/ - /* RadioUart->putc( ( addr & 0xFF00 ) >> 8 );*/ - /* RadioUart->putc( addr & 0x00FF );*/ - /* RadioUart->putc( address + size - addr );*/ - /**/ - /* for( ; addr < ( address + size ); addr++, i++ )*/ - /* {*/ - /* RadioUart->putc( buffer[i] );*/ - /* }*/ - /*}*/ - /**/ - /*WaitOnBusy( );*/ -} - -void SX1280Hal::WriteRegister(uint16_t address, uint8_t value) { - /*WriteRegister( address, &value, 1 );*/ -} - -void SX1280Hal::ReadRegister(uint16_t address, uint8_t* buffer, uint16_t size) { - /*WaitOnBusy( );*/ - /**/ - /*if( RadioSpi != NULL )*/ - /*{*/ - /* RadioNss = 0;*/ - /* RadioSpi->write( RADIO_READ_REGISTER );*/ - /* RadioSpi->write( ( address & 0xFF00 ) >> 8 );*/ - /* RadioSpi->write( address & 0x00FF );*/ - /* RadioSpi->write( 0 );*/ - /* for( uint16_t i = 0; i < size; i++ )*/ - /* {*/ - /* buffer[i] = RadioSpi->write( 0 );*/ - /* }*/ - /* RadioNss = 1;*/ - /*}*/ - /*if( RadioUart != NULL )*/ - /*{*/ - /* uint16_t addr = address;*/ - /* uint16_t i = 0;*/ - /* for( addr = address; ( addr + 255 ) < ( address + size ); )*/ - /* {*/ - /* RadioUart->putc( RADIO_READ_REGISTER );*/ - /* RadioUart->putc( ( addr & 0xFF00 ) >> 8 );*/ - /* RadioUart->putc( addr & 0x00FF );*/ - /* RadioUart->putc( 255 );*/ - /* WaitUartReadable( );*/ - /* for( uint16_t lastAddr = addr + 255 ; addr < lastAddr; i++, addr++ )*/ - /* {*/ - /* buffer[i] = RadioUart->getc( );*/ - /* }*/ - /* }*/ - /* RadioUart->putc( RADIO_READ_REGISTER );*/ - /* RadioUart->putc( ( addr & 0xFF00 ) >> 8 );*/ - /* RadioUart->putc( addr & 0x00FF );*/ - /* RadioUart->putc( address + size - addr );*/ - /* WaitUartReadable( );*/ - /* for( ; addr < ( address + size ); addr++, i++ )*/ - /* {*/ - /* buffer[i] = RadioUart->getc( );*/ - /* }*/ - /*}*/ - /**/ - /*WaitOnBusy( );*/ -} - -uint8_t SX1280Hal::ReadRegister(uint16_t address) { - /*uint8_t data;*/ - /**/ - /*ReadRegister( address, &data, 1 );*/ - /*return data;*/ -} - -void SX1280Hal::WriteBuffer(uint8_t offset, uint8_t* buffer, uint8_t size) { - /*WaitOnBusy( );*/ - /**/ - /*if( RadioSpi != NULL )*/ - /*{*/ - /* RadioNss = 0;*/ - /* RadioSpi->write( RADIO_WRITE_BUFFER );*/ - /* RadioSpi->write( offset );*/ - /* for( uint16_t i = 0; i < size; i++ )*/ - /* {*/ - /* RadioSpi->write( buffer[i] );*/ - /* }*/ - /* RadioNss = 1;*/ - /*}*/ - /*if( RadioUart != NULL )*/ - /*{*/ - /* RadioUart->putc( RADIO_WRITE_BUFFER );*/ - /* RadioUart->putc( offset );*/ - /* RadioUart->putc( size );*/ - /* for( uint16_t i = 0; i < size; i++ )*/ - /* {*/ - /* RadioUart->putc( buffer[i] );*/ - /* }*/ - /*}*/ - /**/ - /*WaitOnBusy( );*/ -} - -void SX1280Hal::ReadBuffer(uint8_t offset, uint8_t* buffer, uint8_t size) { - /*WaitOnBusy( );*/ - /**/ - /*if( RadioSpi != NULL )*/ - /*{*/ - /* RadioNss = 0;*/ - /* RadioSpi->write( RADIO_READ_BUFFER );*/ - /* RadioSpi->write( offset );*/ - /* RadioSpi->write( 0 );*/ - /* for( uint16_t i = 0; i < size; i++ )*/ - /* {*/ - /* buffer[i] = RadioSpi->write( 0 );*/ - /* }*/ - /* RadioNss = 1;*/ - /*}*/ - /*if( RadioUart != NULL )*/ - /*{*/ - /* RadioUart->putc( RADIO_READ_BUFFER );*/ - /* RadioUart->putc( offset );*/ - /* RadioUart->putc( size );*/ - /* WaitUartReadable( );*/ - /* for( uint16_t i = 0; i < size; i++ )*/ - /* {*/ - /* buffer[i] = RadioUart->getc( );*/ - /* }*/ - /*}*/ - /**/ - /*WaitOnBusy( );*/ -} - -uint8_t SX1280Hal::GetDioStatus(void) { - /*return ( *DIO3 << 3 ) | ( *DIO2 << 2 ) | ( *DIO1 << 1 ) | ( BUSY << 0 );*/ -} diff --git a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h b/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h deleted file mode 100644 index 95e3d6a2..00000000 --- a/FprimeZephyrReference/Components/MyComponent/sx1280-hal.h +++ /dev/null @@ -1,192 +0,0 @@ -/* - ______ _ - / _____) _ | | -( (____ _____ ____ _| |_ _____ ____| |__ - \____ \| ___ | (_ _) ___ |/ ___) _ \ - _____) ) ____| | | || |_| ____( (___| | | | -(______/|_____)_|_|_| \__)_____)\____)_| |_| - (C)2015 Semtech - -Description: Handling of the node configuration protocol - -License: Revised BSD License, see LICENSE.TXT file include in the project - -Maintainer: Miguel Luis and Gregory Cristian -*/ -#ifndef __SX1280_HAL_H__ -#define __SX1280_HAL_H__ - -#include "sx1280.h" - -/*! - * \brief Actual implementation of a SX1280 radio - */ -class SX1280Hal : public SX1280 { - public: - /*! - * \brief Constructor for SX1280Hal with SPI support - * - * Represents the physical connectivity with the radio and set callback functions on radio interrupts - */ - /*SX1280Hal(PinName mosi,*/ - /* PinName miso,*/ - /* PinName sclk,*/ - /* PinName nss,*/ - /* PinName busy,*/ - /* PinName dio1,*/ - /* PinName dio2,*/ - /* PinName dio3,*/ - /* PinName rst,*/ - /* RadioCallbacks_t* callbacks);*/ - - /*! - * \brief Constructor for SX1280Hal with UART support - * - * Represents the physical connectivity with the radio and set callback functions on radio interrupts - */ - /*SX1280Hal(PinName tx,*/ - /* PinName rx,*/ - /* PinName ctsn,*/ - /* PinName busy,*/ - /* PinName dio1,*/ - /* PinName dio2,*/ - /* PinName dio3,*/ - /* PinName rst,*/ - /* RadioCallbacks_t* callbacks);*/ - - /*! - * \brief Destructor for SX1280Hal with UART support - * - * Take care of the correct destruction of the communication objects - */ - virtual ~SX1280Hal(void); - - /*! - * \brief Soft resets the radio - */ - virtual void Reset(void); - - /*! - * \brief Wakes up the radio - */ - virtual void Wakeup(void); - - /*! - * \brief Set the SPI Speed - * - * \param [in] spiSpeed Speed of the SPI in Hz - */ - void SetSpiSpeed(uint32_t spiSpeed); - - /*! - * \brief Send a command that write data to the radio - * - * \param [in] opcode Opcode of the command - * \param [in] buffer Buffer to be send to the radio - * \param [in] size Size of the buffer to send - */ - virtual void WriteCommand(RadioCommands_t opcode, uint8_t* buffer, uint16_t size); - - /*! - * \brief Send a command that read data from the radio - * - * \param [in] opcode Opcode of the command - * \param [out] buffer Buffer holding data from the radio - * \param [in] size Size of the buffer - */ - virtual void ReadCommand(RadioCommands_t opcode, uint8_t* buffer, uint16_t size); - - /*! - * \brief Write data to the radio memory - * - * \param [in] address The address of the first byte to write in the radio - * \param [in] buffer The data to be written in radio's memory - * \param [in] size The number of bytes to write in radio's memory - */ - virtual void WriteRegister(uint16_t address, uint8_t* buffer, uint16_t size); - - /*! - * \brief Write a single byte of data to the radio memory - * - * \param [in] address The address of the first byte to write in the radio - * \param [in] value The data to be written in radio's memory - */ - virtual void WriteRegister(uint16_t address, uint8_t value); - - /*! - * \brief Read data from the radio memory - * - * \param [in] address The address of the first byte to read from the radio - * \param [out] buffer The buffer that holds data read from radio - * \param [in] size The number of bytes to read from radio's memory - */ - virtual void ReadRegister(uint16_t address, uint8_t* buffer, uint16_t size); - - /*! - * \brief Read a single byte of data from the radio memory - * - * \param [in] address The address of the first byte to write in the - * radio - * - * \retval value The value of the byte at the given address in - * radio's memory - */ - virtual uint8_t ReadRegister(uint16_t address); - - /*! - * \brief Write data to the buffer holding the payload in the radio - * - * \param [in] offset The offset to start writing the payload - * \param [in] buffer The data to be written (the payload) - * \param [in] size The number of byte to be written - */ - virtual void WriteBuffer(uint8_t offset, uint8_t* buffer, uint8_t size); - - /*! - * \brief Read data from the buffer holding the payload in the radio - * - * \param [in] offset The offset to start reading the payload - * \param [out] buffer A pointer to a buffer holding the data from the radio - * \param [in] size The number of byte to be read - */ - virtual void ReadBuffer(uint8_t offset, uint8_t* buffer, uint8_t size); - - /*! - * \brief Returns the status of DIOs pins - * - * \retval dioStatus A byte where each bit represents a DIO state: - * [ DIO3 | DIO2 | DIO1 | BUSY ] - */ - virtual uint8_t GetDioStatus(void); - - protected: - // SPI* RadioSpi; //!< The SPI object used to communicate with the radio - // Serial* RadioUart; //!< The UART object used to communicate with the radio - // DigitalOut RadioNss; //!< The pin connected to Radio chip select (active low) - // DigitalInOut RadioReset; //!< The reset pin connected to the radio - // DigitalOut RadioCtsn; //!< The Clear To Send radio pin (active low) - /**/ - // DigitalIn BUSY; //!< The pin connected to BUSY - // Interrupt* DIO1; //!< The pin connected to DIO1 - // Interrupt* DIO2; //!< The pin connected to DIO2 - // Interrupt* DIO3; //!< The pin connected to DIO3 - - /*! - * \brief Initializes SPI object used to communicate with the radio - */ - virtual void SpiInit(void); - - /*! - * \brief Initializes UART object used to communicate with the radio - */ - virtual void UartInit(void); - - /*! - * \brief Sets the callback functions to be run on DIO1..3 interrupt - * - * \param [in] irqHandler A function pointer of the function to be run on every DIO interrupt - */ - virtual void IoIrqInit(DioIrqHandler irqHandler); -}; - -#endif // __SX1280_HAL_H__ diff --git a/FprimeZephyrReference/Components/MyComponent/sx1280.cpp b/FprimeZephyrReference/Components/MyComponent/sx1280.cpp deleted file mode 100644 index 654dfcb3..00000000 --- a/FprimeZephyrReference/Components/MyComponent/sx1280.cpp +++ /dev/null @@ -1,1063 +0,0 @@ -/* - ______ _ - / _____) _ | | -( (____ _____ ____ _| |_ _____ ____| |__ - \____ \| ___ | (_ _) ___ |/ ___) _ \ - _____) ) ____| | | || |_| ____( (___| | | | -(______/|_____)_|_|_| \__)_____)\____)_| |_| - (C)2016 Semtech - -Description: Driver for SX1280 devices - -License: Revised BSD License, see LICENSE.TXT file include in the project - -Maintainer: Miguel Luis, Gregory Cristian and Matthieu Verdy -*/ -/*#include "mbed.h"*/ -#include "sx1280.h" - -#include -#include - -#include "sx1280-hal.h" - -/*! - * \brief Radio registers definition - * - */ -typedef struct { - uint16_t Addr; //!< The address of the register - uint8_t Value; //!< The value of the register -} RadioRegisters_t; - -/*! - * \brief Radio hardware registers initialization definition - */ -#define RADIO_INIT_REGISTERS_VALUE \ - { \ - } - -/*! - * \brief Radio hardware registers initialization - */ -const RadioRegisters_t RadioRegsInit[] = RADIO_INIT_REGISTERS_VALUE; - -void SX1280::Init(void) { - Reset(); - IoIrqInit(dioIrq); - Wakeup(); - SetRegistersDefault(); -} - -void SX1280::SetRegistersDefault(void) { - for (int16_t i = 0; i < sizeof(RadioRegsInit) / sizeof(RadioRegisters_t); i++) { - WriteRegister(RadioRegsInit[i].Addr, RadioRegsInit[i].Value); - } -} - -uint16_t SX1280::GetFirmwareVersion(void) { - return (((ReadRegister(REG_LR_FIRMWARE_VERSION_MSB)) << 8) | (ReadRegister(REG_LR_FIRMWARE_VERSION_MSB + 1))); -} - -RadioStatus_t SX1280::GetStatus(void) { - uint8_t stat = 0; - RadioStatus_t status; - - ReadCommand(RADIO_GET_STATUS, (uint8_t*)&stat, 1); - status.Value = stat; - return (status); -} - -RadioOperatingModes_t SX1280::GetOpMode(void) { - return (OperatingMode); -} - -void SX1280::SetSleep(SleepParams_t sleepConfig) { - uint8_t sleep = (sleepConfig.WakeUpRTC << 3) | (sleepConfig.InstructionRamRetention << 2) | - (sleepConfig.DataBufferRetention << 1) | (sleepConfig.DataRamRetention); - - OperatingMode = MODE_SLEEP; - WriteCommand(RADIO_SET_SLEEP, &sleep, 1); -} - -void SX1280::SetStandby(RadioStandbyModes_t standbyConfig) { - WriteCommand(RADIO_SET_STANDBY, (uint8_t*)&standbyConfig, 1); - if (standbyConfig == STDBY_RC) { - OperatingMode = MODE_STDBY_RC; - } else { - OperatingMode = MODE_STDBY_XOSC; - } -} - -void SX1280::SetFs(void) { - WriteCommand(RADIO_SET_FS, 0, 0); - OperatingMode = MODE_FS; -} - -void SX1280::SetTx(TickTime_t timeout) { - uint8_t buf[3]; - buf[0] = timeout.PeriodBase; - buf[1] = (uint8_t)((timeout.PeriodBaseCount >> 8) & 0x00FF); - buf[2] = (uint8_t)(timeout.PeriodBaseCount & 0x00FF); - - ClearIrqStatus(IRQ_RADIO_ALL); - - // If the radio is doing ranging operations, then apply the specific calls - // prior to SetTx - if (GetPacketType(true) == PACKET_TYPE_RANGING) { - SetRangingRole(RADIO_RANGING_ROLE_MASTER); - } - WriteCommand(RADIO_SET_TX, buf, 3); - OperatingMode = MODE_TX; -} - -void SX1280::SetRx(TickTime_t timeout) { - uint8_t buf[3]; - buf[0] = timeout.PeriodBase; - buf[1] = (uint8_t)((timeout.PeriodBaseCount >> 8) & 0x00FF); - buf[2] = (uint8_t)(timeout.PeriodBaseCount & 0x00FF); - - ClearIrqStatus(IRQ_RADIO_ALL); - - // If the radio is doing ranging operations, then apply the specific calls - // prior to SetRx - if (GetPacketType(true) == PACKET_TYPE_RANGING) { - SetRangingRole(RADIO_RANGING_ROLE_SLAVE); - } - WriteCommand(RADIO_SET_RX, buf, 3); - OperatingMode = MODE_RX; -} - -void SX1280::SetRxDutyCycle(RadioTickSizes_t periodBase, uint16_t periodBaseCountRx, uint16_t periodBaseCountSleep) { - uint8_t buf[5]; - - buf[0] = periodBase; - buf[1] = (uint8_t)((periodBaseCountRx >> 8) & 0x00FF); - buf[2] = (uint8_t)(periodBaseCountRx & 0x00FF); - buf[3] = (uint8_t)((periodBaseCountSleep >> 8) & 0x00FF); - buf[4] = (uint8_t)(periodBaseCountSleep & 0x00FF); - WriteCommand(RADIO_SET_RXDUTYCYCLE, buf, 5); - OperatingMode = MODE_RX; -} - -void SX1280::SetCad(void) { - WriteCommand(RADIO_SET_CAD, 0, 0); - OperatingMode = MODE_CAD; -} - -void SX1280::SetTxContinuousWave(void) { - WriteCommand(RADIO_SET_TXCONTINUOUSWAVE, 0, 0); -} - -void SX1280::SetTxContinuousPreamble(void) { - WriteCommand(RADIO_SET_TXCONTINUOUSPREAMBLE, 0, 0); -} - -void SX1280::SetPacketType(RadioPacketTypes_t packetType) { - // Save packet type internally to avoid questioning the radio - this->PacketType = packetType; - - WriteCommand(RADIO_SET_PACKETTYPE, (uint8_t*)&packetType, 1); -} - -RadioPacketTypes_t SX1280::GetPacketType(bool returnLocalCopy) { - RadioPacketTypes_t packetType = PACKET_TYPE_NONE; - if (returnLocalCopy == false) { - ReadCommand(RADIO_GET_PACKETTYPE, (uint8_t*)&packetType, 1); - if (this->PacketType != packetType) { - this->PacketType = packetType; - } - } else { - packetType = this->PacketType; - } - return packetType; -} - -void SX1280::SetRfFrequency(uint32_t rfFrequency) { - uint8_t buf[3]; - uint32_t freq = 0; - - freq = (uint32_t)((double)rfFrequency / (double)FREQ_STEP); - buf[0] = (uint8_t)((freq >> 16) & 0xFF); - buf[1] = (uint8_t)((freq >> 8) & 0xFF); - buf[2] = (uint8_t)(freq & 0xFF); - WriteCommand(RADIO_SET_RFFREQUENCY, buf, 3); -} - -void SX1280::SetTxParams(int8_t power, RadioRampTimes_t rampTime) { - uint8_t buf[2]; - - // The power value to send on SPI/UART is in the range [0..31] and the - // physical output power is in the range [-18..13]dBm - buf[0] = power + 18; - buf[1] = (uint8_t)rampTime; - WriteCommand(RADIO_SET_TXPARAMS, buf, 2); -} - -void SX1280::SetCadParams(RadioLoRaCadSymbols_t cadSymbolNum) { - WriteCommand(RADIO_SET_CADPARAMS, (uint8_t*)&cadSymbolNum, 1); - OperatingMode = MODE_CAD; -} - -void SX1280::SetBufferBaseAddresses(uint8_t txBaseAddress, uint8_t rxBaseAddress) { - uint8_t buf[2]; - - buf[0] = txBaseAddress; - buf[1] = rxBaseAddress; - WriteCommand(RADIO_SET_BUFFERBASEADDRESS, buf, 2); -} - -void SX1280::SetModulationParams(ModulationParams_t* modParams) { - uint8_t buf[3]; - - // Check if required configuration corresponds to the stored packet type - // If not, silently update radio packet type - if (this->PacketType != modParams->PacketType) { - this->SetPacketType(modParams->PacketType); - } - - switch (modParams->PacketType) { - case PACKET_TYPE_GFSK: - buf[0] = modParams->Params.Gfsk.BitrateBandwidth; - buf[1] = modParams->Params.Gfsk.ModulationIndex; - buf[2] = modParams->Params.Gfsk.ModulationShaping; - break; - case PACKET_TYPE_LORA: - case PACKET_TYPE_RANGING: - buf[0] = modParams->Params.LoRa.SpreadingFactor; - buf[1] = modParams->Params.LoRa.Bandwidth; - buf[2] = modParams->Params.LoRa.CodingRate; - this->LoRaBandwidth = modParams->Params.LoRa.Bandwidth; - break; - case PACKET_TYPE_FLRC: - buf[0] = modParams->Params.Flrc.BitrateBandwidth; - buf[1] = modParams->Params.Flrc.CodingRate; - buf[2] = modParams->Params.Flrc.ModulationShaping; - break; - case PACKET_TYPE_BLE: - buf[0] = modParams->Params.Ble.BitrateBandwidth; - buf[1] = modParams->Params.Ble.ModulationIndex; - buf[2] = modParams->Params.Ble.ModulationShaping; - break; - case PACKET_TYPE_NONE: - buf[0] = NULL; - buf[1] = NULL; - buf[2] = NULL; - break; - } - WriteCommand(RADIO_SET_MODULATIONPARAMS, buf, 3); -} - -void SX1280::SetPacketParams(PacketParams_t* packetParams) { - uint8_t buf[7]; - // Check if required configuration corresponds to the stored packet type - // If not, silently update radio packet type - if (this->PacketType != packetParams->PacketType) { - this->SetPacketType(packetParams->PacketType); - } - - switch (packetParams->PacketType) { - case PACKET_TYPE_GFSK: - buf[0] = packetParams->Params.Gfsk.PreambleLength; - buf[1] = packetParams->Params.Gfsk.SyncWordLength; - buf[2] = packetParams->Params.Gfsk.SyncWordMatch; - buf[3] = packetParams->Params.Gfsk.HeaderType; - buf[4] = packetParams->Params.Gfsk.PayloadLength; - buf[5] = packetParams->Params.Gfsk.CrcLength; - buf[6] = packetParams->Params.Gfsk.Whitening; - break; - case PACKET_TYPE_LORA: - case PACKET_TYPE_RANGING: - buf[0] = packetParams->Params.LoRa.PreambleLength; - buf[1] = packetParams->Params.LoRa.HeaderType; - buf[2] = packetParams->Params.LoRa.PayloadLength; - buf[3] = packetParams->Params.LoRa.Crc; - buf[4] = packetParams->Params.LoRa.InvertIQ; - buf[5] = NULL; - buf[6] = NULL; - break; - case PACKET_TYPE_FLRC: - buf[0] = packetParams->Params.Flrc.PreambleLength; - buf[1] = packetParams->Params.Flrc.SyncWordLength; - buf[2] = packetParams->Params.Flrc.SyncWordMatch; - buf[3] = packetParams->Params.Flrc.HeaderType; - buf[4] = packetParams->Params.Flrc.PayloadLength; - buf[5] = packetParams->Params.Flrc.CrcLength; - buf[6] = packetParams->Params.Flrc.Whitening; - break; - case PACKET_TYPE_BLE: - buf[0] = packetParams->Params.Ble.ConnectionState; - buf[1] = packetParams->Params.Ble.CrcLength; - buf[2] = packetParams->Params.Ble.BleTestPayload; - buf[3] = packetParams->Params.Ble.Whitening; - buf[4] = NULL; - buf[5] = NULL; - buf[6] = NULL; - break; - case PACKET_TYPE_NONE: - buf[0] = NULL; - buf[1] = NULL; - buf[2] = NULL; - buf[3] = NULL; - buf[4] = NULL; - buf[5] = NULL; - buf[6] = NULL; - break; - } - WriteCommand(RADIO_SET_PACKETPARAMS, buf, 7); -} - -void SX1280::ForcePreambleLength(RadioPreambleLengths_t preambleLength) { - this->WriteRegister(REG_LR_PREAMBLELENGTH, - (this->ReadRegister(REG_LR_PREAMBLELENGTH) & MASK_FORCE_PREAMBLELENGTH) | preambleLength); -} - -void SX1280::GetRxBufferStatus(uint8_t* rxPayloadLength, uint8_t* rxStartBufferPointer) { - uint8_t status[2]; - - ReadCommand(RADIO_GET_RXBUFFERSTATUS, status, 2); - - // In case of LORA fixed header, the rxPayloadLength is obtained by reading - // the register REG_LR_PAYLOADLENGTH - if ((this->GetPacketType(true) == PACKET_TYPE_LORA) && (ReadRegister(REG_LR_PACKETPARAMS) >> 7 == 1)) { - *rxPayloadLength = ReadRegister(REG_LR_PAYLOADLENGTH); - } else if (this->GetPacketType(true) == PACKET_TYPE_BLE) { - // In the case of BLE, the size returned in status[0] do not include the 2-byte length PDU header - // so it is added there - *rxPayloadLength = status[0] + 2; - } else { - *rxPayloadLength = status[0]; - } - - *rxStartBufferPointer = status[1]; -} - -void SX1280::GetPacketStatus(PacketStatus_t* packetStatus) { - uint8_t status[5]; - - ReadCommand(RADIO_GET_PACKETSTATUS, status, 5); - - packetStatus->packetType = this->GetPacketType(true); - switch (packetStatus->packetType) { - case PACKET_TYPE_GFSK: - packetStatus->Gfsk.RssiSync = -(status[1] / 2); - - packetStatus->Gfsk.ErrorStatus.SyncError = (status[2] >> 6) & 0x01; - packetStatus->Gfsk.ErrorStatus.LengthError = (status[2] >> 5) & 0x01; - packetStatus->Gfsk.ErrorStatus.CrcError = (status[2] >> 4) & 0x01; - packetStatus->Gfsk.ErrorStatus.AbortError = (status[2] >> 3) & 0x01; - packetStatus->Gfsk.ErrorStatus.HeaderReceived = (status[2] >> 2) & 0x01; - packetStatus->Gfsk.ErrorStatus.PacketReceived = (status[2] >> 1) & 0x01; - packetStatus->Gfsk.ErrorStatus.PacketControlerBusy = status[2] & 0x01; - - packetStatus->Gfsk.TxRxStatus.RxNoAck = (status[3] >> 5) & 0x01; - packetStatus->Gfsk.TxRxStatus.PacketSent = status[3] & 0x01; - - packetStatus->Gfsk.SyncAddrStatus = status[4] & 0x07; - break; - - case PACKET_TYPE_LORA: - case PACKET_TYPE_RANGING: - packetStatus->LoRa.RssiPkt = -(status[0] / 2); - (status[1] < 128) ? (packetStatus->LoRa.SnrPkt = status[1] / 4) - : (packetStatus->LoRa.SnrPkt = ((status[1] - 256) / 4)); - break; - - case PACKET_TYPE_FLRC: - packetStatus->Flrc.RssiSync = -(status[1] / 2); - - packetStatus->Flrc.ErrorStatus.SyncError = (status[2] >> 6) & 0x01; - packetStatus->Flrc.ErrorStatus.LengthError = (status[2] >> 5) & 0x01; - packetStatus->Flrc.ErrorStatus.CrcError = (status[2] >> 4) & 0x01; - packetStatus->Flrc.ErrorStatus.AbortError = (status[2] >> 3) & 0x01; - packetStatus->Flrc.ErrorStatus.HeaderReceived = (status[2] >> 2) & 0x01; - packetStatus->Flrc.ErrorStatus.PacketReceived = (status[2] >> 1) & 0x01; - packetStatus->Flrc.ErrorStatus.PacketControlerBusy = status[2] & 0x01; - - packetStatus->Flrc.TxRxStatus.RxPid = (status[3] >> 6) & 0x03; - packetStatus->Flrc.TxRxStatus.RxNoAck = (status[3] >> 5) & 0x01; - packetStatus->Flrc.TxRxStatus.RxPidErr = (status[3] >> 4) & 0x01; - packetStatus->Flrc.TxRxStatus.PacketSent = status[3] & 0x01; - - packetStatus->Flrc.SyncAddrStatus = status[4] & 0x07; - break; - - case PACKET_TYPE_BLE: - packetStatus->Ble.RssiSync = -(status[1] / 2); - - packetStatus->Ble.ErrorStatus.SyncError = (status[2] >> 6) & 0x01; - packetStatus->Ble.ErrorStatus.LengthError = (status[2] >> 5) & 0x01; - packetStatus->Ble.ErrorStatus.CrcError = (status[2] >> 4) & 0x01; - packetStatus->Ble.ErrorStatus.AbortError = (status[2] >> 3) & 0x01; - packetStatus->Ble.ErrorStatus.HeaderReceived = (status[2] >> 2) & 0x01; - packetStatus->Ble.ErrorStatus.PacketReceived = (status[2] >> 1) & 0x01; - packetStatus->Ble.ErrorStatus.PacketControlerBusy = status[2] & 0x01; - - packetStatus->Ble.TxRxStatus.PacketSent = status[3] & 0x01; - - packetStatus->Ble.SyncAddrStatus = status[4] & 0x07; - break; - - case PACKET_TYPE_NONE: - // In that specific case, we set everything in the packetStatus to zeros - // and reset the packet type accordingly - memset(packetStatus, 0, sizeof(PacketStatus_t)); - packetStatus->packetType = PACKET_TYPE_NONE; - break; - } -} - -int8_t SX1280::GetRssiInst(void) { - uint8_t raw = 0; - - ReadCommand(RADIO_GET_RSSIINST, &raw, 1); - - return (int8_t)(-raw / 2); -} - -void SX1280::SetDioIrqParams(uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask) { - uint8_t buf[8]; - - buf[0] = (uint8_t)((irqMask >> 8) & 0x00FF); - buf[1] = (uint8_t)(irqMask & 0x00FF); - buf[2] = (uint8_t)((dio1Mask >> 8) & 0x00FF); - buf[3] = (uint8_t)(dio1Mask & 0x00FF); - buf[4] = (uint8_t)((dio2Mask >> 8) & 0x00FF); - buf[5] = (uint8_t)(dio2Mask & 0x00FF); - buf[6] = (uint8_t)((dio3Mask >> 8) & 0x00FF); - buf[7] = (uint8_t)(dio3Mask & 0x00FF); - WriteCommand(RADIO_SET_DIOIRQPARAMS, buf, 8); -} - -uint16_t SX1280::GetIrqStatus(void) { - uint8_t irqStatus[2]; - ReadCommand(RADIO_GET_IRQSTATUS, irqStatus, 2); - return (irqStatus[0] << 8) | irqStatus[1]; -} - -void SX1280::ClearIrqStatus(uint16_t irqMask) { - uint8_t buf[2]; - - buf[0] = (uint8_t)(((uint16_t)irqMask >> 8) & 0x00FF); - buf[1] = (uint8_t)((uint16_t)irqMask & 0x00FF); - WriteCommand(RADIO_CLR_IRQSTATUS, buf, 2); -} - -void SX1280::Calibrate(CalibrationParams_t calibParam) { - uint8_t cal = (calibParam.ADCBulkPEnable << 5) | (calibParam.ADCBulkNEnable << 4) | - (calibParam.ADCPulseEnable << 3) | (calibParam.PLLEnable << 2) | (calibParam.RC13MEnable << 1) | - (calibParam.RC64KEnable); - WriteCommand(RADIO_CALIBRATE, &cal, 1); -} - -void SX1280::SetRegulatorMode(RadioRegulatorModes_t mode) { - WriteCommand(RADIO_SET_REGULATORMODE, (uint8_t*)&mode, 1); -} - -void SX1280::SetSaveContext(void) { - WriteCommand(RADIO_SET_SAVECONTEXT, 0, 0); -} - -void SX1280::SetAutoTx(uint16_t time) { - uint16_t compensatedTime = time - (uint16_t)AUTO_TX_OFFSET; - uint8_t buf[2]; - - buf[0] = (uint8_t)((compensatedTime >> 8) & 0x00FF); - buf[1] = (uint8_t)(compensatedTime & 0x00FF); - WriteCommand(RADIO_SET_AUTOTX, buf, 2); -} - -void SX1280::StopAutoTx(void) { - uint8_t buf[2] = {0x00, 0x00}; - WriteCommand(RADIO_SET_AUTOTX, buf, 2); -} - -void SX1280::SetAutoFs(bool enableAutoFs) { - WriteCommand(RADIO_SET_AUTOFS, (uint8_t*)&enableAutoFs, 1); -} - -void SX1280::SetLongPreamble(bool enable) { - WriteCommand(RADIO_SET_LONGPREAMBLE, (uint8_t*)&enable, 1); -} - -void SX1280::SetPayload(uint8_t* buffer, uint8_t size, uint8_t offset) { - WriteBuffer(offset, buffer, size); -} - -uint8_t SX1280::GetPayload(uint8_t* buffer, uint8_t* size, uint8_t maxSize) { - uint8_t offset; - - GetRxBufferStatus(size, &offset); - if (*size > maxSize) { - return 1; - } - ReadBuffer(offset, buffer, *size); - return 0; -} - -void SX1280::SendPayload(uint8_t* payload, uint8_t size, TickTime_t timeout, uint8_t offset) { - SetPayload(payload, size, offset); - SetTx(timeout); -} - -uint8_t SX1280::SetSyncWord(uint8_t syncWordIdx, uint8_t* syncWord) { - uint16_t addr; - uint8_t syncwordSize = 0; - - switch (GetPacketType(true)) { - case PACKET_TYPE_GFSK: - syncwordSize = 5; - switch (syncWordIdx) { - case 1: - addr = REG_LR_SYNCWORDBASEADDRESS1; - break; - case 2: - addr = REG_LR_SYNCWORDBASEADDRESS2; - break; - case 3: - addr = REG_LR_SYNCWORDBASEADDRESS3; - break; - default: - return 1; - } - break; - case PACKET_TYPE_FLRC: - // For FLRC packet type, the SyncWord is one byte shorter and - // the base address is shifted by one byte - syncwordSize = 4; - switch (syncWordIdx) { - case 1: - addr = REG_LR_SYNCWORDBASEADDRESS1 + 1; - break; - case 2: - addr = REG_LR_SYNCWORDBASEADDRESS2 + 1; - break; - case 3: - addr = REG_LR_SYNCWORDBASEADDRESS3 + 1; - break; - default: - return 1; - } - break; - case PACKET_TYPE_BLE: - // For Ble packet type, only the first SyncWord is used and its - // address is shifted by one byte - syncwordSize = 4; - switch (syncWordIdx) { - case 1: - addr = REG_LR_SYNCWORDBASEADDRESS1 + 1; - break; - default: - return 1; - } - break; - default: - return 1; - } - WriteRegister(addr, syncWord, syncwordSize); - return 0; -} - -void SX1280::SetSyncWordErrorTolerance(uint8_t ErrorBits) { - ErrorBits = (ReadRegister(REG_LR_SYNCWORDTOLERANCE) & 0xF0) | (ErrorBits & 0x0F); - WriteRegister(REG_LR_SYNCWORDTOLERANCE, ErrorBits); -} - -uint8_t SX1280::SetCrcSeed(uint8_t* seed) { - uint8_t updated = 0; - switch (GetPacketType(true)) { - case PACKET_TYPE_GFSK: - case PACKET_TYPE_FLRC: - WriteRegister(REG_LR_CRCSEEDBASEADDR, seed, 2); - updated = 1; - break; - case PACKET_TYPE_BLE: - this->WriteRegister(0x9c7, seed[2]); - this->WriteRegister(0x9c8, seed[1]); - this->WriteRegister(0x9c9, seed[0]); - updated = 1; - break; - default: - break; - } - return updated; -} - -void SX1280::SetBleAccessAddress(uint32_t accessAddress) { - this->WriteRegister(REG_LR_BLE_ACCESS_ADDRESS, (accessAddress >> 24) & 0x000000FF); - this->WriteRegister(REG_LR_BLE_ACCESS_ADDRESS + 1, (accessAddress >> 16) & 0x000000FF); - this->WriteRegister(REG_LR_BLE_ACCESS_ADDRESS + 2, (accessAddress >> 8) & 0x000000FF); - this->WriteRegister(REG_LR_BLE_ACCESS_ADDRESS + 3, accessAddress & 0x000000FF); -} - -void SX1280::SetBleAdvertizerAccessAddress(void) { - this->SetBleAccessAddress(BLE_ADVERTIZER_ACCESS_ADDRESS); -} - -void SX1280::SetCrcPolynomial(uint16_t polynomial) { - uint8_t val[2]; - - val[0] = (uint8_t)(polynomial >> 8) & 0xFF; - val[1] = (uint8_t)(polynomial & 0xFF); - - switch (GetPacketType(true)) { - case PACKET_TYPE_GFSK: - case PACKET_TYPE_FLRC: - WriteRegister(REG_LR_CRCPOLYBASEADDR, val, 2); - break; - default: - break; - } -} - -void SX1280::SetWhiteningSeed(uint8_t seed) { - switch (GetPacketType(true)) { - case PACKET_TYPE_GFSK: - case PACKET_TYPE_FLRC: - case PACKET_TYPE_BLE: - WriteRegister(REG_LR_WHITSEEDBASEADDR, seed); - break; - default: - break; - } -} - -void SX1280::EnableManualGain(void) { - this->WriteRegister(REG_ENABLE_MANUAL_GAIN_CONTROL, - this->ReadRegister(REG_ENABLE_MANUAL_GAIN_CONTROL) | MASK_MANUAL_GAIN_CONTROL); - this->WriteRegister(REG_DEMOD_DETECTION, this->ReadRegister(REG_DEMOD_DETECTION) & MASK_DEMOD_DETECTION); -} - -void SX1280::DisableManualGain(void) { - this->WriteRegister(REG_ENABLE_MANUAL_GAIN_CONTROL, - this->ReadRegister(REG_ENABLE_MANUAL_GAIN_CONTROL) & (~MASK_MANUAL_GAIN_CONTROL)); - this->WriteRegister(REG_DEMOD_DETECTION, this->ReadRegister(REG_DEMOD_DETECTION) | (~MASK_DEMOD_DETECTION)); -} - -void SX1280::SetManualGainValue(uint8_t gain) { - this->WriteRegister(REG_MANUAL_GAIN_VALUE, - (this->ReadRegister(REG_MANUAL_GAIN_VALUE) & MASK_MANUAL_GAIN_VALUE) | gain); -} - -void SX1280::SetLNAGainSetting(const RadioLnaSettings_t lnaSetting) { - switch (lnaSetting) { - case LNA_HIGH_SENSITIVITY_MODE: { - this->WriteRegister(REG_LNA_REGIME, this->ReadRegister(REG_LNA_REGIME) | MASK_LNA_REGIME); - break; - } - case LNA_LOW_POWER_MODE: { - this->WriteRegister(REG_LNA_REGIME, this->ReadRegister(REG_LNA_REGIME) & ~MASK_LNA_REGIME); - break; - } - } -} - -void SX1280::SetRangingIdLength(RadioRangingIdCheckLengths_t length) { - switch (GetPacketType(true)) { - case PACKET_TYPE_RANGING: - WriteRegister(REG_LR_RANGINGIDCHECKLENGTH, - ((((uint8_t)length) & 0x03) << 6) | (ReadRegister(REG_LR_RANGINGIDCHECKLENGTH) & 0x3F)); - break; - default: - break; - } -} - -void SX1280::SetDeviceRangingAddress(uint32_t address) { - uint8_t addrArray[] = {address >> 24, address >> 16, address >> 8, address}; - - switch (GetPacketType(true)) { - case PACKET_TYPE_RANGING: - WriteRegister(REG_LR_DEVICERANGINGADDR, addrArray, 4); - break; - default: - break; - } -} - -void SX1280::SetRangingRequestAddress(uint32_t address) { - uint8_t addrArray[] = {address >> 24, address >> 16, address >> 8, address}; - - switch (GetPacketType(true)) { - case PACKET_TYPE_RANGING: - WriteRegister(REG_LR_REQUESTRANGINGADDR, addrArray, 4); - break; - default: - break; - } -} - -double SX1280::GetRangingResult(RadioRangingResultTypes_t resultType) { - uint32_t valLsb = 0; - double val = 0.0; - - switch (GetPacketType(true)) { - case PACKET_TYPE_RANGING: - this->SetStandby(STDBY_XOSC); - this->WriteRegister(0x97F, this->ReadRegister(0x97F) | (1 << 1)); // enable LORA modem clock - WriteRegister(REG_LR_RANGINGRESULTCONFIG, (ReadRegister(REG_LR_RANGINGRESULTCONFIG) & MASK_RANGINGMUXSEL) | - ((((uint8_t)resultType) & 0x03) << 4)); - valLsb = ((ReadRegister(REG_LR_RANGINGRESULTBASEADDR) << 16) | - (ReadRegister(REG_LR_RANGINGRESULTBASEADDR + 1) << 8) | - (ReadRegister(REG_LR_RANGINGRESULTBASEADDR + 2))); - this->SetStandby(STDBY_RC); - - // Conversion from LSB to distance. For explanation on the formula, refer to Datasheet of SX1280 - switch (resultType) { - case RANGING_RESULT_RAW: - // Convert the ranging LSB to distance in meter - // The theoretical conversion from register value to distance [m] is given by: - // distance [m] = ( complement2( register ) * 150 ) / ( 2^12 * bandwidth[MHz] ) ) - // The API provide BW in [Hz] so the implemented formula is complement2( register ) / bandwidth[Hz] - // * A, where A = 150 / (2^12 / 1e6) = 36621.09 - val = (double)complement2(valLsb, 24) / (double)this->GetLoRaBandwidth() * 36621.09375; - break; - - case RANGING_RESULT_AVERAGED: - case RANGING_RESULT_DEBIASED: - case RANGING_RESULT_FILTERED: - val = (double)valLsb * 20.0 / 100.0; - break; - default: - val = 0.0; - } - break; - default: - break; - } - return val; -} - -uint8_t SX1280::GetRangingPowerDeltaThresholdIndicator(void) { - SetStandby(STDBY_XOSC); - WriteRegister(0x97F, ReadRegister(0x97F) | (1 << 1)); // enable LoRa modem clock - WriteRegister(REG_LR_RANGINGRESULTCONFIG, (ReadRegister(REG_LR_RANGINGRESULTCONFIG) & MASK_RANGINGMUXSEL) | - ((((uint8_t)RANGING_RESULT_RAW) & 0x03) << 4)); // Select raw results - return ReadRegister(REG_RANGING_RSSI); -} - -void SX1280::SetRangingCalibration(uint16_t cal) { - switch (GetPacketType(true)) { - case PACKET_TYPE_RANGING: - WriteRegister(REG_LR_RANGINGRERXTXDELAYCAL, (uint8_t)((cal >> 8) & 0xFF)); - WriteRegister(REG_LR_RANGINGRERXTXDELAYCAL + 1, (uint8_t)((cal) & 0xFF)); - break; - default: - break; - } -} - -void SX1280::RangingClearFilterResult(void) { - uint8_t regVal = ReadRegister(REG_LR_RANGINGRESULTCLEARREG); - - // To clear result, set bit 5 to 1 then to 0 - WriteRegister(REG_LR_RANGINGRESULTCLEARREG, regVal | (1 << 5)); - WriteRegister(REG_LR_RANGINGRESULTCLEARREG, regVal & (~(1 << 5))); -} - -void SX1280::RangingSetFilterNumSamples(uint8_t num) { - // Silently set 8 as minimum value - WriteRegister(REG_LR_RANGINGFILTERWINDOWSIZE, - (num < DEFAULT_RANGING_FILTER_SIZE) ? DEFAULT_RANGING_FILTER_SIZE : num); -} - -void SX1280::SetRangingRole(RadioRangingRoles_t role) { - uint8_t buf[1]; - - buf[0] = role; - WriteCommand(RADIO_SET_RANGING_ROLE, &buf[0], 1); -} - -double SX1280::GetFrequencyError() { - uint8_t efeRaw[3] = {0}; - uint32_t efe = 0; - double efeHz = 0.0; - - switch (this->GetPacketType(true)) { - case PACKET_TYPE_LORA: - case PACKET_TYPE_RANGING: - efeRaw[0] = this->ReadRegister(REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB); - efeRaw[1] = this->ReadRegister(REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB + 1); - efeRaw[2] = this->ReadRegister(REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB + 2); - efe = (efeRaw[0] << 16) | (efeRaw[1] << 8) | efeRaw[2]; - efe &= REG_LR_ESTIMATED_FREQUENCY_ERROR_MASK; - - efeHz = 1.55 * (double)complement2(efe, 20) / (1600.0 / (double)this->GetLoRaBandwidth() * 1000.0); - break; - - case PACKET_TYPE_NONE: - case PACKET_TYPE_BLE: - case PACKET_TYPE_FLRC: - case PACKET_TYPE_GFSK: - break; - } - - return efeHz; -} - -void SX1280::SetPollingMode(void) { - this->PollingMode = true; -} - -int32_t SX1280::complement2(const uint32_t num, const uint8_t bitCnt) { - int32_t retVal = (int32_t)num; - if (num >= 2 << (bitCnt - 2)) { - retVal -= 2 << (bitCnt - 1); - } - return retVal; -} - -int32_t SX1280::GetLoRaBandwidth() { - int32_t bwValue = 0; - - switch (this->LoRaBandwidth) { - case LORA_BW_0200: - bwValue = 203125; - break; - case LORA_BW_0400: - bwValue = 406250; - break; - case LORA_BW_0800: - bwValue = 812500; - break; - case LORA_BW_1600: - bwValue = 1625000; - break; - default: - bwValue = 0; - } - return bwValue; -} - -void SX1280::SetInterruptMode(void) { - this->PollingMode = false; -} - -void SX1280::OnDioIrq(void) { - /* - * When polling mode is activated, it is up to the application to call - * ProcessIrqs( ). Otherwise, the driver automatically calls ProcessIrqs( ) - * on radio interrupt. - */ - if (this->PollingMode == true) { - this->IrqState = true; - } else { - this->ProcessIrqs(); - } -} - -void SX1280::ProcessIrqs(void) { - RadioPacketTypes_t packetType = PACKET_TYPE_NONE; - - if (this->PollingMode == true) { - if (this->IrqState == true) { - /*__disable_irq( );*/ - this->IrqState = false; - /*__enable_irq( );*/ - } else { - return; - } - } - - packetType = GetPacketType(true); - uint16_t irqRegs = GetIrqStatus(); - ClearIrqStatus(IRQ_RADIO_ALL); - -#if (SX1280_DEBUG == 1) - DigitalOut TEST_PIN_1(D14); - DigitalOut TEST_PIN_2(D15); - for (int i = 0x8000; i != 0; i >>= 1) { - TEST_PIN_2 = 0; - TEST_PIN_1 = ((irqRegs & i) != 0) ? 1 : 0; - TEST_PIN_2 = 1; - } - TEST_PIN_1 = 0; - TEST_PIN_2 = 0; -#endif - - switch (packetType) { - case PACKET_TYPE_GFSK: - case PACKET_TYPE_FLRC: - case PACKET_TYPE_BLE: - switch (OperatingMode) { - case MODE_RX: - if ((irqRegs & IRQ_RX_DONE) == IRQ_RX_DONE) { - if ((irqRegs & IRQ_CRC_ERROR) == IRQ_CRC_ERROR) { - if (rxError != NULL) { - rxError(IRQ_CRC_ERROR_CODE); - } - } else if ((irqRegs & IRQ_SYNCWORD_ERROR) == IRQ_SYNCWORD_ERROR) { - if (rxError != NULL) { - rxError(IRQ_SYNCWORD_ERROR_CODE); - } - } else { - if (rxDone != NULL) { - rxDone(); - } - } - } - if ((irqRegs & IRQ_SYNCWORD_VALID) == IRQ_SYNCWORD_VALID) { - if (rxSyncWordDone != NULL) { - rxSyncWordDone(); - } - } - if ((irqRegs & IRQ_SYNCWORD_ERROR) == IRQ_SYNCWORD_ERROR) { - if (rxError != NULL) { - rxError(IRQ_SYNCWORD_ERROR_CODE); - } - } - if ((irqRegs & IRQ_RX_TX_TIMEOUT) == IRQ_RX_TX_TIMEOUT) { - if (rxTimeout != NULL) { - rxTimeout(); - } - } - if ((irqRegs & IRQ_TX_DONE) == IRQ_TX_DONE) { - if (txDone != NULL) { - txDone(); - } - } - break; - case MODE_TX: - if ((irqRegs & IRQ_TX_DONE) == IRQ_TX_DONE) { - if (txDone != NULL) { - txDone(); - } - } - if ((irqRegs & IRQ_RX_TX_TIMEOUT) == IRQ_RX_TX_TIMEOUT) { - if (txTimeout != NULL) { - txTimeout(); - } - } - break; - default: - // Unexpected IRQ: silently returns - break; - } - break; - case PACKET_TYPE_LORA: - switch (OperatingMode) { - case MODE_RX: - if ((irqRegs & IRQ_RX_DONE) == IRQ_RX_DONE) { - if ((irqRegs & IRQ_CRC_ERROR) == IRQ_CRC_ERROR) { - if (rxError != NULL) { - rxError(IRQ_CRC_ERROR_CODE); - } - } else { - if (rxDone != NULL) { - rxDone(); - } - } - } - if ((irqRegs & IRQ_HEADER_VALID) == IRQ_HEADER_VALID) { - if (rxHeaderDone != NULL) { - rxHeaderDone(); - } - } - if ((irqRegs & IRQ_HEADER_ERROR) == IRQ_HEADER_ERROR) { - if (rxError != NULL) { - rxError(IRQ_HEADER_ERROR_CODE); - } - } - if ((irqRegs & IRQ_RX_TX_TIMEOUT) == IRQ_RX_TX_TIMEOUT) { - if (rxTimeout != NULL) { - rxTimeout(); - } - } - if ((irqRegs & IRQ_RANGING_SLAVE_REQUEST_DISCARDED) == IRQ_RANGING_SLAVE_REQUEST_DISCARDED) { - if (rxError != NULL) { - rxError(IRQ_RANGING_ON_LORA_ERROR_CODE); - } - } - break; - case MODE_TX: - if ((irqRegs & IRQ_TX_DONE) == IRQ_TX_DONE) { - if (txDone != NULL) { - txDone(); - } - } - if ((irqRegs & IRQ_RX_TX_TIMEOUT) == IRQ_RX_TX_TIMEOUT) { - if (txTimeout != NULL) { - txTimeout(); - } - } - break; - case MODE_CAD: - if ((irqRegs & IRQ_CAD_DONE) == IRQ_CAD_DONE) { - if ((irqRegs & IRQ_CAD_DETECTED) == IRQ_CAD_DETECTED) { - if (cadDone != NULL) { - cadDone(true); - } - } else { - if (cadDone != NULL) { - cadDone(false); - } - } - } else if ((irqRegs & IRQ_RX_TX_TIMEOUT) == IRQ_RX_TX_TIMEOUT) { - if (rxTimeout != NULL) { - rxTimeout(); - } - } - break; - default: - // Unexpected IRQ: silently returns - break; - } - break; - case PACKET_TYPE_RANGING: - switch (OperatingMode) { - // MODE_RX indicates an IRQ on the Slave side - case MODE_RX: - if ((irqRegs & IRQ_RANGING_SLAVE_REQUEST_DISCARDED) == IRQ_RANGING_SLAVE_REQUEST_DISCARDED) { - if (rangingDone != NULL) { - rangingDone(IRQ_RANGING_SLAVE_ERROR_CODE); - } - } - if ((irqRegs & IRQ_RANGING_SLAVE_REQUEST_VALID) == IRQ_RANGING_SLAVE_REQUEST_VALID) { - if (rangingDone != NULL) { - rangingDone(IRQ_RANGING_SLAVE_VALID_CODE); - } - } - if ((irqRegs & IRQ_RANGING_SLAVE_RESPONSE_DONE) == IRQ_RANGING_SLAVE_RESPONSE_DONE) { - if (rangingDone != NULL) { - rangingDone(IRQ_RANGING_SLAVE_VALID_CODE); - } - } - if ((irqRegs & IRQ_RX_TX_TIMEOUT) == IRQ_RX_TX_TIMEOUT) { - if (rangingDone != NULL) { - rangingDone(IRQ_RANGING_SLAVE_ERROR_CODE); - } - } - if ((irqRegs & IRQ_HEADER_VALID) == IRQ_HEADER_VALID) { - if (rxHeaderDone != NULL) { - rxHeaderDone(); - } - } - if ((irqRegs & IRQ_HEADER_ERROR) == IRQ_HEADER_ERROR) { - if (rxError != NULL) { - rxError(IRQ_HEADER_ERROR_CODE); - } - } - break; - // MODE_TX indicates an IRQ on the Master side - case MODE_TX: - if ((irqRegs & IRQ_RANGING_MASTER_TIMEOUT) == IRQ_RANGING_MASTER_TIMEOUT) { - if (rangingDone != NULL) { - rangingDone(IRQ_RANGING_MASTER_ERROR_CODE); - } - } - if ((irqRegs & IRQ_RANGING_MASTER_RESULT_VALID) == IRQ_RANGING_MASTER_RESULT_VALID) { - if (rangingDone != NULL) { - rangingDone(IRQ_RANGING_MASTER_VALID_CODE); - } - } - break; - default: - // Unexpected IRQ: silently returns - break; - } - break; - default: - // Unexpected IRQ: silently returns - break; - } -} diff --git a/FprimeZephyrReference/Components/MyComponent/sx1280.h b/FprimeZephyrReference/Components/MyComponent/sx1280.h deleted file mode 100644 index d29a5796..00000000 --- a/FprimeZephyrReference/Components/MyComponent/sx1280.h +++ /dev/null @@ -1,1627 +0,0 @@ -/* - ______ _ - / _____) _ | | -( (____ _____ ____ _| |_ _____ ____| |__ - \____ \| ___ | (_ _) ___ |/ ___) _ \ - _____) ) ____| | | || |_| ____( (___| | | | -(______/|_____)_|_|_| \__)_____)\____)_| |_| - (C)2016 Semtech - -Description: Driver for SX1280 devices - -License: Revised BSD License, see LICENSE.TXT file include in the project - -Maintainer: Miguel Luis, Gregory Cristian and Matthieu Verdy -*/ -#ifndef __SX1280_H__ -#define __SX1280_H__ - -#include "radio.h" - -/*! - * \brief Enables/disables driver debug features - */ -#define SX1280_DEBUG 0 - -/*! - * \brief Hardware IO IRQ callback function definition - */ -class SX1280; -typedef void (SX1280::*DioIrqHandler)(void); - -/*! - * \brief IRQ triggers callback function definition - */ -class SX1280Hal; -typedef void (SX1280Hal::*Trigger)(void); - -/*! - * \brief Provides the frequency of the chip running on the radio and the frequency step - * - * \remark These defines are used for computing the frequency divider to set the RF frequency - */ -#define XTAL_FREQ 52000000 -#define FREQ_STEP ((double)(XTAL_FREQ / (double)(1 << 18))) - -/*! - * \brief Compensation delay for SetAutoTx method in microseconds - */ -#define AUTO_TX_OFFSET 33 - -/*! - * \brief The address of the register holding the firmware version MSB - */ -#define REG_LR_FIRMWARE_VERSION_MSB 0x0153 - -/*! - * \brief The address of the register holding the first byte defining the CRC seed - * - * \remark Only used for packet types GFSK and Flrc - */ -#define REG_LR_CRCSEEDBASEADDR 0x09C8 - -/*! - * \brief The address of the register holding the first byte defining the CRC polynomial - * - * \remark Only used for packet types GFSK and Flrc - */ -#define REG_LR_CRCPOLYBASEADDR 0x09C6 - -/*! - * \brief The address of the register holding the first byte defining the whitening seed - * - * \remark Only used for packet types GFSK, FLRC and BLE - */ -#define REG_LR_WHITSEEDBASEADDR 0x09C5 - -/*! - * \brief The address of the register holding the ranging id check length - * - * \remark Only used for packet type Ranging - */ -#define REG_LR_RANGINGIDCHECKLENGTH 0x0931 - -/*! - * \brief The address of the register holding the device ranging id - * - * \remark Only used for packet type Ranging - */ -#define REG_LR_DEVICERANGINGADDR 0x0916 - -/*! - * \brief The address of the register holding the device ranging id - * - * \remark Only used for packet type Ranging - */ -#define REG_LR_REQUESTRANGINGADDR 0x0912 - -/*! - * \brief The address of the register holding ranging results configuration - * and the corresponding mask - * - * \remark Only used for packet type Ranging - */ -#define REG_LR_RANGINGRESULTCONFIG 0x0924 -#define MASK_RANGINGMUXSEL 0xCF - -/*! - * \brief The address of the register holding the first byte of ranging results - * Only used for packet type Ranging - */ -#define REG_LR_RANGINGRESULTBASEADDR 0x0961 - -/*! - * \brief The address of the register allowing to read ranging results - * - * \remark Only used for packet type Ranging - */ -#define REG_LR_RANGINGRESULTSFREEZE 0x097F - -/*! - * \brief The address of the register holding the first byte of ranging calibration - * - * \remark Only used for packet type Ranging - */ -#define REG_LR_RANGINGRERXTXDELAYCAL 0x092C - -/*! - *\brief The address of the register holding the ranging filter window size - * - * \remark Only used for packet type Ranging - */ -#define REG_LR_RANGINGFILTERWINDOWSIZE 0x091E - -/*! - *\brief The address of the register to reset for clearing ranging filter - * - * \remark Only used for packet type Ranging - */ -#define REG_LR_RANGINGRESULTCLEARREG 0x0923 - -#define REG_RANGING_RSSI 0x0964 - -/*! - * \brief The default number of samples considered in built-in ranging filter - */ -#define DEFAULT_RANGING_FILTER_SIZE 127 - -/*! - * \brief The address of the register holding LORA packet parameters - */ -#define REG_LR_PACKETPARAMS 0x903 - -/*! - * \brief The address of the register holding payload length - * - * \remark Do NOT try to read it directly. Use GetRxBuffer( ) instead. - */ -#define REG_LR_PAYLOADLENGTH 0x901 - -/*! - * \brief The addresses of the registers holding SyncWords values - * - * \remark The addresses depends on the Packet Type in use, and not all - * SyncWords are available for every Packet Type - */ -#define REG_LR_SYNCWORDBASEADDRESS1 0x09CE -#define REG_LR_SYNCWORDBASEADDRESS2 0x09D3 -#define REG_LR_SYNCWORDBASEADDRESS3 0x09D8 - -/*! - * \brief The MSB address and mask used to read the estimated frequency - * error - */ -#define REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB 0x0954 -#define REG_LR_ESTIMATED_FREQUENCY_ERROR_MASK 0x0FFFFF - -/*! - * \brief Defines how many bit errors are tolerated in sync word detection - */ -#define REG_LR_SYNCWORDTOLERANCE 0x09CD - -/*! - * \brief Register and mask for GFSK and BLE preamble length forcing - */ -#define REG_LR_PREAMBLELENGTH 0x09C1 -#define MASK_FORCE_PREAMBLELENGTH 0x8F - -/*! - * \brief Register for MSB Access Address (BLE) - */ -#define REG_LR_BLE_ACCESS_ADDRESS 0x09CF -#define BLE_ADVERTIZER_ACCESS_ADDRESS 0x8E89BED6 - -/*! - * \brief Select high sensitivity versus power consumption - */ -#define REG_LNA_REGIME 0x0891 -#define MASK_LNA_REGIME 0xC0 - -/* - * \brief Register and mask controlling the enabling of manual gain control - */ -#define REG_ENABLE_MANUAL_GAIN_CONTROL 0x089F -#define MASK_MANUAL_GAIN_CONTROL 0x80 - -/*! - * \brief Register and mask controlling the demodulation detection - */ -#define REG_DEMOD_DETECTION 0x0895 -#define MASK_DEMOD_DETECTION 0xFE - -/*! - * Register and mask to set the manual gain parameter - */ -#define REG_MANUAL_GAIN_VALUE 0x089E -#define MASK_MANUAL_GAIN_VALUE 0xF0 - -/*! - * \brief Represents the states of the radio - */ -typedef enum { - RF_IDLE = 0x00, //!< The radio is idle - RF_RX_RUNNING, //!< The radio is in reception state - RF_TX_RUNNING, //!< The radio is in transmission state - RF_CAD, //!< The radio is doing channel activity detection -} RadioStates_t; - -/*! - * \brief Represents the operating mode the radio is actually running - */ -typedef enum { - MODE_SLEEP = 0x00, //! The radio is in sleep mode - MODE_CALIBRATION, //! The radio is in calibration mode - MODE_STDBY_RC, //! The radio is in standby mode with RC oscillator - MODE_STDBY_XOSC, //! The radio is in standby mode with XOSC oscillator - MODE_FS, //! The radio is in frequency synthesis mode - MODE_RX, //! The radio is in receive mode - MODE_TX, //! The radio is in transmit mode - MODE_CAD //! The radio is in channel activity detection mode -} RadioOperatingModes_t; - -/*! - * \brief Declares the oscillator in use while in standby mode - * - * Using the STDBY_RC standby mode allow to reduce the energy consumption - * STDBY_XOSC should be used for time critical applications - */ -typedef enum { - STDBY_RC = 0x00, - STDBY_XOSC = 0x01, -} RadioStandbyModes_t; - -/*! - * \brief Declares the power regulation used to power the device - * - * This command allows the user to specify if DC-DC or LDO is used for power regulation. - * Using only LDO implies that the Rx or Tx current is doubled - */ -typedef enum { - USE_LDO = 0x00, //! Use LDO (default value) - USE_DCDC = 0x01, //! Use DCDC -} RadioRegulatorModes_t; - -/*! - * \brief Represents the possible packet type (i.e. modem) used - */ -typedef enum { - PACKET_TYPE_GFSK = 0x00, - PACKET_TYPE_LORA, - PACKET_TYPE_RANGING, - PACKET_TYPE_FLRC, - PACKET_TYPE_BLE, - PACKET_TYPE_NONE = 0x0F, -} RadioPacketTypes_t; - -/*! - * \brief Represents the ramping time for power amplifier - */ -typedef enum { - RADIO_RAMP_02_US = 0x00, - RADIO_RAMP_04_US = 0x20, - RADIO_RAMP_06_US = 0x40, - RADIO_RAMP_08_US = 0x60, - RADIO_RAMP_10_US = 0x80, - RADIO_RAMP_12_US = 0xA0, - RADIO_RAMP_16_US = 0xC0, - RADIO_RAMP_20_US = 0xE0, -} RadioRampTimes_t; - -/*! - * \brief Represents the number of symbols to be used for channel activity detection operation - */ -typedef enum { - LORA_CAD_01_SYMBOL = 0x00, - LORA_CAD_02_SYMBOLS = 0x20, - LORA_CAD_04_SYMBOLS = 0x40, - LORA_CAD_08_SYMBOLS = 0x60, - LORA_CAD_16_SYMBOLS = 0x80, -} RadioLoRaCadSymbols_t; - -/*! - * \brief Represents the possible combinations of bitrate and bandwidth for - * GFSK and BLE packet types - * - * The bitrate is expressed in Mb/s and the bandwidth in MHz - */ -typedef enum { - GFSK_BLE_BR_2_000_BW_2_4 = 0x04, - GFSK_BLE_BR_1_600_BW_2_4 = 0x28, - GFSK_BLE_BR_1_000_BW_2_4 = 0x4C, - GFSK_BLE_BR_1_000_BW_1_2 = 0x45, - GFSK_BLE_BR_0_800_BW_2_4 = 0x70, - GFSK_BLE_BR_0_800_BW_1_2 = 0x69, - GFSK_BLE_BR_0_500_BW_1_2 = 0x8D, - GFSK_BLE_BR_0_500_BW_0_6 = 0x86, - GFSK_BLE_BR_0_400_BW_1_2 = 0xB1, - GFSK_BLE_BR_0_400_BW_0_6 = 0xAA, - GFSK_BLE_BR_0_250_BW_0_6 = 0xCE, - GFSK_BLE_BR_0_250_BW_0_3 = 0xC7, - GFSK_BLE_BR_0_125_BW_0_3 = 0xEF, -} RadioGfskBleBitrates_t; - -/*! - * \brief Represents the modulation index used in GFSK and BLE packet - * types - */ -typedef enum { - GFSK_BLE_MOD_IND_0_35 = 0, - GFSK_BLE_MOD_IND_0_50 = 1, - GFSK_BLE_MOD_IND_0_75 = 2, - GFSK_BLE_MOD_IND_1_00 = 3, - GFSK_BLE_MOD_IND_1_25 = 4, - GFSK_BLE_MOD_IND_1_50 = 5, - GFSK_BLE_MOD_IND_1_75 = 6, - GFSK_BLE_MOD_IND_2_00 = 7, - GFSK_BLE_MOD_IND_2_25 = 8, - GFSK_BLE_MOD_IND_2_50 = 9, - GFSK_BLE_MOD_IND_2_75 = 10, - GFSK_BLE_MOD_IND_3_00 = 11, - GFSK_BLE_MOD_IND_3_25 = 12, - GFSK_BLE_MOD_IND_3_50 = 13, - GFSK_BLE_MOD_IND_3_75 = 14, - GFSK_BLE_MOD_IND_4_00 = 15, -} RadioGfskBleModIndexes_t; - -/*! - * \brief Represents the possible combination of bitrate and bandwidth for FLRC - * packet type - * - * The bitrate is in Mb/s and the bitrate in MHz - */ -typedef enum { - FLRC_BR_1_300_BW_1_2 = 0x45, - FLRC_BR_1_040_BW_1_2 = 0x69, - FLRC_BR_0_650_BW_0_6 = 0x86, - FLRC_BR_0_520_BW_0_6 = 0xAA, - FLRC_BR_0_325_BW_0_3 = 0xC7, - FLRC_BR_0_260_BW_0_3 = 0xEB, -} RadioFlrcBitrates_t; - -/*! - * \brief Represents the possible values for coding rate parameter in FLRC - * packet type - */ -typedef enum { - FLRC_CR_1_2 = 0x00, - FLRC_CR_3_4 = 0x02, - FLRC_CR_1_0 = 0x04, -} RadioFlrcCodingRates_t; - -/*! - * \brief Represents the modulation shaping parameter for GFSK, FLRC and BLE - * packet types - */ -typedef enum { - RADIO_MOD_SHAPING_BT_OFF = 0x00, //! No filtering - RADIO_MOD_SHAPING_BT_1_0 = 0x10, - RADIO_MOD_SHAPING_BT_0_5 = 0x20, -} RadioModShapings_t; - -/*! - * \brief Represents the possible spreading factor values in LORA packet types - */ -typedef enum { - LORA_SF5 = 0x50, - LORA_SF6 = 0x60, - LORA_SF7 = 0x70, - LORA_SF8 = 0x80, - LORA_SF9 = 0x90, - LORA_SF10 = 0xA0, - LORA_SF11 = 0xB0, - LORA_SF12 = 0xC0, -} RadioLoRaSpreadingFactors_t; - -/*! - * \brief Represents the bandwidth values for LORA packet type - */ -typedef enum { - LORA_BW_0200 = 0x34, - LORA_BW_0400 = 0x26, - LORA_BW_0800 = 0x18, - LORA_BW_1600 = 0x0A, -} RadioLoRaBandwidths_t; - -/*! - * \brief Represents the coding rate values for LORA packet type - */ -typedef enum { - LORA_CR_4_5 = 0x01, - LORA_CR_4_6 = 0x02, - LORA_CR_4_7 = 0x03, - LORA_CR_4_8 = 0x04, - LORA_CR_LI_4_5 = 0x05, - LORA_CR_LI_4_6 = 0x06, - LORA_CR_LI_4_7 = 0x07, -} RadioLoRaCodingRates_t; - -/*! - * \brief Represents the preamble length values for GFSK and FLRC packet - * types - */ -typedef enum { - PREAMBLE_LENGTH_04_BITS = 0x00, //!< Preamble length: 04 bits - PREAMBLE_LENGTH_08_BITS = 0x10, //!< Preamble length: 08 bits - PREAMBLE_LENGTH_12_BITS = 0x20, //!< Preamble length: 12 bits - PREAMBLE_LENGTH_16_BITS = 0x30, //!< Preamble length: 16 bits - PREAMBLE_LENGTH_20_BITS = 0x40, //!< Preamble length: 20 bits - PREAMBLE_LENGTH_24_BITS = 0x50, //!< Preamble length: 24 bits - PREAMBLE_LENGTH_28_BITS = 0x60, //!< Preamble length: 28 bits - PREAMBLE_LENGTH_32_BITS = 0x70, //!< Preamble length: 32 bits -} RadioPreambleLengths_t; - -/*! - * \brief Represents the SyncWord length for FLRC packet type - */ -typedef enum { - FLRC_NO_SYNCWORD = 0x00, - FLRC_SYNCWORD_LENGTH_4_BYTE = 0x04, -} RadioFlrcSyncWordLengths_t; - -/*! - * \brief The length of sync words for GFSK packet type - */ -typedef enum { - GFSK_SYNCWORD_LENGTH_1_BYTE = 0x00, //!< Sync word length: 1 byte - GFSK_SYNCWORD_LENGTH_2_BYTE = 0x02, //!< Sync word length: 2 bytes - GFSK_SYNCWORD_LENGTH_3_BYTE = 0x04, //!< Sync word length: 3 bytes - GFSK_SYNCWORD_LENGTH_4_BYTE = 0x06, //!< Sync word length: 4 bytes - GFSK_SYNCWORD_LENGTH_5_BYTE = 0x08, //!< Sync word length: 5 bytes -} RadioSyncWordLengths_t; - -/*! - * \brief Represents the possible combinations of SyncWord correlators - * activated for GFSK and FLRC packet types - */ -typedef enum { - RADIO_RX_MATCH_SYNCWORD_OFF = 0x00, //!< No correlator turned on, i.e. do not search for SyncWord - RADIO_RX_MATCH_SYNCWORD_1 = 0x10, - RADIO_RX_MATCH_SYNCWORD_2 = 0x20, - RADIO_RX_MATCH_SYNCWORD_1_2 = 0x30, - RADIO_RX_MATCH_SYNCWORD_3 = 0x40, - RADIO_RX_MATCH_SYNCWORD_1_3 = 0x50, - RADIO_RX_MATCH_SYNCWORD_2_3 = 0x60, - RADIO_RX_MATCH_SYNCWORD_1_2_3 = 0x70, -} RadioSyncWordRxMatchs_t; - -/*! - * \brief Radio packet length mode for GFSK and FLRC packet types - */ -typedef enum { - RADIO_PACKET_FIXED_LENGTH = 0x00, //!< The packet is known on both sides, no header included in the packet - RADIO_PACKET_VARIABLE_LENGTH = 0x20, //!< The packet is on variable size, header included -} RadioPacketLengthModes_t; - -/*! - * \brief Represents the CRC length for GFSK and FLRC packet types - * - * \warning Not all configurations are available for both GFSK and FLRC - * packet type. Refer to the datasheet for possible configuration. - */ -typedef enum { - RADIO_CRC_OFF = 0x00, //!< No CRC in use - RADIO_CRC_1_BYTES = 0x10, - RADIO_CRC_2_BYTES = 0x20, - RADIO_CRC_3_BYTES = 0x30, -} RadioCrcTypes_t; - -/*! - * \brief Radio whitening mode activated or deactivated for GFSK, FLRC and - * BLE packet types - */ -typedef enum { - RADIO_WHITENING_ON = 0x00, - RADIO_WHITENING_OFF = 0x08, -} RadioWhiteningModes_t; - -/*! - * \brief Holds the packet length mode of a LORA packet type - */ -typedef enum { - LORA_PACKET_VARIABLE_LENGTH = 0x00, //!< The packet is on variable size, header included - LORA_PACKET_FIXED_LENGTH = 0x80, //!< The packet is known on both sides, no header included in the packet - LORA_PACKET_EXPLICIT = LORA_PACKET_VARIABLE_LENGTH, - LORA_PACKET_IMPLICIT = LORA_PACKET_FIXED_LENGTH, -} RadioLoRaPacketLengthsModes_t; - -/*! - * \brief Represents the CRC mode for LORA packet type - */ -typedef enum { - LORA_CRC_ON = 0x20, //!< CRC activated - LORA_CRC_OFF = 0x00, //!< CRC not used -} RadioLoRaCrcModes_t; - -/*! - * \brief Represents the IQ mode for LORA packet type - */ -typedef enum { - LORA_IQ_NORMAL = 0x40, - LORA_IQ_INVERTED = 0x00, -} RadioLoRaIQModes_t; - -/*! - * \brief Represents the length of the ID to check in ranging operation - */ -typedef enum { - RANGING_IDCHECK_LENGTH_08_BITS = 0x00, - RANGING_IDCHECK_LENGTH_16_BITS, - RANGING_IDCHECK_LENGTH_24_BITS, - RANGING_IDCHECK_LENGTH_32_BITS, -} RadioRangingIdCheckLengths_t; - -/*! - * \brief Represents the result type to be used in ranging operation - */ -typedef enum { - RANGING_RESULT_RAW = 0x00, - RANGING_RESULT_AVERAGED = 0x01, - RANGING_RESULT_DEBIASED = 0x02, - RANGING_RESULT_FILTERED = 0x03, -} RadioRangingResultTypes_t; - -/*! - * \brief Represents the connection state for BLE packet type - */ -typedef enum { - BLE_PAYLOAD_LENGTH_MAX_31_BYTES = 0x00, - BLE_PAYLOAD_LENGTH_MAX_37_BYTES = 0x20, - BLE_TX_TEST_MODE = 0x40, - BLE_PAYLOAD_LENGTH_MAX_255_BYTES = 0x80, -} RadioBleConnectionStates_t; - -/*! - * \brief Represents the CRC field length for BLE packet type - */ -typedef enum { - BLE_CRC_OFF = 0x00, - BLE_CRC_3B = 0x10, -} RadioBleCrcTypes_t; - -/*! - * \brief Represents the specific packets to use in BLE packet type - */ -typedef enum { - BLE_PRBS_9 = 0x00, //!< Pseudo Random Binary Sequence based on 9th degree polynomial - BLE_PRBS_15 = 0x0C, //!< Pseudo Random Binary Sequence based on 15th degree polynomial - BLE_EYELONG_1_0 = 0x04, //!< Repeated '11110000' sequence - BLE_EYELONG_0_1 = 0x18, //!< Repeated '00001111' sequence - BLE_EYESHORT_1_0 = 0x08, //!< Repeated '10101010' sequence - BLE_EYESHORT_0_1 = 0x1C, //!< Repeated '01010101' sequence - BLE_ALL_1 = 0x10, //!< Repeated '11111111' sequence - BLE_ALL_0 = 0x14, //!< Repeated '00000000' sequence -} RadioBleTestPayloads_t; - -/*! - * \brief Represents the interruption masks available for the radio - * - * \remark Note that not all these interruptions are available for all packet types - */ -typedef enum { - IRQ_RADIO_NONE = 0x0000, - IRQ_TX_DONE = 0x0001, - IRQ_RX_DONE = 0x0002, - IRQ_SYNCWORD_VALID = 0x0004, - IRQ_SYNCWORD_ERROR = 0x0008, - IRQ_HEADER_VALID = 0x0010, - IRQ_HEADER_ERROR = 0x0020, - IRQ_CRC_ERROR = 0x0040, - IRQ_RANGING_SLAVE_RESPONSE_DONE = 0x0080, - IRQ_RANGING_SLAVE_REQUEST_DISCARDED = 0x0100, - IRQ_RANGING_MASTER_RESULT_VALID = 0x0200, - IRQ_RANGING_MASTER_TIMEOUT = 0x0400, - IRQ_RANGING_SLAVE_REQUEST_VALID = 0x0800, - IRQ_CAD_DONE = 0x1000, - IRQ_CAD_DETECTED = 0x2000, - IRQ_RX_TX_TIMEOUT = 0x4000, - IRQ_PREAMBLE_DETECTED = 0x8000, - IRQ_RADIO_ALL = 0xFFFF, -} RadioIrqMasks_t; - -/*! - * \brief Represents the digital input/output of the radio - */ -typedef enum { - RADIO_DIO1 = 0x02, - RADIO_DIO2 = 0x04, - RADIO_DIO3 = 0x08, -} RadioDios_t; - -/*! - * \brief Represents the tick size available for Rx/Tx timeout operations - */ -typedef enum { - RADIO_TICK_SIZE_0015_US = 0x00, - RADIO_TICK_SIZE_0062_US = 0x01, - RADIO_TICK_SIZE_1000_US = 0x02, - RADIO_TICK_SIZE_4000_US = 0x03, -} RadioTickSizes_t; - -/*! - * \brief Represents the role of the radio during ranging operations - */ -typedef enum { - RADIO_RANGING_ROLE_SLAVE = 0x00, - RADIO_RANGING_ROLE_MASTER = 0x01, -} RadioRangingRoles_t; - -/*! - * \brief Represents the possible Mask settings for sensitivity selection - */ -typedef enum { - LNA_LOW_POWER_MODE, - LNA_HIGH_SENSITIVITY_MODE, -} RadioLnaSettings_t; - -/*! - * \brief Represents an amount of time measurable by the radio clock - * - * @code - * Time = PeriodBase * PeriodBaseCount - * Example: - * PeriodBase = RADIO_TICK_SIZE_4000_US( 4 ms ) - * PeriodBaseCount = 1000 - * Time = 4e-3 * 1000 = 4 seconds - * @endcode - */ -typedef struct TickTime_s { - RadioTickSizes_t PeriodBase; //!< The base time of ticktime - /*! - * \brief The number of periodBase for ticktime - * Special values are: - * - 0x0000 for single mode - * - 0xFFFF for continuous mode - */ - uint16_t PeriodBaseCount; -} TickTime_t; - -/*! - * \brief RX_TX_CONTINUOUS and RX_TX_SINGLE are two particular values for TickTime. - * The former keep the radio in Rx or Tx mode, even after successful reception - * or transmission. It should never generate Timeout interrupt. - * The later let the radio enough time to make one reception or transmission. - * No Timeout interrupt is generated, and the radio fall in StandBy mode after - * reception or transmission. - */ -#define RX_TX_CONTINUOUS (TickTime_t){RADIO_TICK_SIZE_0015_US, 0xFFFF} -#define RX_TX_SINGLE (TickTime_t){RADIO_TICK_SIZE_0015_US, 0} - -/*! - * \brief The type describing the modulation parameters for every packet types - */ -typedef struct { - RadioPacketTypes_t PacketType; //!< Packet to which the modulation parameters are referring to. - struct { - /*! - * \brief Holds the GFSK modulation parameters - * - * In GFSK modulation, the bit-rate and bandwidth are linked together. In this structure, its values are set - * using the same token. - */ - struct { - RadioGfskBleBitrates_t - BitrateBandwidth; //!< The bandwidth and bit-rate values for BLE and GFSK modulations - RadioGfskBleModIndexes_t ModulationIndex; //!< The coding rate for BLE and GFSK modulations - RadioModShapings_t ModulationShaping; //!< The modulation shaping for BLE and GFSK modulations - } Gfsk; - /*! - * \brief Holds the LORA modulation parameters - * - * LORA modulation is defined by Spreading Factor (SF), Bandwidth and Coding Rate - */ - struct { - RadioLoRaSpreadingFactors_t SpreadingFactor; //!< Spreading Factor for the LORA modulation - RadioLoRaBandwidths_t Bandwidth; //!< Bandwidth for the LORA modulation - RadioLoRaCodingRates_t CodingRate; //!< Coding rate for the LORA modulation - } LoRa; - /*! - * \brief Holds the FLRC modulation parameters - * - * In FLRC modulation, the bit-rate and bandwidth are linked together. In this structure, its values are set - * using the same token. - */ - struct { - RadioFlrcBitrates_t BitrateBandwidth; //!< The bandwidth and bit-rate values for FLRC modulation - RadioFlrcCodingRates_t CodingRate; //!< The coding rate for FLRC modulation - RadioModShapings_t ModulationShaping; //!< The modulation shaping for FLRC modulation - } Flrc; - /*! - * \brief Holds the BLE modulation parameters - * - * In BLE modulation, the bit-rate and bandwidth are linked together. In this structure, its values are set - * using the same token. - */ - struct { - RadioGfskBleBitrates_t - BitrateBandwidth; //!< The bandwidth and bit-rate values for BLE and GFSK modulations - RadioGfskBleModIndexes_t ModulationIndex; //!< The coding rate for BLE and GFSK modulations - RadioModShapings_t ModulationShaping; //!< The modulation shaping for BLE and GFSK modulations - } Ble; - } Params; //!< Holds the modulation parameters structure -} ModulationParams_t; - -/*! - * \brief The type describing the packet parameters for every packet types - */ -typedef struct { - RadioPacketTypes_t PacketType; //!< Packet to which the packet parameters are referring to. - struct { - /*! - * \brief Holds the GFSK packet parameters - */ - struct { - RadioPreambleLengths_t PreambleLength; //!< The preamble length for GFSK packet type - RadioSyncWordLengths_t SyncWordLength; //!< The synchronization word length for GFSK packet type - RadioSyncWordRxMatchs_t - SyncWordMatch; //!< The synchronization correlator to use to check synchronization word - RadioPacketLengthModes_t HeaderType; //!< If the header is explicit, it will be transmitted in the GFSK - //!< packet. If the header is implicit, it will not be transmitted - uint8_t PayloadLength; //!< Size of the payload in the GFSK packet - RadioCrcTypes_t CrcLength; //!< Size of the CRC block in the GFSK packet - RadioWhiteningModes_t Whitening; //!< Usage of whitening on payload and CRC blocks plus header block if - //!< header type is variable - } Gfsk; - /*! - * \brief Holds the LORA packet parameters - */ - struct { - uint8_t PreambleLength; //!< The preamble length is the number of LORA symbols in the preamble. To set it, - //!< use the following formula @code Number of symbols = PreambleLength[3:0] * ( - //!< 2^PreambleLength[7:4] ) @endcode - RadioLoRaPacketLengthsModes_t - HeaderType; //!< If the header is explicit, it will be transmitted in the LORA packet. If the header is - //!< implicit, it will not be transmitted - uint8_t PayloadLength; //!< Size of the payload in the LORA packet - RadioLoRaCrcModes_t Crc; //!< Size of CRC block in LORA packet - RadioLoRaIQModes_t InvertIQ; //!< Allows to swap IQ for LORA packet - } LoRa; - /*! - * \brief Holds the FLRC packet parameters - */ - struct { - RadioPreambleLengths_t PreambleLength; //!< The preamble length for FLRC packet type - RadioFlrcSyncWordLengths_t SyncWordLength; //!< The synchronization word length for FLRC packet type - RadioSyncWordRxMatchs_t - SyncWordMatch; //!< The synchronization correlator to use to check synchronization word - RadioPacketLengthModes_t HeaderType; //!< If the header is explicit, it will be transmitted in the FLRC - //!< packet. If the header is implicit, it will not be transmitted. - uint8_t PayloadLength; //!< Size of the payload in the FLRC packet - RadioCrcTypes_t CrcLength; //!< Size of the CRC block in the FLRC packet - RadioWhiteningModes_t Whitening; //!< Usage of whitening on payload and CRC blocks plus header block if - //!< header type is variable - } Flrc; - /*! - * \brief Holds the BLE packet parameters - */ - struct { - RadioBleConnectionStates_t ConnectionState; //!< The BLE state - RadioBleCrcTypes_t CrcLength; //!< Size of the CRC block in the BLE packet - RadioBleTestPayloads_t BleTestPayload; //!< Special BLE payload for test purpose - RadioWhiteningModes_t Whitening; //!< Usage of whitening on PDU and CRC blocks of BLE packet - } Ble; - } Params; //!< Holds the packet parameters structure -} PacketParams_t; - -/*! - * \brief Represents the packet status for every packet type - */ -typedef struct { - RadioPacketTypes_t packetType; //!< Packet to which the packet status are referring to. - union { - struct { - int8_t RssiSync; //!< The RSSI measured on last packet - struct { - bool SyncError : 1; //!< SyncWord error on last packet - bool LengthError : 1; //!< Length error on last packet - bool CrcError : 1; //!< CRC error on last packet - bool AbortError : 1; //!< Abort error on last packet - bool HeaderReceived : 1; //!< Header received on last packet - bool PacketReceived : 1; //!< Packet received - bool PacketControlerBusy : 1; //!< Packet controller busy - } ErrorStatus; //!< The error status Byte - struct { - bool RxNoAck : 1; //!< No acknowledgment received for Rx with variable length packets - bool PacketSent : 1; //!< Packet sent, only relevant in Tx mode - } TxRxStatus; //!< The Tx/Rx status Byte - uint8_t SyncAddrStatus : 3; //!< The id of the correlator who found the packet - } Gfsk; - struct { - int8_t RssiPkt; //!< The RSSI of the last packet - int8_t SnrPkt; //!< The SNR of the last packet - } LoRa; - struct { - int8_t RssiSync; //!< The RSSI of the last packet - struct { - bool SyncError : 1; //!< SyncWord error on last packet - bool LengthError : 1; //!< Length error on last packet - bool CrcError : 1; //!< CRC error on last packet - bool AbortError : 1; //!< Abort error on last packet - bool HeaderReceived : 1; //!< Header received on last packet - bool PacketReceived : 1; //!< Packet received - bool PacketControlerBusy : 1; //!< Packet controller busy - } ErrorStatus; //!< The error status Byte - struct { - uint8_t RxPid : 2; //!< PID of the Rx - bool RxNoAck : 1; //!< No acknowledgment received for Rx with variable length packets - bool RxPidErr : 1; //!< Received PID error - bool PacketSent : 1; //!< Packet sent, only relevant in Tx mode - } TxRxStatus; //!< The Tx/Rx status Byte - uint8_t SyncAddrStatus : 3; //!< The id of the correlator who found the packet - } Flrc; - struct { - int8_t RssiSync; //!< The RSSI of the last packet - struct { - bool SyncError : 1; //!< SyncWord error on last packet - bool LengthError : 1; //!< Length error on last packet - bool CrcError : 1; //!< CRC error on last packet - bool AbortError : 1; //!< Abort error on last packet - bool HeaderReceived : 1; //!< Header received on last packet - bool PacketReceived : 1; //!< Packet received - bool PacketControlerBusy : 1; //!< Packet controller busy - } ErrorStatus; //!< The error status Byte - struct { - bool PacketSent : 1; //!< Packet sent, only relevant in Tx mode - } TxRxStatus; //!< The Tx/Rx status Byte - uint8_t SyncAddrStatus : 3; //!< The id of the correlator who found the packet - } Ble; - }; -} PacketStatus_t; - -/*! - * \brief Represents the Rx internal counters values when GFSK or LORA packet type is used - */ -typedef struct { - RadioPacketTypes_t packetType; //!< Packet to which the packet status are referring to. - union { - struct { - uint16_t PacketReceived; //!< Number of received packets - uint16_t CrcError; //!< Number of CRC errors - uint16_t LengthError; //!< Number of length errors - uint16_t SyncwordError; //!< Number of sync-word errors - } Gfsk; - struct { - uint16_t PacketReceived; //!< Number of received packets - uint16_t CrcError; //!< Number of CRC errors - uint16_t HeaderValid; //!< Number of valid headers - } LoRa; - }; -} RxCounter_t; - -/*! - * \brief Represents a calibration configuration - */ -typedef struct { - uint8_t RC64KEnable : 1; //!< Calibrate RC64K clock - uint8_t RC13MEnable : 1; //!< Calibrate RC13M clock - uint8_t PLLEnable : 1; //!< Calibrate PLL - uint8_t ADCPulseEnable : 1; //!< Calibrate ADC Pulse - uint8_t ADCBulkNEnable : 1; //!< Calibrate ADC bulkN - uint8_t ADCBulkPEnable : 1; //!< Calibrate ADC bulkP -} CalibrationParams_t; - -/*! - * \brief Represents a sleep mode configuration - */ -typedef struct { - uint8_t WakeUpRTC : 1; //!< Get out of sleep mode if wakeup signal received from RTC - uint8_t InstructionRamRetention : 1; //!< InstructionRam is conserved during sleep - uint8_t DataBufferRetention : 1; //!< Data buffer is conserved during sleep - uint8_t DataRamRetention : 1; //!< Data ram is conserved during sleep -} SleepParams_t; - -/*! - * \brief Represents the SX1280 and its features - * - * It implements the commands the SX1280 can understands - */ -class SX1280 : public Radio { - public: - /*! - * \brief Instantiates a SX1280 object and provides API functions to communicates with the radio - * - * \param [in] callbacks Pointer to the callbacks structure defining - * all callbacks function pointers - */ - explicit SX1280(RadioCallbacks_t* callbacks) - : // The class members are value-initialiazed in member-initilaizer list - Radio(callbacks), - OperatingMode(MODE_STDBY_RC), - PacketType(PACKET_TYPE_NONE), - LoRaBandwidth(LORA_BW_1600), - IrqState(false), - PollingMode(false) { - this->dioIrq = &SX1280::OnDioIrq; - - // Warning: this constructor set the LoRaBandwidth member to a valid - // value, but it is not related to the actual radio configuration! - } - - virtual ~SX1280() {} - - private: - /*! - * \brief Holds the internal operating mode of the radio - */ - RadioOperatingModes_t OperatingMode; - - /*! - * \brief Stores the current packet type set in the radio - */ - RadioPacketTypes_t PacketType; - - /*! - * \brief Stores the current LORA bandwidth set in the radio - */ - RadioLoRaBandwidths_t LoRaBandwidth; - - /*! - * \brief Holds a flag raised on radio interrupt - */ - bool IrqState; - - /*! - * \brief Hardware DIO IRQ functions - */ - DioIrqHandler dioIrq; - - /*! - * \brief Holds the polling state of the driver - */ - bool PollingMode; - - /*! - * \brief Compute the two's complement for a register of size lower than - * 32bits - * - * \param [in] num The register to be two's complemented - * \param [in] bitCnt The position of the sign bit - */ - static int32_t complement2(const uint32_t num, const uint8_t bitCnt); - - /*! - * \brief Returns the value of LoRa bandwidth from driver's value - * - * The value is returned in Hz so that it can be represented as an integer - * type. Most computation should be done as integer to reduce floating - * point related errors. - * - * \retval loRaBw The value of the current bandwidth in Hz - */ - int32_t GetLoRaBandwidth(void); - - protected: - /*! - * \brief Sets a function to be triggered on radio interrupt - * - * \param [in] irqHandler A pointer to a function to be run on interrupt - * from the radio - */ - virtual void IoIrqInit(DioIrqHandler irqHandler) = 0; - - /*! - * \brief DIOs interrupt callback - * - * \remark Called to handle all 3 DIOs pins - */ - void OnDioIrq(void); - - /*! - * \brief Set the role of the radio during ranging operations - * - * \param [in] role Role of the radio - */ - void SetRangingRole(RadioRangingRoles_t role); - - public: - /*! - * \brief Initializes the radio driver - */ - void Init(void); - - /*! - * \brief Set the driver in polling mode. - * - * In polling mode the application is responsible to call ProcessIrqs( ) to - * execute callbacks functions. - * The default mode is Interrupt Mode. - * @code - * // Initializations and callbacks declaration/definition - * radio = SX1280( mosi, miso, sclk, nss, busy, int1, int2, int3, rst, &callbacks ); - * radio.Init( ); - * radio.SetPollingMode( ); - * - * while( true ) - * { - * // IRQ processing is automatically done - * radio.ProcessIrqs( ); // <-- here, as well as callback functions - * // calls - * // Do some applicative work - * } - * @endcode - * - * \see SX1280::SetInterruptMode - */ - void SetPollingMode(void); - - /*! - * \brief Set the driver in interrupt mode. - * - * In interrupt mode, the driver communicate with the radio during the - * interruption by direct calls to ProcessIrqs( ). The main advantage is - * the possibility to have low power application architecture. - * This is the default mode. - * @code - * // Initializations and callbacks declaration/definition - * radio = SX1280( mosi, miso, sclk, nss, busy, int1, int2, int3, rst, &callbacks ); - * radio.Init( ); - * radio.SetInterruptMode( ); // Optional. Driver default behavior - * - * while( true ) - * { - * // Do some applicative work - * } - * @endcode - * - * \see SX1280::SetPollingMode - */ - void SetInterruptMode(void); - - /*! - * \brief Initializes the radio registers to the recommended default values - */ - void SetRegistersDefault(void); - - /*! - * \brief Returns the current device firmware version - * - * \retval version Firmware version - */ - virtual uint16_t GetFirmwareVersion(void); - - /*! - * \brief Resets the radio - */ - virtual void Reset(void) = 0; - - /*! - * \brief Wake-ups the radio from Sleep mode - */ - virtual void Wakeup(void) = 0; - - /*! - * \brief Writes the given command to the radio - * - * \param [in] opcode Command opcode - * \param [in] buffer Command parameters byte array - * \param [in] size Command parameters byte array size - */ - virtual void WriteCommand(RadioCommands_t opcode, uint8_t* buffer, uint16_t size) = 0; - - /*! - * \brief Reads the given command from the radio - * - * \param [in] opcode Command opcode - * \param [in] buffer Command parameters byte array - * \param [in] size Command parameters byte array size - */ - virtual void ReadCommand(RadioCommands_t opcode, uint8_t* buffer, uint16_t size) = 0; - - /*! - * \brief Writes multiple radio registers starting at address - * - * \param [in] address First Radio register address - * \param [in] buffer Buffer containing the new register's values - * \param [in] size Number of registers to be written - */ - virtual void WriteRegister(uint16_t address, uint8_t* buffer, uint16_t size) = 0; - - /*! - * \brief Writes the radio register at the specified address - * - * \param [in] address Register address - * \param [in] value New register value - */ - virtual void WriteRegister(uint16_t address, uint8_t value) = 0; - - /*! - * \brief Reads multiple radio registers starting at address - * - * \param [in] address First Radio register address - * \param [out] buffer Buffer where to copy the registers data - * \param [in] size Number of registers to be read - */ - virtual void ReadRegister(uint16_t address, uint8_t* buffer, uint16_t size) = 0; - - /*! - * \brief Reads the radio register at the specified address - * - * \param [in] address Register address - * - * \retval data Register value - */ - virtual uint8_t ReadRegister(uint16_t address) = 0; - - /*! - * \brief Writes Radio Data Buffer with buffer of size starting at offset. - * - * \param [in] offset Offset where to start writing - * \param [in] buffer Buffer pointer - * \param [in] size Buffer size - */ - virtual void WriteBuffer(uint8_t offset, uint8_t* buffer, uint8_t size) = 0; - - /*! - * \brief Reads Radio Data Buffer at offset to buffer of size - * - * \param [in] offset Offset where to start reading - * \param [out] buffer Buffer pointer - * \param [in] size Buffer size - */ - virtual void ReadBuffer(uint8_t offset, uint8_t* buffer, uint8_t size) = 0; - - /*! - * \brief Gets the current status of the radio DIOs - * - * \retval status [Bit #3: DIO3, Bit #2: DIO2, - * Bit #1: DIO1, Bit #0: BUSY] - */ - virtual uint8_t GetDioStatus(void) = 0; - - /*! - * \brief Gets the current Operation Mode of the Radio - * - * \retval opMode Last operating mode - */ - virtual RadioOperatingModes_t GetOpMode(void); - - /*! - * \brief Gets the current radio status - * - * \retval status Radio status - */ - virtual RadioStatus_t GetStatus(void); - - /*! - * \brief Sets the radio in sleep mode - * - * \param [in] sleepConfig The sleep configuration describing data - * retention and RTC wake-up - */ - void SetSleep(SleepParams_t sleepConfig); - - /*! - * \brief Sets the radio in configuration mode - * - * \param [in] mode The standby mode to put the radio into - */ - void SetStandby(RadioStandbyModes_t mode); - - /*! - * \brief Sets the radio in FS mode - */ - void SetFs(void); - - /*! - * \brief Sets the radio in transmission mode - * - * \param [in] timeout Structure describing the transmission timeout value - */ - void SetTx(TickTime_t timeout); - - /*! - * \brief Sets the radio in reception mode - * - * \param [in] timeout Structure describing the reception timeout value - */ - void SetRx(TickTime_t timeout); - - /*! - * \brief Sets the Rx duty cycle management parameters - * - * \param [in] periodBase Base time for Rx and Sleep TickTime sequences - * \param [in] periodBaseCountRx Number of base time for Rx TickTime sequence - * \param [in] periodBaseCountSleep Number of base time for Sleep TickTime sequence - */ - void SetRxDutyCycle(RadioTickSizes_t periodBase, uint16_t periodBaseCountRx, uint16_t periodBaseCountSleep); - - /*! - * \brief Sets the radio in CAD mode - * - * \see SX1280::SetCadParams - */ - void SetCad(void); - - /*! - * \brief Sets the radio in continuous wave transmission mode - */ - void SetTxContinuousWave(void); - - /*! - * \brief Sets the radio in continuous preamble transmission mode - */ - void SetTxContinuousPreamble(void); - - /*! - * \brief Sets the radio for the given protocol - * - * \param [in] packetType [PACKET_TYPE_GFSK, PACKET_TYPE_LORA, - * PACKET_TYPE_RANGING, PACKET_TYPE_FLRC, - * PACKET_TYPE_BLE] - * - * \remark This method has to be called before SetRfFrequency, - * SetModulationParams and SetPacketParams - */ - void SetPacketType(RadioPacketTypes_t packetType); - - /*! - * \brief Gets the current radio protocol - * - * Default behavior return the packet type returned by the radio. To - * reduce the SPI activity and return the packet type stored by the - * driver, a second optional argument must be provided evaluating as true - * boolean - * - * \param [in] returnLocalCopy If true, the packet type returned is the last - * saved in the driver - * If false, the packet type is obtained from - * the chip - * Default: false - * - * \retval packetType [PACKET_TYPE_GENERIC, PACKET_TYPE_LORA, - * PACKET_TYPE_RANGING, PACKET_TYPE_FLRC, - * PACKET_TYPE_BLE, PACKET_TYPE_NONE] - */ - RadioPacketTypes_t GetPacketType(bool returnLocalCopy = false); - - /*! - * \brief Sets the RF frequency - * - * \param [in] rfFrequency RF frequency [Hz] - */ - void SetRfFrequency(uint32_t rfFrequency); - - /*! - * \brief Sets the transmission parameters - * - * \param [in] power RF output power [-18..13] dBm - * \param [in] rampTime Transmission ramp up time - */ - void SetTxParams(int8_t power, RadioRampTimes_t rampTime); - - /*! - * \brief Sets the number of symbols to be used for Channel Activity - * Detection operation - * - * \param [in] cadSymbolNum The number of symbol to use for Channel Activity - * Detection operations [LORA_CAD_01_SYMBOL, LORA_CAD_02_SYMBOLS, - * LORA_CAD_04_SYMBOLS, LORA_CAD_08_SYMBOLS, LORA_CAD_16_SYMBOLS] - */ - void SetCadParams(RadioLoRaCadSymbols_t cadSymbolNum); - - /*! - * \brief Sets the data buffer base address for transmission and reception - * - * \param [in] txBaseAddress Transmission base address - * \param [in] rxBaseAddress Reception base address - */ - void SetBufferBaseAddresses(uint8_t txBaseAddress, uint8_t rxBaseAddress); - - /*! - * \brief Set the modulation parameters - * - * \param [in] modParams A structure describing the modulation parameters - */ - void SetModulationParams(ModulationParams_t* modParams); - - /*! - * \brief Sets the packet parameters - * - * \param [in] packetParams A structure describing the packet parameters - */ - void SetPacketParams(PacketParams_t* packetParams); - - /*! - * \brief Gets the last received packet buffer status - * - * \param [out] rxPayloadLength Last received packet payload length - * \param [out] rxStartBufferPointer Last received packet buffer address pointer - */ - void GetRxBufferStatus(uint8_t* rxPayloadLength, uint8_t* rxStartBufferPointer); - - /*! - * \brief Gets the last received packet status - * - * The packet status structure returned depends on the modem type selected - * - * \see PacketStatus_t for the description of available information - * - * \param [out] packetStatus A structure of packet status - */ - void GetPacketStatus(PacketStatus_t* packetStatus); - - /*! - * \brief Returns the instantaneous RSSI value for the last packet received - * - * \retval rssiInst Instantaneous RSSI - */ - int8_t GetRssiInst(void); - - /*! - * \brief Sets the IRQ mask and DIO masks - * - * \param [in] irqMask General IRQ mask - * \param [in] dio1Mask DIO1 mask - * \param [in] dio2Mask DIO2 mask - * \param [in] dio3Mask DIO3 mask - */ - void SetDioIrqParams(uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask); - - /*! - * \brief Returns the current IRQ status - * - * \retval irqStatus IRQ status - */ - uint16_t GetIrqStatus(void); - - /*! - * \brief Clears the IRQs - * - * \param [in] irqMask IRQ(s) to be cleared - */ - void ClearIrqStatus(uint16_t irqMask); - - /*! - * \brief Calibrates the given radio block - * - * \param [in] calibParam The description of blocks to be calibrated - */ - void Calibrate(CalibrationParams_t calibParam); - - /*! - * \brief Sets the power regulators operating mode - * - * \param [in] mode [0: LDO, 1:DC_DC] - */ - void SetRegulatorMode(RadioRegulatorModes_t mode); - - /*! - * \brief Saves the current selected modem configuration into data RAM - */ - void SetSaveContext(void); - - /*! - * \brief Sets the chip to automatically send a packet after the end of a packet reception - * - * \remark The offset is automatically compensated inside the function - * - * \param [in] time The delay in us after which a Tx is done - */ - void SetAutoTx(uint16_t time); - - /*! - * \brief Stop the chip to automatically send a packet after the end of a packet reception - * if previously activated with SX1280::SetAutoTx command - */ - void StopAutoTx(void); - - /*! - * \brief Sets the chip to stay in FS mode after sending a packet - * - * \param [in] enableAutoFs Turn on auto FS - */ - void SetAutoFs(bool enableAutoFs); - - /*! - * \brief Enables or disables long preamble detection mode - * - * \param [in] enable Turn on long preamble mode - */ - void SetLongPreamble(bool enable); - - /*! - * \brief Saves the payload to be send in the radio buffer - * - * \param [in] payload A pointer to the payload - * \param [in] size The size of the payload - * \param [in] offset The address in FIFO where writing first byte (default = 0x00) - */ - void SetPayload(uint8_t* payload, uint8_t size, uint8_t offset = 0x00); - - /*! - * \brief Reads the payload received. If the received payload is longer - * than maxSize, then the method returns 1 and do not set size and payload. - * - * \param [out] payload A pointer to a buffer into which the payload will be copied - * \param [out] size A pointer to the size of the payload received - * \param [in] maxSize The maximal size allowed to copy into the buffer - */ - uint8_t GetPayload(uint8_t* payload, uint8_t* size, uint8_t maxSize); - - /*! - * \brief Sends a payload - * - * \param [in] payload A pointer to the payload to send - * \param [in] size The size of the payload to send - * \param [in] timeout The timeout for Tx operation - * \param [in] offset The address in FIFO where writing first byte (default = 0x00) - */ - void SendPayload(uint8_t* payload, uint8_t size, TickTime_t timeout, uint8_t offset = 0x00); - - /*! - * \brief Sets the Sync Word given by index used in GFSK, FLRC and BLE protocols - * - * \remark 5th byte isn't used in FLRC and BLE protocols - * - * \param [in] syncWordIdx Index of SyncWord to be set [1..3] - * \param [in] syncWord SyncWord bytes ( 5 bytes ) - * - * \retval status [0: OK, 1: NOK] - */ - uint8_t SetSyncWord(uint8_t syncWordIdx, uint8_t* syncWord); - - /*! - * \brief Defines how many error bits are tolerated in sync word detection - * - * \param [in] errorBits Number of error bits supported to validate the Sync word detection - * ( default is 4 bit, minimum is 1 bit ) - */ - void SetSyncWordErrorTolerance(uint8_t errorBits); - - /*! - * \brief Sets the Initial value for the LFSR used for the CRC calculation - * - * \param [in] seed Initial LFSR value ( 4 bytes ) - * - * \retval updated [0: failure, 1: success] - * - */ - uint8_t SetCrcSeed(uint8_t* seed); - - /*! - * \brief Set the Access Address field of BLE packet - * - * \param [in] accessAddress The access address to be used for next BLE packet sent - * - * \see SX1280::SetBleAdvertizerAccessAddress - */ - void SetBleAccessAddress(uint32_t accessAddress); - - /*! - * \brief Set the Access Address for Advertizer BLE packets - * - * All advertizer BLE packets must use a particular value for Access - * Address field. This method sets it. - * - * \see SX1280::SetBleAccessAddress - */ - void SetBleAdvertizerAccessAddress(void); - - /*! - * \brief Sets the seed used for the CRC calculation - * - * \param [in] polynomial The seed value - * - */ - void SetCrcPolynomial(uint16_t polynomial); - - /*! - * \brief Sets the Initial value of the LFSR used for the whitening in GFSK, FLRC and BLE protocols - * - * \param [in] seed Initial LFSR value - */ - void SetWhiteningSeed(uint8_t seed); - - /*! - * \brief Enable manual gain control and disable AGC - * - * \see SX1280::SetManualGainValue, SX1280::DisableManualGain - */ - void EnableManualGain(void); - - /*! - * \brief Disable the manual gain control and enable AGC - * - * \see SX1280::EnableManualGain - */ - void DisableManualGain(void); - - /*! - * \brief Set the gain for the AGC - * - * SX1280::EnableManualGain must be called before using this method - * - * \param [in] gain The value of gain to set, refer to datasheet for value meaning - * - * \see SX1280::EnableManualGain, SX1280::DisableManualGain - */ - void SetManualGainValue(uint8_t gain); - - /*! - * \brief Configure the LNA regime of operation - * - * \param [in] lnaSetting The LNA setting. Possible values are - * LNA_LOW_POWER_MODE and - * LNA_HIGH_SENSITIVITY_MODE - */ - void SetLNAGainSetting(const RadioLnaSettings_t lnaSetting); - - /*! - * \brief Sets the number of bits used to check that ranging request match ranging ID - * - * \param [in] length [0: 8 bits, 1: 16 bits, - * 2: 24 bits, 3: 32 bits] - */ - void SetRangingIdLength(RadioRangingIdCheckLengths_t length); - - /*! - * \brief Sets ranging device id - * - * \param [in] address Device address - */ - void SetDeviceRangingAddress(uint32_t address); - - /*! - * \brief Sets the device id to ping in a ranging request - * - * \param [in] address Address of the device to ping - */ - void SetRangingRequestAddress(uint32_t address); - - /*! - * \brief Return the ranging result value - * - * \param [in] resultType Specifies the type of result. - * [0: RAW, 1: Averaged, - * 2: De-biased, 3:Filtered] - * - * \retval ranging The ranging measure filtered according to resultType [m] - */ - double GetRangingResult(RadioRangingResultTypes_t resultType); - - /*! - * \brief Return the last ranging result power indicator - * - * The value returned is not an absolute power measurement. It is - * a relative power measurement. - * - * \retval deltaThreshold A relative power indicator - */ - uint8_t GetRangingPowerDeltaThresholdIndicator(void); - - /*! - * \brief Sets the standard processing delay between Master and Slave - * - * \param [in] cal RxTx delay offset for correcting ranging bias. - * - * The calibration value reflects the group delay of the radio front end and - * must be re-performed for each new SX1280 PCB design. The value is obtained - * empirically by either conducted measurement in a known electrical length - * coaxial RF cable (where the design is connectorised) or by radiated - * measurement, at a known distance, where an antenna is present. - * The result of the calibration process is that the SX1280 ranging result - * accurately reflects the physical range, the calibration procedure therefore - * removes the average timing error from the time-of-flight measurement for a - * given design. - * - * The values are Spreading Factor dependents, and depend also of the board - * design. - */ - void SetRangingCalibration(uint16_t cal); - - /*! - * \brief Clears the ranging filter - */ - void RangingClearFilterResult(void); - - /*! - * \brief Set the number of samples considered in the built-in filter - * - * \param [in] numSample The number of samples to use built-in filter - * [8..255] - * - * \remark Value inferior to 8 will be silently set to 8 - */ - void RangingSetFilterNumSamples(uint8_t numSample); - - /*! - * \brief Return the Estimated Frequency Error in LORA and RANGING operations - * - * \retval efe The estimated frequency error [Hz] - */ - double GetFrequencyError(); - - /*! - * \brief Process the analysis of radio IRQs and calls callback functions - * depending on radio state - */ - void ProcessIrqs(void); - - /*! - * \brief Force the preamble length in GFSK and BLE mode - * - * \param [in] preambleLength The desired preamble length - */ - void ForcePreambleLength(RadioPreambleLengths_t preambleLength); -}; - -#endif // __SX1280_H__ From 0074422aee63e407c001e6f21023dca121e58fa2 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 19:05:48 -0800 Subject: [PATCH 030/101] import some Radiolib files --- .../Components/MyComponent/FprimeHal.cpp | 49 + .../Components/MyComponent/FprimeHal.h | 142 ++ .../Components/MyComponent/Hal.cpp | 44 + .../Components/MyComponent/Hal.h | 219 ++ .../Components/MyComponent/SX1281.cpp | 8 + .../Components/MyComponent/SX1281.h | 33 + .../Components/MyComponent/SX128x.cpp | 1848 +++++++++++++++++ .../Components/MyComponent/SX128x.h | 1045 ++++++++++ 8 files changed, 3388 insertions(+) create mode 100644 FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp create mode 100644 FprimeZephyrReference/Components/MyComponent/FprimeHal.h create mode 100644 FprimeZephyrReference/Components/MyComponent/Hal.cpp create mode 100644 FprimeZephyrReference/Components/MyComponent/Hal.h create mode 100644 FprimeZephyrReference/Components/MyComponent/SX1281.cpp create mode 100644 FprimeZephyrReference/Components/MyComponent/SX1281.h create mode 100644 FprimeZephyrReference/Components/MyComponent/SX128x.cpp create mode 100644 FprimeZephyrReference/Components/MyComponent/SX128x.h diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp new file mode 100644 index 00000000..7b23f9e4 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp @@ -0,0 +1,49 @@ +#include "PicoHal.h" + +#if defined(RADIOLIB_BUILD_RPI_PICO) + +// pre-calculated pulse-widths for 1200 and 2200Hz +// we do this to save calculation time (see https://github.com/khoih-prog/RP2040_PWM/issues/6) +#define SLEEP_1200 416.666 +#define SLEEP_2200 227.272 + +static uint32_t toneLoopPin; +static unsigned int toneLoopFrequency; +static unsigned long toneLoopDuration; + +// === NOTE === +// The tone(...) implementation uses the second core on the RPi Pico. This is to diminish as much +// jitter in the output tones as possible. + +static void toneLoop() { + gpio_set_dir(toneLoopPin, GPIO_OUT); + + uint32_t sleep_dur; + if (toneLoopFrequency == 1200) { + sleep_dur = SLEEP_1200; + } else if (toneLoopFrequency == 2200) { + sleep_dur = SLEEP_2200; + } else { + sleep_dur = 500000 / toneLoopFrequency; + } + + // tone bitbang + while (1) { + gpio_put(toneLoopPin, 1); + sleep_us(sleep_dur); + gpio_put(toneLoopPin, 0); + sleep_us(sleep_dur); + tight_loop_contents(); + } +} + +void PicoHal::tone(uint32_t pin, unsigned int frequency, unsigned long duration) { + // tones on the Pico are generated using bitbanging. This process is offloaded to the Pico's second core + multicore_reset_core1(); + toneLoopPin = pin; + toneLoopFrequency = frequency; + toneLoopDuration = duration; + multicore_launch_core1(toneLoop); +} + +#endif diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.h b/FprimeZephyrReference/Components/MyComponent/FprimeHal.h new file mode 100644 index 00000000..c25a1716 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.h @@ -0,0 +1,142 @@ +#ifndef PICO_HAL_H +#define PICO_HAL_H + +#if defined(RADIOLIB_BUILD_RPI_PICO) + +// include RadioLib +#include + +// include the necessary Pico libraries +#include + +#include "hardware/clocks.h" +#include "hardware/pwm.h" +#include "hardware/spi.h" +#include "hardware/timer.h" +#include "pico/multicore.h" + +// create a new Raspberry Pi Pico hardware abstraction +// layer using the Pico SDK +// the HAL must inherit from the base RadioLibHal class +// and implement all of its virtual methods +class PicoHal : public RadioLibHal { + public: + PicoHal(spi_inst_t* spiChannel, uint32_t misoPin, uint32_t mosiPin, uint32_t sckPin, uint32_t spiSpeed = 500 * 1000) + : RadioLibHal(GPIO_IN, GPIO_OUT, 0, 1, GPIO_IRQ_EDGE_RISE, GPIO_IRQ_EDGE_FALL), + _spiChannel(spiChannel), + _spiSpeed(spiSpeed), + _misoPin(misoPin), + _mosiPin(mosiPin), + _sckPin(sckPin) {} + + void init() override { + stdio_init_all(); + spiBegin(); + } + + void term() override { spiEnd(); } + + // GPIO-related methods (pinMode, digitalWrite etc.) should check + // RADIOLIB_NC as an alias for non-connected pins + void pinMode(uint32_t pin, uint32_t mode) override { + if (pin == RADIOLIB_NC) { + return; + } + + gpio_init(pin); + gpio_set_dir(pin, mode); + } + + void digitalWrite(uint32_t pin, uint32_t value) override { + if (pin == RADIOLIB_NC) { + return; + } + + gpio_put(pin, (bool)value); + } + + uint32_t digitalRead(uint32_t pin) override { + if (pin == RADIOLIB_NC) { + return 0; + } + + return gpio_get(pin); + } + + void attachInterrupt(uint32_t interruptNum, void (*interruptCb)(void), uint32_t mode) override { + if (interruptNum == RADIOLIB_NC) { + return; + } + + gpio_set_irq_enabled_with_callback(interruptNum, mode, true, (gpio_irq_callback_t)interruptCb); + } + + void detachInterrupt(uint32_t interruptNum) override { + if (interruptNum == RADIOLIB_NC) { + return; + } + + gpio_set_irq_enabled_with_callback(interruptNum, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, false, NULL); + } + + void delay(unsigned long ms) override { sleep_ms(ms); } + + void delayMicroseconds(unsigned long us) override { sleep_us(us); } + + unsigned long millis() override { return to_ms_since_boot(get_absolute_time()); } + + unsigned long micros() override { return to_us_since_boot(get_absolute_time()); } + + long pulseIn(uint32_t pin, uint32_t state, unsigned long timeout) override { + if (pin == RADIOLIB_NC) { + return 0; + } + + this->pinMode(pin, GPIO_IN); + uint32_t start = this->micros(); + uint32_t curtick = this->micros(); + + while (this->digitalRead(pin) == state) { + if ((this->micros() - curtick) > timeout) { + return 0; + } + } + + return (this->micros() - start); + } + + void tone(uint32_t pin, unsigned int frequency, unsigned long duration = 0) override; + + void noTone(uint32_t pin) override { multicore_reset_core1(); } + + void spiBegin() { + spi_init(_spiChannel, _spiSpeed); + spi_set_format(_spiChannel, 8, SPI_CPOL_0, SPI_CPHA_0, SPI_MSB_FIRST); + + gpio_set_function(_sckPin, GPIO_FUNC_SPI); + gpio_set_function(_mosiPin, GPIO_FUNC_SPI); + gpio_set_function(_misoPin, GPIO_FUNC_SPI); + } + + void spiBeginTransaction() {} + + void spiTransfer(uint8_t* out, size_t len, uint8_t* in) { spi_write_read_blocking(_spiChannel, out, in, len); } + + void yield() override { tight_loop_contents(); } + + void spiEndTransaction() {} + + void spiEnd() { spi_deinit(_spiChannel); } + + private: + // the HAL can contain any additional private members + spi_inst_t* _spiChannel; + uint32_t _spiSpeed; + uint32_t _misoPin; + uint32_t _mosiPin; + uint32_t _sckPin; +}; + +#endif + +#endif diff --git a/FprimeZephyrReference/Components/MyComponent/Hal.cpp b/FprimeZephyrReference/Components/MyComponent/Hal.cpp new file mode 100644 index 00000000..54e438b7 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/Hal.cpp @@ -0,0 +1,44 @@ +#include "Hal.h" + +static RadioLibHal* rlb_timestamp_hal = nullptr; + +RadioLibHal::RadioLibHal(const uint32_t input, + const uint32_t output, + const uint32_t low, + const uint32_t high, + const uint32_t rising, + const uint32_t falling) + : GpioModeInput(input), + GpioModeOutput(output), + GpioLevelLow(low), + GpioLevelHigh(high), + GpioInterruptRising(rising), + GpioInterruptFalling(falling) { + if (!rlb_timestamp_hal) { + rlb_timestamp_hal = this; + } +} + +void RadioLibHal::init() {} + +void RadioLibHal::term() {} + +void RadioLibHal::tone(uint32_t pin, unsigned int frequency, RadioLibTime_t duration) { + (void)pin; + (void)frequency; + (void)duration; +} + +void RadioLibHal::noTone(uint32_t pin) { + (void)pin; +} + +void RadioLibHal::yield() {} + +uint32_t RadioLibHal::pinToInterrupt(uint32_t pin) { + return (pin); +} + +RadioLibTime_t rlb_time_us() { + return (rlb_timestamp_hal == nullptr ? 0 : rlb_timestamp_hal->micros()); +} diff --git a/FprimeZephyrReference/Components/MyComponent/Hal.h b/FprimeZephyrReference/Components/MyComponent/Hal.h new file mode 100644 index 00000000..bebeaf49 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/Hal.h @@ -0,0 +1,219 @@ +#if !defined(_RADIOLIB_HAL_H) +#define _RADIOLIB_HAL_H + +#include +#include + +#include "BuildOpt.h" + +/*! \brief Global-scope function that returns timestamp since start (in microseconds). */ +RadioLibTime_t rlb_time_us(); + +/*! + \class RadioLibHal + \brief Hardware abstraction library base interface. +*/ +class RadioLibHal { + public: + // values for pin modes, levels and change directions + // these tell RadioLib how are different logic states represented on a given platform + + /*! + \brief Value to be used as the "input" GPIO direction. + */ + const uint32_t GpioModeInput; + + /*! + \brief Value to be used as the "output" GPIO direction. + */ + const uint32_t GpioModeOutput; + + /*! + \brief Value to be used as the "low" GPIO level. + */ + const uint32_t GpioLevelLow; + + /*! + \brief Value to be used as the "high" GPIO level. + */ + const uint32_t GpioLevelHigh; + + /*! + \brief Value to be used as the "rising" GPIO level change direction. + */ + const uint32_t GpioInterruptRising; + + /*! + \brief Value to be used as the "falling" GPIO level change direction. + */ + const uint32_t GpioInterruptFalling; + + /*! + \brief Default constructor. + \param input Value to be used as the "input" GPIO direction. + \param output Value to be used as the "output" GPIO direction. + \param low Value to be used as the "low" GPIO level. + \param high Value to be used as the "high" GPIO level. + \param rising Value to be used as the "rising" GPIO level change direction. + \param falling Value to be used as the "falling" GPIO level change direction. + */ + RadioLibHal(const uint32_t input, + const uint32_t output, + const uint32_t low, + const uint32_t high, + const uint32_t rising, + const uint32_t falling); + + // pure virtual methods - these must be implemented by the hardware abstraction for RadioLib to function + + /*! + \brief GPIO pin mode (input/output/...) configuration method. + Must be implemented by the platform-specific hardware abstraction! + \param pin Pin to be changed (platform-specific). + \param mode Mode to be set (platform-specific). + */ + virtual void pinMode(uint32_t pin, uint32_t mode) = 0; + + /*! + \brief Digital write method. + Must be implemented by the platform-specific hardware abstraction! + \param pin Pin to be changed (platform-specific). + \param value Value to set (platform-specific). + */ + virtual void digitalWrite(uint32_t pin, uint32_t value) = 0; + + /*! + \brief Digital read method. + Must be implemented by the platform-specific hardware abstraction! + \param pin Pin to be changed (platform-specific). + \returns Value read on the pin (platform-specific). + */ + virtual uint32_t digitalRead(uint32_t pin) = 0; + + /*! + \brief Method to attach function to an external interrupt. + Must be implemented by the platform-specific hardware abstraction! + \param interruptNum Interrupt number to attach to (platform-specific). + \param interruptCb Interrupt service routine to execute. + \param mode Rising/falling mode (platform-specific). + */ + virtual void attachInterrupt(uint32_t interruptNum, void (*interruptCb)(void), uint32_t mode) = 0; + + /*! + \brief Method to detach function from an external interrupt. + Must be implemented by the platform-specific hardware abstraction! + \param interruptNum Interrupt number to detach from (platform-specific). + */ + virtual void detachInterrupt(uint32_t interruptNum) = 0; + + /*! + \brief Blocking wait function. + Must be implemented by the platform-specific hardware abstraction! + \param ms Number of milliseconds to wait. + */ + virtual void delay(RadioLibTime_t ms) = 0; + + /*! + \brief Blocking microsecond wait function. + Must be implemented by the platform-specific hardware abstraction! + \param us Number of microseconds to wait. + */ + virtual void delayMicroseconds(RadioLibTime_t us) = 0; + + /*! + \brief Get number of milliseconds since start. + Must be implemented by the platform-specific hardware abstraction! + \returns Number of milliseconds since start. + */ + virtual RadioLibTime_t millis() = 0; + + /*! + \brief Get number of microseconds since start. + Must be implemented by the platform-specific hardware abstraction! + \returns Number of microseconds since start. + */ + virtual RadioLibTime_t micros() = 0; + + /*! + \brief Measure the length of incoming digital pulse in microseconds. + Must be implemented by the platform-specific hardware abstraction! + \param pin Pin to measure on (platform-specific). + \param state Pin level to monitor (platform-specific). + \param timeout Timeout in microseconds. + \returns Pulse length in microseconds, or 0 if the pulse did not start before timeout. + */ + virtual long pulseIn(uint32_t pin, uint32_t state, RadioLibTime_t timeout) = 0; + + /*! + \brief SPI initialization method. + */ + virtual void spiBegin() = 0; + + /*! + \brief Method to start SPI transaction. + */ + virtual void spiBeginTransaction() = 0; + + /*! + \brief Method to transfer buffer over SPI. + \param out Buffer to send. + \param len Number of data to send or receive. + \param in Buffer to save received data into. + */ + virtual void spiTransfer(uint8_t* out, size_t len, uint8_t* in) = 0; + + /*! + \brief Method to end SPI transaction. + */ + virtual void spiEndTransaction() = 0; + + /*! + \brief SPI termination method. + */ + virtual void spiEnd() = 0; + + // virtual methods - these may or may not exists on a given platform + // they exist in this implementation, but do nothing + + /*! + \brief Module initialization method. + This will be called by all radio modules at the beginning of startup. + Can be used to e.g., initialize SPI interface. + */ + virtual void init(); + + /*! + \brief Module termination method. + This will be called by all radio modules when the destructor is called. + Can be used to e.g., stop SPI interface. + */ + virtual void term(); + + /*! + \brief Method to produce a square-wave with 50% duty cycle ("tone") of a given frequency at some pin. + \param pin Pin to be used as the output. + \param frequency Frequency of the square wave. + \param duration Duration of the tone in ms. When set to 0, the tone will be infinite. + */ + virtual void tone(uint32_t pin, unsigned int frequency, RadioLibTime_t duration = 0); + + /*! + \brief Method to stop producing a tone. + \param pin Pin which is currently producing the tone. + */ + virtual void noTone(uint32_t pin); + + /*! + \brief Yield method, called from long loops in multi-threaded environment (to prevent blocking other threads). + */ + virtual void yield(); + + /*! + \brief Function to convert from pin number to interrupt number. + \param pin Pin to convert from. + \returns The interrupt number of a given pin. + */ + virtual uint32_t pinToInterrupt(uint32_t pin); +}; + +#endif diff --git a/FprimeZephyrReference/Components/MyComponent/SX1281.cpp b/FprimeZephyrReference/Components/MyComponent/SX1281.cpp new file mode 100644 index 00000000..780ad9cd --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/SX1281.cpp @@ -0,0 +1,8 @@ +#include "SX1281.h" +#if !RADIOLIB_EXCLUDE_SX128X + +SX1281::SX1281(Module* mod) : SX128x(mod) { + chipType = RADIOLIB_SX1281_CHIP_TYPE; +} + +#endif diff --git a/FprimeZephyrReference/Components/MyComponent/SX1281.h b/FprimeZephyrReference/Components/MyComponent/SX1281.h new file mode 100644 index 00000000..dbbd4f63 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/SX1281.h @@ -0,0 +1,33 @@ +#if !defined(_RADIOLIB_SX1281_H) +#define _RADIOLIB_SX1281_H + +#include "../../TypeDef.h" + +#if !RADIOLIB_EXCLUDE_SX128X + +#include "../../Module.h" +#include "SX128x.h" + +// RADIOLIB_SX128X_REG_VERSION_STRING +#define RADIOLIB_SX1281_CHIP_TYPE "SX1281" + +/*! + \class SX1281 + \brief Derived class for %SX1281 modules. +*/ +class SX1281 : public SX128x { + public: + /*! + \brief Default constructor. + \param mod Instance of Module that will be used to communicate with the radio. + */ + explicit SX1281(Module* mod); + +#if !RADIOLIB_GODMODE + private: +#endif +}; + +#endif + +#endif diff --git a/FprimeZephyrReference/Components/MyComponent/SX128x.cpp b/FprimeZephyrReference/Components/MyComponent/SX128x.cpp new file mode 100644 index 00000000..bfe12e65 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/SX128x.cpp @@ -0,0 +1,1848 @@ +#include "SX128x.h" + +#include +#include +#if !RADIOLIB_EXCLUDE_SX128X + +SX128x::SX128x(Module* mod) : PhysicalLayer() { + this->freqStep = RADIOLIB_SX128X_FREQUENCY_STEP_SIZE; + this->maxPacketLength = RADIOLIB_SX128X_MAX_PACKET_LENGTH; + this->mod = mod; + this->irqMap[RADIOLIB_IRQ_TX_DONE] = RADIOLIB_SX128X_IRQ_TX_DONE; + this->irqMap[RADIOLIB_IRQ_RX_DONE] = RADIOLIB_SX128X_IRQ_RX_DONE; + this->irqMap[RADIOLIB_IRQ_PREAMBLE_DETECTED] = RADIOLIB_SX128X_IRQ_PREAMBLE_DETECTED; + this->irqMap[RADIOLIB_IRQ_SYNC_WORD_VALID] = RADIOLIB_SX128X_IRQ_SYNC_WORD_VALID; + this->irqMap[RADIOLIB_IRQ_HEADER_VALID] = RADIOLIB_SX128X_IRQ_HEADER_VALID; + this->irqMap[RADIOLIB_IRQ_HEADER_ERR] = RADIOLIB_SX128X_IRQ_HEADER_ERROR; + this->irqMap[RADIOLIB_IRQ_CRC_ERR] = RADIOLIB_SX128X_IRQ_CRC_ERROR; + this->irqMap[RADIOLIB_IRQ_CAD_DONE] = RADIOLIB_SX128X_IRQ_CAD_DONE; + this->irqMap[RADIOLIB_IRQ_CAD_DETECTED] = RADIOLIB_SX128X_IRQ_CAD_DETECTED; + this->irqMap[RADIOLIB_IRQ_TIMEOUT] = RADIOLIB_SX128X_IRQ_RX_TX_TIMEOUT; +} + +int16_t +SX128x::begin(float freq, float bw, uint8_t sf, uint8_t cr, uint8_t syncWord, int8_t pwr, uint16_t preambleLength) { + // initialize LoRa modulation variables + this->bandwidthKhz = bw; + this->spreadingFactor = RADIOLIB_SX128X_LORA_SF_9; + this->codingRateLoRa = RADIOLIB_SX128X_LORA_CR_4_7; + + // initialize LoRa packet variables + this->preambleLengthLoRa = preambleLength; + this->headerType = RADIOLIB_SX128X_LORA_HEADER_EXPLICIT; + this->payloadLen = 0xFF; + this->crcLoRa = RADIOLIB_SX128X_LORA_CRC_ON; + + // set module properties and perform initial setup + int16_t state = this->modSetup(RADIOLIB_SX128X_PACKET_TYPE_LORA); + RADIOLIB_ASSERT(state); + + // configure publicly accessible settings + state = setFrequency(freq); + RADIOLIB_ASSERT(state); + + state = setBandwidth(bw); + RADIOLIB_ASSERT(state); + + state = setSpreadingFactor(sf); + RADIOLIB_ASSERT(state); + + state = setCodingRate(cr); + RADIOLIB_ASSERT(state); + + state = setSyncWord(syncWord); + RADIOLIB_ASSERT(state); + + state = setPreambleLength(preambleLength); + RADIOLIB_ASSERT(state); + + state = setOutputPower(pwr); + RADIOLIB_ASSERT(state); + + return (state); +} + +int16_t SX128x::beginGFSK(float freq, uint16_t br, float freqDev, int8_t pwr, uint16_t preambleLength) { + // initialize GFSK modulation variables + this->bitRateKbps = br; + this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_800_BW_2_4; + this->modIndexReal = 1.0; + this->modIndex = RADIOLIB_SX128X_BLE_GFSK_MOD_IND_1_00; + this->shaping = RADIOLIB_SX128X_BLE_GFSK_BT_0_5; + + // initialize GFSK packet variables + this->preambleLengthGFSK = preambleLength; + this->syncWordLen = 2; + this->syncWordMatch = RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1; + this->crcGFSK = RADIOLIB_SX128X_GFSK_FLRC_CRC_2_BYTE; + this->whitening = RADIOLIB_SX128X_GFSK_BLE_WHITENING_ON; + + // set module properties and perform initial setup + int16_t state = this->modSetup(RADIOLIB_SX128X_PACKET_TYPE_GFSK); + RADIOLIB_ASSERT(state); + + // configure publicly accessible settings + state = setFrequency(freq); + RADIOLIB_ASSERT(state); + + state = setBitRate(br); + RADIOLIB_ASSERT(state); + + state = setFrequencyDeviation(freqDev); + RADIOLIB_ASSERT(state); + + state = setOutputPower(pwr); + RADIOLIB_ASSERT(state); + + state = setPreambleLength(preambleLength); + RADIOLIB_ASSERT(state); + + state = setDataShaping(RADIOLIB_SHAPING_0_5); + RADIOLIB_ASSERT(state); + + // set publicly accessible settings that are not a part of begin method + uint8_t sync[] = {0x12, 0xAD}; + state = setSyncWord(sync, 2); + RADIOLIB_ASSERT(state); + + state = setEncoding(RADIOLIB_ENCODING_NRZ); + RADIOLIB_ASSERT(state); + + return (state); +} + +int16_t SX128x::beginBLE(float freq, uint16_t br, float freqDev, int8_t pwr, uint8_t dataShaping) { + // initialize BLE modulation variables + this->bitRateKbps = br; + this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_800_BW_2_4; + this->modIndexReal = 1.0; + this->modIndex = RADIOLIB_SX128X_BLE_GFSK_MOD_IND_1_00; + this->shaping = RADIOLIB_SX128X_BLE_GFSK_BT_0_5; + + // initialize BLE packet variables + this->crcGFSK = RADIOLIB_SX128X_BLE_CRC_3_BYTE; + this->whitening = RADIOLIB_SX128X_GFSK_BLE_WHITENING_ON; + + // set module properties and perform initial setup + int16_t state = this->modSetup(RADIOLIB_SX128X_PACKET_TYPE_BLE); + RADIOLIB_ASSERT(state); + + // configure publicly accessible settings + state = setFrequency(freq); + RADIOLIB_ASSERT(state); + + state = setBitRate(br); + RADIOLIB_ASSERT(state); + + state = setFrequencyDeviation(freqDev); + RADIOLIB_ASSERT(state); + + state = setOutputPower(pwr); + RADIOLIB_ASSERT(state); + + state = setDataShaping(dataShaping); + RADIOLIB_ASSERT(state); + + return (state); +} + +int16_t SX128x::beginFLRC(float freq, + uint16_t br, + uint8_t cr, + int8_t pwr, + uint16_t preambleLength, + uint8_t dataShaping) { + // initialize FLRC modulation variables + this->bitRateKbps = br; + this->bitRate = RADIOLIB_SX128X_FLRC_BR_0_650_BW_0_6; + this->codingRateFLRC = RADIOLIB_SX128X_FLRC_CR_3_4; + this->shaping = RADIOLIB_SX128X_FLRC_BT_0_5; + + // initialize FLRC packet variables + this->preambleLengthGFSK = preambleLength; + this->syncWordLen = 2; + this->syncWordMatch = RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1; + this->crcGFSK = RADIOLIB_SX128X_GFSK_FLRC_CRC_2_BYTE; + this->whitening = RADIOLIB_SX128X_GFSK_BLE_WHITENING_OFF; + + // set module properties and perform initial setup + int16_t state = this->modSetup(RADIOLIB_SX128X_PACKET_TYPE_FLRC); + RADIOLIB_ASSERT(state); + + // configure publicly accessible settings + state = setFrequency(freq); + RADIOLIB_ASSERT(state); + + state = setBitRate(br); + RADIOLIB_ASSERT(state); + + state = setCodingRate(cr); + RADIOLIB_ASSERT(state); + + state = setOutputPower(pwr); + RADIOLIB_ASSERT(state); + + state = setPreambleLength(preambleLength); + RADIOLIB_ASSERT(state); + + state = setDataShaping(dataShaping); + RADIOLIB_ASSERT(state); + + // set publicly accessible settings that are not a part of begin method + uint8_t sync[] = {0x2D, 0x01, 0x4B, 0x1D}; + state = setSyncWord(sync, 4); + RADIOLIB_ASSERT(state); + + return (state); +} + +int16_t SX128x::reset(bool verify) { + // run the reset sequence - same as SX126x, as SX128x docs don't seem to mention this + this->mod->hal->pinMode(this->mod->getRst(), this->mod->hal->GpioModeOutput); + this->mod->hal->digitalWrite(this->mod->getRst(), this->mod->hal->GpioLevelLow); + this->mod->hal->delay(1); + this->mod->hal->digitalWrite(this->mod->getRst(), this->mod->hal->GpioLevelHigh); + + // return immediately when verification is disabled + if (!verify) { + return (RADIOLIB_ERR_NONE); + } + + // set mode to standby + RadioLibTime_t start = this->mod->hal->millis(); + while (true) { + // try to set mode to standby + int16_t state = standby(); + if (state == RADIOLIB_ERR_NONE) { + // standby command successful + return (RADIOLIB_ERR_NONE); + } + + // standby command failed, check timeout and try again + if (this->mod->hal->millis() - start >= 3000) { + // timed out, possibly incorrect wiring + return (state); + } + + // wait a bit to not spam the module + this->mod->hal->delay(10); + } +} + +int16_t SX128x::transmit(const uint8_t* data, size_t len, uint8_t addr) { + // check packet length + if (this->codingRateLoRa == RADIOLIB_SX128X_LORA_CR_4_8_LI && this->crcLoRa == RADIOLIB_SX128X_LORA_CRC_ON) { + // Long Interleaver at CR 4/8 supports up to 253 bytes if CRC is enabled + if (len > RADIOLIB_SX128X_MAX_PACKET_LENGTH - 2) { + return (RADIOLIB_ERR_PACKET_TOO_LONG); + } + } else if (len > RADIOLIB_SX128X_MAX_PACKET_LENGTH) { + return (RADIOLIB_ERR_PACKET_TOO_LONG); + } + + // check active modem + uint8_t modem = getPacketType(); + if (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // set mode to standby + int16_t state = standby(); + RADIOLIB_ASSERT(state); + + // calculate timeout in ms (5ms + 500 % of expected time-on-air) + RadioLibTime_t timeout = 5 + (getTimeOnAir(len) * 5) / 1000; + RADIOLIB_DEBUG_BASIC_PRINTLN("Timeout in %lu ms", timeout); + + // start transmission + state = startTransmit(data, len, addr); + RADIOLIB_ASSERT(state); + + // wait for packet transmission or timeout + RadioLibTime_t start = this->mod->hal->millis(); + while (!this->mod->hal->digitalRead(this->mod->getIrq())) { + this->mod->hal->yield(); + if (this->mod->hal->millis() - start > timeout) { + finishTransmit(); + return (RADIOLIB_ERR_TX_TIMEOUT); + } + } + + return (finishTransmit()); +} + +int16_t SX128x::receive(uint8_t* data, size_t len, RadioLibTime_t timeout) { + // check active modem + uint8_t modem = getPacketType(); + if (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // set mode to standby + int16_t state = standby(); + RADIOLIB_ASSERT(state); + + // calculate timeout (1000% of expected time-on-air) + // for most other modules, it is 500%, however, the overall datarates of SX128x are higher + // so we use higher value for the default timeout + RadioLibTime_t timeoutInternal = timeout; + if (!timeoutInternal) { + timeoutInternal = getTimeOnAir(len) * 10; + } + RADIOLIB_DEBUG_BASIC_PRINTLN("Timeout in %lu ms", (uint32_t)((timeout + 999) / 1000)); + + // start reception + uint32_t timeoutValue = (uint32_t)((float)timeoutInternal / 15.625f); + state = startReceive(timeoutValue); + RADIOLIB_ASSERT(state); + + // wait for packet reception or timeout + bool softTimeout = false; + RadioLibTime_t start = this->mod->hal->micros(); + while (!this->mod->hal->digitalRead(this->mod->getIrq())) { + this->mod->hal->yield(); + // safety check, the timeout should be done by the radio + if (this->mod->hal->micros() - start > timeout) { + softTimeout = true; + break; + } + } + + // if it was a timeout, this will return an error code + state = standby(); + if ((state != RADIOLIB_ERR_NONE) && (state != RADIOLIB_ERR_SPI_CMD_TIMEOUT)) { + return (state); + } + + // check whether this was a timeout or not + if (softTimeout || (getIrqStatus() & this->irqMap[RADIOLIB_IRQ_TIMEOUT])) { + (void)finishReceive(); + return (RADIOLIB_ERR_RX_TIMEOUT); + } + + // read the received data + return (readData(data, len)); +} + +int16_t SX128x::transmitDirect(uint32_t frf) { + // set RF switch (if present) + this->mod->setRfSwitchState(Module::MODE_TX); + + // user requested to start transmitting immediately (required for RTTY) + int16_t state = RADIOLIB_ERR_NONE; + if (frf != 0) { + state = setRfFrequency(frf); + } + RADIOLIB_ASSERT(state); + + // start transmitting + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_TX_CONTINUOUS_WAVE, NULL, 0)); +} + +int16_t SX128x::receiveDirect() { + // set RF switch (if present) + this->mod->setRfSwitchState(Module::MODE_RX); + + // SX128x is unable to output received data directly + return (RADIOLIB_ERR_UNKNOWN); +} + +int16_t SX128x::scanChannel() { + ChannelScanConfig_t cfg = { + .cad = + { + .symNum = RADIOLIB_SX128X_CAD_PARAM_DEFAULT, + .detPeak = 0, + .detMin = 0, + .exitMode = 0, + .timeout = 0, + .irqFlags = RADIOLIB_IRQ_CAD_DEFAULT_FLAGS, + .irqMask = RADIOLIB_IRQ_CAD_DEFAULT_MASK, + }, + }; + return (this->scanChannel(cfg)); +} + +int16_t SX128x::scanChannel(const ChannelScanConfig_t& config) { + // set mode to CAD + int16_t state = startChannelScan(config); + RADIOLIB_ASSERT(state); + + // wait for channel activity detected or timeout + while (!this->mod->hal->digitalRead(this->mod->getIrq())) { + this->mod->hal->yield(); + } + + // check CAD result + return (getChannelScanResult()); +} + +int16_t SX128x::sleep() { + return (SX128x::sleep(true)); +} + +int16_t SX128x::sleep(bool retainConfig) { + // set RF switch (if present) + this->mod->setRfSwitchState(Module::MODE_IDLE); + + uint8_t sleepConfig = RADIOLIB_SX128X_SLEEP_DATA_BUFFER_RETAIN | RADIOLIB_SX128X_SLEEP_DATA_RAM_RETAIN; + if (!retainConfig) { + sleepConfig = RADIOLIB_SX128X_SLEEP_DATA_BUFFER_FLUSH | RADIOLIB_SX128X_SLEEP_DATA_RAM_FLUSH; + } + int16_t state = this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SAVE_CONTEXT, 0, 1, false, false); + RADIOLIB_ASSERT(state); + state = this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_SLEEP, &sleepConfig, 1, false, false); + + // wait for SX128x to safely enter sleep mode + this->mod->hal->delay(1); + + return (state); +} + +int16_t SX128x::standby() { + return (SX128x::standby(RADIOLIB_SX128X_STANDBY_RC)); +} + +int16_t SX128x::standby(uint8_t mode, bool wakeup) { + // set RF switch (if present) + this->mod->setRfSwitchState(Module::MODE_IDLE); + + if (wakeup) { + // send a NOP command - this pulls the NSS low to exit the sleep mode, + // while preventing interference with possible other SPI transactions + (void)this->mod->SPIwriteStream((uint16_t)RADIOLIB_SX128X_CMD_NOP, NULL, 0, false, false); + } + + const uint8_t data[] = {mode}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_STANDBY, data, 1)); +} + +void SX128x::setDio1Action(void (*func)(void)) { + this->mod->hal->attachInterrupt(this->mod->hal->pinToInterrupt(this->mod->getIrq()), func, + this->mod->hal->GpioInterruptRising); +} + +void SX128x::clearDio1Action() { + this->mod->hal->detachInterrupt(this->mod->hal->pinToInterrupt(this->mod->getIrq())); +} + +void SX128x::setPacketReceivedAction(void (*func)(void)) { + this->setDio1Action(func); +} + +void SX128x::clearPacketReceivedAction() { + this->clearDio1Action(); +} + +void SX128x::setPacketSentAction(void (*func)(void)) { + this->setDio1Action(func); +} + +void SX128x::clearPacketSentAction() { + this->clearDio1Action(); +} + +int16_t SX128x::finishTransmit() { + // clear interrupt flags + clearIrqStatus(); + + // set mode to standby to disable transmitter/RF switch + return (standby()); +} + +int16_t SX128x::startReceive() { + return (this->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF, RADIOLIB_IRQ_RX_DEFAULT_FLAGS, + RADIOLIB_IRQ_RX_DEFAULT_MASK, 0)); +} + +int16_t SX128x::readData(uint8_t* data, size_t len) { + // check active modem + if (getPacketType() == RADIOLIB_SX128X_PACKET_TYPE_RANGING) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // set mode to standby + int16_t state = standby(); + RADIOLIB_ASSERT(state); + + // check integrity CRC + uint16_t irq = getIrqStatus(); + int16_t crcState = RADIOLIB_ERR_NONE; + // Report CRC mismatch when there's a payload CRC error, or a header error and no valid header (to avoid false alarm + // from previous packet) + if ((irq & RADIOLIB_SX128X_IRQ_CRC_ERROR) || + ((irq & RADIOLIB_SX128X_IRQ_HEADER_ERROR) && !(irq & RADIOLIB_SX128X_IRQ_HEADER_VALID))) { + crcState = RADIOLIB_ERR_CRC_MISMATCH; + } + + // get packet length and Rx buffer offset + uint8_t offset = 0; + size_t length = getPacketLength(true, &offset); + if ((len != 0) && (len < length)) { + // user requested less data than we got, only return what was requested + length = len; + } + + // read packet data starting at offset + state = readBuffer(data, length, offset); + RADIOLIB_ASSERT(state); + + // clear interrupt flags + state = clearIrqStatus(); + + // check if CRC failed - this is done after reading data to give user the option to keep them + RADIOLIB_ASSERT(crcState); + + return (state); +} + +int16_t SX128x::finishReceive() { + // set mode to standby to disable RF switch + int16_t state = standby(); + RADIOLIB_ASSERT(state); + + // clear interrupt flags + return (clearIrqStatus()); +} + +uint32_t SX128x::getIrqFlags() { + return ((uint32_t)this->getIrqStatus()); +} + +int16_t SX128x::setIrqFlags(uint32_t irq) { + return (this->setDioIrqParams(irq, irq)); +} + +int16_t SX128x::clearIrqFlags(uint32_t irq) { + return (this->clearIrqStatus(irq)); +} + +int16_t SX128x::startChannelScan() { + ChannelScanConfig_t cfg = { + .cad = + { + .symNum = RADIOLIB_SX128X_CAD_PARAM_DEFAULT, + .detPeak = 0, + .detMin = 0, + .exitMode = 0, + .timeout = 0, + .irqFlags = RADIOLIB_IRQ_CAD_DEFAULT_FLAGS, + .irqMask = RADIOLIB_IRQ_CAD_DEFAULT_MASK, + }, + }; + return (this->startChannelScan(cfg)); +} + +int16_t SX128x::startChannelScan(const ChannelScanConfig_t& config) { + // check active modem + if (getPacketType() != RADIOLIB_SX128X_PACKET_TYPE_LORA) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // set mode to standby + int16_t state = standby(); + RADIOLIB_ASSERT(state); + + // set DIO pin mapping + state = setDioIrqParams(getIrqMapped(config.cad.irqFlags), getIrqMapped(config.cad.irqMask)); + RADIOLIB_ASSERT(state); + + // clear interrupt flags + state = clearIrqStatus(); + RADIOLIB_ASSERT(state); + + // set RF switch (if present) + this->mod->setRfSwitchState(Module::MODE_RX); + + // set mode to CAD + return (setCad(config.cad.symNum)); +} + +int16_t SX128x::getChannelScanResult() { + // check active modem + if (getPacketType() != RADIOLIB_SX128X_PACKET_TYPE_LORA) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // check CAD result + uint16_t cadResult = getIrqStatus(); + int16_t state = RADIOLIB_ERR_UNKNOWN; + if (cadResult & RADIOLIB_SX128X_IRQ_CAD_DETECTED) { + // detected some LoRa activity + state = RADIOLIB_LORA_DETECTED; + } else if (cadResult & RADIOLIB_SX128X_IRQ_CAD_DONE) { + // channel is free + state = RADIOLIB_CHANNEL_FREE; + } + + clearIrqStatus(); + return (state); +} + +int16_t SX128x::setFrequency(float freq) { + RADIOLIB_CHECK_RANGE(freq, 2400.0f, 2500.0f, RADIOLIB_ERR_INVALID_FREQUENCY); + + // calculate raw value + uint32_t frf = (freq * (uint32_t(1) << RADIOLIB_SX128X_DIV_EXPONENT)) / RADIOLIB_SX128X_CRYSTAL_FREQ; + return (setRfFrequency(frf)); +} + +int16_t SX128x::setBandwidth(float bw) { + // check active modem + uint8_t modem = getPacketType(); + if (modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) { + // check range for LoRa + RADIOLIB_CHECK_RANGE(bw, 203.125f, 1625.0f, RADIOLIB_ERR_INVALID_BANDWIDTH); + } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING) { + // check range for ranging + RADIOLIB_CHECK_RANGE(bw, 406.25f, 1625.0f, RADIOLIB_ERR_INVALID_BANDWIDTH); + } else { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + if (fabsf(bw - 203.125f) <= 0.001f) { + this->bandwidth = RADIOLIB_SX128X_LORA_BW_203_125; + } else if (fabsf(bw - 406.25f) <= 0.001f) { + this->bandwidth = RADIOLIB_SX128X_LORA_BW_406_25; + } else if (fabsf(bw - 812.5f) <= 0.001f) { + this->bandwidth = RADIOLIB_SX128X_LORA_BW_812_50; + } else if (fabsf(bw - 1625.0f) <= 0.001f) { + this->bandwidth = RADIOLIB_SX128X_LORA_BW_1625_00; + } else { + return (RADIOLIB_ERR_INVALID_BANDWIDTH); + } + + // update modulation parameters + this->bandwidthKhz = bw; + return (setModulationParams(this->spreadingFactor, this->bandwidth, this->codingRateLoRa)); +} + +int16_t SX128x::setSpreadingFactor(uint8_t sf) { + // check active modem + uint8_t modem = getPacketType(); + if (modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) { + // check range for LoRa + RADIOLIB_CHECK_RANGE(sf, 5, 12, RADIOLIB_ERR_INVALID_SPREADING_FACTOR); + } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING) { + // check range for ranging + RADIOLIB_CHECK_RANGE(sf, 5, 10, RADIOLIB_ERR_INVALID_SPREADING_FACTOR); + } else { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // update modulation parameters + this->spreadingFactor = sf << 4; + int16_t state = setModulationParams(this->spreadingFactor, this->bandwidth, this->codingRateLoRa); + RADIOLIB_ASSERT(state); + + // update mystery register in LoRa mode - SX1280 datasheet rev 3.2 section 14.4.1 + if (modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) { + uint8_t data = 0; + if ((this->spreadingFactor == RADIOLIB_SX128X_LORA_SF_5) || + (this->spreadingFactor == RADIOLIB_SX128X_LORA_SF_6)) { + data = 0x1E; + } else if ((this->spreadingFactor == RADIOLIB_SX128X_LORA_SF_7) || + (this->spreadingFactor == RADIOLIB_SX128X_LORA_SF_8)) { + data = 0x37; + } else { + data = 0x32; + } + state = SX128x::writeRegister(RADIOLIB_SX128X_REG_LORA_SF_CONFIG, &data, 1); + RADIOLIB_ASSERT(state); + + // this register must also be updated for some reason + state = SX128x::readRegister(RADIOLIB_SX128X_REG_FREQ_ERROR_CORRECTION, &data, 1); + RADIOLIB_ASSERT(state); + + data |= 0x01; + state = SX128x::writeRegister(RADIOLIB_SX128X_REG_FREQ_ERROR_CORRECTION, &data, 1); + RADIOLIB_ASSERT(state); + } + + return (state); +} + +int16_t SX128x::setCodingRate(uint8_t cr, bool longInterleaving) { + // check active modem + uint8_t modem = getPacketType(); + + // LoRa/ranging + if ((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING)) { + RADIOLIB_CHECK_RANGE(cr, 4, 8, RADIOLIB_ERR_INVALID_CODING_RATE); + + // update modulation parameters + if (longInterleaving && (modem == RADIOLIB_SX128X_PACKET_TYPE_LORA)) { + switch (cr) { + case 4: + this->codingRateLoRa = 0; + break; + case 5: + case 6: + this->codingRateLoRa = cr; + break; + case 8: + this->codingRateLoRa = cr - 1; + break; + default: + return (RADIOLIB_ERR_INVALID_CODING_RATE); + } + } else { + this->codingRateLoRa = cr - 4; + } + return (setModulationParams(this->spreadingFactor, this->bandwidth, this->codingRateLoRa)); + + // FLRC + } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC) { + RADIOLIB_CHECK_RANGE(cr, 2, 4, RADIOLIB_ERR_INVALID_CODING_RATE); + + // update modulation parameters + this->codingRateFLRC = (cr - 2) * 2; + return (setModulationParams(this->bitRate, this->codingRateFLRC, this->shaping)); + } + + return (RADIOLIB_ERR_WRONG_MODEM); +} + +int16_t SX128x::setOutputPower(int8_t pwr) { + // check if power value is configurable + int16_t state = checkOutputPower(pwr, NULL); + RADIOLIB_ASSERT(state); + + this->power = pwr + 18; + return (setTxParams(this->power)); +} + +int16_t SX128x::checkOutputPower(int8_t pwr, int8_t* clipped) { + if (clipped) { + *clipped = RADIOLIB_MAX(-18, RADIOLIB_MIN(13, pwr)); + } + RADIOLIB_CHECK_RANGE(pwr, -18, 13, RADIOLIB_ERR_INVALID_OUTPUT_POWER); + return (RADIOLIB_ERR_NONE); +} + +int16_t SX128x::setModem(ModemType_t modem) { + switch (modem) { + case (ModemType_t::RADIOLIB_MODEM_LORA): { + return (this->begin()); + } break; + case (ModemType_t::RADIOLIB_MODEM_FSK): { + return (this->beginGFSK()); + } break; + default: + return (RADIOLIB_ERR_WRONG_MODEM); + } +} + +int16_t SX128x::getModem(ModemType_t* modem) { + RADIOLIB_ASSERT_PTR(modem); + + switch (getPacketType()) { + case (RADIOLIB_SX128X_PACKET_TYPE_LORA): + *modem = ModemType_t::RADIOLIB_MODEM_LORA; + return (RADIOLIB_ERR_NONE); + case (RADIOLIB_SX128X_PACKET_TYPE_GFSK): + *modem = ModemType_t::RADIOLIB_MODEM_FSK; + return (RADIOLIB_ERR_NONE); + } + + return (RADIOLIB_ERR_WRONG_MODEM); +} + +int16_t SX128x::setPreambleLength(size_t preambleLength) { + uint8_t modem = getPacketType(); + if ((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING)) { + // LoRa or ranging + // the actual limit is 491520, however, some platforms (notably AVR) limit size_t to 16 bits + RADIOLIB_CHECK_RANGE(preambleLength, 2, 65534, RADIOLIB_ERR_INVALID_PREAMBLE_LENGTH); + + // check preamble length is even - no point even trying odd numbers + if (preambleLength % 2 != 0) { + return (RADIOLIB_ERR_INVALID_PREAMBLE_LENGTH); + } + + // calculate exponent and mantissa values (use the next longer preamble if there's no exact match) + uint8_t e = 1; + uint8_t m = 1; + uint32_t len = 0; + for (; e <= 15; e++) { + for (m = 1; m <= 15; m++) { + len = m * (uint32_t(1) << e); + if (len >= preambleLength) { + break; + } + } + if (len >= preambleLength) { + break; + } + } + + // update packet parameters + this->preambleLengthLoRa = (e << 4) | m; + return (setPacketParamsLoRa(this->preambleLengthLoRa, this->headerType, this->payloadLen, this->crcLoRa, + this->invertIQEnabled)); + + } else if ((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC)) { + // GFSK or FLRC + RADIOLIB_CHECK_RANGE(preambleLength, 4, 32, RADIOLIB_ERR_INVALID_PREAMBLE_LENGTH); + + // check preamble length is multiple of 4 + if (preambleLength % 4 != 0) { + return (RADIOLIB_ERR_INVALID_PREAMBLE_LENGTH); + } + + // update packet parameters + this->preambleLengthGFSK = ((preambleLength / 4) - 1) << 4; + return (setPacketParamsGFSK(this->preambleLengthGFSK, this->syncWordLen, this->syncWordMatch, this->crcGFSK, + this->whitening, this->packetType)); + } + + return (RADIOLIB_ERR_WRONG_MODEM); +} + +int16_t SX128x::setDataRate(DataRate_t dr, ModemType_t modem) { + // get the current modem + ModemType_t currentModem; + int16_t state = this->getModem(¤tModem); + RADIOLIB_ASSERT(state); + + // switch over if the requested modem is different + if (modem != RADIOLIB_MODEM_NONE && modem != currentModem) { + state = this->standby(); + RADIOLIB_ASSERT(state); + state = this->setModem(modem); + RADIOLIB_ASSERT(state); + } + + if (modem == RADIOLIB_MODEM_NONE) { + modem = currentModem; + } + + // select interpretation based on modem + if (modem == RADIOLIB_MODEM_LORA) { + state = this->setBandwidth(dr.lora.bandwidth); + RADIOLIB_ASSERT(state); + state = this->setSpreadingFactor(dr.lora.spreadingFactor); + RADIOLIB_ASSERT(state); + state = this->setCodingRate(dr.lora.codingRate); + } else { + return (RADIOLIB_ERR_WRONG_MODEM); + } + return (state); +} + +int16_t SX128x::setBitRate(float br) { + // check active modem + uint8_t modem = getPacketType(); + + // GFSK/BLE + if ((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE)) { + if ((uint16_t)br == 125) { + this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_125_BW_0_3; + } else if ((uint16_t)br == 250) { + this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_250_BW_0_6; + } else if ((uint16_t)br == 400) { + this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_400_BW_1_2; + } else if ((uint16_t)br == 500) { + this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_500_BW_1_2; + } else if ((uint16_t)br == 800) { + this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_800_BW_2_4; + } else if ((uint16_t)br == 1000) { + this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_1_000_BW_2_4; + } else if ((uint16_t)br == 1600) { + this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_1_600_BW_2_4; + } else if ((uint16_t)br == 2000) { + this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_2_000_BW_2_4; + } else { + return (RADIOLIB_ERR_INVALID_BIT_RATE); + } + + // update modulation parameters + this->bitRateKbps = (uint16_t)br; + return (setModulationParams(this->bitRate, this->modIndex, this->shaping)); + + // FLRC + } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC) { + if ((uint16_t)br == 260) { + this->bitRate = RADIOLIB_SX128X_FLRC_BR_0_260_BW_0_3; + } else if ((uint16_t)br == 325) { + this->bitRate = RADIOLIB_SX128X_FLRC_BR_0_325_BW_0_3; + } else if ((uint16_t)br == 520) { + this->bitRate = RADIOLIB_SX128X_FLRC_BR_0_520_BW_0_6; + } else if ((uint16_t)br == 650) { + this->bitRate = RADIOLIB_SX128X_FLRC_BR_0_650_BW_0_6; + } else if ((uint16_t)br == 1000) { + this->bitRate = RADIOLIB_SX128X_FLRC_BR_1_000_BW_1_2; + } else if ((uint16_t)br == 1300) { + this->bitRate = RADIOLIB_SX128X_FLRC_BR_1_300_BW_1_2; + } else { + return (RADIOLIB_ERR_INVALID_BIT_RATE); + } + + // update modulation parameters + this->bitRateKbps = (uint16_t)br; + return (setModulationParams(this->bitRate, this->codingRateFLRC, this->shaping)); + } + + return (RADIOLIB_ERR_WRONG_MODEM); +} + +int16_t SX128x::setFrequencyDeviation(float freqDev) { + // check active modem + uint8_t modem = getPacketType(); + if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE))) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // set frequency deviation to lowest available setting (required for digimodes) + float newFreqDev = freqDev; + if (freqDev < 0.0f) { + newFreqDev = 62.5f; + } + + RADIOLIB_CHECK_RANGE(newFreqDev, 62.5f, 1000.0f, RADIOLIB_ERR_INVALID_FREQUENCY_DEVIATION); + + // override for the lowest possible frequency deviation - required for some PhysicalLayer protocols + if (newFreqDev == 0.0f) { + this->modIndex = RADIOLIB_SX128X_BLE_GFSK_MOD_IND_0_35; + this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_125_BW_0_3; + return (setModulationParams(this->bitRate, this->modIndex, this->shaping)); + } + + // update modulation parameters + uint8_t modInd = (uint8_t)((8.0f * (newFreqDev / (float)this->bitRateKbps)) - 1.0f); + if (modInd > RADIOLIB_SX128X_BLE_GFSK_MOD_IND_4_00) { + return (RADIOLIB_ERR_INVALID_MODULATION_PARAMETERS); + } + + // update modulation parameters + this->frequencyDev = newFreqDev; + this->modIndex = modInd; + return (setModulationParams(this->bitRate, this->modIndex, this->shaping)); +} + +int16_t SX128x::setDataShaping(uint8_t sh) { + // check active modem + uint8_t modem = getPacketType(); + if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE) || + (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC))) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // set data this->shaping + switch (sh) { + case RADIOLIB_SHAPING_NONE: + this->shaping = RADIOLIB_SX128X_BLE_GFSK_BT_OFF; + break; + case RADIOLIB_SHAPING_0_5: + this->shaping = RADIOLIB_SX128X_BLE_GFSK_BT_0_5; + break; + case RADIOLIB_SHAPING_1_0: + this->shaping = RADIOLIB_SX128X_BLE_GFSK_BT_1_0; + break; + default: + return (RADIOLIB_ERR_INVALID_DATA_SHAPING); + } + + // update modulation parameters + if ((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE)) { + return (setModulationParams(this->bitRate, this->modIndex, this->shaping)); + } else { + return (setModulationParams(this->bitRate, this->codingRateFLRC, this->shaping)); + } +} + +int16_t SX128x::setSyncWord(const uint8_t* syncWord, uint8_t len) { + // check active modem + uint8_t modem = getPacketType(); + if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC))) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + if (modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) { + // GFSK can use up to 5 bytes as sync word + if (len > 5) { + return (RADIOLIB_ERR_INVALID_SYNC_WORD); + } + + // calculate sync word length parameter value + if (len > 0) { + this->syncWordLen = (len - 1) * 2; + } + + } else { + // FLRC requires 32-bit sync word + if (!((len == 0) || (len == 4))) { + return (RADIOLIB_ERR_INVALID_SYNC_WORD); + } + + // save sync word length parameter value + this->syncWordLen = len; + } + + // update sync word + int16_t state = + SX128x::writeRegister(RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_4 + (5 - len), const_cast(syncWord), len); + RADIOLIB_ASSERT(state); + + // update packet parameters + if (this->syncWordLen == 0) { + this->syncWordMatch = RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_OFF; + } else { + /// \todo add support for multiple sync words + this->syncWordMatch = RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1; + } + return (setPacketParamsGFSK(this->preambleLengthGFSK, this->syncWordLen, this->syncWordMatch, this->crcGFSK, + this->whitening, this->packetType)); +} + +int16_t SX128x::setSyncWord(uint8_t syncWord, uint8_t controlBits) { + // check active modem + if (getPacketType() != RADIOLIB_SX128X_PACKET_TYPE_LORA) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // update register + const uint8_t data[2] = {(uint8_t)((syncWord & 0xF0) | ((controlBits & 0xF0) >> 4)), + (uint8_t)(((syncWord & 0x0F) << 4) | (controlBits & 0x0F))}; + return (writeRegister(RADIOLIB_SX128X_REG_LORA_SYNC_WORD_MSB, data, 2)); +} + +int16_t SX128x::setCRC(uint8_t len, uint32_t initial, uint16_t polynomial) { + // check active modem + uint8_t modem = getPacketType(); + + int16_t state; + if ((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC)) { + // update packet parameters + if (modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) { + if (len > 2) { + return (RADIOLIB_ERR_INVALID_CRC_CONFIGURATION); + } + } else { + if (len > 3) { + return (RADIOLIB_ERR_INVALID_CRC_CONFIGURATION); + } + } + this->crcGFSK = len << 4; + state = setPacketParamsGFSK(this->preambleLengthGFSK, this->syncWordLen, this->syncWordMatch, this->crcGFSK, + this->whitening, this->packetType); + RADIOLIB_ASSERT(state); + + // set initial CRC value + uint8_t data[] = {(uint8_t)((initial >> 8) & 0xFF), (uint8_t)(initial & 0xFF)}; + state = writeRegister(RADIOLIB_SX128X_REG_CRC_INITIAL_MSB, data, 2); + RADIOLIB_ASSERT(state); + + // set CRC polynomial + data[0] = (uint8_t)((polynomial >> 8) & 0xFF); + data[1] = (uint8_t)(polynomial & 0xFF); + state = writeRegister(RADIOLIB_SX128X_REG_CRC_POLYNOMIAL_MSB, data, 2); + return (state); + + } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE) { + // update packet parameters + if (len == 0) { + this->crcBLE = RADIOLIB_SX128X_BLE_CRC_OFF; + } else if (len == 3) { + this->crcBLE = RADIOLIB_SX128X_BLE_CRC_3_BYTE; + } else { + return (RADIOLIB_ERR_INVALID_CRC_CONFIGURATION); + } + state = setPacketParamsBLE(this->connectionState, this->crcBLE, this->bleTestPayload, this->whitening); + RADIOLIB_ASSERT(state); + + // set initial CRC value + const uint8_t data[] = {(uint8_t)((initial >> 16) & 0xFF), (uint8_t)((initial >> 8) & 0xFF), + (uint8_t)(initial & 0xFF)}; + state = writeRegister(RADIOLIB_SX128X_REG_BLE_CRC_INITIAL_MSB, data, 3); + return (state); + + } else if ((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING)) { + // update packet parameters + if (len == 0) { + this->crcLoRa = RADIOLIB_SX128X_LORA_CRC_OFF; + } else if (len == 2) { + this->crcLoRa = RADIOLIB_SX128X_LORA_CRC_ON; + } else { + return (RADIOLIB_ERR_INVALID_CRC_CONFIGURATION); + } + state = setPacketParamsLoRa(this->preambleLengthLoRa, this->headerType, this->payloadLen, this->crcLoRa, + this->invertIQEnabled); + return (state); + } + + return (RADIOLIB_ERR_UNKNOWN); +} + +int16_t SX128x::setWhitening(bool enabled) { + // check active modem + uint8_t modem = getPacketType(); + if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE))) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // update packet parameters + if (enabled) { + this->whitening = RADIOLIB_SX128X_GFSK_BLE_WHITENING_ON; + } else { + this->whitening = RADIOLIB_SX128X_GFSK_BLE_WHITENING_OFF; + } + + if (modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) { + return (setPacketParamsGFSK(this->preambleLengthGFSK, this->syncWordLen, this->syncWordMatch, this->crcGFSK, + this->whitening, this->packetType)); + } + return (setPacketParamsBLE(this->connectionState, this->crcBLE, this->bleTestPayload, this->whitening)); +} + +int16_t SX128x::setAccessAddress(uint32_t addr) { + // check active modem + if (getPacketType() != RADIOLIB_SX128X_PACKET_TYPE_BLE) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // set the address + const uint8_t addrBuff[] = {(uint8_t)((addr >> 24) & 0xFF), (uint8_t)((addr >> 16) & 0xFF), + (uint8_t)((addr >> 8) & 0xFF), (uint8_t)(addr & 0xFF)}; + return (SX128x::writeRegister(RADIOLIB_SX128X_REG_ACCESS_ADDRESS_BYTE_3, addrBuff, 4)); +} + +int16_t SX128x::setHighSensitivityMode(bool enable) { + // read the current registers + uint8_t RxGain = 0; + int16_t state = readRegister(RADIOLIB_SX128X_REG_GAIN_MODE, &RxGain, 1); + RADIOLIB_ASSERT(state); + + if (enable) { + RxGain |= 0xC0; // Set bits 6 and 7 + } else { + RxGain &= ~0xC0; // Unset bits 6 and 7 + } + + // update all values + state = writeRegister(RADIOLIB_SX128X_REG_GAIN_MODE, &RxGain, 1); + return (state); +} + +int16_t SX128x::setGainControl(uint8_t gain) { + // read the current registers + uint8_t ManualGainSetting = 0; + int16_t state = readRegister(RADIOLIB_SX128X_REG_MANUAL_GAIN_CONTROL_ENABLE_2, &ManualGainSetting, 1); + RADIOLIB_ASSERT(state); + uint8_t LNAGainValue = 0; + state = readRegister(RADIOLIB_SX128X_REG_MANUAL_GAIN_SETTING, &LNAGainValue, 1); + RADIOLIB_ASSERT(state); + uint8_t LNAGainControl = 0; + state = readRegister(RADIOLIB_SX128X_REG_MANUAL_GAIN_CONTROL_ENABLE_1, &LNAGainControl, 1); + RADIOLIB_ASSERT(state); + + // set the gain + if (gain > 0 && gain < 14) { + // Set manual gain + ManualGainSetting &= ~0x01; // Set bit 0 to 0 (Enable Manual Gain Control) + LNAGainValue &= 0xF0; // Bits 0, 1, 2 and 3 to 0 + LNAGainValue |= gain; // Set bits 0, 1, 2 and 3 to Manual Gain Setting (1-13) + LNAGainControl |= 0x80; // Set bit 7 to 1 (Enable Manual Gain Control) + } else { + // Set automatic gain if 0 or out of range + ManualGainSetting |= 0x01; // Set bit 0 to 1 (Enable Automatic Gain Control) + LNAGainValue &= 0xF0; // Bits 0, 1, 2 and 3 to 0 + LNAGainValue |= 0x0A; // Set bits 0, 1, 2 and 3 to Manual Gain Setting (1-13) + LNAGainControl &= ~0x80; // Set bit 7 to 0 (Enable Automatic Gain Control) + } + + // update all values + state = writeRegister(RADIOLIB_SX128X_REG_MANUAL_GAIN_CONTROL_ENABLE_2, &ManualGainSetting, 1); + RADIOLIB_ASSERT(state); + state = writeRegister(RADIOLIB_SX128X_REG_MANUAL_GAIN_SETTING, &LNAGainValue, 1); + RADIOLIB_ASSERT(state); + state = writeRegister(RADIOLIB_SX128X_REG_MANUAL_GAIN_CONTROL_ENABLE_1, &LNAGainControl, 1); + return (state); +} + +float SX128x::getRSSI() { + // get packet status + uint8_t packetStatus[5]; + this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_PACKET_STATUS, packetStatus, 5); + + // check active modem + uint8_t modem = getPacketType(); + if ((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING)) { + // LoRa or ranging + uint8_t rssiSync = packetStatus[0]; + float rssiMeasured = -1.0 * rssiSync / 2.0; + float snr = getSNR(); + if (snr <= 0.0f) { + return (rssiMeasured - snr); + } else { + return (rssiMeasured); + } + } else { + // GFSK, BLE or FLRC + uint8_t rssiSync = packetStatus[1]; + return (-1.0 * rssiSync / 2.0); + } +} + +float SX128x::getRSSI(bool packet) { + if (!packet) { + // get instantaneous RSSI value + uint8_t data[3] = {0, 0, 0}; // RssiInst, Status, RFU + this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_RSSI_INST, data, 3); + return ((float)data[0] / (-2.0f)); + } else { + return this->getRSSI(); + } +} + +float SX128x::getSNR() { + // check active modem + uint8_t modem = getPacketType(); + if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING))) { + return (0.0); + } + + // get packet status + uint8_t packetStatus[5]; + this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_PACKET_STATUS, packetStatus, 5); + + // calculate real SNR + uint8_t snr = packetStatus[1]; + if (snr < 128) { + return (snr / 4.0); + } else { + return ((snr - 256) / 4.0f); + } +} + +float SX128x::getFrequencyError() { + // check active modem + uint8_t modem = getPacketType(); + if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING))) { + return (0.0); + } + + // read the raw frequency error register values + uint8_t efeRaw[3] = {0}; + int16_t state = readRegister(RADIOLIB_SX128X_REG_FEI_MSB, &efeRaw[0], 1); + RADIOLIB_ASSERT(state); + state = readRegister(RADIOLIB_SX128X_REG_FEI_MID, &efeRaw[1], 1); + RADIOLIB_ASSERT(state); + state = readRegister(RADIOLIB_SX128X_REG_FEI_LSB, &efeRaw[2], 1); + RADIOLIB_ASSERT(state); + uint32_t efe = ((uint32_t)efeRaw[0] << 16) | ((uint32_t)efeRaw[1] << 8) | efeRaw[2]; + efe &= 0x0FFFFF; + + float error = 0; + + // check the first bit + if (efe & 0x80000) { + // frequency error is negative + efe |= (uint32_t)0xFFF00000; + efe = ~efe + 1; + error = 1.55f * (float)efe / (1600.0f / this->bandwidthKhz) * -1.0f; + } else { + error = 1.55f * (float)efe / (1600.0f / this->bandwidthKhz); + } + + return (error); +} + +size_t SX128x::getPacketLength(bool update) { + return (this->getPacketLength(update, NULL)); +} + +size_t SX128x::getPacketLength(bool update, uint8_t* offset) { + (void)update; + + // in implicit mode, return the cached value + if ((getPacketType() == RADIOLIB_SX128X_PACKET_TYPE_LORA) && + (this->headerType == RADIOLIB_SX128X_LORA_HEADER_IMPLICIT)) { + return (this->payloadLen); + } + + uint8_t rxBufStatus[2] = {0, 0}; + this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_RX_BUFFER_STATUS, rxBufStatus, 2); + + if (offset) { + *offset = rxBufStatus[1]; + } + + return ((size_t)rxBufStatus[0]); +} + +int16_t SX128x::getLoRaRxHeaderInfo(uint8_t* cr, bool* hasCRC) { + int16_t state = RADIOLIB_ERR_NONE; + + // check if in explicit header mode + if (this->headerType == RADIOLIB_SX128X_LORA_HEADER_IMPLICIT) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + if (cr) { + *cr = this->mod->SPIgetRegValue(RADIOLIB_SX128X_REG_LORA_RX_CODING_RATE, 6, 4) >> 4; + } + if (hasCRC) { + *hasCRC = (this->mod->SPIgetRegValue(RADIOLIB_SX128X_REG_FEI_MSB, 4, 4) != 0); + } + + return (state); +} + +int16_t SX128x::fixedPacketLengthMode(uint8_t len) { + return (setPacketMode(RADIOLIB_SX128X_GFSK_FLRC_PACKET_FIXED, len)); +} + +int16_t SX128x::variablePacketLengthMode(uint8_t maxLen) { + return (setPacketMode(RADIOLIB_SX128X_GFSK_FLRC_PACKET_VARIABLE, maxLen)); +} + +RadioLibTime_t SX128x::calculateTimeOnAir(ModemType_t modem, DataRate_t dr, PacketConfig_t pc, size_t len) { + switch (modem) { + case (ModemType_t::RADIOLIB_MODEM_LORA): { + // calculate number of symbols + float N_symbol = 0; + uint8_t sf = dr.lora.spreadingFactor; + float cr = (float)dr.lora.codingRate; + + // get SF coefficients + float coeff1 = 0; + int16_t coeff2 = 0; + int16_t coeff3 = 0; + if (sf < 7) { + // SF5, SF6 + coeff1 = 6.25; + coeff2 = 4 * sf; + coeff3 = 4 * sf; + } else if (sf < 11) { + // SF7. SF8, SF9, SF10 + coeff1 = 4.25; + coeff2 = 4 * sf + 8; + coeff3 = 4 * sf; + } else { + // SF11, SF12 + coeff1 = 4.25; + coeff2 = 4 * sf + 8; + coeff3 = 4 * (sf - 2); + } + + // get CRC length + int16_t N_bitCRC = 16; + if (!pc.lora.crcEnabled) { + N_bitCRC = 0; + } + + // get header length + int16_t N_symbolHeader = 20; + if (pc.lora.implicitHeader) { + N_symbolHeader = 0; + } + + // calculate number of LoRa preamble symbols + uint32_t N_symbolPreamble = pc.lora.preambleLength; + + // calculate the number of symbols + N_symbol = (float)N_symbolPreamble + coeff1 + 8.0f + + ceilf((float)RADIOLIB_MAX((int16_t)(8 * len + N_bitCRC - coeff2 + N_symbolHeader), (int16_t)0) / + (float)coeff3) * + cr; + + // get time-on-air in us + return (((uint32_t(1) << sf) / dr.lora.bandwidth) * N_symbol * 1000.0f); + } + case (ModemType_t::RADIOLIB_MODEM_FSK): + return ( + (((float)(pc.fsk.crcLength * 8) + pc.fsk.syncWordLength + pc.fsk.preambleLength + (uint32_t)len * 8) / + (dr.fsk.bitRate / 1000.0f))); + + default: + return (RADIOLIB_ERR_WRONG_MODEM); + } +} + +RadioLibTime_t SX128x::getTimeOnAir(size_t len) { + // check active modem + uint8_t modem = getPacketType(); + DataRate_t dr = {}; + PacketConfig_t pc = {}; + + if (modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) { + uint8_t sf = this->spreadingFactor >> 4; + uint8_t cr = this->codingRateLoRa; + // We assume same calculation for short and long interleaving, so map CR values 0-4 and 5-7 to the same values + if (cr < 5) { + cr = cr + 4; + } else if (cr == 7) { + cr = cr + 1; + } + + dr.lora.spreadingFactor = sf; + dr.lora.codingRate = cr; + dr.lora.bandwidth = this->bandwidthKhz; + + uint16_t preambleLength = + (this->preambleLengthLoRa & 0x0F) * (uint32_t(1) << ((this->preambleLengthLoRa & 0xF0) >> 4)); + + pc.lora.preambleLength = preambleLength; + pc.lora.implicitHeader = this->headerType == RADIOLIB_SX128X_LORA_HEADER_IMPLICIT; + pc.lora.crcEnabled = this->crcLoRa == RADIOLIB_SX128X_LORA_CRC_ON; + pc.lora.ldrOptimize = false; + + return (calculateTimeOnAir(ModemType_t::RADIOLIB_MODEM_LORA, dr, pc, len)); + } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) { + dr.fsk.bitRate = (float)this->bitRateKbps; + dr.fsk.freqDev = this->frequencyDev; + + pc.fsk.preambleLength = ((uint16_t)this->preambleLengthGFSK >> 2) + 4; + pc.fsk.syncWordLength = ((this->syncWordLen >> 1) + 1) * 8; + pc.fsk.crcLength = this->crcGFSK >> 4; + + return (calculateTimeOnAir(ModemType_t::RADIOLIB_MODEM_FSK, dr, pc, len)); + } else { + return (RADIOLIB_ERR_WRONG_MODEM); + } +} + +int16_t SX128x::implicitHeader(size_t len) { + return (setHeaderType(RADIOLIB_SX128X_LORA_HEADER_IMPLICIT, len)); +} + +int16_t SX128x::explicitHeader() { + return (setHeaderType(RADIOLIB_SX128X_LORA_HEADER_EXPLICIT)); +} + +int16_t SX128x::setEncoding(uint8_t encoding) { + return (setWhitening(encoding)); +} + +void SX128x::setRfSwitchPins(uint32_t rxEn, uint32_t txEn) { + this->mod->setRfSwitchPins(rxEn, txEn); +} + +void SX128x::setRfSwitchTable(const uint32_t (&pins)[Module::RFSWITCH_MAX_PINS], const Module::RfSwitchMode_t table[]) { + this->mod->setRfSwitchTable(pins, table); +} + +uint8_t SX128x::randomByte() { + // it's unclear whether SX128x can measure RSSI while not receiving a packet + // this method is implemented only for PhysicalLayer compatibility + return (0); +} + +int16_t SX128x::invertIQ(bool enable) { + if (getPacketType() != RADIOLIB_SX128X_PACKET_TYPE_LORA) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + if (enable) { + this->invertIQEnabled = RADIOLIB_SX128X_LORA_IQ_INVERTED; + } else { + this->invertIQEnabled = RADIOLIB_SX128X_LORA_IQ_STANDARD; + } + + return (setPacketParamsLoRa(this->preambleLengthLoRa, this->headerType, this->payloadLen, this->crcLoRa, + this->invertIQEnabled)); +} + +int16_t SX128x::stageMode(RadioModeType_t mode, RadioModeConfig_t* cfg) { + int16_t state; + + switch (mode) { + case (RADIOLIB_RADIO_MODE_RX): { + // in implicit header mode, use the provided length if it is nonzero + // otherwise we trust the user has previously set the payload length manually + if ((this->headerType == RADIOLIB_SX128X_LORA_HEADER_IMPLICIT) && (cfg->receive.len != 0)) { + this->payloadLen = cfg->receive.len; + } + + // check active modem + if (getPacketType() == RADIOLIB_SX128X_PACKET_TYPE_RANGING) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // set DIO mapping + if (cfg->receive.timeout != RADIOLIB_SX128X_RX_TIMEOUT_INF) { + cfg->receive.irqMask |= (1UL << RADIOLIB_IRQ_TIMEOUT); + } + + state = setDioIrqParams(getIrqMapped(cfg->receive.irqFlags), getIrqMapped(cfg->receive.irqMask)); + RADIOLIB_ASSERT(state); + + // set buffer pointers + state = setBufferBaseAddress(); + RADIOLIB_ASSERT(state); + + // clear interrupt flags + state = clearIrqStatus(); + RADIOLIB_ASSERT(state); + + // set implicit mode and expected len if applicable + if ((this->headerType == RADIOLIB_SX128X_LORA_HEADER_IMPLICIT) && + (getPacketType() == RADIOLIB_SX128X_PACKET_TYPE_LORA)) { + state = setPacketParamsLoRa(this->preambleLengthLoRa, this->headerType, this->payloadLen, this->crcLoRa, + this->invertIQEnabled); + RADIOLIB_ASSERT(state); + } + // if max(uint32_t) is used, revert to RxContinuous + if (cfg->receive.timeout == 0xFFFFFFFF) { + cfg->receive.timeout = 0xFFFF; + } + this->rxTimeout = cfg->receive.timeout; + } break; + + case (RADIOLIB_RADIO_MODE_TX): { + // check packet length + if (cfg->transmit.len > RADIOLIB_SX128X_MAX_PACKET_LENGTH) { + return (RADIOLIB_ERR_PACKET_TOO_LONG); + } + + // set packet Length + uint8_t modem = getPacketType(); + if (modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) { + state = setPacketParamsLoRa(this->preambleLengthLoRa, this->headerType, cfg->transmit.len, + this->crcLoRa, this->invertIQEnabled); + } else if ((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC)) { + state = setPacketParamsGFSK(this->preambleLengthGFSK, this->syncWordLen, this->syncWordMatch, + this->crcGFSK, this->whitening, this->packetType, cfg->transmit.len); + } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE) { + state = setPacketParamsBLE(this->connectionState, this->crcBLE, this->bleTestPayload, this->whitening); + } else { + return (RADIOLIB_ERR_WRONG_MODEM); + } + RADIOLIB_ASSERT(state); + + // update output power + state = setTxParams(this->power); + RADIOLIB_ASSERT(state); + + // set buffer pointers + state = setBufferBaseAddress(); + RADIOLIB_ASSERT(state); + + // write packet to buffer + if (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE) { + // first 2 bytes of BLE payload are PDU header + state = writeBuffer(cfg->transmit.data, cfg->transmit.len, 2); + RADIOLIB_ASSERT(state); + } else { + state = writeBuffer(cfg->transmit.data, cfg->transmit.len); + RADIOLIB_ASSERT(state); + } + + // set DIO mapping + state = setDioIrqParams(RADIOLIB_SX128X_IRQ_TX_DONE | RADIOLIB_SX128X_IRQ_RX_TX_TIMEOUT, + RADIOLIB_SX128X_IRQ_TX_DONE); + RADIOLIB_ASSERT(state); + + // clear interrupt flags + state = clearIrqStatus(); + RADIOLIB_ASSERT(state); + } break; + + default: + return (RADIOLIB_ERR_UNSUPPORTED); + } + + this->stagedMode = mode; + return (state); +} + +int16_t SX128x::launchMode() { + int16_t state; + switch (this->stagedMode) { + case (RADIOLIB_RADIO_MODE_RX): { + this->mod->setRfSwitchState(Module::MODE_RX); + state = setRx(this->rxTimeout); + RADIOLIB_ASSERT(state); + } break; + + case (RADIOLIB_RADIO_MODE_TX): { + this->mod->setRfSwitchState(Module::MODE_TX); + state = setTx(RADIOLIB_SX128X_TX_TIMEOUT_NONE); + RADIOLIB_ASSERT(state); + + // wait for BUSY to go low (= PA ramp up done) + while (this->mod->hal->digitalRead(this->mod->getGpio())) { + this->mod->hal->yield(); + } + } break; + + default: + return (RADIOLIB_ERR_UNSUPPORTED); + } + + this->stagedMode = RADIOLIB_RADIO_MODE_NONE; + return (state); +} + +#if !RADIOLIB_EXCLUDE_DIRECT_RECEIVE +void SX128x::setDirectAction(void (*func)(void)) { + // SX128x is unable to perform direct mode reception + // this method is implemented only for PhysicalLayer compatibility + (void)func; +} + +void SX128x::readBit(uint32_t pin) { + // SX128x is unable to perform direct mode reception + // this method is implemented only for PhysicalLayer compatibility + (void)pin; +} +#endif + +Module* SX128x::getMod() { + return (this->mod); +} + +int16_t SX128x::modSetup(uint8_t modem) { + // set module properties + this->mod->init(); + this->mod->hal->pinMode(this->mod->getIrq(), this->mod->hal->GpioModeInput); + this->mod->hal->pinMode(this->mod->getGpio(), this->mod->hal->GpioModeInput); + this->mod->spiConfig.widths[RADIOLIB_MODULE_SPI_WIDTH_ADDR] = Module::BITS_16; + this->mod->spiConfig.widths[RADIOLIB_MODULE_SPI_WIDTH_CMD] = Module::BITS_8; + this->mod->spiConfig.statusPos = 1; + this->mod->spiConfig.cmds[RADIOLIB_MODULE_SPI_COMMAND_READ] = RADIOLIB_SX128X_CMD_READ_REGISTER; + this->mod->spiConfig.cmds[RADIOLIB_MODULE_SPI_COMMAND_WRITE] = RADIOLIB_SX128X_CMD_WRITE_REGISTER; + this->mod->spiConfig.cmds[RADIOLIB_MODULE_SPI_COMMAND_NOP] = RADIOLIB_SX128X_CMD_NOP; + this->mod->spiConfig.cmds[RADIOLIB_MODULE_SPI_COMMAND_STATUS] = RADIOLIB_SX128X_CMD_GET_STATUS; + this->mod->spiConfig.stream = true; + this->mod->spiConfig.parseStatusCb = SPIparseStatus; + + // find the chip - this will also reset the module and verify the module + if (!SX128x::findChip(this->chipType)) { + RADIOLIB_DEBUG_BASIC_PRINTLN("No SX128x found!"); + this->mod->term(); + return (RADIOLIB_ERR_CHIP_NOT_FOUND); + } + RADIOLIB_DEBUG_BASIC_PRINTLN("M\tSX128x"); + + // configure settings not accessible by API + return (config(modem)); +} + +uint8_t SX128x::getStatus() { + uint8_t data = 0; + this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_STATUS, &data, 0); + return (data); +} + +int16_t SX128x::writeRegister(uint16_t addr, const uint8_t* data, uint8_t numBytes) { + this->mod->SPIwriteRegisterBurst(addr, data, numBytes); + return (RADIOLIB_ERR_NONE); +} + +int16_t SX128x::readRegister(uint16_t addr, uint8_t* data, uint8_t numBytes) { + // send the command + this->mod->SPIreadRegisterBurst(addr, numBytes, data); + + // check the status + int16_t state = this->mod->SPIcheckStream(); + return (state); +} + +int16_t SX128x::writeBuffer(const uint8_t* data, uint8_t numBytes, uint8_t offset) { + const uint8_t cmd[] = {RADIOLIB_SX128X_CMD_WRITE_BUFFER, offset}; + return (this->mod->SPIwriteStream(cmd, 2, data, numBytes)); +} + +int16_t SX128x::readBuffer(uint8_t* data, uint8_t numBytes, uint8_t offset) { + const uint8_t cmd[] = {RADIOLIB_SX128X_CMD_READ_BUFFER, offset}; + return (this->mod->SPIreadStream(cmd, 2, data, numBytes)); +} + +int16_t SX128x::setTx(uint16_t periodBaseCount, uint8_t periodBase) { + const uint8_t data[] = {periodBase, (uint8_t)((periodBaseCount >> 8) & 0xFF), (uint8_t)(periodBaseCount & 0xFF)}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_TX, data, 3)); +} + +int16_t SX128x::setRx(uint16_t periodBaseCount, uint8_t periodBase) { + const uint8_t data[] = {periodBase, (uint8_t)((periodBaseCount >> 8) & 0xFF), (uint8_t)(periodBaseCount & 0xFF)}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_RX, data, 3)); +} + +int16_t SX128x::setCad(uint8_t symbolNum) { + // configure parameters + int16_t state = this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_CAD_PARAMS, &symbolNum, 1); + RADIOLIB_ASSERT(state); + + // start CAD + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_CAD, NULL, 0)); +} + +uint8_t SX128x::getPacketType() { + uint8_t data = 0xFF; + this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_PACKET_TYPE, &data, 1); + return (data); +} + +int16_t SX128x::setRfFrequency(uint32_t frf) { + const uint8_t data[] = {(uint8_t)((frf >> 16) & 0xFF), (uint8_t)((frf >> 8) & 0xFF), (uint8_t)(frf & 0xFF)}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_RF_FREQUENCY, data, 3)); +} + +int16_t SX128x::setTxParams(uint8_t pwr, uint8_t rampTime) { + const uint8_t data[] = {pwr, rampTime}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_TX_PARAMS, data, 2)); +} + +int16_t SX128x::setBufferBaseAddress(uint8_t txBaseAddress, uint8_t rxBaseAddress) { + const uint8_t data[] = {txBaseAddress, rxBaseAddress}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_BUFFER_BASE_ADDRESS, data, 2)); +} + +int16_t SX128x::setModulationParams(uint8_t modParam1, uint8_t modParam2, uint8_t modParam3) { + const uint8_t data[] = {modParam1, modParam2, modParam3}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_MODULATION_PARAMS, data, 3)); +} + +int16_t SX128x::setPacketParamsGFSK(uint8_t preambleLen, + uint8_t syncLen, + uint8_t syncMatch, + uint8_t crcLen, + uint8_t whiten, + uint8_t hdrType, + uint8_t payLen) { + const uint8_t data[] = {preambleLen, syncLen, syncMatch, hdrType, payLen, crcLen, whiten}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_PACKET_PARAMS, data, 7)); +} + +int16_t SX128x::setPacketParamsBLE(uint8_t connState, uint8_t crcLen, uint8_t bleTest, uint8_t whiten) { + const uint8_t data[] = {connState, crcLen, bleTest, whiten, 0x00, 0x00, 0x00}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_PACKET_PARAMS, data, 7)); +} + +int16_t SX128x::setPacketParamsLoRa(uint8_t preambleLen, uint8_t hdrType, uint8_t payLen, uint8_t crc, uint8_t invIQ) { + const uint8_t data[] = {preambleLen, hdrType, payLen, crc, invIQ, 0x00, 0x00}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_PACKET_PARAMS, data, 7)); +} + +int16_t SX128x::setDioIrqParams(uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask) { + const uint8_t data[] = {(uint8_t)((irqMask >> 8) & 0xFF), (uint8_t)(irqMask & 0xFF), + (uint8_t)((dio1Mask >> 8) & 0xFF), (uint8_t)(dio1Mask & 0xFF), + (uint8_t)((dio2Mask >> 8) & 0xFF), (uint8_t)(dio2Mask & 0xFF), + (uint8_t)((dio3Mask >> 8) & 0xFF), (uint8_t)(dio3Mask & 0xFF)}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_DIO_IRQ_PARAMS, data, 8)); +} + +uint16_t SX128x::getIrqStatus() { + uint8_t data[] = {0x00, 0x00}; + this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_IRQ_STATUS, data, 2); + return (((uint16_t)(data[0]) << 8) | data[1]); +} + +int16_t SX128x::clearIrqStatus(uint16_t clearIrqParams) { + const uint8_t data[] = {(uint8_t)((clearIrqParams >> 8) & 0xFF), (uint8_t)(clearIrqParams & 0xFF)}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_CLEAR_IRQ_STATUS, data, 2)); +} + +int16_t SX128x::setRangingRole(uint8_t role) { + const uint8_t data[] = {role}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_RANGING_ROLE, data, 1)); +} + +int16_t SX128x::setPacketType(uint8_t type) { + const uint8_t data[] = {type}; + return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_PACKET_TYPE, data, 1)); +} + +int16_t SX128x::setPacketMode(uint8_t mode, uint8_t len) { + // check active modem + uint8_t modem = getPacketType(); + if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC))) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // set requested packet mode + int16_t state = setPacketParamsGFSK(this->preambleLengthGFSK, this->syncWordLen, this->syncWordMatch, this->crcGFSK, + this->whitening, mode, len); + RADIOLIB_ASSERT(state); + + // update cached value + this->packetType = mode; + return (state); +} + +int16_t SX128x::setHeaderType(uint8_t hdrType, size_t len) { + // check active modem + uint8_t modem = getPacketType(); + if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING))) { + return (RADIOLIB_ERR_WRONG_MODEM); + } + + // update packet parameters + this->headerType = hdrType; + this->payloadLen = len; + return (setPacketParamsLoRa(this->preambleLengthLoRa, this->headerType, this->payloadLen, this->crcLoRa, + this->invertIQEnabled)); +} + +int16_t SX128x::config(uint8_t modem) { + // reset buffer base address + int16_t state = setBufferBaseAddress(); + RADIOLIB_ASSERT(state); + + // set modem + uint8_t data[1]; + data[0] = modem; + state = this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_PACKET_TYPE, data, 1); + RADIOLIB_ASSERT(state); + + // set CAD parameters + data[0] = RADIOLIB_SX128X_CAD_ON_8_SYMB; + state = this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_CAD_PARAMS, data, 1); + RADIOLIB_ASSERT(state); + + // set regulator mode to DC-DC + data[0] = RADIOLIB_SX128X_REGULATOR_DC_DC; + state = this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_REGULATOR_MODE, data, 1); + RADIOLIB_ASSERT(state); + + return (RADIOLIB_ERR_NONE); +} + +int16_t SX128x::SPIparseStatus(uint8_t in) { + if ((in & 0b00011100) == RADIOLIB_SX128X_STATUS_CMD_TIMEOUT) { + return (RADIOLIB_ERR_SPI_CMD_TIMEOUT); + } else if ((in & 0b00011100) == RADIOLIB_SX128X_STATUS_CMD_ERROR) { + return (RADIOLIB_ERR_SPI_CMD_INVALID); + } else if ((in & 0b00011100) == RADIOLIB_SX128X_STATUS_CMD_FAILED) { + return (RADIOLIB_ERR_SPI_CMD_FAILED); + } else if ((in == 0x00) || (in == 0xFF)) { + return (RADIOLIB_ERR_CHIP_NOT_FOUND); + } + return (RADIOLIB_ERR_NONE); +} + +bool SX128x::findChip(const char* verStr) { + uint8_t i = 0; + bool flagFound = false; + while ((i < 10) && !flagFound) { + // reset the module + reset(true); + + // read the version string + char version[16] = {0}; + this->mod->SPIreadRegisterBurst(RADIOLIB_SX128X_REG_VERSION_STRING, 16, reinterpret_cast(version)); + + // check version register + if (strncmp(verStr, version, 6) == 0) { + RADIOLIB_DEBUG_BASIC_PRINTLN("Found SX128x: RADIOLIB_SX128X_REG_VERSION_STRING:"); + RADIOLIB_DEBUG_BASIC_HEXDUMP(reinterpret_cast(version), 16, RADIOLIB_SX128X_REG_VERSION_STRING); + RADIOLIB_DEBUG_BASIC_PRINTLN(); + flagFound = true; + } else { +#if RADIOLIB_DEBUG_BASIC + RADIOLIB_DEBUG_BASIC_PRINTLN("SX128x not found! (%d of 10 tries) RADIOLIB_SX128X_REG_VERSION_STRING:", + i + 1); + RADIOLIB_DEBUG_BASIC_HEXDUMP(reinterpret_cast(version), 16, RADIOLIB_SX128X_REG_VERSION_STRING); + RADIOLIB_DEBUG_BASIC_PRINTLN("Expected string: %s", verStr); +#endif + this->mod->hal->delay(10); + i++; + } + } + + return (flagFound); +} + +#endif diff --git a/FprimeZephyrReference/Components/MyComponent/SX128x.h b/FprimeZephyrReference/Components/MyComponent/SX128x.h new file mode 100644 index 00000000..6f69e752 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/SX128x.h @@ -0,0 +1,1045 @@ +#if !defined(_RADIOLIB_SX128X_H) +#define _RADIOLIB_SX128X_H + +#include "../../TypeDef.h" + +#if !RADIOLIB_EXCLUDE_SX128X + +#include "../../Module.h" +#include "../../protocols/PhysicalLayer/PhysicalLayer.h" + +// SX128X physical layer properties +#define RADIOLIB_SX128X_FREQUENCY_STEP_SIZE 198.3642578 +#define RADIOLIB_SX128X_MAX_PACKET_LENGTH 255 +#define RADIOLIB_SX128X_CRYSTAL_FREQ 52.0f +#define RADIOLIB_SX128X_DIV_EXPONENT 18 + +// SX128X SPI commands +#define RADIOLIB_SX128X_CMD_NOP 0x00 +#define RADIOLIB_SX128X_CMD_GET_STATUS 0xC0 +#define RADIOLIB_SX128X_CMD_WRITE_REGISTER 0x18 +#define RADIOLIB_SX128X_CMD_READ_REGISTER 0x19 +#define RADIOLIB_SX128X_CMD_WRITE_BUFFER 0x1A +#define RADIOLIB_SX128X_CMD_READ_BUFFER 0x1B +#define RADIOLIB_SX128X_CMD_SAVE_CONTEXT 0xD5 +#define RADIOLIB_SX128X_CMD_SET_SLEEP 0x84 +#define RADIOLIB_SX128X_CMD_SET_STANDBY 0x80 +#define RADIOLIB_SX128X_CMD_SET_FS 0xC1 +#define RADIOLIB_SX128X_CMD_SET_TX 0x83 +#define RADIOLIB_SX128X_CMD_SET_RX 0x82 +#define RADIOLIB_SX128X_CMD_SET_RX_DUTY_CYCLE 0x94 +#define RADIOLIB_SX128X_CMD_SET_CAD 0xC5 +#define RADIOLIB_SX128X_CMD_SET_TX_CONTINUOUS_WAVE 0xD1 +#define RADIOLIB_SX128X_CMD_SET_TX_CONTINUOUS_PREAMBLE 0xD2 +#define RADIOLIB_SX128X_CMD_SET_PACKET_TYPE 0x8A +#define RADIOLIB_SX128X_CMD_GET_PACKET_TYPE 0x03 +#define RADIOLIB_SX128X_CMD_SET_RF_FREQUENCY 0x86 +#define RADIOLIB_SX128X_CMD_SET_TX_PARAMS 0x8E +#define RADIOLIB_SX128X_CMD_SET_CAD_PARAMS 0x88 +#define RADIOLIB_SX128X_CMD_SET_BUFFER_BASE_ADDRESS 0x8F +#define RADIOLIB_SX128X_CMD_SET_MODULATION_PARAMS 0x8B +#define RADIOLIB_SX128X_CMD_SET_PACKET_PARAMS 0x8C +#define RADIOLIB_SX128X_CMD_GET_RX_BUFFER_STATUS 0x17 +#define RADIOLIB_SX128X_CMD_GET_PACKET_STATUS 0x1D +#define RADIOLIB_SX128X_CMD_GET_RSSI_INST 0x1F +#define RADIOLIB_SX128X_CMD_SET_DIO_IRQ_PARAMS 0x8D +#define RADIOLIB_SX128X_CMD_GET_IRQ_STATUS 0x15 +#define RADIOLIB_SX128X_CMD_CLEAR_IRQ_STATUS 0x97 +#define RADIOLIB_SX128X_CMD_SET_REGULATOR_MODE 0x96 +#define RADIOLIB_SX128X_CMD_SET_SAVE_CONTEXT 0xD5 +#define RADIOLIB_SX128X_CMD_SET_AUTO_TX 0x98 +#define RADIOLIB_SX128X_CMD_SET_AUTO_FS 0x9E +#define RADIOLIB_SX128X_CMD_SET_PERF_COUNTER_MODE 0x9C +#define RADIOLIB_SX128X_CMD_SET_LONG_PREAMBLE 0x9B +#define RADIOLIB_SX128X_CMD_SET_UART_SPEED 0x9D +#define RADIOLIB_SX128X_CMD_SET_RANGING_ROLE 0xA3 +#define RADIOLIB_SX128X_CMD_SET_ADVANCED_RANGING 0x9A + +// SX128X register map +#define RADIOLIB_SX128X_REG_VERSION_STRING 0x01F0 +#define RADIOLIB_SX128X_REG_GAIN_MODE 0x0891 +#define RADIOLIB_SX128X_REG_MANUAL_GAIN_CONTROL_ENABLE_2 0x0895 +#define RADIOLIB_SX128X_REG_MANUAL_GAIN_SETTING 0x089E +#define RADIOLIB_SX128X_REG_MANUAL_GAIN_CONTROL_ENABLE_1 0x089F +#define RADIOLIB_SX128X_REG_SYNCH_PEAK_ATTENUATION 0x08C2 +#define RADIOLIB_SX128X_REG_LORA_FIXED_PAYLOAD_LENGTH 0x0901 +#define RADIOLIB_SX128X_REG_LORA_HEADER_MODE 0x0903 +#define RADIOLIB_SX128X_REG_MASTER_RANGING_ADDRESS_BYTE_3 0x0912 +#define RADIOLIB_SX128X_REG_MASTER_RANGING_ADDRESS_BYTE_2 0x0913 +#define RADIOLIB_SX128X_REG_MASTER_RANGING_ADDRESS_BYTE_1 0x0914 +#define RADIOLIB_SX128X_REG_MASTER_RANGING_ADDRESS_BYTE_0 0x0915 +#define RADIOLIB_SX128X_REG_SLAVE_RANGING_ADDRESS_BYTE_3 0x0916 +#define RADIOLIB_SX128X_REG_SLAVE_RANGING_ADDRESS_BYTE_2 0x0917 +#define RADIOLIB_SX128X_REG_SLAVE_RANGING_ADDRESS_BYTE_1 0x0918 +#define RADIOLIB_SX128X_REG_SLAVE_RANGING_ADDRESS_BYTE_0 0x0919 +#define RADIOLIB_SX128X_REG_RANGING_FILTER_WINDOW_SIZE 0x091E +#define RADIOLIB_SX128X_REG_RANGING_FILTER_RESET 0x0923 +#define RADIOLIB_SX128X_REG_RANGING_TYPE 0x0924 +#define RADIOLIB_SX128X_REG_LORA_SF_CONFIG 0x0925 +#define RADIOLIB_SX128X_REG_RANGING_ADDRESS_SWITCH 0x0927 +#define RADIOLIB_SX128X_REG_RANGING_CALIBRATION_BYTE_2 0x092B +#define RADIOLIB_SX128X_REG_RANGING_CALIBRATION_MSB 0x092C +#define RADIOLIB_SX128X_REG_RANGING_CALIBRATION_LSB 0x092D +#define RADIOLIB_SX128X_REG_SLAVE_RANGING_ADDRESS_WIDTH 0x0931 +#define RADIOLIB_SX128X_REG_FREQ_ERROR_CORRECTION 0x093C +#define RADIOLIB_SX128X_REG_LORA_SYNC_WORD_MSB 0x0944 +#define RADIOLIB_SX128X_REG_LORA_SYNC_WORD_LSB 0x0945 +#define RADIOLIB_SX128X_REG_LORA_RX_CODING_RATE 0x0950 +#define RADIOLIB_SX128X_REG_RANGING_FILTER_RSSI_OFFSET 0x0953 +#define RADIOLIB_SX128X_REG_FEI_MSB 0x0954 +#define RADIOLIB_SX128X_REG_FEI_MID 0x0955 +#define RADIOLIB_SX128X_REG_FEI_LSB 0x0956 +#define RADIOLIB_SX128X_REG_RANGING_ADDRESS_MSB 0x095F +#define RADIOLIB_SX128X_REG_RANGING_ADDRESS_LSB 0x0960 +#define RADIOLIB_SX128X_REG_RANGING_RESULT_MSB 0x0961 +#define RADIOLIB_SX128X_REG_RANGING_RESULT_MID 0x0962 +#define RADIOLIB_SX128X_REG_RANGING_RESULT_LSB 0x0963 +#define RADIOLIB_SX128X_REG_RANGING_RSSI 0x0964 +#define RADIOLIB_SX128X_REG_RANGING_LORA_CLOCK_ENABLE 0x097F +#define RADIOLIB_SX128X_REG_PACKET_PREAMBLE_SETTINGS 0x09C1 +#define RADIOLIB_SX128X_REG_WHITENING_INITIAL_VALUE 0x09C5 +#define RADIOLIB_SX128X_REG_CRC_POLYNOMIAL_MSB 0x09C6 +#define RADIOLIB_SX128X_REG_CRC_POLYNOMIAL_LSB 0x09C7 +#define RADIOLIB_SX128X_REG_CRC_INITIAL_MSB 0x09C8 +#define RADIOLIB_SX128X_REG_CRC_INITIAL_LSB 0x09C9 +#define RADIOLIB_SX128X_REG_BLE_CRC_INITIAL_MSB 0x09C7 +#define RADIOLIB_SX128X_REG_BLE_CRC_INITIAL_MID (RADIOLIB_SX128X_REG_CRC_INITIAL_MSB) +#define RADIOLIB_SX128X_REG_BLE_CRC_INITIAL_LSB (RADIOLIB_SX128X_REG_CRC_INITIAL_LSB) +#define RADIOLIB_SX128X_REG_SYNCH_ADDRESS_CONTROL 0x09CD +#define RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_4 0x09CE +#define RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_3 0x09CF +#define RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_2 0x09D0 +#define RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_1 0x09D1 +#define RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_0 0x09D2 +#define RADIOLIB_SX128X_REG_SYNC_WORD_2_BYTE_4 0x09D3 +#define RADIOLIB_SX128X_REG_SYNC_WORD_2_BYTE_3 0x09D4 +#define RADIOLIB_SX128X_REG_SYNC_WORD_2_BYTE_2 0x09D5 +#define RADIOLIB_SX128X_REG_SYNC_WORD_2_BYTE_1 0x09D6 +#define RADIOLIB_SX128X_REG_SYNC_WORD_2_BYTE_0 0x09D7 +#define RADIOLIB_SX128X_REG_SYNC_WORD_3_BYTE_4 0x09D8 +#define RADIOLIB_SX128X_REG_SYNC_WORD_3_BYTE_3 0x09D9 +#define RADIOLIB_SX128X_REG_SYNC_WORD_3_BYTE_2 0x09DA +#define RADIOLIB_SX128X_REG_SYNC_WORD_3_BYTE_1 0x09DB +#define RADIOLIB_SX128X_REG_SYNC_WORD_3_BYTE_0 0x09DC +#define RADIOLIB_SX128X_REG_ACCESS_ADDRESS_BYTE_3 (RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_3) +#define RADIOLIB_SX128X_REG_ACCESS_ADDRESS_BYTE_2 (RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_2) +#define RADIOLIB_SX128X_REG_ACCESS_ADDRESS_BYTE_1 (RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_1) +#define RADIOLIB_SX128X_REG_ACCESS_ADDRESS_BYTE_0 (RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_0) + +// SX128X SPI command variables +// RADIOLIB_SX128X_CMD_GET_STATUS MSB LSB DESCRIPTION +#define RADIOLIB_SX128X_STATUS_MODE_STDBY_RC 0b01000000 // 7 5 current chip mode: STDBY_RC +#define RADIOLIB_SX128X_STATUS_MODE_STDBY_XOSC 0b01100000 // 7 5 STDBY_XOSC +#define RADIOLIB_SX128X_STATUS_MODE_FS 0b10000000 // 7 5 FS +#define RADIOLIB_SX128X_STATUS_MODE_RX 0b10100000 // 7 5 Rx +#define RADIOLIB_SX128X_STATUS_MODE_TX 0b11000000 // 7 5 Tx +#define RADIOLIB_SX128X_STATUS_CMD_PROCESSED 0b00000100 // 4 2 command status: processing OK +#define RADIOLIB_SX128X_STATUS_DATA_AVAILABLE 0b00001000 // 4 2 data available +#define RADIOLIB_SX128X_STATUS_CMD_TIMEOUT 0b00001100 // 4 2 timeout +#define RADIOLIB_SX128X_STATUS_CMD_ERROR 0b00010000 // 4 2 processing error +#define RADIOLIB_SX128X_STATUS_CMD_FAILED 0b00010100 // 4 2 failed to execute +#define RADIOLIB_SX128X_STATUS_TX_DONE 0b00011000 // 4 2 transmission finished +#define RADIOLIB_SX128X_STATUS_BUSY 0b00000001 // 0 0 chip busy +#define RADIOLIB_SX128X_STATUS_SPI_FAILED 0b11111111 // 7 0 SPI transaction failed + +// RADIOLIB_SX128X_CMD_SET_SLEEP +#define RADIOLIB_SX128X_SLEEP_DATA_BUFFER_FLUSH 0b00000000 // 1 1 data buffer behavior in sleep mode: flush +#define RADIOLIB_SX128X_SLEEP_DATA_BUFFER_RETAIN 0b00000010 // 1 1 retain +#define RADIOLIB_SX128X_SLEEP_DATA_RAM_FLUSH \ + 0b00000000 // 0 0 data RAM (configuration) behavior in sleep mode: flush +#define RADIOLIB_SX128X_SLEEP_DATA_RAM_RETAIN \ + 0b00000001 // 0 0 retain + +// RADIOLIB_SX128X_CMD_SET_STANDBY +#define RADIOLIB_SX128X_STANDBY_RC 0x00 // 7 0 standby mode: 13 MHz RC oscillator +#define RADIOLIB_SX128X_STANDBY_XOSC 0x01 // 7 0 52 MHz crystal oscillator + +// RADIOLIB_SX128X_CMD_SET_TX + RADIOLIB_SX128X_CMD_SET_RX + RADIOLIB_SX128X_CMD_SET_RX_DUTY_CYCLE +#define RADIOLIB_SX128X_PERIOD_BASE_15_625_US 0x00 // 7 0 time period step: 15.625 us +#define RADIOLIB_SX128X_PERIOD_BASE_62_5_US 0x01 // 7 0 62.5 us +#define RADIOLIB_SX128X_PERIOD_BASE_1_MS 0x02 // 7 0 1 ms +#define RADIOLIB_SX128X_PERIOD_BASE_4_MS 0x03 // 7 0 4 ms + +// RADIOLIB_SX128X_CMD_SET_TX +#define RADIOLIB_SX128X_TX_TIMEOUT_NONE 0x0000 // 15 0 Tx timeout duration: no timeout (Tx single mode) + +// RADIOLIB_SX128X_CMD_SET_RX +#define RADIOLIB_SX128X_RX_TIMEOUT_NONE 0x0000 // 15 0 Rx timeout duration: no timeout (Rx single mode) +#define RADIOLIB_SX128X_RX_TIMEOUT_INF 0xFFFF // 15 0 infinite (Rx continuous mode) + +// RADIOLIB_SX128X_CMD_SET_PACKET_TYPE +#define RADIOLIB_SX128X_PACKET_TYPE_GFSK 0x00 // 7 0 packet type: (G)FSK +#define RADIOLIB_SX128X_PACKET_TYPE_LORA 0x01 // 7 0 LoRa +#define RADIOLIB_SX128X_PACKET_TYPE_RANGING 0x02 // 7 0 ranging engine +#define RADIOLIB_SX128X_PACKET_TYPE_FLRC 0x03 // 7 0 FLRC +#define RADIOLIB_SX128X_PACKET_TYPE_BLE 0x04 // 7 0 BLE + +// RADIOLIB_SX128X_CMD_SET_TX_PARAMS +#define RADIOLIB_SX128X_PA_RAMP_02_US 0x00 // 7 0 PA ramp time: 2 us +#define RADIOLIB_SX128X_PA_RAMP_04_US 0x20 // 7 0 4 us +#define RADIOLIB_SX128X_PA_RAMP_06_US 0x40 // 7 0 6 us +#define RADIOLIB_SX128X_PA_RAMP_08_US 0x60 // 7 0 8 us +#define RADIOLIB_SX128X_PA_RAMP_10_US 0x80 // 7 0 10 us +#define RADIOLIB_SX128X_PA_RAMP_12_US 0xA0 // 7 0 12 us +#define RADIOLIB_SX128X_PA_RAMP_16_US 0xC0 // 7 0 16 us +#define RADIOLIB_SX128X_PA_RAMP_20_US 0xE0 // 7 0 20 us + +// RADIOLIB_SX128X_CMD_SET_CAD_PARAMS +#define RADIOLIB_SX128X_CAD_ON_1_SYMB 0x00 // 7 0 number of symbols used for CAD: 1 +#define RADIOLIB_SX128X_CAD_ON_2_SYMB 0x20 // 7 0 2 +#define RADIOLIB_SX128X_CAD_ON_4_SYMB 0x40 // 7 0 4 +#define RADIOLIB_SX128X_CAD_ON_8_SYMB 0x60 // 7 0 8 +#define RADIOLIB_SX128X_CAD_ON_16_SYMB 0x80 // 7 0 16 +#define RADIOLIB_SX128X_CAD_PARAM_DEFAULT RADIOLIB_SX128X_CAD_ON_8_SYMB + +// RADIOLIB_SX128X_CMD_SET_MODULATION_PARAMS +#define RADIOLIB_SX128X_BLE_GFSK_BR_2_000_BW_2_4 \ + 0x04 // 7 0 GFSK/BLE bit rate and bandwidth setting: 2.0 Mbps 2.4 MHz +#define RADIOLIB_SX128X_BLE_GFSK_BR_1_600_BW_2_4 \ + 0x28 // 7 0 1.6 Mbps 2.4 MHz +#define RADIOLIB_SX128X_BLE_GFSK_BR_1_000_BW_2_4 \ + 0x4C // 7 0 1.0 Mbps 2.4 MHz +#define RADIOLIB_SX128X_BLE_GFSK_BR_1_000_BW_1_2 \ + 0x45 // 7 0 1.0 Mbps 1.2 MHz +#define RADIOLIB_SX128X_BLE_GFSK_BR_0_800_BW_2_4 \ + 0x70 // 7 0 0.8 Mbps 2.4 MHz +#define RADIOLIB_SX128X_BLE_GFSK_BR_0_800_BW_1_2 \ + 0x69 // 7 0 0.8 Mbps 1.2 MHz +#define RADIOLIB_SX128X_BLE_GFSK_BR_0_500_BW_1_2 \ + 0x8D // 7 0 0.5 Mbps 1.2 MHz +#define RADIOLIB_SX128X_BLE_GFSK_BR_0_500_BW_0_6 \ + 0x86 // 7 0 0.5 Mbps 0.6 MHz +#define RADIOLIB_SX128X_BLE_GFSK_BR_0_400_BW_1_2 \ + 0xB1 // 7 0 0.4 Mbps 1.2 MHz +#define RADIOLIB_SX128X_BLE_GFSK_BR_0_400_BW_0_6 \ + 0xAA // 7 0 0.4 Mbps 0.6 MHz +#define RADIOLIB_SX128X_BLE_GFSK_BR_0_250_BW_0_6 \ + 0xCE // 7 0 0.25 Mbps 0.6 MHz +#define RADIOLIB_SX128X_BLE_GFSK_BR_0_250_BW_0_3 \ + 0xC7 // 7 0 0.25 Mbps 0.3 MHz +#define RADIOLIB_SX128X_BLE_GFSK_BR_0_125_BW_0_3 \ + 0xEF // 7 0 0.125 Mbps 0.3 MHz +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_0_35 0x00 // 7 0 GFSK/BLE modulation index: 0.35 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_0_50 0x01 // 7 0 0.50 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_0_75 0x02 // 7 0 0.75 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_1_00 0x03 // 7 0 1.00 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_1_25 0x04 // 7 0 1.25 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_1_50 0x05 // 7 0 1.50 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_1_75 0x06 // 7 0 1.75 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_2_00 0x07 // 7 0 2.00 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_2_25 0x08 // 7 0 2.25 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_2_50 0x09 // 7 0 2.50 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_2_75 0x0A // 7 0 2.75 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_3_00 0x0B // 7 0 3.00 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_3_25 0x0C // 7 0 3.25 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_3_50 0x0D // 7 0 3.50 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_3_75 0x0E // 7 0 3.75 +#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_4_00 0x0F // 7 0 4.00 +#define RADIOLIB_SX128X_BLE_GFSK_BT_OFF 0x00 // 7 0 GFSK Gaussian filter BT product: filter disabled +#define RADIOLIB_SX128X_BLE_GFSK_BT_1_0 0x10 // 7 0 1.0 +#define RADIOLIB_SX128X_BLE_GFSK_BT_0_5 0x20 // 7 0 0.5 +#define RADIOLIB_SX128X_FLRC_BR_1_300_BW_1_2 0x45 // 7 0 FLRC bit rate and bandwidth setting: 1.3 Mbps 1.2 MHz +#define RADIOLIB_SX128X_FLRC_BR_1_000_BW_1_2 0x69 // 7 0 1.04 Mbps 1.2 MHz +#define RADIOLIB_SX128X_FLRC_BR_0_650_BW_0_6 0x86 // 7 0 0.65 Mbps 0.6 MHz +#define RADIOLIB_SX128X_FLRC_BR_0_520_BW_0_6 0xAA // 7 0 0.52 Mbps 0.6 MHz +#define RADIOLIB_SX128X_FLRC_BR_0_325_BW_0_3 0xC7 // 7 0 0.325 Mbps 0.3 MHz +#define RADIOLIB_SX128X_FLRC_BR_0_260_BW_0_3 0xEB // 7 0 0.260 Mbps 0.3 MHz +#define RADIOLIB_SX128X_FLRC_CR_1_2 0x00 // 7 0 FLRC coding rate: 1/2 +#define RADIOLIB_SX128X_FLRC_CR_3_4 0x02 // 7 0 3/4 +#define RADIOLIB_SX128X_FLRC_CR_1_0 0x04 // 7 0 1/1 +#define RADIOLIB_SX128X_FLRC_BT_OFF 0x00 // 7 0 FLRC Gaussian filter BT product: filter disabled +#define RADIOLIB_SX128X_FLRC_BT_1_0 0x10 // 7 0 1.0 +#define RADIOLIB_SX128X_FLRC_BT_0_5 0x20 // 7 0 0.5 +#define RADIOLIB_SX128X_LORA_SF_5 0x50 // 7 0 LoRa spreading factor: 5 +#define RADIOLIB_SX128X_LORA_SF_6 0x60 // 7 0 6 +#define RADIOLIB_SX128X_LORA_SF_7 0x70 // 7 0 7 +#define RADIOLIB_SX128X_LORA_SF_8 0x80 // 7 0 8 +#define RADIOLIB_SX128X_LORA_SF_9 0x90 // 7 0 9 +#define RADIOLIB_SX128X_LORA_SF_10 0xA0 // 7 0 10 +#define RADIOLIB_SX128X_LORA_SF_11 0xB0 // 7 0 11 +#define RADIOLIB_SX128X_LORA_SF_12 0xC0 // 7 0 12 +#define RADIOLIB_SX128X_LORA_BW_1625_00 0x0A // 7 0 LoRa bandwidth: 1625.0 kHz +#define RADIOLIB_SX128X_LORA_BW_812_50 0x18 // 7 0 812.5 kHz +#define RADIOLIB_SX128X_LORA_BW_406_25 0x26 // 7 0 406.25 kHz +#define RADIOLIB_SX128X_LORA_BW_203_125 0x34 // 7 0 203.125 kHz +#define RADIOLIB_SX128X_LORA_CR_4_5 0x01 // 7 0 LoRa coding rate: 4/5 +#define RADIOLIB_SX128X_LORA_CR_4_6 0x02 // 7 0 4/6 +#define RADIOLIB_SX128X_LORA_CR_4_7 0x03 // 7 0 4/7 +#define RADIOLIB_SX128X_LORA_CR_4_8 0x04 // 7 0 4/8 +#define RADIOLIB_SX128X_LORA_CR_4_5_LI 0x05 // 7 0 4/5, long interleaving +#define RADIOLIB_SX128X_LORA_CR_4_6_LI 0x06 // 7 0 4/6, long interleaving +#define RADIOLIB_SX128X_LORA_CR_4_8_LI 0x07 // 7 0 4/8, long interleaving + +// RADIOLIB_SX128X_CMD_SET_PACKET_PARAMS +#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_OFF 0x00 // 7 0 GFSK/FLRC sync word used: none +#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1 0x10 // 7 0 sync word 1 +#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_2 0x20 // 7 0 sync word 2 +#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1_2 0x30 // 7 0 sync words 1 and 2 +#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_3 0x40 // 7 0 sync word 3 +#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1_3 0x50 // 7 0 sync words 1 and 3 +#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_2_3 0x60 // 7 0 sync words 2 and 3 +#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1_2_3 0x70 // 7 0 sync words 1, 2 and 3 +#define RADIOLIB_SX128X_GFSK_FLRC_PACKET_FIXED 0x00 // 7 0 GFSK/FLRC packet length mode: fixed +#define RADIOLIB_SX128X_GFSK_FLRC_PACKET_VARIABLE 0x20 // 7 0 variable +#define RADIOLIB_SX128X_GFSK_FLRC_CRC_OFF 0x00 // 7 0 GFSK/FLRC packet CRC: none +#define RADIOLIB_SX128X_GFSK_FLRC_CRC_1_BYTE 0x10 // 7 0 1 byte +#define RADIOLIB_SX128X_GFSK_FLRC_CRC_2_BYTE 0x20 // 7 0 2 bytes +#define RADIOLIB_SX128X_GFSK_FLRC_CRC_3_BYTE 0x30 // 7 0 3 bytes (FLRC only) +#define RADIOLIB_SX128X_GFSK_BLE_WHITENING_ON 0x00 // 7 0 GFSK/BLE whitening: enabled +#define RADIOLIB_SX128X_GFSK_BLE_WHITENING_OFF 0x08 // 7 0 disabled +#define RADIOLIB_SX128X_BLE_PAYLOAD_LENGTH_MAX_31 0x00 // 7 0 BLE maximum payload length: 31 bytes +#define RADIOLIB_SX128X_BLE_PAYLOAD_LENGTH_MAX_37 0x20 // 7 0 37 bytes +#define RADIOLIB_SX128X_BLE_PAYLOAD_LENGTH_TEST 0x40 // 7 0 63 bytes (test mode) +#define RADIOLIB_SX128X_BLE_PAYLOAD_LENGTH_MAX_255 \ + 0x80 // 7 0 255 bytes (Bluetooth 4.2 and above) +#define RADIOLIB_SX128X_BLE_CRC_OFF 0x00 // 7 0 BLE packet CRC: none +#define RADIOLIB_SX128X_BLE_CRC_3_BYTE 0x10 // 7 0 3 byte +#define RADIOLIB_SX128X_BLE_PRBS_9 0x00 // 7 0 BLE test payload contents: PRNG sequence using x^9 + x^5 + x +#define RADIOLIB_SX128X_BLE_EYELONG 0x04 // 7 0 repeated 0xF0 +#define RADIOLIB_SX128X_BLE_EYESHORT 0x08 // 7 0 repeated 0xAA +#define RADIOLIB_SX128X_BLE_PRBS_15 \ + 0x0C // 7 0 PRNG sequence using x^15 + x^14 + x^13 + x^12 + x^2 + x + 1 +#define RADIOLIB_SX128X_BLE_ALL_1 0x10 // 7 0 repeated 0xFF +#define RADIOLIB_SX128X_BLE_ALL_0 0x14 // 7 0 repeated 0x00 +#define RADIOLIB_SX128X_BLE_EYELONG_INV 0x18 // 7 0 repeated 0x0F +#define RADIOLIB_SX128X_BLE_EYESHORT_INV 0x1C // 7 0 repeated 0x55 +#define RADIOLIB_SX128X_FLRC_SYNC_WORD_OFF 0x00 // 7 0 FLRC sync word: disabled +#define RADIOLIB_SX128X_FLRC_SYNC_WORD_ON 0x04 // 7 0 enabled +#define RADIOLIB_SX128X_LORA_HEADER_EXPLICIT 0x00 // 7 0 LoRa header mode: explicit +#define RADIOLIB_SX128X_LORA_HEADER_IMPLICIT 0x80 // 7 0 implicit +#define RADIOLIB_SX128X_LORA_CRC_OFF 0x00 // 7 0 LoRa packet CRC: disabled +#define RADIOLIB_SX128X_LORA_CRC_ON 0x20 // 7 0 enabled +#define RADIOLIB_SX128X_LORA_IQ_STANDARD 0x40 // 7 0 LoRa IQ: standard +#define RADIOLIB_SX128X_LORA_IQ_INVERTED 0x00 // 7 0 inverted + +// RADIOLIB_SX128X_CMD_GET_PACKET_STATUS +#define RADIOLIB_SX128X_PACKET_STATUS_SYNC_ERROR 0b01000000 // 6 6 packet status errors byte: sync word error +#define RADIOLIB_SX128X_PACKET_STATUS_LENGTH_ERROR \ + 0b00100000 // 5 5 packet length error +#define RADIOLIB_SX128X_PACKET_STATUS_CRC_ERROR 0b00010000 // 4 4 CRC error +#define RADIOLIB_SX128X_PACKET_STATUS_ABORT_ERROR \ + 0b00001000 // 3 3 packet reception aborted +#define RADIOLIB_SX128X_PACKET_STATUS_HEADER_RECEIVED \ + 0b00000100 // 2 2 header received +#define RADIOLIB_SX128X_PACKET_STATUS_PACKET_RECEIVED \ + 0b00000010 // 1 1 packet received +#define RADIOLIB_SX128X_PACKET_STATUS_PACKET_CTRL_BUSY \ + 0b00000001 // 0 0 packet controller is busy +#define RADIOLIB_SX128X_PACKET_STATUS_RX_PID \ + 0b11000000 // 7 6 packet status status byte: PID field of the received packet +#define RADIOLIB_SX128X_PACKET_STATUS_NO_ACK \ + 0b00100000 // 5 5 NO_ACK field of the received packet +#define RADIOLIB_SX128X_PACKET_STATUS_RX_PID_ERROR 0b00010000 // 4 4 PID field error +#define RADIOLIB_SX128X_PACKET_STATUS_PACKET_SENT 0b00000001 // 0 0 packet sent +#define RADIOLIB_SX128X_PACKET_STATUS_SYNC_DET_ERROR \ + 0b00000000 // 2 0 packet status sync byte: sync word detection error +#define RADIOLIB_SX128X_PACKET_STATUS_SYNC_DET_1 0b00000001 // 2 0 detected sync word 1 +#define RADIOLIB_SX128X_PACKET_STATUS_SYNC_DET_2 0b00000010 // 2 0 detected sync word 2 +#define RADIOLIB_SX128X_PACKET_STATUS_SYNC_DET_3 0b00000100 // 2 0 detected sync word 3 + +// RADIOLIB_SX128X_CMD_SET_DIO_IRQ_PARAMS +#define RADIOLIB_SX128X_IRQ_PREAMBLE_DETECTED 0x8000 // 15 15 interrupt source: preamble detected +#define RADIOLIB_SX128X_IRQ_ADVANCED_RANGING_DONE 0x8000 // 15 15 advanced ranging done +#define RADIOLIB_SX128X_IRQ_RX_TX_TIMEOUT 0x4000 // 14 14 Rx or Tx timeout +#define RADIOLIB_SX128X_IRQ_CAD_DETECTED 0x2000 // 13 13 channel activity detected +#define RADIOLIB_SX128X_IRQ_CAD_DONE 0x1000 // 12 12 CAD finished +#define RADIOLIB_SX128X_IRQ_RANGING_SLAVE_REQ_VALID \ + 0x0800 // 11 11 ranging request valid (slave) +#define RADIOLIB_SX128X_IRQ_RANGING_MASTER_TIMEOUT 0x0400 // 10 10 ranging timeout (master) +#define RADIOLIB_SX128X_IRQ_RANGING_MASTER_RES_VALID \ + 0x0200 // 9 9 ranging result valid (master) +#define RADIOLIB_SX128X_IRQ_RANGING_SLAVE_REQ_DISCARD \ + 0x0100 // 8 8 ranging result valid (master) +#define RADIOLIB_SX128X_IRQ_RANGING_SLAVE_RESP_DONE \ + 0x0080 // 7 7 ranging response complete (slave) +#define RADIOLIB_SX128X_IRQ_CRC_ERROR 0x0040 // 6 6 CRC error +#define RADIOLIB_SX128X_IRQ_HEADER_ERROR 0x0020 // 5 5 header error +#define RADIOLIB_SX128X_IRQ_HEADER_VALID 0x0010 // 4 4 header valid +#define RADIOLIB_SX128X_IRQ_SYNC_WORD_ERROR 0x0008 // 3 3 sync word error +#define RADIOLIB_SX128X_IRQ_SYNC_WORD_VALID 0x0004 // 2 2 sync word valid +#define RADIOLIB_SX128X_IRQ_RX_DONE 0x0002 // 1 1 Rx done +#define RADIOLIB_SX128X_IRQ_TX_DONE 0x0001 // 0 0 Tx done +#define RADIOLIB_SX128X_IRQ_NONE 0x0000 // 15 0 none +#define RADIOLIB_SX128X_IRQ_ALL 0xFFFF // 15 0 all + +// RADIOLIB_SX128X_CMD_SET_REGULATOR_MODE +#define RADIOLIB_SX128X_REGULATOR_LDO 0x00 // 7 0 set regulator mode: LDO (default) +#define RADIOLIB_SX128X_REGULATOR_DC_DC 0x01 // 7 0 DC-DC + +// RADIOLIB_SX128X_CMD_SET_RANGING_ROLE +#define RADIOLIB_SX128X_RANGING_ROLE_MASTER 0x01 // 7 0 ranging role: master +#define RADIOLIB_SX128X_RANGING_ROLE_SLAVE 0x00 // 7 0 slave + +// RADIOLIB_SX128X_REG_LORA_SYNC_WORD_1 - RADIOLIB_SX128X_REG_LORA_SYNC_WORD_2 +#define RADIOLIB_SX128X_SYNC_WORD_PRIVATE 0x12 + +/*! + \class SX128x + \brief Base class for %SX128x series. All derived classes for %SX128x (e.g. SX1280 or SX1281) inherit from this base + class. This class should not be instantiated directly from Arduino sketch, only from its derived classes. +*/ +class SX128x : public PhysicalLayer { + public: + // introduce PhysicalLayer overloads + using PhysicalLayer::readData; + using PhysicalLayer::receive; + using PhysicalLayer::startReceive; + using PhysicalLayer::startTransmit; + using PhysicalLayer::transmit; + + /*! + \brief Default constructor. + \param mod Instance of Module that will be used to communicate with the radio. + */ + explicit SX128x(Module* mod); + + // basic methods + + /*! + \brief Initialization method for LoRa modem. + \param freq Carrier frequency in MHz. Defaults to 2400.0 MHz. + \param bw LoRa bandwidth in kHz. Defaults to 812.5 kHz. + \param sf LoRa spreading factor. Defaults to 9. + \param cr LoRa coding rate denominator. Defaults to 7 (coding rate 4/7). Allowed values range from 4 to 8. Note + that a value of 4 means no coding, is undocumented and not recommended without your own FEC. + \param syncWord 2-byte LoRa sync word. Defaults to RADIOLIB_SX128X_SYNC_WORD_PRIVATE (0x12). + \param pwr Output power in dBm. Defaults to 10 dBm. + \param preambleLength LoRa preamble length in symbols. Defaults to 12 symbols. + \returns \ref status_codes + */ + int16_t begin(float freq = 2400.0, + float bw = 812.5, + uint8_t sf = 9, + uint8_t cr = 7, + uint8_t syncWord = RADIOLIB_SX128X_SYNC_WORD_PRIVATE, + int8_t pwr = 10, + uint16_t preambleLength = 12); + + /*! + \brief Initialization method for GFSK modem. + \param freq Carrier frequency in MHz. Defaults to 2400.0 MHz. + \param br FSK bit rate in kbps. Defaults to 800 kbps. + \param freqDev Frequency deviation from carrier frequency in kHz. Defaults to 400.0 kHz. + \param pwr Output power in dBm. Defaults to 10 dBm. + \param preambleLength FSK preamble length in bits. Defaults to 16 bits. + \returns \ref status_codes + */ + int16_t beginGFSK(float freq = 2400.0, + uint16_t br = 800, + float freqDev = 400.0, + int8_t pwr = 10, + uint16_t preambleLength = 16); + + /*! + \brief Initialization method for BLE modem. + \param freq Carrier frequency in MHz. Defaults to 2400.0 MHz. + \param br BLE bit rate in kbps. Defaults to 800 kbps. + \param freqDev Frequency deviation from carrier frequency in kHz. Defaults to 400.0 kHz. + \param pwr Output power in dBm. Defaults to 10 dBm. + \param dataShaping Time-bandwidth product of the Gaussian filter to be used for shaping. Defaults to 0.5. + \returns \ref status_codes + */ + int16_t beginBLE(float freq = 2400.0, + uint16_t br = 800, + float freqDev = 400.0, + int8_t pwr = 10, + uint8_t dataShaping = RADIOLIB_SHAPING_0_5); + + /*! + \brief Initialization method for FLRC modem. + \param freq Carrier frequency in MHz. Defaults to 2400.0 MHz. + \param br FLRC bit rate in kbps. Defaults to 650 kbps. + \param cr FLRC coding rate. Defaults to 3 (coding rate 3/4). + \param pwr Output power in dBm. Defaults to 10 dBm. + \param preambleLength FLRC preamble length in bits. Defaults to 16 bits. + \param dataShaping Time-bandwidth product of the Gaussian filter to be used for shaping. Defaults to 0.5. + \returns \ref status_codes + */ + int16_t beginFLRC(float freq = 2400.0, + uint16_t br = 650, + uint8_t cr = 3, + int8_t pwr = 10, + uint16_t preambleLength = 16, + uint8_t dataShaping = RADIOLIB_SHAPING_0_5); + + /*! + \brief Reset method. Will reset the chip to the default state using RST pin. + \param verify Whether correct module startup should be verified. When set to true, RadioLib will attempt to verify + the module has started correctly by repeatedly issuing setStandby command. Enabled by default. + \returns \ref status_codes + */ + int16_t reset(bool verify = true); + + /*! + \brief Blocking binary transmit method. + Overloads for string-based transmissions are implemented in PhysicalLayer. + \param data Binary data to be sent. + \param len Number of bytes to send. + \param addr Address to send the data to. Unsupported, compatibility only. + \returns \ref status_codes + */ + int16_t transmit(const uint8_t* data, size_t len, uint8_t addr = 0) override; + + /*! + \brief Blocking binary receive method. + Overloads for string-based transmissions are implemented in PhysicalLayer. + \param data Pointer to array to save the received binary data. + \param len Number of bytes that will be received. Must be known in advance for binary transmissions. + \param timeout Reception timeout in milliseconds. If set to 0, + timeout period will be calculated automatically based on the radio configuration. + \returns \ref status_codes + */ + int16_t receive(uint8_t* data, size_t len, RadioLibTime_t timeout = 0) override; + + /*! + \brief Starts direct mode transmission. + \param frf Raw RF frequency value. Defaults to 0, required for quick frequency shifts in RTTY. + \returns \ref status_codes + */ + int16_t transmitDirect(uint32_t frf = 0) override; + + /*! + \brief Starts direct mode reception. Only implemented for PhysicalLayer compatibility, + as %SX128x series does not support direct mode reception. Will always return RADIOLIB_ERR_UNKNOWN. + \returns \ref status_codes + */ + int16_t receiveDirect() override; + + /*! + \brief Performs scan for LoRa transmission in the current channel. Detects both preamble and payload. + \returns \ref status_codes + */ + int16_t scanChannel() override; + + /*! + \brief Performs scan for LoRa transmission in the current channel. Detects both preamble and payload. + \param config CAD configuration structure. + \returns \ref status_codes + */ + int16_t scanChannel(const ChannelScanConfig_t& config) override; + + /*! + \brief Sets the module to sleep mode. To wake the device up, call standby(). + Overload for PhysicalLayer compatibility. + \returns \ref status_codes + */ + int16_t sleep() override; + + /*! + \brief Sets the module to sleep mode. To wake the device up, call standby(). + \param retainConfig Set to true to retain configuration and data buffer or to false + to discard current configuration and data buffer. Defaults to true. + \returns \ref status_codes + */ + int16_t sleep(bool retainConfig); + + /*! + \brief Sets the module to standby mode (overload for PhysicalLayer compatibility, uses 13 MHz RC oscillator). + \returns \ref status_codes + */ + int16_t standby() override; + + /*! + \brief Sets the module to standby mode. + \param mode Oscillator to be used in standby mode. Can be set to RADIOLIB_SX128X_STANDBY_RC + (13 MHz RC oscillator) or RADIOLIB_SX128X_STANDBY_XOSC (52 MHz external crystal oscillator). + \param wakeup Whether to force the module to wake up. Setting to true will immediately attempt to wake up the + module. + \returns \ref status_codes + */ + int16_t standby(uint8_t mode, bool wakeup = false); + + // interrupt methods + + /*! + \brief Sets interrupt service routine to call when DIO1 activates. + \param func ISR to call. + */ + void setDio1Action(void (*func)(void)); + + /*! + \brief Clears interrupt service routine to call when DIO1 activates. + */ + void clearDio1Action(); + + /*! + \brief Sets interrupt service routine to call when a packet is received. + \param func ISR to call. + */ + void setPacketReceivedAction(void (*func)(void)) override; + + /*! + \brief Clears interrupt service routine to call when a packet is received. + */ + void clearPacketReceivedAction() override; + + /*! + \brief Sets interrupt service routine to call when a packet is sent. + \param func ISR to call. + */ + void setPacketSentAction(void (*func)(void)) override; + + /*! + \brief Clears interrupt service routine to call when a packet is sent. + */ + void clearPacketSentAction() override; + + /*! + \brief Clean up after transmission is done. + \returns \ref status_codes + */ + int16_t finishTransmit() override; + + /*! + \brief Interrupt-driven receive method with default parameters. + Implemented for compatibility with PhysicalLayer. + + \returns \ref status_codes + */ + int16_t startReceive() override; + + /*! + \brief Reads the current IRQ status. + \returns IRQ status bits + */ + uint16_t getIrqStatus(); + + /*! + \brief Reads data received after calling startReceive method. When the packet length is not known in advance, + getPacketLength method must be called BEFORE calling readData! + \param data Pointer to array to save the received binary data. + \param len Number of bytes that will be read. When set to 0, the packet length will be retrieved automatically. + When more bytes than received are requested, only the number of bytes requested will be returned. + \returns \ref status_codes + */ + int16_t readData(uint8_t* data, size_t len) override; + + /*! + \brief Clean up after reception is done. + \returns \ref status_codes + */ + int16_t finishReceive() override; + + /*! + \brief Read currently active IRQ flags. + \returns IRQ flags. + */ + uint32_t getIrqFlags() override; + + /*! + \brief Set interrupt on DIO1 to be sent on a specific IRQ bit (e.g. RxTimeout, CadDone). + \param irq Module-specific IRQ flags. + \returns \ref status_codes + */ + int16_t setIrqFlags(uint32_t irq) override; + + /*! + \brief Clear interrupt on a specific IRQ bit (e.g. RxTimeout, CadDone). + \param irq Module-specific IRQ flags. + \returns \ref status_codes + */ + int16_t clearIrqFlags(uint32_t irq) override; + + /*! + \brief Interrupt-driven channel activity detection method. DIO1 will be activated + when LoRa preamble is detected, or upon timeout. + \returns \ref status_codes + */ + int16_t startChannelScan() override; + + /*! + \brief Interrupt-driven channel activity detection method. DIO1 will be activated + when LoRa preamble is detected, or upon timeout. + \param config CAD configuration structure. + \returns \ref status_codes + */ + int16_t startChannelScan(const ChannelScanConfig_t& config) override; + + /*! + \brief Read the channel scan result + \returns \ref status_codes + */ + int16_t getChannelScanResult() override; + + // configuration methods + + /*! + \brief Sets carrier frequency. Allowed values are in range from 2400.0 to 2500.0 MHz. + \param freq Carrier frequency to be set in MHz. + \returns \ref status_codes + */ + int16_t setFrequency(float freq) override; + + /*! + \brief Sets LoRa bandwidth. Allowed values are 203.125, 406.25, 812.5 and 1625.0 kHz. + \param bw LoRa bandwidth to be set in kHz. + \returns \ref status_codes + */ + int16_t setBandwidth(float bw); + + /*! + \brief Sets LoRa spreading factor. Allowed values range from 5 to 12. + \param sf LoRa spreading factor to be set. + \returns \ref status_codes + */ + int16_t setSpreadingFactor(uint8_t sf); + + /*! + \brief Sets LoRa coding rate denominator. Allowed values range from 4 to 8. Note that a value of 4 + means no coding, is undocumented and not recommended without your own FEC. + \param cr LoRa coding rate denominator to be set. + \param longInterleaving Whether to enable long interleaving mode. Not available for coding rate 4/7, + defaults to false. + \returns \ref status_codes + */ + int16_t setCodingRate(uint8_t cr, bool longInterleaving = false); + + /*! + \brief Sets output power. Allowed values are in range from -18 to 13 dBm. + \param pwr Output power to be set in dBm. + \returns \ref status_codes + */ + int16_t setOutputPower(int8_t pwr) override; + + /*! + \brief Check if output power is configurable. + \param pwr Output power in dBm. + \param clipped Clipped output power value to what is possible within the module's range. + \returns \ref status_codes + */ + int16_t checkOutputPower(int8_t pwr, int8_t* clipped) override; + + /*! + \brief Set modem for the radio to use. Will perform full reset and reconfigure the radio + using its default parameters. + \param modem Modem type to set - FSK, LoRa or LR-FHSS. + \returns \ref status_codes + */ + int16_t setModem(ModemType_t modem) override; + + /*! + \brief Get modem currently in use by the radio. + \param modem Pointer to a variable to save the retrieved configuration into. + \returns \ref status_codes + */ + int16_t getModem(ModemType_t* modem) override; + + /*! + \brief Sets preamble length for currently active modem. Allowed values range from 1 to 65535. + \param preambleLength Preamble length to be set in symbols (LoRa) or bits (FSK/BLE/FLRC). + \returns \ref status_codes + */ + int16_t setPreambleLength(size_t preambleLength) override; + + /*! + \brief Set data rate. + \param dr Data rate struct. Interpretation depends on currently active modem (FSK or LoRa). + \returns \ref status_codes + */ + int16_t setDataRate(DataRate_t dr, ModemType_t modem = RADIOLIB_MODEM_NONE) override; + + /*! + \brief Sets FSK or FLRC bit rate. Allowed values are 125, 250, 400, 500, 800, 1000, + 1600 and 2000 kbps (for FSK modem) or 260, 325, 520, 650, 1000 and 1300 (for FLRC modem). + \param br FSK/FLRC bit rate to be set in kbps. + \returns \ref status_codes + */ + int16_t setBitRate(float br) override; + + /*! + \brief Sets FSK frequency deviation. Allowed values range from 0.0 to 3200.0 kHz. + \param freqDev FSK frequency deviation to be set in kHz. + \returns \ref status_codes + */ + int16_t setFrequencyDeviation(float freqDev) override; + + /*! + \brief Sets time-bandwidth product of Gaussian filter applied for shaping. + Allowed values are RADIOLIB_SHAPING_0_5 or RADIOLIB_SHAPING_1_0. Set to RADIOLIB_SHAPING_NONE to disable data + shaping. + \param sh Time-bandwidth product of Gaussian filter to be set. + \returns \ref status_codes + */ + int16_t setDataShaping(uint8_t sh) override; + + /*! + \brief Sets FSK/FLRC sync word in the form of array of up to 5 bytes (FSK). For FLRC modem, + the sync word must be exactly 4 bytes long + \param syncWord Sync word to be set. + \param len Sync word length in bytes. + \returns \ref status_codes + */ + int16_t setSyncWord(const uint8_t* syncWord, uint8_t len); + + /*! + \brief Sets LoRa sync word. + \param syncWord LoRa sync word to be set. + \param controlBits Undocumented control bits, required for compatibility purposes. + \returns \ref status_codes + */ + int16_t setSyncWord(uint8_t syncWord, uint8_t controlBits = 0x44); + + /*! + \brief Sets CRC configuration. + \param len CRC length in bytes, Allowed values are 1, 2 or 3, set to 0 to disable CRC. + \param initial Initial CRC value. Defaults to 0x1D0F (CCIT CRC), not available for LoRa modem. + \param polynomial Polynomial for CRC calculation. Defaults to 0x1021 (CCIT CRC), not available for LoRa or BLE + modem. + \returns \ref status_codes + */ + int16_t setCRC(uint8_t len, uint32_t initial = 0x1D0F, uint16_t polynomial = 0x1021); + + /*! + \brief Sets whitening parameters, not available for LoRa or FLRC modem. + \param enabled Set to true to enable whitening. + \returns \ref status_codes + */ + int16_t setWhitening(bool enabled); + + /*! + \brief Sets BLE access address. + \param addr BLE access address. + \returns \ref status_codes + */ + int16_t setAccessAddress(uint32_t addr); + + /*! + \brief Enables or disables receiver high sensitivity mode. + \param enable True to enable and false to disable. + \returns \ref status_codes + */ + int16_t setHighSensitivityMode(bool enable); + + /*! + \brief Enables or disables receiver manual gain control. + \param gain Use 0 for automatic gain, 1 for minimum gain and up to 13 for maximum gain. + \returns \ref status_codes + */ + int16_t setGainControl(uint8_t gain = 0); + + /*! + \brief Gets RSSI (Recorded Signal Strength Indicator) of the last received packet. + \returns RSSI of the last received packet in dBm. + */ + float getRSSI() override; + + /*! + \brief Gets RSSI (Recorded Signal Strength Indicator). + \param packet Whether to read last packet RSSI, or the current value. + \returns RSSI value in dBm. + */ + float getRSSI(bool packet); + + /*! + \brief Gets SNR (Signal to Noise Ratio) of the last received packet. Only available for LoRa or ranging modem. + \returns SNR of the last received packet in dB. + */ + float getSNR() override; + + /*! + \brief Gets frequency error of the latest received packet. + \returns Frequency error in Hz. + */ + float getFrequencyError(); + + /*! + \brief Query modem for the packet length of received payload. + \param update Update received packet length. Will return cached value when set to false. + \returns Length of last received packet in bytes. + */ + size_t getPacketLength(bool update = true) override; + + /*! + \brief Query modem for the packet length of received payload and Rx buffer offset. + \param update Update received packet length. Will return cached value when set to false. + \param offset Pointer to variable to store the Rx buffer offset. + \returns Length of last received packet in bytes. + */ + size_t getPacketLength(bool update, uint8_t* offset); + + /*! + \brief Get LoRa header information from last received packet. Only valid in explicit header mode. + \param cr Pointer to variable to store the coding rate. + \param hasCRC Pointer to variable to store the CRC status. + \returns \ref status_codes + */ + int16_t getLoRaRxHeaderInfo(uint8_t* cr, bool* hasCRC); + + /*! + \brief Set modem in fixed packet length mode. Available in GFSK and FLRC modes only. + \param len Packet length. + \returns \ref status_codes + */ + int16_t fixedPacketLengthMode(uint8_t len = RADIOLIB_SX128X_MAX_PACKET_LENGTH); + + /*! + \brief Set modem in variable packet length mode. Available in GFSK and FLRC modes only. + \param maxLen Maximum packet length. + \returns \ref status_codes + */ + int16_t variablePacketLengthMode(uint8_t maxLen = RADIOLIB_SX128X_MAX_PACKET_LENGTH); + + /*! + \brief Calculate the expected time-on-air for a given modem, data rate, packet configuration and payload size. + \param modem Modem type. + \param dr Data rate. + \param pc Packet config. + \param len Payload length in bytes. + \returns Expected time-on-air in microseconds. + */ + RadioLibTime_t calculateTimeOnAir(ModemType_t modem, DataRate_t dr, PacketConfig_t pc, size_t len) override; + + /*! + \brief Get expected time-on-air for a given size of payload. + \param len Payload length in bytes. + \returns Expected time-on-air in microseconds. + */ + RadioLibTime_t getTimeOnAir(size_t len) override; + + /*! + \brief Set implicit header mode for future reception/transmission. + \returns \ref status_codes + */ + int16_t implicitHeader(size_t len); + + /*! + \brief Set explicit header mode for future reception/transmission. + \param len Payload length in bytes. + \returns \ref status_codes + */ + int16_t explicitHeader(); + + /*! + \brief Sets transmission encoding. Serves only as alias for PhysicalLayer compatibility. + \param encoding Encoding to be used. Set to 0 for NRZ, and 2 for whitening. + \returns \ref status_codes + */ + int16_t setEncoding(uint8_t encoding) override; + + /*! \copydoc Module::setRfSwitchPins */ + void setRfSwitchPins(uint32_t rxEn, uint32_t txEn); + + /*! \copydoc Module::setRfSwitchTable */ + void setRfSwitchTable(const uint32_t (&pins)[Module::RFSWITCH_MAX_PINS], const Module::RfSwitchMode_t table[]); + + /*! + \brief Dummy random method, to ensure PhysicalLayer compatibility. + \returns Always returns 0. + */ + uint8_t randomByte() override; + + /*! + \brief Enable/disable inversion of the I and Q signals + \param enable IQ inversion enabled (true) or disabled (false); + \returns \ref status_codes + */ + int16_t invertIQ(bool enable) override; + + /*! \copydoc PhysicalLayer::stageMode */ + int16_t stageMode(RadioModeType_t mode, RadioModeConfig_t* cfg) override; + + /*! \copydoc PhysicalLayer::launchMode */ + int16_t launchMode() override; + +#if !RADIOLIB_EXCLUDE_DIRECT_RECEIVE + /*! + \brief Dummy method, to ensure PhysicalLayer compatibility. + \param func Ignored. + */ + void setDirectAction(void (*func)(void)) override; + + /*! + \brief Dummy method, to ensure PhysicalLayer compatibility. + \param pin Ignored. + */ + void readBit(uint32_t pin) override; +#endif + +#if !RADIOLIB_GODMODE && !RADIOLIB_LOW_LEVEL + protected: +#endif + const char* chipType = NULL; + + Module* getMod() override; + int16_t modSetup(uint8_t modem); + + // cached LoRa parameters + float bandwidthKhz = 0; + uint8_t bandwidth = 0, spreadingFactor = 0, codingRateLoRa = 0; + uint8_t preambleLengthLoRa = 0, headerType = 0, payloadLen = 0, crcLoRa = 0; + + // SX128x SPI command implementations + uint8_t getStatus(); + int16_t writeRegister(uint16_t addr, const uint8_t* data, uint8_t numBytes); + int16_t readRegister(uint16_t addr, uint8_t* data, uint8_t numBytes); + int16_t writeBuffer(const uint8_t* data, uint8_t numBytes, uint8_t offset = 0x00); + int16_t readBuffer(uint8_t* data, uint8_t numBytes, uint8_t offset = 0x00); + int16_t setTx(uint16_t periodBaseCount = RADIOLIB_SX128X_TX_TIMEOUT_NONE, + uint8_t periodBase = RADIOLIB_SX128X_PERIOD_BASE_15_625_US); + int16_t setRx(uint16_t periodBaseCount, uint8_t periodBase = RADIOLIB_SX128X_PERIOD_BASE_15_625_US); + int16_t setCad(uint8_t symbolNum); + uint8_t getPacketType(); + int16_t setRfFrequency(uint32_t frf); + int16_t setTxParams(uint8_t pwr, uint8_t rampTime = RADIOLIB_SX128X_PA_RAMP_10_US); + int16_t setBufferBaseAddress(uint8_t txBaseAddress = 0x00, uint8_t rxBaseAddress = 0x00); + int16_t setModulationParams(uint8_t modParam1, uint8_t modParam2, uint8_t modParam3); + int16_t setPacketParamsGFSK(uint8_t preambleLen, + uint8_t syncLen, + uint8_t syncMatch, + uint8_t crcLen, + uint8_t whiten, + uint8_t hdrType, + uint8_t payLen = 0xFF); + int16_t setPacketParamsBLE(uint8_t connState, uint8_t crcLen, uint8_t bleTest, uint8_t whiten); + int16_t setPacketParamsLoRa(uint8_t preambleLen, + uint8_t hdrType, + uint8_t payLen, + uint8_t crc, + uint8_t invIQ = RADIOLIB_SX128X_LORA_IQ_STANDARD); + int16_t setDioIrqParams(uint16_t irqMask, + uint16_t dio1Mask, + uint16_t dio2Mask = RADIOLIB_SX128X_IRQ_NONE, + uint16_t dio3Mask = RADIOLIB_SX128X_IRQ_NONE); + int16_t clearIrqStatus(uint16_t clearIrqParams = RADIOLIB_SX128X_IRQ_ALL); + int16_t setRangingRole(uint8_t role); + int16_t setPacketType(uint8_t type); + + // protected so that it can be accessed from SX1280 class during reinit after ranging is complete + int16_t config(uint8_t modem); + +#if !RADIOLIB_GODMODE + private: +#endif + Module* mod; + + // common low-level SPI interface + static int16_t SPIparseStatus(uint8_t in); + + // common parameters + uint8_t power = 0; + uint32_t rxTimeout = 0; + + // cached LoRa parameters + uint8_t invertIQEnabled = RADIOLIB_SX128X_LORA_IQ_STANDARD; + + // cached GFSK parameters + float modIndexReal = 0, frequencyDev = 0; + uint16_t bitRateKbps = 0; + uint8_t bitRate = 0, modIndex = 0, shaping = 0; + uint8_t preambleLengthGFSK = 0, syncWordLen = 0, syncWordMatch = 0, crcGFSK = 0, whitening = 0; + uint8_t packetType = RADIOLIB_SX128X_GFSK_FLRC_PACKET_VARIABLE; + + // cached FLRC parameters + uint8_t codingRateFLRC = 0; + + // cached BLE parameters + uint8_t connectionState = 0, crcBLE = 0, bleTestPayload = 0; + + bool findChip(const char* verStr); + int16_t setPacketMode(uint8_t mode, uint8_t len); + int16_t setHeaderType(uint8_t hdrType, size_t len = 0xFF); +}; + +#endif + +#endif From 8bf5308dd1767dd3fcb569c515372191978549d3 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 19:12:51 -0800 Subject: [PATCH 031/101] update submodule lib/fprime-zephyr --- lib/fprime-zephyr | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/fprime-zephyr b/lib/fprime-zephyr index 3b2775dc..55501061 160000 --- a/lib/fprime-zephyr +++ b/lib/fprime-zephyr @@ -1 +1 @@ -Subproject commit 3b2775dc284073685752d046be899b9d47709934 +Subproject commit 55501061461d55b0c0c4962e74a885e3437d7246 From d060d4789c92c72bcf0c310ffdc92f2b6a6268ea Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 19:26:12 -0800 Subject: [PATCH 032/101] add RadioLib submodule --- .gitmodules | 3 +++ lib/RadioLib | 1 + 2 files changed, 4 insertions(+) create mode 160000 lib/RadioLib diff --git a/.gitmodules b/.gitmodules index ef179827..50f7b4d9 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,6 @@ [submodule "lib/fprime-zephyr"] path = lib/fprime-zephyr url = https://github.com/fprime-community/fprime-zephyr.git +[submodule "lib/RadioLib"] + path = lib/RadioLib + url = https://github.com/jgromes/RadioLib diff --git a/lib/RadioLib b/lib/RadioLib new file mode 160000 index 00000000..3a5dac09 --- /dev/null +++ b/lib/RadioLib @@ -0,0 +1 @@ +Subproject commit 3a5dac095d9f4c823fdc8a9e8f8e427244134981 From 2c8c06982d5a02ea8f70e3c998738e41b1fac4f3 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 19:26:57 -0800 Subject: [PATCH 033/101] Revert "import some Radiolib files" This reverts commit 0074422aee63e407c001e6f21023dca121e58fa2. --- .../Components/MyComponent/FprimeHal.cpp | 49 - .../Components/MyComponent/FprimeHal.h | 142 -- .../Components/MyComponent/Hal.cpp | 44 - .../Components/MyComponent/Hal.h | 219 -- .../Components/MyComponent/SX1281.cpp | 8 - .../Components/MyComponent/SX1281.h | 33 - .../Components/MyComponent/SX128x.cpp | 1848 ----------------- .../Components/MyComponent/SX128x.h | 1045 ---------- 8 files changed, 3388 deletions(-) delete mode 100644 FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp delete mode 100644 FprimeZephyrReference/Components/MyComponent/FprimeHal.h delete mode 100644 FprimeZephyrReference/Components/MyComponent/Hal.cpp delete mode 100644 FprimeZephyrReference/Components/MyComponent/Hal.h delete mode 100644 FprimeZephyrReference/Components/MyComponent/SX1281.cpp delete mode 100644 FprimeZephyrReference/Components/MyComponent/SX1281.h delete mode 100644 FprimeZephyrReference/Components/MyComponent/SX128x.cpp delete mode 100644 FprimeZephyrReference/Components/MyComponent/SX128x.h diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp deleted file mode 100644 index 7b23f9e4..00000000 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp +++ /dev/null @@ -1,49 +0,0 @@ -#include "PicoHal.h" - -#if defined(RADIOLIB_BUILD_RPI_PICO) - -// pre-calculated pulse-widths for 1200 and 2200Hz -// we do this to save calculation time (see https://github.com/khoih-prog/RP2040_PWM/issues/6) -#define SLEEP_1200 416.666 -#define SLEEP_2200 227.272 - -static uint32_t toneLoopPin; -static unsigned int toneLoopFrequency; -static unsigned long toneLoopDuration; - -// === NOTE === -// The tone(...) implementation uses the second core on the RPi Pico. This is to diminish as much -// jitter in the output tones as possible. - -static void toneLoop() { - gpio_set_dir(toneLoopPin, GPIO_OUT); - - uint32_t sleep_dur; - if (toneLoopFrequency == 1200) { - sleep_dur = SLEEP_1200; - } else if (toneLoopFrequency == 2200) { - sleep_dur = SLEEP_2200; - } else { - sleep_dur = 500000 / toneLoopFrequency; - } - - // tone bitbang - while (1) { - gpio_put(toneLoopPin, 1); - sleep_us(sleep_dur); - gpio_put(toneLoopPin, 0); - sleep_us(sleep_dur); - tight_loop_contents(); - } -} - -void PicoHal::tone(uint32_t pin, unsigned int frequency, unsigned long duration) { - // tones on the Pico are generated using bitbanging. This process is offloaded to the Pico's second core - multicore_reset_core1(); - toneLoopPin = pin; - toneLoopFrequency = frequency; - toneLoopDuration = duration; - multicore_launch_core1(toneLoop); -} - -#endif diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.h b/FprimeZephyrReference/Components/MyComponent/FprimeHal.h deleted file mode 100644 index c25a1716..00000000 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.h +++ /dev/null @@ -1,142 +0,0 @@ -#ifndef PICO_HAL_H -#define PICO_HAL_H - -#if defined(RADIOLIB_BUILD_RPI_PICO) - -// include RadioLib -#include - -// include the necessary Pico libraries -#include - -#include "hardware/clocks.h" -#include "hardware/pwm.h" -#include "hardware/spi.h" -#include "hardware/timer.h" -#include "pico/multicore.h" - -// create a new Raspberry Pi Pico hardware abstraction -// layer using the Pico SDK -// the HAL must inherit from the base RadioLibHal class -// and implement all of its virtual methods -class PicoHal : public RadioLibHal { - public: - PicoHal(spi_inst_t* spiChannel, uint32_t misoPin, uint32_t mosiPin, uint32_t sckPin, uint32_t spiSpeed = 500 * 1000) - : RadioLibHal(GPIO_IN, GPIO_OUT, 0, 1, GPIO_IRQ_EDGE_RISE, GPIO_IRQ_EDGE_FALL), - _spiChannel(spiChannel), - _spiSpeed(spiSpeed), - _misoPin(misoPin), - _mosiPin(mosiPin), - _sckPin(sckPin) {} - - void init() override { - stdio_init_all(); - spiBegin(); - } - - void term() override { spiEnd(); } - - // GPIO-related methods (pinMode, digitalWrite etc.) should check - // RADIOLIB_NC as an alias for non-connected pins - void pinMode(uint32_t pin, uint32_t mode) override { - if (pin == RADIOLIB_NC) { - return; - } - - gpio_init(pin); - gpio_set_dir(pin, mode); - } - - void digitalWrite(uint32_t pin, uint32_t value) override { - if (pin == RADIOLIB_NC) { - return; - } - - gpio_put(pin, (bool)value); - } - - uint32_t digitalRead(uint32_t pin) override { - if (pin == RADIOLIB_NC) { - return 0; - } - - return gpio_get(pin); - } - - void attachInterrupt(uint32_t interruptNum, void (*interruptCb)(void), uint32_t mode) override { - if (interruptNum == RADIOLIB_NC) { - return; - } - - gpio_set_irq_enabled_with_callback(interruptNum, mode, true, (gpio_irq_callback_t)interruptCb); - } - - void detachInterrupt(uint32_t interruptNum) override { - if (interruptNum == RADIOLIB_NC) { - return; - } - - gpio_set_irq_enabled_with_callback(interruptNum, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, false, NULL); - } - - void delay(unsigned long ms) override { sleep_ms(ms); } - - void delayMicroseconds(unsigned long us) override { sleep_us(us); } - - unsigned long millis() override { return to_ms_since_boot(get_absolute_time()); } - - unsigned long micros() override { return to_us_since_boot(get_absolute_time()); } - - long pulseIn(uint32_t pin, uint32_t state, unsigned long timeout) override { - if (pin == RADIOLIB_NC) { - return 0; - } - - this->pinMode(pin, GPIO_IN); - uint32_t start = this->micros(); - uint32_t curtick = this->micros(); - - while (this->digitalRead(pin) == state) { - if ((this->micros() - curtick) > timeout) { - return 0; - } - } - - return (this->micros() - start); - } - - void tone(uint32_t pin, unsigned int frequency, unsigned long duration = 0) override; - - void noTone(uint32_t pin) override { multicore_reset_core1(); } - - void spiBegin() { - spi_init(_spiChannel, _spiSpeed); - spi_set_format(_spiChannel, 8, SPI_CPOL_0, SPI_CPHA_0, SPI_MSB_FIRST); - - gpio_set_function(_sckPin, GPIO_FUNC_SPI); - gpio_set_function(_mosiPin, GPIO_FUNC_SPI); - gpio_set_function(_misoPin, GPIO_FUNC_SPI); - } - - void spiBeginTransaction() {} - - void spiTransfer(uint8_t* out, size_t len, uint8_t* in) { spi_write_read_blocking(_spiChannel, out, in, len); } - - void yield() override { tight_loop_contents(); } - - void spiEndTransaction() {} - - void spiEnd() { spi_deinit(_spiChannel); } - - private: - // the HAL can contain any additional private members - spi_inst_t* _spiChannel; - uint32_t _spiSpeed; - uint32_t _misoPin; - uint32_t _mosiPin; - uint32_t _sckPin; -}; - -#endif - -#endif diff --git a/FprimeZephyrReference/Components/MyComponent/Hal.cpp b/FprimeZephyrReference/Components/MyComponent/Hal.cpp deleted file mode 100644 index 54e438b7..00000000 --- a/FprimeZephyrReference/Components/MyComponent/Hal.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include "Hal.h" - -static RadioLibHal* rlb_timestamp_hal = nullptr; - -RadioLibHal::RadioLibHal(const uint32_t input, - const uint32_t output, - const uint32_t low, - const uint32_t high, - const uint32_t rising, - const uint32_t falling) - : GpioModeInput(input), - GpioModeOutput(output), - GpioLevelLow(low), - GpioLevelHigh(high), - GpioInterruptRising(rising), - GpioInterruptFalling(falling) { - if (!rlb_timestamp_hal) { - rlb_timestamp_hal = this; - } -} - -void RadioLibHal::init() {} - -void RadioLibHal::term() {} - -void RadioLibHal::tone(uint32_t pin, unsigned int frequency, RadioLibTime_t duration) { - (void)pin; - (void)frequency; - (void)duration; -} - -void RadioLibHal::noTone(uint32_t pin) { - (void)pin; -} - -void RadioLibHal::yield() {} - -uint32_t RadioLibHal::pinToInterrupt(uint32_t pin) { - return (pin); -} - -RadioLibTime_t rlb_time_us() { - return (rlb_timestamp_hal == nullptr ? 0 : rlb_timestamp_hal->micros()); -} diff --git a/FprimeZephyrReference/Components/MyComponent/Hal.h b/FprimeZephyrReference/Components/MyComponent/Hal.h deleted file mode 100644 index bebeaf49..00000000 --- a/FprimeZephyrReference/Components/MyComponent/Hal.h +++ /dev/null @@ -1,219 +0,0 @@ -#if !defined(_RADIOLIB_HAL_H) -#define _RADIOLIB_HAL_H - -#include -#include - -#include "BuildOpt.h" - -/*! \brief Global-scope function that returns timestamp since start (in microseconds). */ -RadioLibTime_t rlb_time_us(); - -/*! - \class RadioLibHal - \brief Hardware abstraction library base interface. -*/ -class RadioLibHal { - public: - // values for pin modes, levels and change directions - // these tell RadioLib how are different logic states represented on a given platform - - /*! - \brief Value to be used as the "input" GPIO direction. - */ - const uint32_t GpioModeInput; - - /*! - \brief Value to be used as the "output" GPIO direction. - */ - const uint32_t GpioModeOutput; - - /*! - \brief Value to be used as the "low" GPIO level. - */ - const uint32_t GpioLevelLow; - - /*! - \brief Value to be used as the "high" GPIO level. - */ - const uint32_t GpioLevelHigh; - - /*! - \brief Value to be used as the "rising" GPIO level change direction. - */ - const uint32_t GpioInterruptRising; - - /*! - \brief Value to be used as the "falling" GPIO level change direction. - */ - const uint32_t GpioInterruptFalling; - - /*! - \brief Default constructor. - \param input Value to be used as the "input" GPIO direction. - \param output Value to be used as the "output" GPIO direction. - \param low Value to be used as the "low" GPIO level. - \param high Value to be used as the "high" GPIO level. - \param rising Value to be used as the "rising" GPIO level change direction. - \param falling Value to be used as the "falling" GPIO level change direction. - */ - RadioLibHal(const uint32_t input, - const uint32_t output, - const uint32_t low, - const uint32_t high, - const uint32_t rising, - const uint32_t falling); - - // pure virtual methods - these must be implemented by the hardware abstraction for RadioLib to function - - /*! - \brief GPIO pin mode (input/output/...) configuration method. - Must be implemented by the platform-specific hardware abstraction! - \param pin Pin to be changed (platform-specific). - \param mode Mode to be set (platform-specific). - */ - virtual void pinMode(uint32_t pin, uint32_t mode) = 0; - - /*! - \brief Digital write method. - Must be implemented by the platform-specific hardware abstraction! - \param pin Pin to be changed (platform-specific). - \param value Value to set (platform-specific). - */ - virtual void digitalWrite(uint32_t pin, uint32_t value) = 0; - - /*! - \brief Digital read method. - Must be implemented by the platform-specific hardware abstraction! - \param pin Pin to be changed (platform-specific). - \returns Value read on the pin (platform-specific). - */ - virtual uint32_t digitalRead(uint32_t pin) = 0; - - /*! - \brief Method to attach function to an external interrupt. - Must be implemented by the platform-specific hardware abstraction! - \param interruptNum Interrupt number to attach to (platform-specific). - \param interruptCb Interrupt service routine to execute. - \param mode Rising/falling mode (platform-specific). - */ - virtual void attachInterrupt(uint32_t interruptNum, void (*interruptCb)(void), uint32_t mode) = 0; - - /*! - \brief Method to detach function from an external interrupt. - Must be implemented by the platform-specific hardware abstraction! - \param interruptNum Interrupt number to detach from (platform-specific). - */ - virtual void detachInterrupt(uint32_t interruptNum) = 0; - - /*! - \brief Blocking wait function. - Must be implemented by the platform-specific hardware abstraction! - \param ms Number of milliseconds to wait. - */ - virtual void delay(RadioLibTime_t ms) = 0; - - /*! - \brief Blocking microsecond wait function. - Must be implemented by the platform-specific hardware abstraction! - \param us Number of microseconds to wait. - */ - virtual void delayMicroseconds(RadioLibTime_t us) = 0; - - /*! - \brief Get number of milliseconds since start. - Must be implemented by the platform-specific hardware abstraction! - \returns Number of milliseconds since start. - */ - virtual RadioLibTime_t millis() = 0; - - /*! - \brief Get number of microseconds since start. - Must be implemented by the platform-specific hardware abstraction! - \returns Number of microseconds since start. - */ - virtual RadioLibTime_t micros() = 0; - - /*! - \brief Measure the length of incoming digital pulse in microseconds. - Must be implemented by the platform-specific hardware abstraction! - \param pin Pin to measure on (platform-specific). - \param state Pin level to monitor (platform-specific). - \param timeout Timeout in microseconds. - \returns Pulse length in microseconds, or 0 if the pulse did not start before timeout. - */ - virtual long pulseIn(uint32_t pin, uint32_t state, RadioLibTime_t timeout) = 0; - - /*! - \brief SPI initialization method. - */ - virtual void spiBegin() = 0; - - /*! - \brief Method to start SPI transaction. - */ - virtual void spiBeginTransaction() = 0; - - /*! - \brief Method to transfer buffer over SPI. - \param out Buffer to send. - \param len Number of data to send or receive. - \param in Buffer to save received data into. - */ - virtual void spiTransfer(uint8_t* out, size_t len, uint8_t* in) = 0; - - /*! - \brief Method to end SPI transaction. - */ - virtual void spiEndTransaction() = 0; - - /*! - \brief SPI termination method. - */ - virtual void spiEnd() = 0; - - // virtual methods - these may or may not exists on a given platform - // they exist in this implementation, but do nothing - - /*! - \brief Module initialization method. - This will be called by all radio modules at the beginning of startup. - Can be used to e.g., initialize SPI interface. - */ - virtual void init(); - - /*! - \brief Module termination method. - This will be called by all radio modules when the destructor is called. - Can be used to e.g., stop SPI interface. - */ - virtual void term(); - - /*! - \brief Method to produce a square-wave with 50% duty cycle ("tone") of a given frequency at some pin. - \param pin Pin to be used as the output. - \param frequency Frequency of the square wave. - \param duration Duration of the tone in ms. When set to 0, the tone will be infinite. - */ - virtual void tone(uint32_t pin, unsigned int frequency, RadioLibTime_t duration = 0); - - /*! - \brief Method to stop producing a tone. - \param pin Pin which is currently producing the tone. - */ - virtual void noTone(uint32_t pin); - - /*! - \brief Yield method, called from long loops in multi-threaded environment (to prevent blocking other threads). - */ - virtual void yield(); - - /*! - \brief Function to convert from pin number to interrupt number. - \param pin Pin to convert from. - \returns The interrupt number of a given pin. - */ - virtual uint32_t pinToInterrupt(uint32_t pin); -}; - -#endif diff --git a/FprimeZephyrReference/Components/MyComponent/SX1281.cpp b/FprimeZephyrReference/Components/MyComponent/SX1281.cpp deleted file mode 100644 index 780ad9cd..00000000 --- a/FprimeZephyrReference/Components/MyComponent/SX1281.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include "SX1281.h" -#if !RADIOLIB_EXCLUDE_SX128X - -SX1281::SX1281(Module* mod) : SX128x(mod) { - chipType = RADIOLIB_SX1281_CHIP_TYPE; -} - -#endif diff --git a/FprimeZephyrReference/Components/MyComponent/SX1281.h b/FprimeZephyrReference/Components/MyComponent/SX1281.h deleted file mode 100644 index dbbd4f63..00000000 --- a/FprimeZephyrReference/Components/MyComponent/SX1281.h +++ /dev/null @@ -1,33 +0,0 @@ -#if !defined(_RADIOLIB_SX1281_H) -#define _RADIOLIB_SX1281_H - -#include "../../TypeDef.h" - -#if !RADIOLIB_EXCLUDE_SX128X - -#include "../../Module.h" -#include "SX128x.h" - -// RADIOLIB_SX128X_REG_VERSION_STRING -#define RADIOLIB_SX1281_CHIP_TYPE "SX1281" - -/*! - \class SX1281 - \brief Derived class for %SX1281 modules. -*/ -class SX1281 : public SX128x { - public: - /*! - \brief Default constructor. - \param mod Instance of Module that will be used to communicate with the radio. - */ - explicit SX1281(Module* mod); - -#if !RADIOLIB_GODMODE - private: -#endif -}; - -#endif - -#endif diff --git a/FprimeZephyrReference/Components/MyComponent/SX128x.cpp b/FprimeZephyrReference/Components/MyComponent/SX128x.cpp deleted file mode 100644 index bfe12e65..00000000 --- a/FprimeZephyrReference/Components/MyComponent/SX128x.cpp +++ /dev/null @@ -1,1848 +0,0 @@ -#include "SX128x.h" - -#include -#include -#if !RADIOLIB_EXCLUDE_SX128X - -SX128x::SX128x(Module* mod) : PhysicalLayer() { - this->freqStep = RADIOLIB_SX128X_FREQUENCY_STEP_SIZE; - this->maxPacketLength = RADIOLIB_SX128X_MAX_PACKET_LENGTH; - this->mod = mod; - this->irqMap[RADIOLIB_IRQ_TX_DONE] = RADIOLIB_SX128X_IRQ_TX_DONE; - this->irqMap[RADIOLIB_IRQ_RX_DONE] = RADIOLIB_SX128X_IRQ_RX_DONE; - this->irqMap[RADIOLIB_IRQ_PREAMBLE_DETECTED] = RADIOLIB_SX128X_IRQ_PREAMBLE_DETECTED; - this->irqMap[RADIOLIB_IRQ_SYNC_WORD_VALID] = RADIOLIB_SX128X_IRQ_SYNC_WORD_VALID; - this->irqMap[RADIOLIB_IRQ_HEADER_VALID] = RADIOLIB_SX128X_IRQ_HEADER_VALID; - this->irqMap[RADIOLIB_IRQ_HEADER_ERR] = RADIOLIB_SX128X_IRQ_HEADER_ERROR; - this->irqMap[RADIOLIB_IRQ_CRC_ERR] = RADIOLIB_SX128X_IRQ_CRC_ERROR; - this->irqMap[RADIOLIB_IRQ_CAD_DONE] = RADIOLIB_SX128X_IRQ_CAD_DONE; - this->irqMap[RADIOLIB_IRQ_CAD_DETECTED] = RADIOLIB_SX128X_IRQ_CAD_DETECTED; - this->irqMap[RADIOLIB_IRQ_TIMEOUT] = RADIOLIB_SX128X_IRQ_RX_TX_TIMEOUT; -} - -int16_t -SX128x::begin(float freq, float bw, uint8_t sf, uint8_t cr, uint8_t syncWord, int8_t pwr, uint16_t preambleLength) { - // initialize LoRa modulation variables - this->bandwidthKhz = bw; - this->spreadingFactor = RADIOLIB_SX128X_LORA_SF_9; - this->codingRateLoRa = RADIOLIB_SX128X_LORA_CR_4_7; - - // initialize LoRa packet variables - this->preambleLengthLoRa = preambleLength; - this->headerType = RADIOLIB_SX128X_LORA_HEADER_EXPLICIT; - this->payloadLen = 0xFF; - this->crcLoRa = RADIOLIB_SX128X_LORA_CRC_ON; - - // set module properties and perform initial setup - int16_t state = this->modSetup(RADIOLIB_SX128X_PACKET_TYPE_LORA); - RADIOLIB_ASSERT(state); - - // configure publicly accessible settings - state = setFrequency(freq); - RADIOLIB_ASSERT(state); - - state = setBandwidth(bw); - RADIOLIB_ASSERT(state); - - state = setSpreadingFactor(sf); - RADIOLIB_ASSERT(state); - - state = setCodingRate(cr); - RADIOLIB_ASSERT(state); - - state = setSyncWord(syncWord); - RADIOLIB_ASSERT(state); - - state = setPreambleLength(preambleLength); - RADIOLIB_ASSERT(state); - - state = setOutputPower(pwr); - RADIOLIB_ASSERT(state); - - return (state); -} - -int16_t SX128x::beginGFSK(float freq, uint16_t br, float freqDev, int8_t pwr, uint16_t preambleLength) { - // initialize GFSK modulation variables - this->bitRateKbps = br; - this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_800_BW_2_4; - this->modIndexReal = 1.0; - this->modIndex = RADIOLIB_SX128X_BLE_GFSK_MOD_IND_1_00; - this->shaping = RADIOLIB_SX128X_BLE_GFSK_BT_0_5; - - // initialize GFSK packet variables - this->preambleLengthGFSK = preambleLength; - this->syncWordLen = 2; - this->syncWordMatch = RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1; - this->crcGFSK = RADIOLIB_SX128X_GFSK_FLRC_CRC_2_BYTE; - this->whitening = RADIOLIB_SX128X_GFSK_BLE_WHITENING_ON; - - // set module properties and perform initial setup - int16_t state = this->modSetup(RADIOLIB_SX128X_PACKET_TYPE_GFSK); - RADIOLIB_ASSERT(state); - - // configure publicly accessible settings - state = setFrequency(freq); - RADIOLIB_ASSERT(state); - - state = setBitRate(br); - RADIOLIB_ASSERT(state); - - state = setFrequencyDeviation(freqDev); - RADIOLIB_ASSERT(state); - - state = setOutputPower(pwr); - RADIOLIB_ASSERT(state); - - state = setPreambleLength(preambleLength); - RADIOLIB_ASSERT(state); - - state = setDataShaping(RADIOLIB_SHAPING_0_5); - RADIOLIB_ASSERT(state); - - // set publicly accessible settings that are not a part of begin method - uint8_t sync[] = {0x12, 0xAD}; - state = setSyncWord(sync, 2); - RADIOLIB_ASSERT(state); - - state = setEncoding(RADIOLIB_ENCODING_NRZ); - RADIOLIB_ASSERT(state); - - return (state); -} - -int16_t SX128x::beginBLE(float freq, uint16_t br, float freqDev, int8_t pwr, uint8_t dataShaping) { - // initialize BLE modulation variables - this->bitRateKbps = br; - this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_800_BW_2_4; - this->modIndexReal = 1.0; - this->modIndex = RADIOLIB_SX128X_BLE_GFSK_MOD_IND_1_00; - this->shaping = RADIOLIB_SX128X_BLE_GFSK_BT_0_5; - - // initialize BLE packet variables - this->crcGFSK = RADIOLIB_SX128X_BLE_CRC_3_BYTE; - this->whitening = RADIOLIB_SX128X_GFSK_BLE_WHITENING_ON; - - // set module properties and perform initial setup - int16_t state = this->modSetup(RADIOLIB_SX128X_PACKET_TYPE_BLE); - RADIOLIB_ASSERT(state); - - // configure publicly accessible settings - state = setFrequency(freq); - RADIOLIB_ASSERT(state); - - state = setBitRate(br); - RADIOLIB_ASSERT(state); - - state = setFrequencyDeviation(freqDev); - RADIOLIB_ASSERT(state); - - state = setOutputPower(pwr); - RADIOLIB_ASSERT(state); - - state = setDataShaping(dataShaping); - RADIOLIB_ASSERT(state); - - return (state); -} - -int16_t SX128x::beginFLRC(float freq, - uint16_t br, - uint8_t cr, - int8_t pwr, - uint16_t preambleLength, - uint8_t dataShaping) { - // initialize FLRC modulation variables - this->bitRateKbps = br; - this->bitRate = RADIOLIB_SX128X_FLRC_BR_0_650_BW_0_6; - this->codingRateFLRC = RADIOLIB_SX128X_FLRC_CR_3_4; - this->shaping = RADIOLIB_SX128X_FLRC_BT_0_5; - - // initialize FLRC packet variables - this->preambleLengthGFSK = preambleLength; - this->syncWordLen = 2; - this->syncWordMatch = RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1; - this->crcGFSK = RADIOLIB_SX128X_GFSK_FLRC_CRC_2_BYTE; - this->whitening = RADIOLIB_SX128X_GFSK_BLE_WHITENING_OFF; - - // set module properties and perform initial setup - int16_t state = this->modSetup(RADIOLIB_SX128X_PACKET_TYPE_FLRC); - RADIOLIB_ASSERT(state); - - // configure publicly accessible settings - state = setFrequency(freq); - RADIOLIB_ASSERT(state); - - state = setBitRate(br); - RADIOLIB_ASSERT(state); - - state = setCodingRate(cr); - RADIOLIB_ASSERT(state); - - state = setOutputPower(pwr); - RADIOLIB_ASSERT(state); - - state = setPreambleLength(preambleLength); - RADIOLIB_ASSERT(state); - - state = setDataShaping(dataShaping); - RADIOLIB_ASSERT(state); - - // set publicly accessible settings that are not a part of begin method - uint8_t sync[] = {0x2D, 0x01, 0x4B, 0x1D}; - state = setSyncWord(sync, 4); - RADIOLIB_ASSERT(state); - - return (state); -} - -int16_t SX128x::reset(bool verify) { - // run the reset sequence - same as SX126x, as SX128x docs don't seem to mention this - this->mod->hal->pinMode(this->mod->getRst(), this->mod->hal->GpioModeOutput); - this->mod->hal->digitalWrite(this->mod->getRst(), this->mod->hal->GpioLevelLow); - this->mod->hal->delay(1); - this->mod->hal->digitalWrite(this->mod->getRst(), this->mod->hal->GpioLevelHigh); - - // return immediately when verification is disabled - if (!verify) { - return (RADIOLIB_ERR_NONE); - } - - // set mode to standby - RadioLibTime_t start = this->mod->hal->millis(); - while (true) { - // try to set mode to standby - int16_t state = standby(); - if (state == RADIOLIB_ERR_NONE) { - // standby command successful - return (RADIOLIB_ERR_NONE); - } - - // standby command failed, check timeout and try again - if (this->mod->hal->millis() - start >= 3000) { - // timed out, possibly incorrect wiring - return (state); - } - - // wait a bit to not spam the module - this->mod->hal->delay(10); - } -} - -int16_t SX128x::transmit(const uint8_t* data, size_t len, uint8_t addr) { - // check packet length - if (this->codingRateLoRa == RADIOLIB_SX128X_LORA_CR_4_8_LI && this->crcLoRa == RADIOLIB_SX128X_LORA_CRC_ON) { - // Long Interleaver at CR 4/8 supports up to 253 bytes if CRC is enabled - if (len > RADIOLIB_SX128X_MAX_PACKET_LENGTH - 2) { - return (RADIOLIB_ERR_PACKET_TOO_LONG); - } - } else if (len > RADIOLIB_SX128X_MAX_PACKET_LENGTH) { - return (RADIOLIB_ERR_PACKET_TOO_LONG); - } - - // check active modem - uint8_t modem = getPacketType(); - if (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // set mode to standby - int16_t state = standby(); - RADIOLIB_ASSERT(state); - - // calculate timeout in ms (5ms + 500 % of expected time-on-air) - RadioLibTime_t timeout = 5 + (getTimeOnAir(len) * 5) / 1000; - RADIOLIB_DEBUG_BASIC_PRINTLN("Timeout in %lu ms", timeout); - - // start transmission - state = startTransmit(data, len, addr); - RADIOLIB_ASSERT(state); - - // wait for packet transmission or timeout - RadioLibTime_t start = this->mod->hal->millis(); - while (!this->mod->hal->digitalRead(this->mod->getIrq())) { - this->mod->hal->yield(); - if (this->mod->hal->millis() - start > timeout) { - finishTransmit(); - return (RADIOLIB_ERR_TX_TIMEOUT); - } - } - - return (finishTransmit()); -} - -int16_t SX128x::receive(uint8_t* data, size_t len, RadioLibTime_t timeout) { - // check active modem - uint8_t modem = getPacketType(); - if (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // set mode to standby - int16_t state = standby(); - RADIOLIB_ASSERT(state); - - // calculate timeout (1000% of expected time-on-air) - // for most other modules, it is 500%, however, the overall datarates of SX128x are higher - // so we use higher value for the default timeout - RadioLibTime_t timeoutInternal = timeout; - if (!timeoutInternal) { - timeoutInternal = getTimeOnAir(len) * 10; - } - RADIOLIB_DEBUG_BASIC_PRINTLN("Timeout in %lu ms", (uint32_t)((timeout + 999) / 1000)); - - // start reception - uint32_t timeoutValue = (uint32_t)((float)timeoutInternal / 15.625f); - state = startReceive(timeoutValue); - RADIOLIB_ASSERT(state); - - // wait for packet reception or timeout - bool softTimeout = false; - RadioLibTime_t start = this->mod->hal->micros(); - while (!this->mod->hal->digitalRead(this->mod->getIrq())) { - this->mod->hal->yield(); - // safety check, the timeout should be done by the radio - if (this->mod->hal->micros() - start > timeout) { - softTimeout = true; - break; - } - } - - // if it was a timeout, this will return an error code - state = standby(); - if ((state != RADIOLIB_ERR_NONE) && (state != RADIOLIB_ERR_SPI_CMD_TIMEOUT)) { - return (state); - } - - // check whether this was a timeout or not - if (softTimeout || (getIrqStatus() & this->irqMap[RADIOLIB_IRQ_TIMEOUT])) { - (void)finishReceive(); - return (RADIOLIB_ERR_RX_TIMEOUT); - } - - // read the received data - return (readData(data, len)); -} - -int16_t SX128x::transmitDirect(uint32_t frf) { - // set RF switch (if present) - this->mod->setRfSwitchState(Module::MODE_TX); - - // user requested to start transmitting immediately (required for RTTY) - int16_t state = RADIOLIB_ERR_NONE; - if (frf != 0) { - state = setRfFrequency(frf); - } - RADIOLIB_ASSERT(state); - - // start transmitting - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_TX_CONTINUOUS_WAVE, NULL, 0)); -} - -int16_t SX128x::receiveDirect() { - // set RF switch (if present) - this->mod->setRfSwitchState(Module::MODE_RX); - - // SX128x is unable to output received data directly - return (RADIOLIB_ERR_UNKNOWN); -} - -int16_t SX128x::scanChannel() { - ChannelScanConfig_t cfg = { - .cad = - { - .symNum = RADIOLIB_SX128X_CAD_PARAM_DEFAULT, - .detPeak = 0, - .detMin = 0, - .exitMode = 0, - .timeout = 0, - .irqFlags = RADIOLIB_IRQ_CAD_DEFAULT_FLAGS, - .irqMask = RADIOLIB_IRQ_CAD_DEFAULT_MASK, - }, - }; - return (this->scanChannel(cfg)); -} - -int16_t SX128x::scanChannel(const ChannelScanConfig_t& config) { - // set mode to CAD - int16_t state = startChannelScan(config); - RADIOLIB_ASSERT(state); - - // wait for channel activity detected or timeout - while (!this->mod->hal->digitalRead(this->mod->getIrq())) { - this->mod->hal->yield(); - } - - // check CAD result - return (getChannelScanResult()); -} - -int16_t SX128x::sleep() { - return (SX128x::sleep(true)); -} - -int16_t SX128x::sleep(bool retainConfig) { - // set RF switch (if present) - this->mod->setRfSwitchState(Module::MODE_IDLE); - - uint8_t sleepConfig = RADIOLIB_SX128X_SLEEP_DATA_BUFFER_RETAIN | RADIOLIB_SX128X_SLEEP_DATA_RAM_RETAIN; - if (!retainConfig) { - sleepConfig = RADIOLIB_SX128X_SLEEP_DATA_BUFFER_FLUSH | RADIOLIB_SX128X_SLEEP_DATA_RAM_FLUSH; - } - int16_t state = this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SAVE_CONTEXT, 0, 1, false, false); - RADIOLIB_ASSERT(state); - state = this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_SLEEP, &sleepConfig, 1, false, false); - - // wait for SX128x to safely enter sleep mode - this->mod->hal->delay(1); - - return (state); -} - -int16_t SX128x::standby() { - return (SX128x::standby(RADIOLIB_SX128X_STANDBY_RC)); -} - -int16_t SX128x::standby(uint8_t mode, bool wakeup) { - // set RF switch (if present) - this->mod->setRfSwitchState(Module::MODE_IDLE); - - if (wakeup) { - // send a NOP command - this pulls the NSS low to exit the sleep mode, - // while preventing interference with possible other SPI transactions - (void)this->mod->SPIwriteStream((uint16_t)RADIOLIB_SX128X_CMD_NOP, NULL, 0, false, false); - } - - const uint8_t data[] = {mode}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_STANDBY, data, 1)); -} - -void SX128x::setDio1Action(void (*func)(void)) { - this->mod->hal->attachInterrupt(this->mod->hal->pinToInterrupt(this->mod->getIrq()), func, - this->mod->hal->GpioInterruptRising); -} - -void SX128x::clearDio1Action() { - this->mod->hal->detachInterrupt(this->mod->hal->pinToInterrupt(this->mod->getIrq())); -} - -void SX128x::setPacketReceivedAction(void (*func)(void)) { - this->setDio1Action(func); -} - -void SX128x::clearPacketReceivedAction() { - this->clearDio1Action(); -} - -void SX128x::setPacketSentAction(void (*func)(void)) { - this->setDio1Action(func); -} - -void SX128x::clearPacketSentAction() { - this->clearDio1Action(); -} - -int16_t SX128x::finishTransmit() { - // clear interrupt flags - clearIrqStatus(); - - // set mode to standby to disable transmitter/RF switch - return (standby()); -} - -int16_t SX128x::startReceive() { - return (this->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF, RADIOLIB_IRQ_RX_DEFAULT_FLAGS, - RADIOLIB_IRQ_RX_DEFAULT_MASK, 0)); -} - -int16_t SX128x::readData(uint8_t* data, size_t len) { - // check active modem - if (getPacketType() == RADIOLIB_SX128X_PACKET_TYPE_RANGING) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // set mode to standby - int16_t state = standby(); - RADIOLIB_ASSERT(state); - - // check integrity CRC - uint16_t irq = getIrqStatus(); - int16_t crcState = RADIOLIB_ERR_NONE; - // Report CRC mismatch when there's a payload CRC error, or a header error and no valid header (to avoid false alarm - // from previous packet) - if ((irq & RADIOLIB_SX128X_IRQ_CRC_ERROR) || - ((irq & RADIOLIB_SX128X_IRQ_HEADER_ERROR) && !(irq & RADIOLIB_SX128X_IRQ_HEADER_VALID))) { - crcState = RADIOLIB_ERR_CRC_MISMATCH; - } - - // get packet length and Rx buffer offset - uint8_t offset = 0; - size_t length = getPacketLength(true, &offset); - if ((len != 0) && (len < length)) { - // user requested less data than we got, only return what was requested - length = len; - } - - // read packet data starting at offset - state = readBuffer(data, length, offset); - RADIOLIB_ASSERT(state); - - // clear interrupt flags - state = clearIrqStatus(); - - // check if CRC failed - this is done after reading data to give user the option to keep them - RADIOLIB_ASSERT(crcState); - - return (state); -} - -int16_t SX128x::finishReceive() { - // set mode to standby to disable RF switch - int16_t state = standby(); - RADIOLIB_ASSERT(state); - - // clear interrupt flags - return (clearIrqStatus()); -} - -uint32_t SX128x::getIrqFlags() { - return ((uint32_t)this->getIrqStatus()); -} - -int16_t SX128x::setIrqFlags(uint32_t irq) { - return (this->setDioIrqParams(irq, irq)); -} - -int16_t SX128x::clearIrqFlags(uint32_t irq) { - return (this->clearIrqStatus(irq)); -} - -int16_t SX128x::startChannelScan() { - ChannelScanConfig_t cfg = { - .cad = - { - .symNum = RADIOLIB_SX128X_CAD_PARAM_DEFAULT, - .detPeak = 0, - .detMin = 0, - .exitMode = 0, - .timeout = 0, - .irqFlags = RADIOLIB_IRQ_CAD_DEFAULT_FLAGS, - .irqMask = RADIOLIB_IRQ_CAD_DEFAULT_MASK, - }, - }; - return (this->startChannelScan(cfg)); -} - -int16_t SX128x::startChannelScan(const ChannelScanConfig_t& config) { - // check active modem - if (getPacketType() != RADIOLIB_SX128X_PACKET_TYPE_LORA) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // set mode to standby - int16_t state = standby(); - RADIOLIB_ASSERT(state); - - // set DIO pin mapping - state = setDioIrqParams(getIrqMapped(config.cad.irqFlags), getIrqMapped(config.cad.irqMask)); - RADIOLIB_ASSERT(state); - - // clear interrupt flags - state = clearIrqStatus(); - RADIOLIB_ASSERT(state); - - // set RF switch (if present) - this->mod->setRfSwitchState(Module::MODE_RX); - - // set mode to CAD - return (setCad(config.cad.symNum)); -} - -int16_t SX128x::getChannelScanResult() { - // check active modem - if (getPacketType() != RADIOLIB_SX128X_PACKET_TYPE_LORA) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // check CAD result - uint16_t cadResult = getIrqStatus(); - int16_t state = RADIOLIB_ERR_UNKNOWN; - if (cadResult & RADIOLIB_SX128X_IRQ_CAD_DETECTED) { - // detected some LoRa activity - state = RADIOLIB_LORA_DETECTED; - } else if (cadResult & RADIOLIB_SX128X_IRQ_CAD_DONE) { - // channel is free - state = RADIOLIB_CHANNEL_FREE; - } - - clearIrqStatus(); - return (state); -} - -int16_t SX128x::setFrequency(float freq) { - RADIOLIB_CHECK_RANGE(freq, 2400.0f, 2500.0f, RADIOLIB_ERR_INVALID_FREQUENCY); - - // calculate raw value - uint32_t frf = (freq * (uint32_t(1) << RADIOLIB_SX128X_DIV_EXPONENT)) / RADIOLIB_SX128X_CRYSTAL_FREQ; - return (setRfFrequency(frf)); -} - -int16_t SX128x::setBandwidth(float bw) { - // check active modem - uint8_t modem = getPacketType(); - if (modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) { - // check range for LoRa - RADIOLIB_CHECK_RANGE(bw, 203.125f, 1625.0f, RADIOLIB_ERR_INVALID_BANDWIDTH); - } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING) { - // check range for ranging - RADIOLIB_CHECK_RANGE(bw, 406.25f, 1625.0f, RADIOLIB_ERR_INVALID_BANDWIDTH); - } else { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - if (fabsf(bw - 203.125f) <= 0.001f) { - this->bandwidth = RADIOLIB_SX128X_LORA_BW_203_125; - } else if (fabsf(bw - 406.25f) <= 0.001f) { - this->bandwidth = RADIOLIB_SX128X_LORA_BW_406_25; - } else if (fabsf(bw - 812.5f) <= 0.001f) { - this->bandwidth = RADIOLIB_SX128X_LORA_BW_812_50; - } else if (fabsf(bw - 1625.0f) <= 0.001f) { - this->bandwidth = RADIOLIB_SX128X_LORA_BW_1625_00; - } else { - return (RADIOLIB_ERR_INVALID_BANDWIDTH); - } - - // update modulation parameters - this->bandwidthKhz = bw; - return (setModulationParams(this->spreadingFactor, this->bandwidth, this->codingRateLoRa)); -} - -int16_t SX128x::setSpreadingFactor(uint8_t sf) { - // check active modem - uint8_t modem = getPacketType(); - if (modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) { - // check range for LoRa - RADIOLIB_CHECK_RANGE(sf, 5, 12, RADIOLIB_ERR_INVALID_SPREADING_FACTOR); - } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING) { - // check range for ranging - RADIOLIB_CHECK_RANGE(sf, 5, 10, RADIOLIB_ERR_INVALID_SPREADING_FACTOR); - } else { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // update modulation parameters - this->spreadingFactor = sf << 4; - int16_t state = setModulationParams(this->spreadingFactor, this->bandwidth, this->codingRateLoRa); - RADIOLIB_ASSERT(state); - - // update mystery register in LoRa mode - SX1280 datasheet rev 3.2 section 14.4.1 - if (modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) { - uint8_t data = 0; - if ((this->spreadingFactor == RADIOLIB_SX128X_LORA_SF_5) || - (this->spreadingFactor == RADIOLIB_SX128X_LORA_SF_6)) { - data = 0x1E; - } else if ((this->spreadingFactor == RADIOLIB_SX128X_LORA_SF_7) || - (this->spreadingFactor == RADIOLIB_SX128X_LORA_SF_8)) { - data = 0x37; - } else { - data = 0x32; - } - state = SX128x::writeRegister(RADIOLIB_SX128X_REG_LORA_SF_CONFIG, &data, 1); - RADIOLIB_ASSERT(state); - - // this register must also be updated for some reason - state = SX128x::readRegister(RADIOLIB_SX128X_REG_FREQ_ERROR_CORRECTION, &data, 1); - RADIOLIB_ASSERT(state); - - data |= 0x01; - state = SX128x::writeRegister(RADIOLIB_SX128X_REG_FREQ_ERROR_CORRECTION, &data, 1); - RADIOLIB_ASSERT(state); - } - - return (state); -} - -int16_t SX128x::setCodingRate(uint8_t cr, bool longInterleaving) { - // check active modem - uint8_t modem = getPacketType(); - - // LoRa/ranging - if ((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING)) { - RADIOLIB_CHECK_RANGE(cr, 4, 8, RADIOLIB_ERR_INVALID_CODING_RATE); - - // update modulation parameters - if (longInterleaving && (modem == RADIOLIB_SX128X_PACKET_TYPE_LORA)) { - switch (cr) { - case 4: - this->codingRateLoRa = 0; - break; - case 5: - case 6: - this->codingRateLoRa = cr; - break; - case 8: - this->codingRateLoRa = cr - 1; - break; - default: - return (RADIOLIB_ERR_INVALID_CODING_RATE); - } - } else { - this->codingRateLoRa = cr - 4; - } - return (setModulationParams(this->spreadingFactor, this->bandwidth, this->codingRateLoRa)); - - // FLRC - } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC) { - RADIOLIB_CHECK_RANGE(cr, 2, 4, RADIOLIB_ERR_INVALID_CODING_RATE); - - // update modulation parameters - this->codingRateFLRC = (cr - 2) * 2; - return (setModulationParams(this->bitRate, this->codingRateFLRC, this->shaping)); - } - - return (RADIOLIB_ERR_WRONG_MODEM); -} - -int16_t SX128x::setOutputPower(int8_t pwr) { - // check if power value is configurable - int16_t state = checkOutputPower(pwr, NULL); - RADIOLIB_ASSERT(state); - - this->power = pwr + 18; - return (setTxParams(this->power)); -} - -int16_t SX128x::checkOutputPower(int8_t pwr, int8_t* clipped) { - if (clipped) { - *clipped = RADIOLIB_MAX(-18, RADIOLIB_MIN(13, pwr)); - } - RADIOLIB_CHECK_RANGE(pwr, -18, 13, RADIOLIB_ERR_INVALID_OUTPUT_POWER); - return (RADIOLIB_ERR_NONE); -} - -int16_t SX128x::setModem(ModemType_t modem) { - switch (modem) { - case (ModemType_t::RADIOLIB_MODEM_LORA): { - return (this->begin()); - } break; - case (ModemType_t::RADIOLIB_MODEM_FSK): { - return (this->beginGFSK()); - } break; - default: - return (RADIOLIB_ERR_WRONG_MODEM); - } -} - -int16_t SX128x::getModem(ModemType_t* modem) { - RADIOLIB_ASSERT_PTR(modem); - - switch (getPacketType()) { - case (RADIOLIB_SX128X_PACKET_TYPE_LORA): - *modem = ModemType_t::RADIOLIB_MODEM_LORA; - return (RADIOLIB_ERR_NONE); - case (RADIOLIB_SX128X_PACKET_TYPE_GFSK): - *modem = ModemType_t::RADIOLIB_MODEM_FSK; - return (RADIOLIB_ERR_NONE); - } - - return (RADIOLIB_ERR_WRONG_MODEM); -} - -int16_t SX128x::setPreambleLength(size_t preambleLength) { - uint8_t modem = getPacketType(); - if ((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING)) { - // LoRa or ranging - // the actual limit is 491520, however, some platforms (notably AVR) limit size_t to 16 bits - RADIOLIB_CHECK_RANGE(preambleLength, 2, 65534, RADIOLIB_ERR_INVALID_PREAMBLE_LENGTH); - - // check preamble length is even - no point even trying odd numbers - if (preambleLength % 2 != 0) { - return (RADIOLIB_ERR_INVALID_PREAMBLE_LENGTH); - } - - // calculate exponent and mantissa values (use the next longer preamble if there's no exact match) - uint8_t e = 1; - uint8_t m = 1; - uint32_t len = 0; - for (; e <= 15; e++) { - for (m = 1; m <= 15; m++) { - len = m * (uint32_t(1) << e); - if (len >= preambleLength) { - break; - } - } - if (len >= preambleLength) { - break; - } - } - - // update packet parameters - this->preambleLengthLoRa = (e << 4) | m; - return (setPacketParamsLoRa(this->preambleLengthLoRa, this->headerType, this->payloadLen, this->crcLoRa, - this->invertIQEnabled)); - - } else if ((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC)) { - // GFSK or FLRC - RADIOLIB_CHECK_RANGE(preambleLength, 4, 32, RADIOLIB_ERR_INVALID_PREAMBLE_LENGTH); - - // check preamble length is multiple of 4 - if (preambleLength % 4 != 0) { - return (RADIOLIB_ERR_INVALID_PREAMBLE_LENGTH); - } - - // update packet parameters - this->preambleLengthGFSK = ((preambleLength / 4) - 1) << 4; - return (setPacketParamsGFSK(this->preambleLengthGFSK, this->syncWordLen, this->syncWordMatch, this->crcGFSK, - this->whitening, this->packetType)); - } - - return (RADIOLIB_ERR_WRONG_MODEM); -} - -int16_t SX128x::setDataRate(DataRate_t dr, ModemType_t modem) { - // get the current modem - ModemType_t currentModem; - int16_t state = this->getModem(¤tModem); - RADIOLIB_ASSERT(state); - - // switch over if the requested modem is different - if (modem != RADIOLIB_MODEM_NONE && modem != currentModem) { - state = this->standby(); - RADIOLIB_ASSERT(state); - state = this->setModem(modem); - RADIOLIB_ASSERT(state); - } - - if (modem == RADIOLIB_MODEM_NONE) { - modem = currentModem; - } - - // select interpretation based on modem - if (modem == RADIOLIB_MODEM_LORA) { - state = this->setBandwidth(dr.lora.bandwidth); - RADIOLIB_ASSERT(state); - state = this->setSpreadingFactor(dr.lora.spreadingFactor); - RADIOLIB_ASSERT(state); - state = this->setCodingRate(dr.lora.codingRate); - } else { - return (RADIOLIB_ERR_WRONG_MODEM); - } - return (state); -} - -int16_t SX128x::setBitRate(float br) { - // check active modem - uint8_t modem = getPacketType(); - - // GFSK/BLE - if ((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE)) { - if ((uint16_t)br == 125) { - this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_125_BW_0_3; - } else if ((uint16_t)br == 250) { - this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_250_BW_0_6; - } else if ((uint16_t)br == 400) { - this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_400_BW_1_2; - } else if ((uint16_t)br == 500) { - this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_500_BW_1_2; - } else if ((uint16_t)br == 800) { - this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_800_BW_2_4; - } else if ((uint16_t)br == 1000) { - this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_1_000_BW_2_4; - } else if ((uint16_t)br == 1600) { - this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_1_600_BW_2_4; - } else if ((uint16_t)br == 2000) { - this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_2_000_BW_2_4; - } else { - return (RADIOLIB_ERR_INVALID_BIT_RATE); - } - - // update modulation parameters - this->bitRateKbps = (uint16_t)br; - return (setModulationParams(this->bitRate, this->modIndex, this->shaping)); - - // FLRC - } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC) { - if ((uint16_t)br == 260) { - this->bitRate = RADIOLIB_SX128X_FLRC_BR_0_260_BW_0_3; - } else if ((uint16_t)br == 325) { - this->bitRate = RADIOLIB_SX128X_FLRC_BR_0_325_BW_0_3; - } else if ((uint16_t)br == 520) { - this->bitRate = RADIOLIB_SX128X_FLRC_BR_0_520_BW_0_6; - } else if ((uint16_t)br == 650) { - this->bitRate = RADIOLIB_SX128X_FLRC_BR_0_650_BW_0_6; - } else if ((uint16_t)br == 1000) { - this->bitRate = RADIOLIB_SX128X_FLRC_BR_1_000_BW_1_2; - } else if ((uint16_t)br == 1300) { - this->bitRate = RADIOLIB_SX128X_FLRC_BR_1_300_BW_1_2; - } else { - return (RADIOLIB_ERR_INVALID_BIT_RATE); - } - - // update modulation parameters - this->bitRateKbps = (uint16_t)br; - return (setModulationParams(this->bitRate, this->codingRateFLRC, this->shaping)); - } - - return (RADIOLIB_ERR_WRONG_MODEM); -} - -int16_t SX128x::setFrequencyDeviation(float freqDev) { - // check active modem - uint8_t modem = getPacketType(); - if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE))) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // set frequency deviation to lowest available setting (required for digimodes) - float newFreqDev = freqDev; - if (freqDev < 0.0f) { - newFreqDev = 62.5f; - } - - RADIOLIB_CHECK_RANGE(newFreqDev, 62.5f, 1000.0f, RADIOLIB_ERR_INVALID_FREQUENCY_DEVIATION); - - // override for the lowest possible frequency deviation - required for some PhysicalLayer protocols - if (newFreqDev == 0.0f) { - this->modIndex = RADIOLIB_SX128X_BLE_GFSK_MOD_IND_0_35; - this->bitRate = RADIOLIB_SX128X_BLE_GFSK_BR_0_125_BW_0_3; - return (setModulationParams(this->bitRate, this->modIndex, this->shaping)); - } - - // update modulation parameters - uint8_t modInd = (uint8_t)((8.0f * (newFreqDev / (float)this->bitRateKbps)) - 1.0f); - if (modInd > RADIOLIB_SX128X_BLE_GFSK_MOD_IND_4_00) { - return (RADIOLIB_ERR_INVALID_MODULATION_PARAMETERS); - } - - // update modulation parameters - this->frequencyDev = newFreqDev; - this->modIndex = modInd; - return (setModulationParams(this->bitRate, this->modIndex, this->shaping)); -} - -int16_t SX128x::setDataShaping(uint8_t sh) { - // check active modem - uint8_t modem = getPacketType(); - if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE) || - (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC))) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // set data this->shaping - switch (sh) { - case RADIOLIB_SHAPING_NONE: - this->shaping = RADIOLIB_SX128X_BLE_GFSK_BT_OFF; - break; - case RADIOLIB_SHAPING_0_5: - this->shaping = RADIOLIB_SX128X_BLE_GFSK_BT_0_5; - break; - case RADIOLIB_SHAPING_1_0: - this->shaping = RADIOLIB_SX128X_BLE_GFSK_BT_1_0; - break; - default: - return (RADIOLIB_ERR_INVALID_DATA_SHAPING); - } - - // update modulation parameters - if ((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE)) { - return (setModulationParams(this->bitRate, this->modIndex, this->shaping)); - } else { - return (setModulationParams(this->bitRate, this->codingRateFLRC, this->shaping)); - } -} - -int16_t SX128x::setSyncWord(const uint8_t* syncWord, uint8_t len) { - // check active modem - uint8_t modem = getPacketType(); - if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC))) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - if (modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) { - // GFSK can use up to 5 bytes as sync word - if (len > 5) { - return (RADIOLIB_ERR_INVALID_SYNC_WORD); - } - - // calculate sync word length parameter value - if (len > 0) { - this->syncWordLen = (len - 1) * 2; - } - - } else { - // FLRC requires 32-bit sync word - if (!((len == 0) || (len == 4))) { - return (RADIOLIB_ERR_INVALID_SYNC_WORD); - } - - // save sync word length parameter value - this->syncWordLen = len; - } - - // update sync word - int16_t state = - SX128x::writeRegister(RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_4 + (5 - len), const_cast(syncWord), len); - RADIOLIB_ASSERT(state); - - // update packet parameters - if (this->syncWordLen == 0) { - this->syncWordMatch = RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_OFF; - } else { - /// \todo add support for multiple sync words - this->syncWordMatch = RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1; - } - return (setPacketParamsGFSK(this->preambleLengthGFSK, this->syncWordLen, this->syncWordMatch, this->crcGFSK, - this->whitening, this->packetType)); -} - -int16_t SX128x::setSyncWord(uint8_t syncWord, uint8_t controlBits) { - // check active modem - if (getPacketType() != RADIOLIB_SX128X_PACKET_TYPE_LORA) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // update register - const uint8_t data[2] = {(uint8_t)((syncWord & 0xF0) | ((controlBits & 0xF0) >> 4)), - (uint8_t)(((syncWord & 0x0F) << 4) | (controlBits & 0x0F))}; - return (writeRegister(RADIOLIB_SX128X_REG_LORA_SYNC_WORD_MSB, data, 2)); -} - -int16_t SX128x::setCRC(uint8_t len, uint32_t initial, uint16_t polynomial) { - // check active modem - uint8_t modem = getPacketType(); - - int16_t state; - if ((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC)) { - // update packet parameters - if (modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) { - if (len > 2) { - return (RADIOLIB_ERR_INVALID_CRC_CONFIGURATION); - } - } else { - if (len > 3) { - return (RADIOLIB_ERR_INVALID_CRC_CONFIGURATION); - } - } - this->crcGFSK = len << 4; - state = setPacketParamsGFSK(this->preambleLengthGFSK, this->syncWordLen, this->syncWordMatch, this->crcGFSK, - this->whitening, this->packetType); - RADIOLIB_ASSERT(state); - - // set initial CRC value - uint8_t data[] = {(uint8_t)((initial >> 8) & 0xFF), (uint8_t)(initial & 0xFF)}; - state = writeRegister(RADIOLIB_SX128X_REG_CRC_INITIAL_MSB, data, 2); - RADIOLIB_ASSERT(state); - - // set CRC polynomial - data[0] = (uint8_t)((polynomial >> 8) & 0xFF); - data[1] = (uint8_t)(polynomial & 0xFF); - state = writeRegister(RADIOLIB_SX128X_REG_CRC_POLYNOMIAL_MSB, data, 2); - return (state); - - } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE) { - // update packet parameters - if (len == 0) { - this->crcBLE = RADIOLIB_SX128X_BLE_CRC_OFF; - } else if (len == 3) { - this->crcBLE = RADIOLIB_SX128X_BLE_CRC_3_BYTE; - } else { - return (RADIOLIB_ERR_INVALID_CRC_CONFIGURATION); - } - state = setPacketParamsBLE(this->connectionState, this->crcBLE, this->bleTestPayload, this->whitening); - RADIOLIB_ASSERT(state); - - // set initial CRC value - const uint8_t data[] = {(uint8_t)((initial >> 16) & 0xFF), (uint8_t)((initial >> 8) & 0xFF), - (uint8_t)(initial & 0xFF)}; - state = writeRegister(RADIOLIB_SX128X_REG_BLE_CRC_INITIAL_MSB, data, 3); - return (state); - - } else if ((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING)) { - // update packet parameters - if (len == 0) { - this->crcLoRa = RADIOLIB_SX128X_LORA_CRC_OFF; - } else if (len == 2) { - this->crcLoRa = RADIOLIB_SX128X_LORA_CRC_ON; - } else { - return (RADIOLIB_ERR_INVALID_CRC_CONFIGURATION); - } - state = setPacketParamsLoRa(this->preambleLengthLoRa, this->headerType, this->payloadLen, this->crcLoRa, - this->invertIQEnabled); - return (state); - } - - return (RADIOLIB_ERR_UNKNOWN); -} - -int16_t SX128x::setWhitening(bool enabled) { - // check active modem - uint8_t modem = getPacketType(); - if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE))) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // update packet parameters - if (enabled) { - this->whitening = RADIOLIB_SX128X_GFSK_BLE_WHITENING_ON; - } else { - this->whitening = RADIOLIB_SX128X_GFSK_BLE_WHITENING_OFF; - } - - if (modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) { - return (setPacketParamsGFSK(this->preambleLengthGFSK, this->syncWordLen, this->syncWordMatch, this->crcGFSK, - this->whitening, this->packetType)); - } - return (setPacketParamsBLE(this->connectionState, this->crcBLE, this->bleTestPayload, this->whitening)); -} - -int16_t SX128x::setAccessAddress(uint32_t addr) { - // check active modem - if (getPacketType() != RADIOLIB_SX128X_PACKET_TYPE_BLE) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // set the address - const uint8_t addrBuff[] = {(uint8_t)((addr >> 24) & 0xFF), (uint8_t)((addr >> 16) & 0xFF), - (uint8_t)((addr >> 8) & 0xFF), (uint8_t)(addr & 0xFF)}; - return (SX128x::writeRegister(RADIOLIB_SX128X_REG_ACCESS_ADDRESS_BYTE_3, addrBuff, 4)); -} - -int16_t SX128x::setHighSensitivityMode(bool enable) { - // read the current registers - uint8_t RxGain = 0; - int16_t state = readRegister(RADIOLIB_SX128X_REG_GAIN_MODE, &RxGain, 1); - RADIOLIB_ASSERT(state); - - if (enable) { - RxGain |= 0xC0; // Set bits 6 and 7 - } else { - RxGain &= ~0xC0; // Unset bits 6 and 7 - } - - // update all values - state = writeRegister(RADIOLIB_SX128X_REG_GAIN_MODE, &RxGain, 1); - return (state); -} - -int16_t SX128x::setGainControl(uint8_t gain) { - // read the current registers - uint8_t ManualGainSetting = 0; - int16_t state = readRegister(RADIOLIB_SX128X_REG_MANUAL_GAIN_CONTROL_ENABLE_2, &ManualGainSetting, 1); - RADIOLIB_ASSERT(state); - uint8_t LNAGainValue = 0; - state = readRegister(RADIOLIB_SX128X_REG_MANUAL_GAIN_SETTING, &LNAGainValue, 1); - RADIOLIB_ASSERT(state); - uint8_t LNAGainControl = 0; - state = readRegister(RADIOLIB_SX128X_REG_MANUAL_GAIN_CONTROL_ENABLE_1, &LNAGainControl, 1); - RADIOLIB_ASSERT(state); - - // set the gain - if (gain > 0 && gain < 14) { - // Set manual gain - ManualGainSetting &= ~0x01; // Set bit 0 to 0 (Enable Manual Gain Control) - LNAGainValue &= 0xF0; // Bits 0, 1, 2 and 3 to 0 - LNAGainValue |= gain; // Set bits 0, 1, 2 and 3 to Manual Gain Setting (1-13) - LNAGainControl |= 0x80; // Set bit 7 to 1 (Enable Manual Gain Control) - } else { - // Set automatic gain if 0 or out of range - ManualGainSetting |= 0x01; // Set bit 0 to 1 (Enable Automatic Gain Control) - LNAGainValue &= 0xF0; // Bits 0, 1, 2 and 3 to 0 - LNAGainValue |= 0x0A; // Set bits 0, 1, 2 and 3 to Manual Gain Setting (1-13) - LNAGainControl &= ~0x80; // Set bit 7 to 0 (Enable Automatic Gain Control) - } - - // update all values - state = writeRegister(RADIOLIB_SX128X_REG_MANUAL_GAIN_CONTROL_ENABLE_2, &ManualGainSetting, 1); - RADIOLIB_ASSERT(state); - state = writeRegister(RADIOLIB_SX128X_REG_MANUAL_GAIN_SETTING, &LNAGainValue, 1); - RADIOLIB_ASSERT(state); - state = writeRegister(RADIOLIB_SX128X_REG_MANUAL_GAIN_CONTROL_ENABLE_1, &LNAGainControl, 1); - return (state); -} - -float SX128x::getRSSI() { - // get packet status - uint8_t packetStatus[5]; - this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_PACKET_STATUS, packetStatus, 5); - - // check active modem - uint8_t modem = getPacketType(); - if ((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING)) { - // LoRa or ranging - uint8_t rssiSync = packetStatus[0]; - float rssiMeasured = -1.0 * rssiSync / 2.0; - float snr = getSNR(); - if (snr <= 0.0f) { - return (rssiMeasured - snr); - } else { - return (rssiMeasured); - } - } else { - // GFSK, BLE or FLRC - uint8_t rssiSync = packetStatus[1]; - return (-1.0 * rssiSync / 2.0); - } -} - -float SX128x::getRSSI(bool packet) { - if (!packet) { - // get instantaneous RSSI value - uint8_t data[3] = {0, 0, 0}; // RssiInst, Status, RFU - this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_RSSI_INST, data, 3); - return ((float)data[0] / (-2.0f)); - } else { - return this->getRSSI(); - } -} - -float SX128x::getSNR() { - // check active modem - uint8_t modem = getPacketType(); - if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING))) { - return (0.0); - } - - // get packet status - uint8_t packetStatus[5]; - this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_PACKET_STATUS, packetStatus, 5); - - // calculate real SNR - uint8_t snr = packetStatus[1]; - if (snr < 128) { - return (snr / 4.0); - } else { - return ((snr - 256) / 4.0f); - } -} - -float SX128x::getFrequencyError() { - // check active modem - uint8_t modem = getPacketType(); - if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING))) { - return (0.0); - } - - // read the raw frequency error register values - uint8_t efeRaw[3] = {0}; - int16_t state = readRegister(RADIOLIB_SX128X_REG_FEI_MSB, &efeRaw[0], 1); - RADIOLIB_ASSERT(state); - state = readRegister(RADIOLIB_SX128X_REG_FEI_MID, &efeRaw[1], 1); - RADIOLIB_ASSERT(state); - state = readRegister(RADIOLIB_SX128X_REG_FEI_LSB, &efeRaw[2], 1); - RADIOLIB_ASSERT(state); - uint32_t efe = ((uint32_t)efeRaw[0] << 16) | ((uint32_t)efeRaw[1] << 8) | efeRaw[2]; - efe &= 0x0FFFFF; - - float error = 0; - - // check the first bit - if (efe & 0x80000) { - // frequency error is negative - efe |= (uint32_t)0xFFF00000; - efe = ~efe + 1; - error = 1.55f * (float)efe / (1600.0f / this->bandwidthKhz) * -1.0f; - } else { - error = 1.55f * (float)efe / (1600.0f / this->bandwidthKhz); - } - - return (error); -} - -size_t SX128x::getPacketLength(bool update) { - return (this->getPacketLength(update, NULL)); -} - -size_t SX128x::getPacketLength(bool update, uint8_t* offset) { - (void)update; - - // in implicit mode, return the cached value - if ((getPacketType() == RADIOLIB_SX128X_PACKET_TYPE_LORA) && - (this->headerType == RADIOLIB_SX128X_LORA_HEADER_IMPLICIT)) { - return (this->payloadLen); - } - - uint8_t rxBufStatus[2] = {0, 0}; - this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_RX_BUFFER_STATUS, rxBufStatus, 2); - - if (offset) { - *offset = rxBufStatus[1]; - } - - return ((size_t)rxBufStatus[0]); -} - -int16_t SX128x::getLoRaRxHeaderInfo(uint8_t* cr, bool* hasCRC) { - int16_t state = RADIOLIB_ERR_NONE; - - // check if in explicit header mode - if (this->headerType == RADIOLIB_SX128X_LORA_HEADER_IMPLICIT) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - if (cr) { - *cr = this->mod->SPIgetRegValue(RADIOLIB_SX128X_REG_LORA_RX_CODING_RATE, 6, 4) >> 4; - } - if (hasCRC) { - *hasCRC = (this->mod->SPIgetRegValue(RADIOLIB_SX128X_REG_FEI_MSB, 4, 4) != 0); - } - - return (state); -} - -int16_t SX128x::fixedPacketLengthMode(uint8_t len) { - return (setPacketMode(RADIOLIB_SX128X_GFSK_FLRC_PACKET_FIXED, len)); -} - -int16_t SX128x::variablePacketLengthMode(uint8_t maxLen) { - return (setPacketMode(RADIOLIB_SX128X_GFSK_FLRC_PACKET_VARIABLE, maxLen)); -} - -RadioLibTime_t SX128x::calculateTimeOnAir(ModemType_t modem, DataRate_t dr, PacketConfig_t pc, size_t len) { - switch (modem) { - case (ModemType_t::RADIOLIB_MODEM_LORA): { - // calculate number of symbols - float N_symbol = 0; - uint8_t sf = dr.lora.spreadingFactor; - float cr = (float)dr.lora.codingRate; - - // get SF coefficients - float coeff1 = 0; - int16_t coeff2 = 0; - int16_t coeff3 = 0; - if (sf < 7) { - // SF5, SF6 - coeff1 = 6.25; - coeff2 = 4 * sf; - coeff3 = 4 * sf; - } else if (sf < 11) { - // SF7. SF8, SF9, SF10 - coeff1 = 4.25; - coeff2 = 4 * sf + 8; - coeff3 = 4 * sf; - } else { - // SF11, SF12 - coeff1 = 4.25; - coeff2 = 4 * sf + 8; - coeff3 = 4 * (sf - 2); - } - - // get CRC length - int16_t N_bitCRC = 16; - if (!pc.lora.crcEnabled) { - N_bitCRC = 0; - } - - // get header length - int16_t N_symbolHeader = 20; - if (pc.lora.implicitHeader) { - N_symbolHeader = 0; - } - - // calculate number of LoRa preamble symbols - uint32_t N_symbolPreamble = pc.lora.preambleLength; - - // calculate the number of symbols - N_symbol = (float)N_symbolPreamble + coeff1 + 8.0f + - ceilf((float)RADIOLIB_MAX((int16_t)(8 * len + N_bitCRC - coeff2 + N_symbolHeader), (int16_t)0) / - (float)coeff3) * - cr; - - // get time-on-air in us - return (((uint32_t(1) << sf) / dr.lora.bandwidth) * N_symbol * 1000.0f); - } - case (ModemType_t::RADIOLIB_MODEM_FSK): - return ( - (((float)(pc.fsk.crcLength * 8) + pc.fsk.syncWordLength + pc.fsk.preambleLength + (uint32_t)len * 8) / - (dr.fsk.bitRate / 1000.0f))); - - default: - return (RADIOLIB_ERR_WRONG_MODEM); - } -} - -RadioLibTime_t SX128x::getTimeOnAir(size_t len) { - // check active modem - uint8_t modem = getPacketType(); - DataRate_t dr = {}; - PacketConfig_t pc = {}; - - if (modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) { - uint8_t sf = this->spreadingFactor >> 4; - uint8_t cr = this->codingRateLoRa; - // We assume same calculation for short and long interleaving, so map CR values 0-4 and 5-7 to the same values - if (cr < 5) { - cr = cr + 4; - } else if (cr == 7) { - cr = cr + 1; - } - - dr.lora.spreadingFactor = sf; - dr.lora.codingRate = cr; - dr.lora.bandwidth = this->bandwidthKhz; - - uint16_t preambleLength = - (this->preambleLengthLoRa & 0x0F) * (uint32_t(1) << ((this->preambleLengthLoRa & 0xF0) >> 4)); - - pc.lora.preambleLength = preambleLength; - pc.lora.implicitHeader = this->headerType == RADIOLIB_SX128X_LORA_HEADER_IMPLICIT; - pc.lora.crcEnabled = this->crcLoRa == RADIOLIB_SX128X_LORA_CRC_ON; - pc.lora.ldrOptimize = false; - - return (calculateTimeOnAir(ModemType_t::RADIOLIB_MODEM_LORA, dr, pc, len)); - } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) { - dr.fsk.bitRate = (float)this->bitRateKbps; - dr.fsk.freqDev = this->frequencyDev; - - pc.fsk.preambleLength = ((uint16_t)this->preambleLengthGFSK >> 2) + 4; - pc.fsk.syncWordLength = ((this->syncWordLen >> 1) + 1) * 8; - pc.fsk.crcLength = this->crcGFSK >> 4; - - return (calculateTimeOnAir(ModemType_t::RADIOLIB_MODEM_FSK, dr, pc, len)); - } else { - return (RADIOLIB_ERR_WRONG_MODEM); - } -} - -int16_t SX128x::implicitHeader(size_t len) { - return (setHeaderType(RADIOLIB_SX128X_LORA_HEADER_IMPLICIT, len)); -} - -int16_t SX128x::explicitHeader() { - return (setHeaderType(RADIOLIB_SX128X_LORA_HEADER_EXPLICIT)); -} - -int16_t SX128x::setEncoding(uint8_t encoding) { - return (setWhitening(encoding)); -} - -void SX128x::setRfSwitchPins(uint32_t rxEn, uint32_t txEn) { - this->mod->setRfSwitchPins(rxEn, txEn); -} - -void SX128x::setRfSwitchTable(const uint32_t (&pins)[Module::RFSWITCH_MAX_PINS], const Module::RfSwitchMode_t table[]) { - this->mod->setRfSwitchTable(pins, table); -} - -uint8_t SX128x::randomByte() { - // it's unclear whether SX128x can measure RSSI while not receiving a packet - // this method is implemented only for PhysicalLayer compatibility - return (0); -} - -int16_t SX128x::invertIQ(bool enable) { - if (getPacketType() != RADIOLIB_SX128X_PACKET_TYPE_LORA) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - if (enable) { - this->invertIQEnabled = RADIOLIB_SX128X_LORA_IQ_INVERTED; - } else { - this->invertIQEnabled = RADIOLIB_SX128X_LORA_IQ_STANDARD; - } - - return (setPacketParamsLoRa(this->preambleLengthLoRa, this->headerType, this->payloadLen, this->crcLoRa, - this->invertIQEnabled)); -} - -int16_t SX128x::stageMode(RadioModeType_t mode, RadioModeConfig_t* cfg) { - int16_t state; - - switch (mode) { - case (RADIOLIB_RADIO_MODE_RX): { - // in implicit header mode, use the provided length if it is nonzero - // otherwise we trust the user has previously set the payload length manually - if ((this->headerType == RADIOLIB_SX128X_LORA_HEADER_IMPLICIT) && (cfg->receive.len != 0)) { - this->payloadLen = cfg->receive.len; - } - - // check active modem - if (getPacketType() == RADIOLIB_SX128X_PACKET_TYPE_RANGING) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // set DIO mapping - if (cfg->receive.timeout != RADIOLIB_SX128X_RX_TIMEOUT_INF) { - cfg->receive.irqMask |= (1UL << RADIOLIB_IRQ_TIMEOUT); - } - - state = setDioIrqParams(getIrqMapped(cfg->receive.irqFlags), getIrqMapped(cfg->receive.irqMask)); - RADIOLIB_ASSERT(state); - - // set buffer pointers - state = setBufferBaseAddress(); - RADIOLIB_ASSERT(state); - - // clear interrupt flags - state = clearIrqStatus(); - RADIOLIB_ASSERT(state); - - // set implicit mode and expected len if applicable - if ((this->headerType == RADIOLIB_SX128X_LORA_HEADER_IMPLICIT) && - (getPacketType() == RADIOLIB_SX128X_PACKET_TYPE_LORA)) { - state = setPacketParamsLoRa(this->preambleLengthLoRa, this->headerType, this->payloadLen, this->crcLoRa, - this->invertIQEnabled); - RADIOLIB_ASSERT(state); - } - // if max(uint32_t) is used, revert to RxContinuous - if (cfg->receive.timeout == 0xFFFFFFFF) { - cfg->receive.timeout = 0xFFFF; - } - this->rxTimeout = cfg->receive.timeout; - } break; - - case (RADIOLIB_RADIO_MODE_TX): { - // check packet length - if (cfg->transmit.len > RADIOLIB_SX128X_MAX_PACKET_LENGTH) { - return (RADIOLIB_ERR_PACKET_TOO_LONG); - } - - // set packet Length - uint8_t modem = getPacketType(); - if (modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) { - state = setPacketParamsLoRa(this->preambleLengthLoRa, this->headerType, cfg->transmit.len, - this->crcLoRa, this->invertIQEnabled); - } else if ((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC)) { - state = setPacketParamsGFSK(this->preambleLengthGFSK, this->syncWordLen, this->syncWordMatch, - this->crcGFSK, this->whitening, this->packetType, cfg->transmit.len); - } else if (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE) { - state = setPacketParamsBLE(this->connectionState, this->crcBLE, this->bleTestPayload, this->whitening); - } else { - return (RADIOLIB_ERR_WRONG_MODEM); - } - RADIOLIB_ASSERT(state); - - // update output power - state = setTxParams(this->power); - RADIOLIB_ASSERT(state); - - // set buffer pointers - state = setBufferBaseAddress(); - RADIOLIB_ASSERT(state); - - // write packet to buffer - if (modem == RADIOLIB_SX128X_PACKET_TYPE_BLE) { - // first 2 bytes of BLE payload are PDU header - state = writeBuffer(cfg->transmit.data, cfg->transmit.len, 2); - RADIOLIB_ASSERT(state); - } else { - state = writeBuffer(cfg->transmit.data, cfg->transmit.len); - RADIOLIB_ASSERT(state); - } - - // set DIO mapping - state = setDioIrqParams(RADIOLIB_SX128X_IRQ_TX_DONE | RADIOLIB_SX128X_IRQ_RX_TX_TIMEOUT, - RADIOLIB_SX128X_IRQ_TX_DONE); - RADIOLIB_ASSERT(state); - - // clear interrupt flags - state = clearIrqStatus(); - RADIOLIB_ASSERT(state); - } break; - - default: - return (RADIOLIB_ERR_UNSUPPORTED); - } - - this->stagedMode = mode; - return (state); -} - -int16_t SX128x::launchMode() { - int16_t state; - switch (this->stagedMode) { - case (RADIOLIB_RADIO_MODE_RX): { - this->mod->setRfSwitchState(Module::MODE_RX); - state = setRx(this->rxTimeout); - RADIOLIB_ASSERT(state); - } break; - - case (RADIOLIB_RADIO_MODE_TX): { - this->mod->setRfSwitchState(Module::MODE_TX); - state = setTx(RADIOLIB_SX128X_TX_TIMEOUT_NONE); - RADIOLIB_ASSERT(state); - - // wait for BUSY to go low (= PA ramp up done) - while (this->mod->hal->digitalRead(this->mod->getGpio())) { - this->mod->hal->yield(); - } - } break; - - default: - return (RADIOLIB_ERR_UNSUPPORTED); - } - - this->stagedMode = RADIOLIB_RADIO_MODE_NONE; - return (state); -} - -#if !RADIOLIB_EXCLUDE_DIRECT_RECEIVE -void SX128x::setDirectAction(void (*func)(void)) { - // SX128x is unable to perform direct mode reception - // this method is implemented only for PhysicalLayer compatibility - (void)func; -} - -void SX128x::readBit(uint32_t pin) { - // SX128x is unable to perform direct mode reception - // this method is implemented only for PhysicalLayer compatibility - (void)pin; -} -#endif - -Module* SX128x::getMod() { - return (this->mod); -} - -int16_t SX128x::modSetup(uint8_t modem) { - // set module properties - this->mod->init(); - this->mod->hal->pinMode(this->mod->getIrq(), this->mod->hal->GpioModeInput); - this->mod->hal->pinMode(this->mod->getGpio(), this->mod->hal->GpioModeInput); - this->mod->spiConfig.widths[RADIOLIB_MODULE_SPI_WIDTH_ADDR] = Module::BITS_16; - this->mod->spiConfig.widths[RADIOLIB_MODULE_SPI_WIDTH_CMD] = Module::BITS_8; - this->mod->spiConfig.statusPos = 1; - this->mod->spiConfig.cmds[RADIOLIB_MODULE_SPI_COMMAND_READ] = RADIOLIB_SX128X_CMD_READ_REGISTER; - this->mod->spiConfig.cmds[RADIOLIB_MODULE_SPI_COMMAND_WRITE] = RADIOLIB_SX128X_CMD_WRITE_REGISTER; - this->mod->spiConfig.cmds[RADIOLIB_MODULE_SPI_COMMAND_NOP] = RADIOLIB_SX128X_CMD_NOP; - this->mod->spiConfig.cmds[RADIOLIB_MODULE_SPI_COMMAND_STATUS] = RADIOLIB_SX128X_CMD_GET_STATUS; - this->mod->spiConfig.stream = true; - this->mod->spiConfig.parseStatusCb = SPIparseStatus; - - // find the chip - this will also reset the module and verify the module - if (!SX128x::findChip(this->chipType)) { - RADIOLIB_DEBUG_BASIC_PRINTLN("No SX128x found!"); - this->mod->term(); - return (RADIOLIB_ERR_CHIP_NOT_FOUND); - } - RADIOLIB_DEBUG_BASIC_PRINTLN("M\tSX128x"); - - // configure settings not accessible by API - return (config(modem)); -} - -uint8_t SX128x::getStatus() { - uint8_t data = 0; - this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_STATUS, &data, 0); - return (data); -} - -int16_t SX128x::writeRegister(uint16_t addr, const uint8_t* data, uint8_t numBytes) { - this->mod->SPIwriteRegisterBurst(addr, data, numBytes); - return (RADIOLIB_ERR_NONE); -} - -int16_t SX128x::readRegister(uint16_t addr, uint8_t* data, uint8_t numBytes) { - // send the command - this->mod->SPIreadRegisterBurst(addr, numBytes, data); - - // check the status - int16_t state = this->mod->SPIcheckStream(); - return (state); -} - -int16_t SX128x::writeBuffer(const uint8_t* data, uint8_t numBytes, uint8_t offset) { - const uint8_t cmd[] = {RADIOLIB_SX128X_CMD_WRITE_BUFFER, offset}; - return (this->mod->SPIwriteStream(cmd, 2, data, numBytes)); -} - -int16_t SX128x::readBuffer(uint8_t* data, uint8_t numBytes, uint8_t offset) { - const uint8_t cmd[] = {RADIOLIB_SX128X_CMD_READ_BUFFER, offset}; - return (this->mod->SPIreadStream(cmd, 2, data, numBytes)); -} - -int16_t SX128x::setTx(uint16_t periodBaseCount, uint8_t periodBase) { - const uint8_t data[] = {periodBase, (uint8_t)((periodBaseCount >> 8) & 0xFF), (uint8_t)(periodBaseCount & 0xFF)}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_TX, data, 3)); -} - -int16_t SX128x::setRx(uint16_t periodBaseCount, uint8_t periodBase) { - const uint8_t data[] = {periodBase, (uint8_t)((periodBaseCount >> 8) & 0xFF), (uint8_t)(periodBaseCount & 0xFF)}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_RX, data, 3)); -} - -int16_t SX128x::setCad(uint8_t symbolNum) { - // configure parameters - int16_t state = this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_CAD_PARAMS, &symbolNum, 1); - RADIOLIB_ASSERT(state); - - // start CAD - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_CAD, NULL, 0)); -} - -uint8_t SX128x::getPacketType() { - uint8_t data = 0xFF; - this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_PACKET_TYPE, &data, 1); - return (data); -} - -int16_t SX128x::setRfFrequency(uint32_t frf) { - const uint8_t data[] = {(uint8_t)((frf >> 16) & 0xFF), (uint8_t)((frf >> 8) & 0xFF), (uint8_t)(frf & 0xFF)}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_RF_FREQUENCY, data, 3)); -} - -int16_t SX128x::setTxParams(uint8_t pwr, uint8_t rampTime) { - const uint8_t data[] = {pwr, rampTime}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_TX_PARAMS, data, 2)); -} - -int16_t SX128x::setBufferBaseAddress(uint8_t txBaseAddress, uint8_t rxBaseAddress) { - const uint8_t data[] = {txBaseAddress, rxBaseAddress}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_BUFFER_BASE_ADDRESS, data, 2)); -} - -int16_t SX128x::setModulationParams(uint8_t modParam1, uint8_t modParam2, uint8_t modParam3) { - const uint8_t data[] = {modParam1, modParam2, modParam3}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_MODULATION_PARAMS, data, 3)); -} - -int16_t SX128x::setPacketParamsGFSK(uint8_t preambleLen, - uint8_t syncLen, - uint8_t syncMatch, - uint8_t crcLen, - uint8_t whiten, - uint8_t hdrType, - uint8_t payLen) { - const uint8_t data[] = {preambleLen, syncLen, syncMatch, hdrType, payLen, crcLen, whiten}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_PACKET_PARAMS, data, 7)); -} - -int16_t SX128x::setPacketParamsBLE(uint8_t connState, uint8_t crcLen, uint8_t bleTest, uint8_t whiten) { - const uint8_t data[] = {connState, crcLen, bleTest, whiten, 0x00, 0x00, 0x00}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_PACKET_PARAMS, data, 7)); -} - -int16_t SX128x::setPacketParamsLoRa(uint8_t preambleLen, uint8_t hdrType, uint8_t payLen, uint8_t crc, uint8_t invIQ) { - const uint8_t data[] = {preambleLen, hdrType, payLen, crc, invIQ, 0x00, 0x00}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_PACKET_PARAMS, data, 7)); -} - -int16_t SX128x::setDioIrqParams(uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask) { - const uint8_t data[] = {(uint8_t)((irqMask >> 8) & 0xFF), (uint8_t)(irqMask & 0xFF), - (uint8_t)((dio1Mask >> 8) & 0xFF), (uint8_t)(dio1Mask & 0xFF), - (uint8_t)((dio2Mask >> 8) & 0xFF), (uint8_t)(dio2Mask & 0xFF), - (uint8_t)((dio3Mask >> 8) & 0xFF), (uint8_t)(dio3Mask & 0xFF)}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_DIO_IRQ_PARAMS, data, 8)); -} - -uint16_t SX128x::getIrqStatus() { - uint8_t data[] = {0x00, 0x00}; - this->mod->SPIreadStream(RADIOLIB_SX128X_CMD_GET_IRQ_STATUS, data, 2); - return (((uint16_t)(data[0]) << 8) | data[1]); -} - -int16_t SX128x::clearIrqStatus(uint16_t clearIrqParams) { - const uint8_t data[] = {(uint8_t)((clearIrqParams >> 8) & 0xFF), (uint8_t)(clearIrqParams & 0xFF)}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_CLEAR_IRQ_STATUS, data, 2)); -} - -int16_t SX128x::setRangingRole(uint8_t role) { - const uint8_t data[] = {role}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_RANGING_ROLE, data, 1)); -} - -int16_t SX128x::setPacketType(uint8_t type) { - const uint8_t data[] = {type}; - return (this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_PACKET_TYPE, data, 1)); -} - -int16_t SX128x::setPacketMode(uint8_t mode, uint8_t len) { - // check active modem - uint8_t modem = getPacketType(); - if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_GFSK) || (modem == RADIOLIB_SX128X_PACKET_TYPE_FLRC))) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // set requested packet mode - int16_t state = setPacketParamsGFSK(this->preambleLengthGFSK, this->syncWordLen, this->syncWordMatch, this->crcGFSK, - this->whitening, mode, len); - RADIOLIB_ASSERT(state); - - // update cached value - this->packetType = mode; - return (state); -} - -int16_t SX128x::setHeaderType(uint8_t hdrType, size_t len) { - // check active modem - uint8_t modem = getPacketType(); - if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING))) { - return (RADIOLIB_ERR_WRONG_MODEM); - } - - // update packet parameters - this->headerType = hdrType; - this->payloadLen = len; - return (setPacketParamsLoRa(this->preambleLengthLoRa, this->headerType, this->payloadLen, this->crcLoRa, - this->invertIQEnabled)); -} - -int16_t SX128x::config(uint8_t modem) { - // reset buffer base address - int16_t state = setBufferBaseAddress(); - RADIOLIB_ASSERT(state); - - // set modem - uint8_t data[1]; - data[0] = modem; - state = this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_PACKET_TYPE, data, 1); - RADIOLIB_ASSERT(state); - - // set CAD parameters - data[0] = RADIOLIB_SX128X_CAD_ON_8_SYMB; - state = this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_CAD_PARAMS, data, 1); - RADIOLIB_ASSERT(state); - - // set regulator mode to DC-DC - data[0] = RADIOLIB_SX128X_REGULATOR_DC_DC; - state = this->mod->SPIwriteStream(RADIOLIB_SX128X_CMD_SET_REGULATOR_MODE, data, 1); - RADIOLIB_ASSERT(state); - - return (RADIOLIB_ERR_NONE); -} - -int16_t SX128x::SPIparseStatus(uint8_t in) { - if ((in & 0b00011100) == RADIOLIB_SX128X_STATUS_CMD_TIMEOUT) { - return (RADIOLIB_ERR_SPI_CMD_TIMEOUT); - } else if ((in & 0b00011100) == RADIOLIB_SX128X_STATUS_CMD_ERROR) { - return (RADIOLIB_ERR_SPI_CMD_INVALID); - } else if ((in & 0b00011100) == RADIOLIB_SX128X_STATUS_CMD_FAILED) { - return (RADIOLIB_ERR_SPI_CMD_FAILED); - } else if ((in == 0x00) || (in == 0xFF)) { - return (RADIOLIB_ERR_CHIP_NOT_FOUND); - } - return (RADIOLIB_ERR_NONE); -} - -bool SX128x::findChip(const char* verStr) { - uint8_t i = 0; - bool flagFound = false; - while ((i < 10) && !flagFound) { - // reset the module - reset(true); - - // read the version string - char version[16] = {0}; - this->mod->SPIreadRegisterBurst(RADIOLIB_SX128X_REG_VERSION_STRING, 16, reinterpret_cast(version)); - - // check version register - if (strncmp(verStr, version, 6) == 0) { - RADIOLIB_DEBUG_BASIC_PRINTLN("Found SX128x: RADIOLIB_SX128X_REG_VERSION_STRING:"); - RADIOLIB_DEBUG_BASIC_HEXDUMP(reinterpret_cast(version), 16, RADIOLIB_SX128X_REG_VERSION_STRING); - RADIOLIB_DEBUG_BASIC_PRINTLN(); - flagFound = true; - } else { -#if RADIOLIB_DEBUG_BASIC - RADIOLIB_DEBUG_BASIC_PRINTLN("SX128x not found! (%d of 10 tries) RADIOLIB_SX128X_REG_VERSION_STRING:", - i + 1); - RADIOLIB_DEBUG_BASIC_HEXDUMP(reinterpret_cast(version), 16, RADIOLIB_SX128X_REG_VERSION_STRING); - RADIOLIB_DEBUG_BASIC_PRINTLN("Expected string: %s", verStr); -#endif - this->mod->hal->delay(10); - i++; - } - } - - return (flagFound); -} - -#endif diff --git a/FprimeZephyrReference/Components/MyComponent/SX128x.h b/FprimeZephyrReference/Components/MyComponent/SX128x.h deleted file mode 100644 index 6f69e752..00000000 --- a/FprimeZephyrReference/Components/MyComponent/SX128x.h +++ /dev/null @@ -1,1045 +0,0 @@ -#if !defined(_RADIOLIB_SX128X_H) -#define _RADIOLIB_SX128X_H - -#include "../../TypeDef.h" - -#if !RADIOLIB_EXCLUDE_SX128X - -#include "../../Module.h" -#include "../../protocols/PhysicalLayer/PhysicalLayer.h" - -// SX128X physical layer properties -#define RADIOLIB_SX128X_FREQUENCY_STEP_SIZE 198.3642578 -#define RADIOLIB_SX128X_MAX_PACKET_LENGTH 255 -#define RADIOLIB_SX128X_CRYSTAL_FREQ 52.0f -#define RADIOLIB_SX128X_DIV_EXPONENT 18 - -// SX128X SPI commands -#define RADIOLIB_SX128X_CMD_NOP 0x00 -#define RADIOLIB_SX128X_CMD_GET_STATUS 0xC0 -#define RADIOLIB_SX128X_CMD_WRITE_REGISTER 0x18 -#define RADIOLIB_SX128X_CMD_READ_REGISTER 0x19 -#define RADIOLIB_SX128X_CMD_WRITE_BUFFER 0x1A -#define RADIOLIB_SX128X_CMD_READ_BUFFER 0x1B -#define RADIOLIB_SX128X_CMD_SAVE_CONTEXT 0xD5 -#define RADIOLIB_SX128X_CMD_SET_SLEEP 0x84 -#define RADIOLIB_SX128X_CMD_SET_STANDBY 0x80 -#define RADIOLIB_SX128X_CMD_SET_FS 0xC1 -#define RADIOLIB_SX128X_CMD_SET_TX 0x83 -#define RADIOLIB_SX128X_CMD_SET_RX 0x82 -#define RADIOLIB_SX128X_CMD_SET_RX_DUTY_CYCLE 0x94 -#define RADIOLIB_SX128X_CMD_SET_CAD 0xC5 -#define RADIOLIB_SX128X_CMD_SET_TX_CONTINUOUS_WAVE 0xD1 -#define RADIOLIB_SX128X_CMD_SET_TX_CONTINUOUS_PREAMBLE 0xD2 -#define RADIOLIB_SX128X_CMD_SET_PACKET_TYPE 0x8A -#define RADIOLIB_SX128X_CMD_GET_PACKET_TYPE 0x03 -#define RADIOLIB_SX128X_CMD_SET_RF_FREQUENCY 0x86 -#define RADIOLIB_SX128X_CMD_SET_TX_PARAMS 0x8E -#define RADIOLIB_SX128X_CMD_SET_CAD_PARAMS 0x88 -#define RADIOLIB_SX128X_CMD_SET_BUFFER_BASE_ADDRESS 0x8F -#define RADIOLIB_SX128X_CMD_SET_MODULATION_PARAMS 0x8B -#define RADIOLIB_SX128X_CMD_SET_PACKET_PARAMS 0x8C -#define RADIOLIB_SX128X_CMD_GET_RX_BUFFER_STATUS 0x17 -#define RADIOLIB_SX128X_CMD_GET_PACKET_STATUS 0x1D -#define RADIOLIB_SX128X_CMD_GET_RSSI_INST 0x1F -#define RADIOLIB_SX128X_CMD_SET_DIO_IRQ_PARAMS 0x8D -#define RADIOLIB_SX128X_CMD_GET_IRQ_STATUS 0x15 -#define RADIOLIB_SX128X_CMD_CLEAR_IRQ_STATUS 0x97 -#define RADIOLIB_SX128X_CMD_SET_REGULATOR_MODE 0x96 -#define RADIOLIB_SX128X_CMD_SET_SAVE_CONTEXT 0xD5 -#define RADIOLIB_SX128X_CMD_SET_AUTO_TX 0x98 -#define RADIOLIB_SX128X_CMD_SET_AUTO_FS 0x9E -#define RADIOLIB_SX128X_CMD_SET_PERF_COUNTER_MODE 0x9C -#define RADIOLIB_SX128X_CMD_SET_LONG_PREAMBLE 0x9B -#define RADIOLIB_SX128X_CMD_SET_UART_SPEED 0x9D -#define RADIOLIB_SX128X_CMD_SET_RANGING_ROLE 0xA3 -#define RADIOLIB_SX128X_CMD_SET_ADVANCED_RANGING 0x9A - -// SX128X register map -#define RADIOLIB_SX128X_REG_VERSION_STRING 0x01F0 -#define RADIOLIB_SX128X_REG_GAIN_MODE 0x0891 -#define RADIOLIB_SX128X_REG_MANUAL_GAIN_CONTROL_ENABLE_2 0x0895 -#define RADIOLIB_SX128X_REG_MANUAL_GAIN_SETTING 0x089E -#define RADIOLIB_SX128X_REG_MANUAL_GAIN_CONTROL_ENABLE_1 0x089F -#define RADIOLIB_SX128X_REG_SYNCH_PEAK_ATTENUATION 0x08C2 -#define RADIOLIB_SX128X_REG_LORA_FIXED_PAYLOAD_LENGTH 0x0901 -#define RADIOLIB_SX128X_REG_LORA_HEADER_MODE 0x0903 -#define RADIOLIB_SX128X_REG_MASTER_RANGING_ADDRESS_BYTE_3 0x0912 -#define RADIOLIB_SX128X_REG_MASTER_RANGING_ADDRESS_BYTE_2 0x0913 -#define RADIOLIB_SX128X_REG_MASTER_RANGING_ADDRESS_BYTE_1 0x0914 -#define RADIOLIB_SX128X_REG_MASTER_RANGING_ADDRESS_BYTE_0 0x0915 -#define RADIOLIB_SX128X_REG_SLAVE_RANGING_ADDRESS_BYTE_3 0x0916 -#define RADIOLIB_SX128X_REG_SLAVE_RANGING_ADDRESS_BYTE_2 0x0917 -#define RADIOLIB_SX128X_REG_SLAVE_RANGING_ADDRESS_BYTE_1 0x0918 -#define RADIOLIB_SX128X_REG_SLAVE_RANGING_ADDRESS_BYTE_0 0x0919 -#define RADIOLIB_SX128X_REG_RANGING_FILTER_WINDOW_SIZE 0x091E -#define RADIOLIB_SX128X_REG_RANGING_FILTER_RESET 0x0923 -#define RADIOLIB_SX128X_REG_RANGING_TYPE 0x0924 -#define RADIOLIB_SX128X_REG_LORA_SF_CONFIG 0x0925 -#define RADIOLIB_SX128X_REG_RANGING_ADDRESS_SWITCH 0x0927 -#define RADIOLIB_SX128X_REG_RANGING_CALIBRATION_BYTE_2 0x092B -#define RADIOLIB_SX128X_REG_RANGING_CALIBRATION_MSB 0x092C -#define RADIOLIB_SX128X_REG_RANGING_CALIBRATION_LSB 0x092D -#define RADIOLIB_SX128X_REG_SLAVE_RANGING_ADDRESS_WIDTH 0x0931 -#define RADIOLIB_SX128X_REG_FREQ_ERROR_CORRECTION 0x093C -#define RADIOLIB_SX128X_REG_LORA_SYNC_WORD_MSB 0x0944 -#define RADIOLIB_SX128X_REG_LORA_SYNC_WORD_LSB 0x0945 -#define RADIOLIB_SX128X_REG_LORA_RX_CODING_RATE 0x0950 -#define RADIOLIB_SX128X_REG_RANGING_FILTER_RSSI_OFFSET 0x0953 -#define RADIOLIB_SX128X_REG_FEI_MSB 0x0954 -#define RADIOLIB_SX128X_REG_FEI_MID 0x0955 -#define RADIOLIB_SX128X_REG_FEI_LSB 0x0956 -#define RADIOLIB_SX128X_REG_RANGING_ADDRESS_MSB 0x095F -#define RADIOLIB_SX128X_REG_RANGING_ADDRESS_LSB 0x0960 -#define RADIOLIB_SX128X_REG_RANGING_RESULT_MSB 0x0961 -#define RADIOLIB_SX128X_REG_RANGING_RESULT_MID 0x0962 -#define RADIOLIB_SX128X_REG_RANGING_RESULT_LSB 0x0963 -#define RADIOLIB_SX128X_REG_RANGING_RSSI 0x0964 -#define RADIOLIB_SX128X_REG_RANGING_LORA_CLOCK_ENABLE 0x097F -#define RADIOLIB_SX128X_REG_PACKET_PREAMBLE_SETTINGS 0x09C1 -#define RADIOLIB_SX128X_REG_WHITENING_INITIAL_VALUE 0x09C5 -#define RADIOLIB_SX128X_REG_CRC_POLYNOMIAL_MSB 0x09C6 -#define RADIOLIB_SX128X_REG_CRC_POLYNOMIAL_LSB 0x09C7 -#define RADIOLIB_SX128X_REG_CRC_INITIAL_MSB 0x09C8 -#define RADIOLIB_SX128X_REG_CRC_INITIAL_LSB 0x09C9 -#define RADIOLIB_SX128X_REG_BLE_CRC_INITIAL_MSB 0x09C7 -#define RADIOLIB_SX128X_REG_BLE_CRC_INITIAL_MID (RADIOLIB_SX128X_REG_CRC_INITIAL_MSB) -#define RADIOLIB_SX128X_REG_BLE_CRC_INITIAL_LSB (RADIOLIB_SX128X_REG_CRC_INITIAL_LSB) -#define RADIOLIB_SX128X_REG_SYNCH_ADDRESS_CONTROL 0x09CD -#define RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_4 0x09CE -#define RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_3 0x09CF -#define RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_2 0x09D0 -#define RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_1 0x09D1 -#define RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_0 0x09D2 -#define RADIOLIB_SX128X_REG_SYNC_WORD_2_BYTE_4 0x09D3 -#define RADIOLIB_SX128X_REG_SYNC_WORD_2_BYTE_3 0x09D4 -#define RADIOLIB_SX128X_REG_SYNC_WORD_2_BYTE_2 0x09D5 -#define RADIOLIB_SX128X_REG_SYNC_WORD_2_BYTE_1 0x09D6 -#define RADIOLIB_SX128X_REG_SYNC_WORD_2_BYTE_0 0x09D7 -#define RADIOLIB_SX128X_REG_SYNC_WORD_3_BYTE_4 0x09D8 -#define RADIOLIB_SX128X_REG_SYNC_WORD_3_BYTE_3 0x09D9 -#define RADIOLIB_SX128X_REG_SYNC_WORD_3_BYTE_2 0x09DA -#define RADIOLIB_SX128X_REG_SYNC_WORD_3_BYTE_1 0x09DB -#define RADIOLIB_SX128X_REG_SYNC_WORD_3_BYTE_0 0x09DC -#define RADIOLIB_SX128X_REG_ACCESS_ADDRESS_BYTE_3 (RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_3) -#define RADIOLIB_SX128X_REG_ACCESS_ADDRESS_BYTE_2 (RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_2) -#define RADIOLIB_SX128X_REG_ACCESS_ADDRESS_BYTE_1 (RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_1) -#define RADIOLIB_SX128X_REG_ACCESS_ADDRESS_BYTE_0 (RADIOLIB_SX128X_REG_SYNC_WORD_1_BYTE_0) - -// SX128X SPI command variables -// RADIOLIB_SX128X_CMD_GET_STATUS MSB LSB DESCRIPTION -#define RADIOLIB_SX128X_STATUS_MODE_STDBY_RC 0b01000000 // 7 5 current chip mode: STDBY_RC -#define RADIOLIB_SX128X_STATUS_MODE_STDBY_XOSC 0b01100000 // 7 5 STDBY_XOSC -#define RADIOLIB_SX128X_STATUS_MODE_FS 0b10000000 // 7 5 FS -#define RADIOLIB_SX128X_STATUS_MODE_RX 0b10100000 // 7 5 Rx -#define RADIOLIB_SX128X_STATUS_MODE_TX 0b11000000 // 7 5 Tx -#define RADIOLIB_SX128X_STATUS_CMD_PROCESSED 0b00000100 // 4 2 command status: processing OK -#define RADIOLIB_SX128X_STATUS_DATA_AVAILABLE 0b00001000 // 4 2 data available -#define RADIOLIB_SX128X_STATUS_CMD_TIMEOUT 0b00001100 // 4 2 timeout -#define RADIOLIB_SX128X_STATUS_CMD_ERROR 0b00010000 // 4 2 processing error -#define RADIOLIB_SX128X_STATUS_CMD_FAILED 0b00010100 // 4 2 failed to execute -#define RADIOLIB_SX128X_STATUS_TX_DONE 0b00011000 // 4 2 transmission finished -#define RADIOLIB_SX128X_STATUS_BUSY 0b00000001 // 0 0 chip busy -#define RADIOLIB_SX128X_STATUS_SPI_FAILED 0b11111111 // 7 0 SPI transaction failed - -// RADIOLIB_SX128X_CMD_SET_SLEEP -#define RADIOLIB_SX128X_SLEEP_DATA_BUFFER_FLUSH 0b00000000 // 1 1 data buffer behavior in sleep mode: flush -#define RADIOLIB_SX128X_SLEEP_DATA_BUFFER_RETAIN 0b00000010 // 1 1 retain -#define RADIOLIB_SX128X_SLEEP_DATA_RAM_FLUSH \ - 0b00000000 // 0 0 data RAM (configuration) behavior in sleep mode: flush -#define RADIOLIB_SX128X_SLEEP_DATA_RAM_RETAIN \ - 0b00000001 // 0 0 retain - -// RADIOLIB_SX128X_CMD_SET_STANDBY -#define RADIOLIB_SX128X_STANDBY_RC 0x00 // 7 0 standby mode: 13 MHz RC oscillator -#define RADIOLIB_SX128X_STANDBY_XOSC 0x01 // 7 0 52 MHz crystal oscillator - -// RADIOLIB_SX128X_CMD_SET_TX + RADIOLIB_SX128X_CMD_SET_RX + RADIOLIB_SX128X_CMD_SET_RX_DUTY_CYCLE -#define RADIOLIB_SX128X_PERIOD_BASE_15_625_US 0x00 // 7 0 time period step: 15.625 us -#define RADIOLIB_SX128X_PERIOD_BASE_62_5_US 0x01 // 7 0 62.5 us -#define RADIOLIB_SX128X_PERIOD_BASE_1_MS 0x02 // 7 0 1 ms -#define RADIOLIB_SX128X_PERIOD_BASE_4_MS 0x03 // 7 0 4 ms - -// RADIOLIB_SX128X_CMD_SET_TX -#define RADIOLIB_SX128X_TX_TIMEOUT_NONE 0x0000 // 15 0 Tx timeout duration: no timeout (Tx single mode) - -// RADIOLIB_SX128X_CMD_SET_RX -#define RADIOLIB_SX128X_RX_TIMEOUT_NONE 0x0000 // 15 0 Rx timeout duration: no timeout (Rx single mode) -#define RADIOLIB_SX128X_RX_TIMEOUT_INF 0xFFFF // 15 0 infinite (Rx continuous mode) - -// RADIOLIB_SX128X_CMD_SET_PACKET_TYPE -#define RADIOLIB_SX128X_PACKET_TYPE_GFSK 0x00 // 7 0 packet type: (G)FSK -#define RADIOLIB_SX128X_PACKET_TYPE_LORA 0x01 // 7 0 LoRa -#define RADIOLIB_SX128X_PACKET_TYPE_RANGING 0x02 // 7 0 ranging engine -#define RADIOLIB_SX128X_PACKET_TYPE_FLRC 0x03 // 7 0 FLRC -#define RADIOLIB_SX128X_PACKET_TYPE_BLE 0x04 // 7 0 BLE - -// RADIOLIB_SX128X_CMD_SET_TX_PARAMS -#define RADIOLIB_SX128X_PA_RAMP_02_US 0x00 // 7 0 PA ramp time: 2 us -#define RADIOLIB_SX128X_PA_RAMP_04_US 0x20 // 7 0 4 us -#define RADIOLIB_SX128X_PA_RAMP_06_US 0x40 // 7 0 6 us -#define RADIOLIB_SX128X_PA_RAMP_08_US 0x60 // 7 0 8 us -#define RADIOLIB_SX128X_PA_RAMP_10_US 0x80 // 7 0 10 us -#define RADIOLIB_SX128X_PA_RAMP_12_US 0xA0 // 7 0 12 us -#define RADIOLIB_SX128X_PA_RAMP_16_US 0xC0 // 7 0 16 us -#define RADIOLIB_SX128X_PA_RAMP_20_US 0xE0 // 7 0 20 us - -// RADIOLIB_SX128X_CMD_SET_CAD_PARAMS -#define RADIOLIB_SX128X_CAD_ON_1_SYMB 0x00 // 7 0 number of symbols used for CAD: 1 -#define RADIOLIB_SX128X_CAD_ON_2_SYMB 0x20 // 7 0 2 -#define RADIOLIB_SX128X_CAD_ON_4_SYMB 0x40 // 7 0 4 -#define RADIOLIB_SX128X_CAD_ON_8_SYMB 0x60 // 7 0 8 -#define RADIOLIB_SX128X_CAD_ON_16_SYMB 0x80 // 7 0 16 -#define RADIOLIB_SX128X_CAD_PARAM_DEFAULT RADIOLIB_SX128X_CAD_ON_8_SYMB - -// RADIOLIB_SX128X_CMD_SET_MODULATION_PARAMS -#define RADIOLIB_SX128X_BLE_GFSK_BR_2_000_BW_2_4 \ - 0x04 // 7 0 GFSK/BLE bit rate and bandwidth setting: 2.0 Mbps 2.4 MHz -#define RADIOLIB_SX128X_BLE_GFSK_BR_1_600_BW_2_4 \ - 0x28 // 7 0 1.6 Mbps 2.4 MHz -#define RADIOLIB_SX128X_BLE_GFSK_BR_1_000_BW_2_4 \ - 0x4C // 7 0 1.0 Mbps 2.4 MHz -#define RADIOLIB_SX128X_BLE_GFSK_BR_1_000_BW_1_2 \ - 0x45 // 7 0 1.0 Mbps 1.2 MHz -#define RADIOLIB_SX128X_BLE_GFSK_BR_0_800_BW_2_4 \ - 0x70 // 7 0 0.8 Mbps 2.4 MHz -#define RADIOLIB_SX128X_BLE_GFSK_BR_0_800_BW_1_2 \ - 0x69 // 7 0 0.8 Mbps 1.2 MHz -#define RADIOLIB_SX128X_BLE_GFSK_BR_0_500_BW_1_2 \ - 0x8D // 7 0 0.5 Mbps 1.2 MHz -#define RADIOLIB_SX128X_BLE_GFSK_BR_0_500_BW_0_6 \ - 0x86 // 7 0 0.5 Mbps 0.6 MHz -#define RADIOLIB_SX128X_BLE_GFSK_BR_0_400_BW_1_2 \ - 0xB1 // 7 0 0.4 Mbps 1.2 MHz -#define RADIOLIB_SX128X_BLE_GFSK_BR_0_400_BW_0_6 \ - 0xAA // 7 0 0.4 Mbps 0.6 MHz -#define RADIOLIB_SX128X_BLE_GFSK_BR_0_250_BW_0_6 \ - 0xCE // 7 0 0.25 Mbps 0.6 MHz -#define RADIOLIB_SX128X_BLE_GFSK_BR_0_250_BW_0_3 \ - 0xC7 // 7 0 0.25 Mbps 0.3 MHz -#define RADIOLIB_SX128X_BLE_GFSK_BR_0_125_BW_0_3 \ - 0xEF // 7 0 0.125 Mbps 0.3 MHz -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_0_35 0x00 // 7 0 GFSK/BLE modulation index: 0.35 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_0_50 0x01 // 7 0 0.50 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_0_75 0x02 // 7 0 0.75 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_1_00 0x03 // 7 0 1.00 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_1_25 0x04 // 7 0 1.25 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_1_50 0x05 // 7 0 1.50 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_1_75 0x06 // 7 0 1.75 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_2_00 0x07 // 7 0 2.00 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_2_25 0x08 // 7 0 2.25 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_2_50 0x09 // 7 0 2.50 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_2_75 0x0A // 7 0 2.75 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_3_00 0x0B // 7 0 3.00 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_3_25 0x0C // 7 0 3.25 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_3_50 0x0D // 7 0 3.50 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_3_75 0x0E // 7 0 3.75 -#define RADIOLIB_SX128X_BLE_GFSK_MOD_IND_4_00 0x0F // 7 0 4.00 -#define RADIOLIB_SX128X_BLE_GFSK_BT_OFF 0x00 // 7 0 GFSK Gaussian filter BT product: filter disabled -#define RADIOLIB_SX128X_BLE_GFSK_BT_1_0 0x10 // 7 0 1.0 -#define RADIOLIB_SX128X_BLE_GFSK_BT_0_5 0x20 // 7 0 0.5 -#define RADIOLIB_SX128X_FLRC_BR_1_300_BW_1_2 0x45 // 7 0 FLRC bit rate and bandwidth setting: 1.3 Mbps 1.2 MHz -#define RADIOLIB_SX128X_FLRC_BR_1_000_BW_1_2 0x69 // 7 0 1.04 Mbps 1.2 MHz -#define RADIOLIB_SX128X_FLRC_BR_0_650_BW_0_6 0x86 // 7 0 0.65 Mbps 0.6 MHz -#define RADIOLIB_SX128X_FLRC_BR_0_520_BW_0_6 0xAA // 7 0 0.52 Mbps 0.6 MHz -#define RADIOLIB_SX128X_FLRC_BR_0_325_BW_0_3 0xC7 // 7 0 0.325 Mbps 0.3 MHz -#define RADIOLIB_SX128X_FLRC_BR_0_260_BW_0_3 0xEB // 7 0 0.260 Mbps 0.3 MHz -#define RADIOLIB_SX128X_FLRC_CR_1_2 0x00 // 7 0 FLRC coding rate: 1/2 -#define RADIOLIB_SX128X_FLRC_CR_3_4 0x02 // 7 0 3/4 -#define RADIOLIB_SX128X_FLRC_CR_1_0 0x04 // 7 0 1/1 -#define RADIOLIB_SX128X_FLRC_BT_OFF 0x00 // 7 0 FLRC Gaussian filter BT product: filter disabled -#define RADIOLIB_SX128X_FLRC_BT_1_0 0x10 // 7 0 1.0 -#define RADIOLIB_SX128X_FLRC_BT_0_5 0x20 // 7 0 0.5 -#define RADIOLIB_SX128X_LORA_SF_5 0x50 // 7 0 LoRa spreading factor: 5 -#define RADIOLIB_SX128X_LORA_SF_6 0x60 // 7 0 6 -#define RADIOLIB_SX128X_LORA_SF_7 0x70 // 7 0 7 -#define RADIOLIB_SX128X_LORA_SF_8 0x80 // 7 0 8 -#define RADIOLIB_SX128X_LORA_SF_9 0x90 // 7 0 9 -#define RADIOLIB_SX128X_LORA_SF_10 0xA0 // 7 0 10 -#define RADIOLIB_SX128X_LORA_SF_11 0xB0 // 7 0 11 -#define RADIOLIB_SX128X_LORA_SF_12 0xC0 // 7 0 12 -#define RADIOLIB_SX128X_LORA_BW_1625_00 0x0A // 7 0 LoRa bandwidth: 1625.0 kHz -#define RADIOLIB_SX128X_LORA_BW_812_50 0x18 // 7 0 812.5 kHz -#define RADIOLIB_SX128X_LORA_BW_406_25 0x26 // 7 0 406.25 kHz -#define RADIOLIB_SX128X_LORA_BW_203_125 0x34 // 7 0 203.125 kHz -#define RADIOLIB_SX128X_LORA_CR_4_5 0x01 // 7 0 LoRa coding rate: 4/5 -#define RADIOLIB_SX128X_LORA_CR_4_6 0x02 // 7 0 4/6 -#define RADIOLIB_SX128X_LORA_CR_4_7 0x03 // 7 0 4/7 -#define RADIOLIB_SX128X_LORA_CR_4_8 0x04 // 7 0 4/8 -#define RADIOLIB_SX128X_LORA_CR_4_5_LI 0x05 // 7 0 4/5, long interleaving -#define RADIOLIB_SX128X_LORA_CR_4_6_LI 0x06 // 7 0 4/6, long interleaving -#define RADIOLIB_SX128X_LORA_CR_4_8_LI 0x07 // 7 0 4/8, long interleaving - -// RADIOLIB_SX128X_CMD_SET_PACKET_PARAMS -#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_OFF 0x00 // 7 0 GFSK/FLRC sync word used: none -#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1 0x10 // 7 0 sync word 1 -#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_2 0x20 // 7 0 sync word 2 -#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1_2 0x30 // 7 0 sync words 1 and 2 -#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_3 0x40 // 7 0 sync word 3 -#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1_3 0x50 // 7 0 sync words 1 and 3 -#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_2_3 0x60 // 7 0 sync words 2 and 3 -#define RADIOLIB_SX128X_GFSK_FLRC_SYNC_WORD_1_2_3 0x70 // 7 0 sync words 1, 2 and 3 -#define RADIOLIB_SX128X_GFSK_FLRC_PACKET_FIXED 0x00 // 7 0 GFSK/FLRC packet length mode: fixed -#define RADIOLIB_SX128X_GFSK_FLRC_PACKET_VARIABLE 0x20 // 7 0 variable -#define RADIOLIB_SX128X_GFSK_FLRC_CRC_OFF 0x00 // 7 0 GFSK/FLRC packet CRC: none -#define RADIOLIB_SX128X_GFSK_FLRC_CRC_1_BYTE 0x10 // 7 0 1 byte -#define RADIOLIB_SX128X_GFSK_FLRC_CRC_2_BYTE 0x20 // 7 0 2 bytes -#define RADIOLIB_SX128X_GFSK_FLRC_CRC_3_BYTE 0x30 // 7 0 3 bytes (FLRC only) -#define RADIOLIB_SX128X_GFSK_BLE_WHITENING_ON 0x00 // 7 0 GFSK/BLE whitening: enabled -#define RADIOLIB_SX128X_GFSK_BLE_WHITENING_OFF 0x08 // 7 0 disabled -#define RADIOLIB_SX128X_BLE_PAYLOAD_LENGTH_MAX_31 0x00 // 7 0 BLE maximum payload length: 31 bytes -#define RADIOLIB_SX128X_BLE_PAYLOAD_LENGTH_MAX_37 0x20 // 7 0 37 bytes -#define RADIOLIB_SX128X_BLE_PAYLOAD_LENGTH_TEST 0x40 // 7 0 63 bytes (test mode) -#define RADIOLIB_SX128X_BLE_PAYLOAD_LENGTH_MAX_255 \ - 0x80 // 7 0 255 bytes (Bluetooth 4.2 and above) -#define RADIOLIB_SX128X_BLE_CRC_OFF 0x00 // 7 0 BLE packet CRC: none -#define RADIOLIB_SX128X_BLE_CRC_3_BYTE 0x10 // 7 0 3 byte -#define RADIOLIB_SX128X_BLE_PRBS_9 0x00 // 7 0 BLE test payload contents: PRNG sequence using x^9 + x^5 + x -#define RADIOLIB_SX128X_BLE_EYELONG 0x04 // 7 0 repeated 0xF0 -#define RADIOLIB_SX128X_BLE_EYESHORT 0x08 // 7 0 repeated 0xAA -#define RADIOLIB_SX128X_BLE_PRBS_15 \ - 0x0C // 7 0 PRNG sequence using x^15 + x^14 + x^13 + x^12 + x^2 + x + 1 -#define RADIOLIB_SX128X_BLE_ALL_1 0x10 // 7 0 repeated 0xFF -#define RADIOLIB_SX128X_BLE_ALL_0 0x14 // 7 0 repeated 0x00 -#define RADIOLIB_SX128X_BLE_EYELONG_INV 0x18 // 7 0 repeated 0x0F -#define RADIOLIB_SX128X_BLE_EYESHORT_INV 0x1C // 7 0 repeated 0x55 -#define RADIOLIB_SX128X_FLRC_SYNC_WORD_OFF 0x00 // 7 0 FLRC sync word: disabled -#define RADIOLIB_SX128X_FLRC_SYNC_WORD_ON 0x04 // 7 0 enabled -#define RADIOLIB_SX128X_LORA_HEADER_EXPLICIT 0x00 // 7 0 LoRa header mode: explicit -#define RADIOLIB_SX128X_LORA_HEADER_IMPLICIT 0x80 // 7 0 implicit -#define RADIOLIB_SX128X_LORA_CRC_OFF 0x00 // 7 0 LoRa packet CRC: disabled -#define RADIOLIB_SX128X_LORA_CRC_ON 0x20 // 7 0 enabled -#define RADIOLIB_SX128X_LORA_IQ_STANDARD 0x40 // 7 0 LoRa IQ: standard -#define RADIOLIB_SX128X_LORA_IQ_INVERTED 0x00 // 7 0 inverted - -// RADIOLIB_SX128X_CMD_GET_PACKET_STATUS -#define RADIOLIB_SX128X_PACKET_STATUS_SYNC_ERROR 0b01000000 // 6 6 packet status errors byte: sync word error -#define RADIOLIB_SX128X_PACKET_STATUS_LENGTH_ERROR \ - 0b00100000 // 5 5 packet length error -#define RADIOLIB_SX128X_PACKET_STATUS_CRC_ERROR 0b00010000 // 4 4 CRC error -#define RADIOLIB_SX128X_PACKET_STATUS_ABORT_ERROR \ - 0b00001000 // 3 3 packet reception aborted -#define RADIOLIB_SX128X_PACKET_STATUS_HEADER_RECEIVED \ - 0b00000100 // 2 2 header received -#define RADIOLIB_SX128X_PACKET_STATUS_PACKET_RECEIVED \ - 0b00000010 // 1 1 packet received -#define RADIOLIB_SX128X_PACKET_STATUS_PACKET_CTRL_BUSY \ - 0b00000001 // 0 0 packet controller is busy -#define RADIOLIB_SX128X_PACKET_STATUS_RX_PID \ - 0b11000000 // 7 6 packet status status byte: PID field of the received packet -#define RADIOLIB_SX128X_PACKET_STATUS_NO_ACK \ - 0b00100000 // 5 5 NO_ACK field of the received packet -#define RADIOLIB_SX128X_PACKET_STATUS_RX_PID_ERROR 0b00010000 // 4 4 PID field error -#define RADIOLIB_SX128X_PACKET_STATUS_PACKET_SENT 0b00000001 // 0 0 packet sent -#define RADIOLIB_SX128X_PACKET_STATUS_SYNC_DET_ERROR \ - 0b00000000 // 2 0 packet status sync byte: sync word detection error -#define RADIOLIB_SX128X_PACKET_STATUS_SYNC_DET_1 0b00000001 // 2 0 detected sync word 1 -#define RADIOLIB_SX128X_PACKET_STATUS_SYNC_DET_2 0b00000010 // 2 0 detected sync word 2 -#define RADIOLIB_SX128X_PACKET_STATUS_SYNC_DET_3 0b00000100 // 2 0 detected sync word 3 - -// RADIOLIB_SX128X_CMD_SET_DIO_IRQ_PARAMS -#define RADIOLIB_SX128X_IRQ_PREAMBLE_DETECTED 0x8000 // 15 15 interrupt source: preamble detected -#define RADIOLIB_SX128X_IRQ_ADVANCED_RANGING_DONE 0x8000 // 15 15 advanced ranging done -#define RADIOLIB_SX128X_IRQ_RX_TX_TIMEOUT 0x4000 // 14 14 Rx or Tx timeout -#define RADIOLIB_SX128X_IRQ_CAD_DETECTED 0x2000 // 13 13 channel activity detected -#define RADIOLIB_SX128X_IRQ_CAD_DONE 0x1000 // 12 12 CAD finished -#define RADIOLIB_SX128X_IRQ_RANGING_SLAVE_REQ_VALID \ - 0x0800 // 11 11 ranging request valid (slave) -#define RADIOLIB_SX128X_IRQ_RANGING_MASTER_TIMEOUT 0x0400 // 10 10 ranging timeout (master) -#define RADIOLIB_SX128X_IRQ_RANGING_MASTER_RES_VALID \ - 0x0200 // 9 9 ranging result valid (master) -#define RADIOLIB_SX128X_IRQ_RANGING_SLAVE_REQ_DISCARD \ - 0x0100 // 8 8 ranging result valid (master) -#define RADIOLIB_SX128X_IRQ_RANGING_SLAVE_RESP_DONE \ - 0x0080 // 7 7 ranging response complete (slave) -#define RADIOLIB_SX128X_IRQ_CRC_ERROR 0x0040 // 6 6 CRC error -#define RADIOLIB_SX128X_IRQ_HEADER_ERROR 0x0020 // 5 5 header error -#define RADIOLIB_SX128X_IRQ_HEADER_VALID 0x0010 // 4 4 header valid -#define RADIOLIB_SX128X_IRQ_SYNC_WORD_ERROR 0x0008 // 3 3 sync word error -#define RADIOLIB_SX128X_IRQ_SYNC_WORD_VALID 0x0004 // 2 2 sync word valid -#define RADIOLIB_SX128X_IRQ_RX_DONE 0x0002 // 1 1 Rx done -#define RADIOLIB_SX128X_IRQ_TX_DONE 0x0001 // 0 0 Tx done -#define RADIOLIB_SX128X_IRQ_NONE 0x0000 // 15 0 none -#define RADIOLIB_SX128X_IRQ_ALL 0xFFFF // 15 0 all - -// RADIOLIB_SX128X_CMD_SET_REGULATOR_MODE -#define RADIOLIB_SX128X_REGULATOR_LDO 0x00 // 7 0 set regulator mode: LDO (default) -#define RADIOLIB_SX128X_REGULATOR_DC_DC 0x01 // 7 0 DC-DC - -// RADIOLIB_SX128X_CMD_SET_RANGING_ROLE -#define RADIOLIB_SX128X_RANGING_ROLE_MASTER 0x01 // 7 0 ranging role: master -#define RADIOLIB_SX128X_RANGING_ROLE_SLAVE 0x00 // 7 0 slave - -// RADIOLIB_SX128X_REG_LORA_SYNC_WORD_1 - RADIOLIB_SX128X_REG_LORA_SYNC_WORD_2 -#define RADIOLIB_SX128X_SYNC_WORD_PRIVATE 0x12 - -/*! - \class SX128x - \brief Base class for %SX128x series. All derived classes for %SX128x (e.g. SX1280 or SX1281) inherit from this base - class. This class should not be instantiated directly from Arduino sketch, only from its derived classes. -*/ -class SX128x : public PhysicalLayer { - public: - // introduce PhysicalLayer overloads - using PhysicalLayer::readData; - using PhysicalLayer::receive; - using PhysicalLayer::startReceive; - using PhysicalLayer::startTransmit; - using PhysicalLayer::transmit; - - /*! - \brief Default constructor. - \param mod Instance of Module that will be used to communicate with the radio. - */ - explicit SX128x(Module* mod); - - // basic methods - - /*! - \brief Initialization method for LoRa modem. - \param freq Carrier frequency in MHz. Defaults to 2400.0 MHz. - \param bw LoRa bandwidth in kHz. Defaults to 812.5 kHz. - \param sf LoRa spreading factor. Defaults to 9. - \param cr LoRa coding rate denominator. Defaults to 7 (coding rate 4/7). Allowed values range from 4 to 8. Note - that a value of 4 means no coding, is undocumented and not recommended without your own FEC. - \param syncWord 2-byte LoRa sync word. Defaults to RADIOLIB_SX128X_SYNC_WORD_PRIVATE (0x12). - \param pwr Output power in dBm. Defaults to 10 dBm. - \param preambleLength LoRa preamble length in symbols. Defaults to 12 symbols. - \returns \ref status_codes - */ - int16_t begin(float freq = 2400.0, - float bw = 812.5, - uint8_t sf = 9, - uint8_t cr = 7, - uint8_t syncWord = RADIOLIB_SX128X_SYNC_WORD_PRIVATE, - int8_t pwr = 10, - uint16_t preambleLength = 12); - - /*! - \brief Initialization method for GFSK modem. - \param freq Carrier frequency in MHz. Defaults to 2400.0 MHz. - \param br FSK bit rate in kbps. Defaults to 800 kbps. - \param freqDev Frequency deviation from carrier frequency in kHz. Defaults to 400.0 kHz. - \param pwr Output power in dBm. Defaults to 10 dBm. - \param preambleLength FSK preamble length in bits. Defaults to 16 bits. - \returns \ref status_codes - */ - int16_t beginGFSK(float freq = 2400.0, - uint16_t br = 800, - float freqDev = 400.0, - int8_t pwr = 10, - uint16_t preambleLength = 16); - - /*! - \brief Initialization method for BLE modem. - \param freq Carrier frequency in MHz. Defaults to 2400.0 MHz. - \param br BLE bit rate in kbps. Defaults to 800 kbps. - \param freqDev Frequency deviation from carrier frequency in kHz. Defaults to 400.0 kHz. - \param pwr Output power in dBm. Defaults to 10 dBm. - \param dataShaping Time-bandwidth product of the Gaussian filter to be used for shaping. Defaults to 0.5. - \returns \ref status_codes - */ - int16_t beginBLE(float freq = 2400.0, - uint16_t br = 800, - float freqDev = 400.0, - int8_t pwr = 10, - uint8_t dataShaping = RADIOLIB_SHAPING_0_5); - - /*! - \brief Initialization method for FLRC modem. - \param freq Carrier frequency in MHz. Defaults to 2400.0 MHz. - \param br FLRC bit rate in kbps. Defaults to 650 kbps. - \param cr FLRC coding rate. Defaults to 3 (coding rate 3/4). - \param pwr Output power in dBm. Defaults to 10 dBm. - \param preambleLength FLRC preamble length in bits. Defaults to 16 bits. - \param dataShaping Time-bandwidth product of the Gaussian filter to be used for shaping. Defaults to 0.5. - \returns \ref status_codes - */ - int16_t beginFLRC(float freq = 2400.0, - uint16_t br = 650, - uint8_t cr = 3, - int8_t pwr = 10, - uint16_t preambleLength = 16, - uint8_t dataShaping = RADIOLIB_SHAPING_0_5); - - /*! - \brief Reset method. Will reset the chip to the default state using RST pin. - \param verify Whether correct module startup should be verified. When set to true, RadioLib will attempt to verify - the module has started correctly by repeatedly issuing setStandby command. Enabled by default. - \returns \ref status_codes - */ - int16_t reset(bool verify = true); - - /*! - \brief Blocking binary transmit method. - Overloads for string-based transmissions are implemented in PhysicalLayer. - \param data Binary data to be sent. - \param len Number of bytes to send. - \param addr Address to send the data to. Unsupported, compatibility only. - \returns \ref status_codes - */ - int16_t transmit(const uint8_t* data, size_t len, uint8_t addr = 0) override; - - /*! - \brief Blocking binary receive method. - Overloads for string-based transmissions are implemented in PhysicalLayer. - \param data Pointer to array to save the received binary data. - \param len Number of bytes that will be received. Must be known in advance for binary transmissions. - \param timeout Reception timeout in milliseconds. If set to 0, - timeout period will be calculated automatically based on the radio configuration. - \returns \ref status_codes - */ - int16_t receive(uint8_t* data, size_t len, RadioLibTime_t timeout = 0) override; - - /*! - \brief Starts direct mode transmission. - \param frf Raw RF frequency value. Defaults to 0, required for quick frequency shifts in RTTY. - \returns \ref status_codes - */ - int16_t transmitDirect(uint32_t frf = 0) override; - - /*! - \brief Starts direct mode reception. Only implemented for PhysicalLayer compatibility, - as %SX128x series does not support direct mode reception. Will always return RADIOLIB_ERR_UNKNOWN. - \returns \ref status_codes - */ - int16_t receiveDirect() override; - - /*! - \brief Performs scan for LoRa transmission in the current channel. Detects both preamble and payload. - \returns \ref status_codes - */ - int16_t scanChannel() override; - - /*! - \brief Performs scan for LoRa transmission in the current channel. Detects both preamble and payload. - \param config CAD configuration structure. - \returns \ref status_codes - */ - int16_t scanChannel(const ChannelScanConfig_t& config) override; - - /*! - \brief Sets the module to sleep mode. To wake the device up, call standby(). - Overload for PhysicalLayer compatibility. - \returns \ref status_codes - */ - int16_t sleep() override; - - /*! - \brief Sets the module to sleep mode. To wake the device up, call standby(). - \param retainConfig Set to true to retain configuration and data buffer or to false - to discard current configuration and data buffer. Defaults to true. - \returns \ref status_codes - */ - int16_t sleep(bool retainConfig); - - /*! - \brief Sets the module to standby mode (overload for PhysicalLayer compatibility, uses 13 MHz RC oscillator). - \returns \ref status_codes - */ - int16_t standby() override; - - /*! - \brief Sets the module to standby mode. - \param mode Oscillator to be used in standby mode. Can be set to RADIOLIB_SX128X_STANDBY_RC - (13 MHz RC oscillator) or RADIOLIB_SX128X_STANDBY_XOSC (52 MHz external crystal oscillator). - \param wakeup Whether to force the module to wake up. Setting to true will immediately attempt to wake up the - module. - \returns \ref status_codes - */ - int16_t standby(uint8_t mode, bool wakeup = false); - - // interrupt methods - - /*! - \brief Sets interrupt service routine to call when DIO1 activates. - \param func ISR to call. - */ - void setDio1Action(void (*func)(void)); - - /*! - \brief Clears interrupt service routine to call when DIO1 activates. - */ - void clearDio1Action(); - - /*! - \brief Sets interrupt service routine to call when a packet is received. - \param func ISR to call. - */ - void setPacketReceivedAction(void (*func)(void)) override; - - /*! - \brief Clears interrupt service routine to call when a packet is received. - */ - void clearPacketReceivedAction() override; - - /*! - \brief Sets interrupt service routine to call when a packet is sent. - \param func ISR to call. - */ - void setPacketSentAction(void (*func)(void)) override; - - /*! - \brief Clears interrupt service routine to call when a packet is sent. - */ - void clearPacketSentAction() override; - - /*! - \brief Clean up after transmission is done. - \returns \ref status_codes - */ - int16_t finishTransmit() override; - - /*! - \brief Interrupt-driven receive method with default parameters. - Implemented for compatibility with PhysicalLayer. - - \returns \ref status_codes - */ - int16_t startReceive() override; - - /*! - \brief Reads the current IRQ status. - \returns IRQ status bits - */ - uint16_t getIrqStatus(); - - /*! - \brief Reads data received after calling startReceive method. When the packet length is not known in advance, - getPacketLength method must be called BEFORE calling readData! - \param data Pointer to array to save the received binary data. - \param len Number of bytes that will be read. When set to 0, the packet length will be retrieved automatically. - When more bytes than received are requested, only the number of bytes requested will be returned. - \returns \ref status_codes - */ - int16_t readData(uint8_t* data, size_t len) override; - - /*! - \brief Clean up after reception is done. - \returns \ref status_codes - */ - int16_t finishReceive() override; - - /*! - \brief Read currently active IRQ flags. - \returns IRQ flags. - */ - uint32_t getIrqFlags() override; - - /*! - \brief Set interrupt on DIO1 to be sent on a specific IRQ bit (e.g. RxTimeout, CadDone). - \param irq Module-specific IRQ flags. - \returns \ref status_codes - */ - int16_t setIrqFlags(uint32_t irq) override; - - /*! - \brief Clear interrupt on a specific IRQ bit (e.g. RxTimeout, CadDone). - \param irq Module-specific IRQ flags. - \returns \ref status_codes - */ - int16_t clearIrqFlags(uint32_t irq) override; - - /*! - \brief Interrupt-driven channel activity detection method. DIO1 will be activated - when LoRa preamble is detected, or upon timeout. - \returns \ref status_codes - */ - int16_t startChannelScan() override; - - /*! - \brief Interrupt-driven channel activity detection method. DIO1 will be activated - when LoRa preamble is detected, or upon timeout. - \param config CAD configuration structure. - \returns \ref status_codes - */ - int16_t startChannelScan(const ChannelScanConfig_t& config) override; - - /*! - \brief Read the channel scan result - \returns \ref status_codes - */ - int16_t getChannelScanResult() override; - - // configuration methods - - /*! - \brief Sets carrier frequency. Allowed values are in range from 2400.0 to 2500.0 MHz. - \param freq Carrier frequency to be set in MHz. - \returns \ref status_codes - */ - int16_t setFrequency(float freq) override; - - /*! - \brief Sets LoRa bandwidth. Allowed values are 203.125, 406.25, 812.5 and 1625.0 kHz. - \param bw LoRa bandwidth to be set in kHz. - \returns \ref status_codes - */ - int16_t setBandwidth(float bw); - - /*! - \brief Sets LoRa spreading factor. Allowed values range from 5 to 12. - \param sf LoRa spreading factor to be set. - \returns \ref status_codes - */ - int16_t setSpreadingFactor(uint8_t sf); - - /*! - \brief Sets LoRa coding rate denominator. Allowed values range from 4 to 8. Note that a value of 4 - means no coding, is undocumented and not recommended without your own FEC. - \param cr LoRa coding rate denominator to be set. - \param longInterleaving Whether to enable long interleaving mode. Not available for coding rate 4/7, - defaults to false. - \returns \ref status_codes - */ - int16_t setCodingRate(uint8_t cr, bool longInterleaving = false); - - /*! - \brief Sets output power. Allowed values are in range from -18 to 13 dBm. - \param pwr Output power to be set in dBm. - \returns \ref status_codes - */ - int16_t setOutputPower(int8_t pwr) override; - - /*! - \brief Check if output power is configurable. - \param pwr Output power in dBm. - \param clipped Clipped output power value to what is possible within the module's range. - \returns \ref status_codes - */ - int16_t checkOutputPower(int8_t pwr, int8_t* clipped) override; - - /*! - \brief Set modem for the radio to use. Will perform full reset and reconfigure the radio - using its default parameters. - \param modem Modem type to set - FSK, LoRa or LR-FHSS. - \returns \ref status_codes - */ - int16_t setModem(ModemType_t modem) override; - - /*! - \brief Get modem currently in use by the radio. - \param modem Pointer to a variable to save the retrieved configuration into. - \returns \ref status_codes - */ - int16_t getModem(ModemType_t* modem) override; - - /*! - \brief Sets preamble length for currently active modem. Allowed values range from 1 to 65535. - \param preambleLength Preamble length to be set in symbols (LoRa) or bits (FSK/BLE/FLRC). - \returns \ref status_codes - */ - int16_t setPreambleLength(size_t preambleLength) override; - - /*! - \brief Set data rate. - \param dr Data rate struct. Interpretation depends on currently active modem (FSK or LoRa). - \returns \ref status_codes - */ - int16_t setDataRate(DataRate_t dr, ModemType_t modem = RADIOLIB_MODEM_NONE) override; - - /*! - \brief Sets FSK or FLRC bit rate. Allowed values are 125, 250, 400, 500, 800, 1000, - 1600 and 2000 kbps (for FSK modem) or 260, 325, 520, 650, 1000 and 1300 (for FLRC modem). - \param br FSK/FLRC bit rate to be set in kbps. - \returns \ref status_codes - */ - int16_t setBitRate(float br) override; - - /*! - \brief Sets FSK frequency deviation. Allowed values range from 0.0 to 3200.0 kHz. - \param freqDev FSK frequency deviation to be set in kHz. - \returns \ref status_codes - */ - int16_t setFrequencyDeviation(float freqDev) override; - - /*! - \brief Sets time-bandwidth product of Gaussian filter applied for shaping. - Allowed values are RADIOLIB_SHAPING_0_5 or RADIOLIB_SHAPING_1_0. Set to RADIOLIB_SHAPING_NONE to disable data - shaping. - \param sh Time-bandwidth product of Gaussian filter to be set. - \returns \ref status_codes - */ - int16_t setDataShaping(uint8_t sh) override; - - /*! - \brief Sets FSK/FLRC sync word in the form of array of up to 5 bytes (FSK). For FLRC modem, - the sync word must be exactly 4 bytes long - \param syncWord Sync word to be set. - \param len Sync word length in bytes. - \returns \ref status_codes - */ - int16_t setSyncWord(const uint8_t* syncWord, uint8_t len); - - /*! - \brief Sets LoRa sync word. - \param syncWord LoRa sync word to be set. - \param controlBits Undocumented control bits, required for compatibility purposes. - \returns \ref status_codes - */ - int16_t setSyncWord(uint8_t syncWord, uint8_t controlBits = 0x44); - - /*! - \brief Sets CRC configuration. - \param len CRC length in bytes, Allowed values are 1, 2 or 3, set to 0 to disable CRC. - \param initial Initial CRC value. Defaults to 0x1D0F (CCIT CRC), not available for LoRa modem. - \param polynomial Polynomial for CRC calculation. Defaults to 0x1021 (CCIT CRC), not available for LoRa or BLE - modem. - \returns \ref status_codes - */ - int16_t setCRC(uint8_t len, uint32_t initial = 0x1D0F, uint16_t polynomial = 0x1021); - - /*! - \brief Sets whitening parameters, not available for LoRa or FLRC modem. - \param enabled Set to true to enable whitening. - \returns \ref status_codes - */ - int16_t setWhitening(bool enabled); - - /*! - \brief Sets BLE access address. - \param addr BLE access address. - \returns \ref status_codes - */ - int16_t setAccessAddress(uint32_t addr); - - /*! - \brief Enables or disables receiver high sensitivity mode. - \param enable True to enable and false to disable. - \returns \ref status_codes - */ - int16_t setHighSensitivityMode(bool enable); - - /*! - \brief Enables or disables receiver manual gain control. - \param gain Use 0 for automatic gain, 1 for minimum gain and up to 13 for maximum gain. - \returns \ref status_codes - */ - int16_t setGainControl(uint8_t gain = 0); - - /*! - \brief Gets RSSI (Recorded Signal Strength Indicator) of the last received packet. - \returns RSSI of the last received packet in dBm. - */ - float getRSSI() override; - - /*! - \brief Gets RSSI (Recorded Signal Strength Indicator). - \param packet Whether to read last packet RSSI, or the current value. - \returns RSSI value in dBm. - */ - float getRSSI(bool packet); - - /*! - \brief Gets SNR (Signal to Noise Ratio) of the last received packet. Only available for LoRa or ranging modem. - \returns SNR of the last received packet in dB. - */ - float getSNR() override; - - /*! - \brief Gets frequency error of the latest received packet. - \returns Frequency error in Hz. - */ - float getFrequencyError(); - - /*! - \brief Query modem for the packet length of received payload. - \param update Update received packet length. Will return cached value when set to false. - \returns Length of last received packet in bytes. - */ - size_t getPacketLength(bool update = true) override; - - /*! - \brief Query modem for the packet length of received payload and Rx buffer offset. - \param update Update received packet length. Will return cached value when set to false. - \param offset Pointer to variable to store the Rx buffer offset. - \returns Length of last received packet in bytes. - */ - size_t getPacketLength(bool update, uint8_t* offset); - - /*! - \brief Get LoRa header information from last received packet. Only valid in explicit header mode. - \param cr Pointer to variable to store the coding rate. - \param hasCRC Pointer to variable to store the CRC status. - \returns \ref status_codes - */ - int16_t getLoRaRxHeaderInfo(uint8_t* cr, bool* hasCRC); - - /*! - \brief Set modem in fixed packet length mode. Available in GFSK and FLRC modes only. - \param len Packet length. - \returns \ref status_codes - */ - int16_t fixedPacketLengthMode(uint8_t len = RADIOLIB_SX128X_MAX_PACKET_LENGTH); - - /*! - \brief Set modem in variable packet length mode. Available in GFSK and FLRC modes only. - \param maxLen Maximum packet length. - \returns \ref status_codes - */ - int16_t variablePacketLengthMode(uint8_t maxLen = RADIOLIB_SX128X_MAX_PACKET_LENGTH); - - /*! - \brief Calculate the expected time-on-air for a given modem, data rate, packet configuration and payload size. - \param modem Modem type. - \param dr Data rate. - \param pc Packet config. - \param len Payload length in bytes. - \returns Expected time-on-air in microseconds. - */ - RadioLibTime_t calculateTimeOnAir(ModemType_t modem, DataRate_t dr, PacketConfig_t pc, size_t len) override; - - /*! - \brief Get expected time-on-air for a given size of payload. - \param len Payload length in bytes. - \returns Expected time-on-air in microseconds. - */ - RadioLibTime_t getTimeOnAir(size_t len) override; - - /*! - \brief Set implicit header mode for future reception/transmission. - \returns \ref status_codes - */ - int16_t implicitHeader(size_t len); - - /*! - \brief Set explicit header mode for future reception/transmission. - \param len Payload length in bytes. - \returns \ref status_codes - */ - int16_t explicitHeader(); - - /*! - \brief Sets transmission encoding. Serves only as alias for PhysicalLayer compatibility. - \param encoding Encoding to be used. Set to 0 for NRZ, and 2 for whitening. - \returns \ref status_codes - */ - int16_t setEncoding(uint8_t encoding) override; - - /*! \copydoc Module::setRfSwitchPins */ - void setRfSwitchPins(uint32_t rxEn, uint32_t txEn); - - /*! \copydoc Module::setRfSwitchTable */ - void setRfSwitchTable(const uint32_t (&pins)[Module::RFSWITCH_MAX_PINS], const Module::RfSwitchMode_t table[]); - - /*! - \brief Dummy random method, to ensure PhysicalLayer compatibility. - \returns Always returns 0. - */ - uint8_t randomByte() override; - - /*! - \brief Enable/disable inversion of the I and Q signals - \param enable IQ inversion enabled (true) or disabled (false); - \returns \ref status_codes - */ - int16_t invertIQ(bool enable) override; - - /*! \copydoc PhysicalLayer::stageMode */ - int16_t stageMode(RadioModeType_t mode, RadioModeConfig_t* cfg) override; - - /*! \copydoc PhysicalLayer::launchMode */ - int16_t launchMode() override; - -#if !RADIOLIB_EXCLUDE_DIRECT_RECEIVE - /*! - \brief Dummy method, to ensure PhysicalLayer compatibility. - \param func Ignored. - */ - void setDirectAction(void (*func)(void)) override; - - /*! - \brief Dummy method, to ensure PhysicalLayer compatibility. - \param pin Ignored. - */ - void readBit(uint32_t pin) override; -#endif - -#if !RADIOLIB_GODMODE && !RADIOLIB_LOW_LEVEL - protected: -#endif - const char* chipType = NULL; - - Module* getMod() override; - int16_t modSetup(uint8_t modem); - - // cached LoRa parameters - float bandwidthKhz = 0; - uint8_t bandwidth = 0, spreadingFactor = 0, codingRateLoRa = 0; - uint8_t preambleLengthLoRa = 0, headerType = 0, payloadLen = 0, crcLoRa = 0; - - // SX128x SPI command implementations - uint8_t getStatus(); - int16_t writeRegister(uint16_t addr, const uint8_t* data, uint8_t numBytes); - int16_t readRegister(uint16_t addr, uint8_t* data, uint8_t numBytes); - int16_t writeBuffer(const uint8_t* data, uint8_t numBytes, uint8_t offset = 0x00); - int16_t readBuffer(uint8_t* data, uint8_t numBytes, uint8_t offset = 0x00); - int16_t setTx(uint16_t periodBaseCount = RADIOLIB_SX128X_TX_TIMEOUT_NONE, - uint8_t periodBase = RADIOLIB_SX128X_PERIOD_BASE_15_625_US); - int16_t setRx(uint16_t periodBaseCount, uint8_t periodBase = RADIOLIB_SX128X_PERIOD_BASE_15_625_US); - int16_t setCad(uint8_t symbolNum); - uint8_t getPacketType(); - int16_t setRfFrequency(uint32_t frf); - int16_t setTxParams(uint8_t pwr, uint8_t rampTime = RADIOLIB_SX128X_PA_RAMP_10_US); - int16_t setBufferBaseAddress(uint8_t txBaseAddress = 0x00, uint8_t rxBaseAddress = 0x00); - int16_t setModulationParams(uint8_t modParam1, uint8_t modParam2, uint8_t modParam3); - int16_t setPacketParamsGFSK(uint8_t preambleLen, - uint8_t syncLen, - uint8_t syncMatch, - uint8_t crcLen, - uint8_t whiten, - uint8_t hdrType, - uint8_t payLen = 0xFF); - int16_t setPacketParamsBLE(uint8_t connState, uint8_t crcLen, uint8_t bleTest, uint8_t whiten); - int16_t setPacketParamsLoRa(uint8_t preambleLen, - uint8_t hdrType, - uint8_t payLen, - uint8_t crc, - uint8_t invIQ = RADIOLIB_SX128X_LORA_IQ_STANDARD); - int16_t setDioIrqParams(uint16_t irqMask, - uint16_t dio1Mask, - uint16_t dio2Mask = RADIOLIB_SX128X_IRQ_NONE, - uint16_t dio3Mask = RADIOLIB_SX128X_IRQ_NONE); - int16_t clearIrqStatus(uint16_t clearIrqParams = RADIOLIB_SX128X_IRQ_ALL); - int16_t setRangingRole(uint8_t role); - int16_t setPacketType(uint8_t type); - - // protected so that it can be accessed from SX1280 class during reinit after ranging is complete - int16_t config(uint8_t modem); - -#if !RADIOLIB_GODMODE - private: -#endif - Module* mod; - - // common low-level SPI interface - static int16_t SPIparseStatus(uint8_t in); - - // common parameters - uint8_t power = 0; - uint32_t rxTimeout = 0; - - // cached LoRa parameters - uint8_t invertIQEnabled = RADIOLIB_SX128X_LORA_IQ_STANDARD; - - // cached GFSK parameters - float modIndexReal = 0, frequencyDev = 0; - uint16_t bitRateKbps = 0; - uint8_t bitRate = 0, modIndex = 0, shaping = 0; - uint8_t preambleLengthGFSK = 0, syncWordLen = 0, syncWordMatch = 0, crcGFSK = 0, whitening = 0; - uint8_t packetType = RADIOLIB_SX128X_GFSK_FLRC_PACKET_VARIABLE; - - // cached FLRC parameters - uint8_t codingRateFLRC = 0; - - // cached BLE parameters - uint8_t connectionState = 0, crcBLE = 0, bleTestPayload = 0; - - bool findChip(const char* verStr); - int16_t setPacketMode(uint8_t mode, uint8_t len); - int16_t setHeaderType(uint8_t hdrType, size_t len = 0xFF); -}; - -#endif - -#endif From 8f06d13d1db2a3dfbcad9ba20dffa4dd712a56a9 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 19:39:16 -0800 Subject: [PATCH 034/101] make MyComponent depend on RadioLib --- FprimeZephyrReference/Components/MyComponent/CMakeLists.txt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt b/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt index 2667f799..2baf1fb4 100644 --- a/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt +++ b/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt @@ -19,10 +19,12 @@ register_fprime_library( "${CMAKE_CURRENT_LIST_DIR}/MyComponent.fpp" SOURCES "${CMAKE_CURRENT_LIST_DIR}/MyComponent.cpp" -# DEPENDS -# MyPackage_MyOtherModule + DEPENDS + RadioLib ) +add_subdirectory("${FPRIME_PROJECT_ROOT}/lib/RadioLib" "${CMAKE_BINARY_DIR}/RadioLib") + ### Unit Tests ### # register_fprime_ut( # AUTOCODER_INPUTS From 3dd31d2217f688871f8237d8523ed745ae540e0d Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 19:46:57 -0800 Subject: [PATCH 035/101] add skeleton FprimeHal --- .../Components/MyComponent/CMakeLists.txt | 1 + .../Components/MyComponent/FprimeHal.cpp | 56 +++++++++++++++++++ 2 files changed, 57 insertions(+) create mode 100644 FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp diff --git a/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt b/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt index 2baf1fb4..c0b39406 100644 --- a/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt +++ b/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt @@ -19,6 +19,7 @@ register_fprime_library( "${CMAKE_CURRENT_LIST_DIR}/MyComponent.fpp" SOURCES "${CMAKE_CURRENT_LIST_DIR}/MyComponent.cpp" + "${CMAKE_CURRENT_LIST_DIR}/FprimeHal.cpp" DEPENDS RadioLib ) diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp new file mode 100644 index 00000000..e6928b57 --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp @@ -0,0 +1,56 @@ +#ifndef FPRIME_HAL_H +#define FPRIME_HAL_H + +// include RadioLib +#include + +// the HAL must inherit from the base RadioLibHal class +// and implement all of its virtual methods +class FprimeHal : public RadioLibHal { + public: + FprimeHal() : RadioLibHal(0, 0, 0, 0, 0, 0) {} + + void init() override {} + + void term() override {} + + void pinMode(uint32_t pin, uint32_t mode) override {} + + void digitalWrite(uint32_t pin, uint32_t value) override {} + + uint32_t digitalRead(uint32_t pin) override {} + + void attachInterrupt(uint32_t interruptNum, void (*interruptCb)(void), uint32_t mode) override {} + + void detachInterrupt(uint32_t interruptNum) override {} + + void delay(unsigned long ms) override {} + + void delayMicroseconds(unsigned long us) override {} + + unsigned long millis() override {} + + unsigned long micros() override {} + + long pulseIn(uint32_t pin, uint32_t state, unsigned long timeout) override {} + + void tone(uint32_t pin, unsigned int frequency, unsigned long duration = 0) override {} + + void noTone(uint32_t pin) override {} + + void spiBegin() {} + + void spiBeginTransaction() {} + + void spiTransfer(uint8_t* out, size_t len, uint8_t* in) {} + + void yield() override {} + + void spiEndTransaction() {} + + void spiEnd() {} + + private: +}; + +#endif From 366f18d05dcde8a3b2cc1fb0405c6990d67421da Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 14 Nov 2025 20:47:14 -0800 Subject: [PATCH 036/101] split Fprime Hal into hpp and cpp --- .../Components/MyComponent/FprimeHal.cpp | 57 +++++++------------ .../Components/MyComponent/FprimeHal.hpp | 55 ++++++++++++++++++ .../Components/MyComponent/MyComponent.cpp | 18 +++--- 3 files changed, 88 insertions(+), 42 deletions(-) create mode 100644 FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp index e6928b57..6cbe629c 100644 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp @@ -1,56 +1,43 @@ -#ifndef FPRIME_HAL_H -#define FPRIME_HAL_H +#include "FprimeHal.hpp" -// include RadioLib -#include +FprimeHal::FprimeHal() : RadioLibHal(0, 0, 0, 0, 0, 0) {} -// the HAL must inherit from the base RadioLibHal class -// and implement all of its virtual methods -class FprimeHal : public RadioLibHal { - public: - FprimeHal() : RadioLibHal(0, 0, 0, 0, 0, 0) {} +void FprimeHal::init() {} - void init() override {} +void FprimeHal::term() {} - void term() override {} +void FprimeHal::pinMode(uint32_t pin, uint32_t mode) {} - void pinMode(uint32_t pin, uint32_t mode) override {} +void FprimeHal::digitalWrite(uint32_t pin, uint32_t value) {} - void digitalWrite(uint32_t pin, uint32_t value) override {} +uint32_t FprimeHal::digitalRead(uint32_t pin) {} - uint32_t digitalRead(uint32_t pin) override {} +void FprimeHal::attachInterrupt(uint32_t interruptNum, void (*interruptCb)(void), uint32_t mode) {} - void attachInterrupt(uint32_t interruptNum, void (*interruptCb)(void), uint32_t mode) override {} +void FprimeHal::detachInterrupt(uint32_t interruptNum) {} - void detachInterrupt(uint32_t interruptNum) override {} +void FprimeHal::delay(unsigned long ms) {} - void delay(unsigned long ms) override {} +void FprimeHal::delayMicroseconds(unsigned long us) {} - void delayMicroseconds(unsigned long us) override {} +unsigned long FprimeHal::millis() {} - unsigned long millis() override {} +unsigned long FprimeHal::micros() {} - unsigned long micros() override {} +long FprimeHal::pulseIn(uint32_t pin, uint32_t state, unsigned long timeout) {} - long pulseIn(uint32_t pin, uint32_t state, unsigned long timeout) override {} +void FprimeHal::tone(uint32_t pin, unsigned int frequency, unsigned long duration) {} - void tone(uint32_t pin, unsigned int frequency, unsigned long duration = 0) override {} +void FprimeHal::noTone(uint32_t pin) {} - void noTone(uint32_t pin) override {} +void FprimeHal::spiBegin() {} - void spiBegin() {} +void FprimeHal::spiBeginTransaction() {} - void spiBeginTransaction() {} +void FprimeHal::spiTransfer(uint8_t* out, size_t len, uint8_t* in) {} - void spiTransfer(uint8_t* out, size_t len, uint8_t* in) {} +void FprimeHal::yield() {} - void yield() override {} +void FprimeHal::spiEndTransaction() {} - void spiEndTransaction() {} - - void spiEnd() {} - - private: -}; - -#endif +void FprimeHal::spiEnd() {} diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp new file mode 100644 index 00000000..636b22ee --- /dev/null +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp @@ -0,0 +1,55 @@ +#ifndef FPRIME_HAL_H +#define FPRIME_HAL_H + +#include + +// the HAL must inherit from the base RadioLibHal class +// and implement all of its virtual methods +class FprimeHal : public RadioLibHal { + public: + FprimeHal(); + + void init() override; + + void term() override; + + void pinMode(uint32_t pin, uint32_t mode) override; + + void digitalWrite(uint32_t pin, uint32_t value) override; + + uint32_t digitalRead(uint32_t pin) override; + + void attachInterrupt(uint32_t interruptNum, void (*interruptCb)(void), uint32_t mode) override; + + void detachInterrupt(uint32_t interruptNum) override; + + void delay(unsigned long ms) override; + + void delayMicroseconds(unsigned long us) override; + + unsigned long millis() override; + + unsigned long micros() override; + + long pulseIn(uint32_t pin, uint32_t state, unsigned long timeout) override; + + void tone(uint32_t pin, unsigned int frequency, unsigned long duration = 0) override; + + void noTone(uint32_t pin) override; + + void spiBegin(); + + void spiBeginTransaction(); + + void spiTransfer(uint8_t* out, size_t len, uint8_t* in); + + void yield() override; + + void spiEndTransaction(); + + void spiEnd(); + + private: +}; + +#endif diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index e9fcf9e2..87263e15 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -6,8 +6,14 @@ #include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" +#define RADIOLIB_LOW_LEVEL 1 + +#include + #include +#include "FprimeHal.hpp" + #define OP_SET_MODULATION_PARAMS 0x8B #define OP_SET_TX_PARAMS 0x8E #define OP_SET_CONTINUOUS_PREAMBLE 0xD2 @@ -40,13 +46,11 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { // ---------------------------------------------------------------------- void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - this->spiSetStandby(); - this->spiSetRfFrequency(); - this->spiSetTxParams(); - this->spiSetModulationParams(); - this->spiSetTxContinuousPreamble(); - U8 status = this->spiGetStatus(); - Fw::Logger::log("status: %" PRI_U8 "\n", status); + FprimeHal hal; + Module m(&hal, 0, 0, 0); + SX1280 radio(&m); + U8 status = radio.getStatus(); + Fw::Logger::log("radio status: %" PRI_U8 "\n", status); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } From ef651cd6355f7754b386031bef6020391ec3b805 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 15 Nov 2025 09:54:28 -0800 Subject: [PATCH 037/101] implement spiTransfer --- .../Components/MyComponent/FprimeHal.cpp | 26 ++++++++++++++----- .../Components/MyComponent/FprimeHal.hpp | 9 ++++++- .../Components/MyComponent/MyComponent.cpp | 11 ++++---- .../Components/MyComponent/MyComponent.hpp | 3 +++ 4 files changed, 36 insertions(+), 13 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp index 6cbe629c..314b346c 100644 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp @@ -1,6 +1,8 @@ +#define RADIOLIB_LOW_LEVEL 1 + #include "FprimeHal.hpp" -FprimeHal::FprimeHal() : RadioLibHal(0, 0, 0, 0, 0, 0) {} +FprimeHal::FprimeHal(Components::MyComponent* component) : RadioLibHal(0, 0, 0, 0, 0, 0), m_component(component) {} void FprimeHal::init() {} @@ -10,7 +12,9 @@ void FprimeHal::pinMode(uint32_t pin, uint32_t mode) {} void FprimeHal::digitalWrite(uint32_t pin, uint32_t value) {} -uint32_t FprimeHal::digitalRead(uint32_t pin) {} +uint32_t FprimeHal::digitalRead(uint32_t pin) { + return 0; +} void FprimeHal::attachInterrupt(uint32_t interruptNum, void (*interruptCb)(void), uint32_t mode) {} @@ -20,11 +24,17 @@ void FprimeHal::delay(unsigned long ms) {} void FprimeHal::delayMicroseconds(unsigned long us) {} -unsigned long FprimeHal::millis() {} +unsigned long FprimeHal::millis() { + return 0; +} -unsigned long FprimeHal::micros() {} +unsigned long FprimeHal::micros() { + return 0; +} -long FprimeHal::pulseIn(uint32_t pin, uint32_t state, unsigned long timeout) {} +long FprimeHal::pulseIn(uint32_t pin, uint32_t state, unsigned long timeout) { + return 0; +} void FprimeHal::tone(uint32_t pin, unsigned int frequency, unsigned long duration) {} @@ -34,7 +44,11 @@ void FprimeHal::spiBegin() {} void FprimeHal::spiBeginTransaction() {} -void FprimeHal::spiTransfer(uint8_t* out, size_t len, uint8_t* in) {} +void FprimeHal::spiTransfer(uint8_t* out, size_t len, uint8_t* in) { + Fw::Buffer writeBuffer(out, len); + Fw::Buffer readBuffer(in, len); + this->m_component->spiSend_out(0, writeBuffer, readBuffer); +} void FprimeHal::yield() {} diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp index 636b22ee..1fb6916b 100644 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp @@ -3,11 +3,17 @@ #include +#include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" + +namespace Components { +class MyComponent; +} // namespace Components + // the HAL must inherit from the base RadioLibHal class // and implement all of its virtual methods class FprimeHal : public RadioLibHal { public: - FprimeHal(); + explicit FprimeHal(Components::MyComponent* component); void init() override; @@ -50,6 +56,7 @@ class FprimeHal : public RadioLibHal { void spiEnd(); private: + Components::MyComponent* m_component; }; #endif diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 87263e15..fa47897a 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -4,16 +4,14 @@ // \brief cpp file for MyComponent component implementation class // ====================================================================== -#include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" - #define RADIOLIB_LOW_LEVEL 1 +#include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" + #include #include -#include "FprimeHal.hpp" - #define OP_SET_MODULATION_PARAMS 0x8B #define OP_SET_TX_PARAMS 0x8E #define OP_SET_CONTINUOUS_PREAMBLE 0xD2 @@ -46,9 +44,10 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { // ---------------------------------------------------------------------- void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - FprimeHal hal; + FprimeHal hal(this); Module m(&hal, 0, 0, 0); - SX1280 radio(&m); + SX1281 radio(&m); + radio.modSetup(RADIOLIB_SX128X_PACKET_TYPE_GFSK); U8 status = radio.getStatus(); Fw::Logger::log("radio status: %" PRI_U8 "\n", status); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 0ed2a455..20b247b1 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -7,6 +7,7 @@ #ifndef Components_MyComponent_HPP #define Components_MyComponent_HPP +#include "FprimeHal.hpp" #include "FprimeZephyrReference/Components/MyComponent/MyComponentComponentAc.hpp" namespace Components { @@ -24,6 +25,8 @@ class MyComponent final : public MyComponentComponentBase { //! Destroy MyComponent object ~MyComponent(); + using MyComponentComponentBase::spiSend_out; + private: // ---------------------------------------------------------------------- // Handler implementations for typed input ports From 0f371b20a4bb1e55a19f2c70bdbf966b51cc19de Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 19 Nov 2025 21:08:04 -0800 Subject: [PATCH 038/101] implement timing HAL functions --- .../Components/MyComponent/FprimeHal.cpp | 14 ++++++++++---- .../Components/MyComponent/MyComponent.hpp | 1 + 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp index 314b346c..8298678f 100644 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp @@ -20,16 +20,22 @@ void FprimeHal::attachInterrupt(uint32_t interruptNum, void (*interruptCb)(void) void FprimeHal::detachInterrupt(uint32_t interruptNum) {} -void FprimeHal::delay(unsigned long ms) {} +void FprimeHal::delay(unsigned long ms) { + Os::Task::delay(Fw::TimeInterval(0, ms * 1000)); +} -void FprimeHal::delayMicroseconds(unsigned long us) {} +void FprimeHal::delayMicroseconds(unsigned long us) { + Os::Task::delay(Fw::TimeInterval(0, us)); +} unsigned long FprimeHal::millis() { - return 0; + Fw::Time time = this->m_component->getTime(); + return time.getSeconds() * 1000 + time.getUSeconds() / 1000; } unsigned long FprimeHal::micros() { - return 0; + Fw::Time time = this->m_component->getTime(); + return time.getSeconds() * 1000000 + time.getUSeconds(); } long FprimeHal::pulseIn(uint32_t pin, uint32_t state, unsigned long timeout) { diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 20b247b1..d765646f 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -25,6 +25,7 @@ class MyComponent final : public MyComponentComponentBase { //! Destroy MyComponent object ~MyComponent(); + using MyComponentComponentBase::getTime; using MyComponentComponentBase::spiSend_out; private: From 4f3e04643789bfdc81295977e0a3c2b017cf6267 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 19 Nov 2025 21:08:45 -0800 Subject: [PATCH 039/101] use proper radio API --- .../Components/MyComponent/MyComponent.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index fa47897a..08520fc7 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -46,10 +46,14 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { FprimeHal hal(this); Module m(&hal, 0, 0, 0); - SX1281 radio(&m); - radio.modSetup(RADIOLIB_SX128X_PACKET_TYPE_GFSK); - U8 status = radio.getStatus(); - Fw::Logger::log("radio status: %" PRI_U8 "\n", status); + SX1280 radio(&m); + int state = radio.begin(); + if (state == RADIOLIB_ERR_NONE) { + Fw::Logger::log("radio.begin() success!\n"); + } else { + Fw::Logger::log("radio.begin() failed!\n"); + Fw::Logger::log("state: %i\n", state); + } this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } From 7a0074cd83b4ae58f6a31d18bc273e9311a5fa6f Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 21 Nov 2025 17:09:57 -0800 Subject: [PATCH 040/101] add `setOutputPower` and `transmit` calls --- .../Components/MyComponent/MyComponent.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 08520fc7..7be0eea4 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -54,6 +54,24 @@ void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { Fw::Logger::log("radio.begin() failed!\n"); Fw::Logger::log("state: %i\n", state); } + Fw::Logger::log("TIMEOUT ms: %i\n", m.spiConfig.timeout); + state = radio.setOutputPower(13); // 13dB is max + if (state == RADIOLIB_ERR_NONE) { + Fw::Logger::log("radio.setOutputPower() success!\n"); + } else if (state == RADIOLIB_ERR_INVALID_OUTPUT_POWER) { + Fw::Logger::log("Selected output power is invalid for this module!"); + } else { + Fw::Logger::log("radio.setOutputPower() failed!\n"); + Fw::Logger::log("state: %i\n", state); + } + char s[] = "Hello, World\0"; + state = radio.transmit(s, sizeof(s)); + if (state == RADIOLIB_ERR_NONE) { + Fw::Logger::log("radio.transmit() success!\n"); + } else { + Fw::Logger::log("radio.transmit() failed!\n"); + Fw::Logger::log("state: %i\n", state); + } this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } From 23e01c847e79123e459b43a245744cc6e66dc742 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 22 Nov 2025 10:19:24 -0800 Subject: [PATCH 041/101] remove spurious log --- FprimeZephyrReference/Components/MyComponent/MyComponent.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 7be0eea4..a1f14cbc 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -54,7 +54,6 @@ void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { Fw::Logger::log("radio.begin() failed!\n"); Fw::Logger::log("state: %i\n", state); } - Fw::Logger::log("TIMEOUT ms: %i\n", m.spiConfig.timeout); state = radio.setOutputPower(13); // 13dB is max if (state == RADIOLIB_ERR_NONE) { Fw::Logger::log("radio.setOutputPower() success!\n"); From c372675c84d3d56530eab08181b55204739733d6 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 22 Nov 2025 10:21:42 -0800 Subject: [PATCH 042/101] increase message length --- FprimeZephyrReference/Components/MyComponent/MyComponent.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index a1f14cbc..ccd33ca0 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -63,7 +63,10 @@ void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { Fw::Logger::log("radio.setOutputPower() failed!\n"); Fw::Logger::log("state: %i\n", state); } - char s[] = "Hello, World\0"; + char s[] = + "Hello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " + "world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " + "world!\nHello, world!\nHello, world!\nHello, world!\n"; state = radio.transmit(s, sizeof(s)); if (state == RADIOLIB_ERR_NONE) { Fw::Logger::log("radio.transmit() success!\n"); From 69757105087c314ff0f933577c8e57dc85a48392 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Mon, 24 Nov 2025 20:13:52 -0800 Subject: [PATCH 043/101] Match modulation parameters to CircuitPython defaults --- .../Components/MyComponent/MyComponent.cpp | 23 ++++++++----------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index ccd33ca0..7f1d90d2 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -48,21 +48,16 @@ void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { Module m(&hal, 0, 0, 0); SX1280 radio(&m); int state = radio.begin(); - if (state == RADIOLIB_ERR_NONE) { - Fw::Logger::log("radio.begin() success!\n"); - } else { - Fw::Logger::log("radio.begin() failed!\n"); - Fw::Logger::log("state: %i\n", state); - } + FW_ASSERT(state == RADIOLIB_ERR_NONE); state = radio.setOutputPower(13); // 13dB is max - if (state == RADIOLIB_ERR_NONE) { - Fw::Logger::log("radio.setOutputPower() success!\n"); - } else if (state == RADIOLIB_ERR_INVALID_OUTPUT_POWER) { - Fw::Logger::log("Selected output power is invalid for this module!"); - } else { - Fw::Logger::log("radio.setOutputPower() failed!\n"); - Fw::Logger::log("state: %i\n", state); - } + FW_ASSERT(state == RADIOLIB_ERR_NONE); + // Match modulation parameters to CircuitPython defaults + state = radio.setSpreadingFactor(7); + FW_ASSERT(state == RADIOLIB_ERR_NONE); + state = radio.setBandwidth(406.25); + FW_ASSERT(state == RADIOLIB_ERR_NONE); + state = radio.setCodingRate(5); + FW_ASSERT(state == RADIOLIB_ERR_NONE); char s[] = "Hello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " "world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " From cab0843278cb70bd70a062068172d34c00d954bb Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Mon, 24 Nov 2025 20:22:02 -0800 Subject: [PATCH 044/101] match packet params to CircuitPython implementation --- FprimeZephyrReference/Components/MyComponent/MyComponent.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 7f1d90d2..cecf5b64 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -58,6 +58,9 @@ void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { FW_ASSERT(state == RADIOLIB_ERR_NONE); state = radio.setCodingRate(5); FW_ASSERT(state == RADIOLIB_ERR_NONE); + state = radio.setPacketParamsLoRa(12, RADIOLIB_SX128X_LORA_HEADER_EXPLICIT, 255, RADIOLIB_SX128X_LORA_CRC_ON, + RADIOLIB_SX128X_LORA_IQ_STANDARD); + FW_ASSERT(state == RADIOLIB_ERR_NONE); char s[] = "Hello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " "world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " From 99ebeb9f0353e6937c5e37cfd6a84734b4ecfb84 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Tue, 25 Nov 2025 08:32:13 -0800 Subject: [PATCH 045/101] make hal, module, radio component members --- .../Components/MyComponent/FprimeHal.cpp | 6 +++++ .../Components/MyComponent/FprimeHal.hpp | 2 -- .../Components/MyComponent/MyComponent.cpp | 27 ++++++++++--------- .../Components/MyComponent/MyComponent.hpp | 5 ++++ 4 files changed, 26 insertions(+), 14 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp index 8298678f..1ea86223 100644 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp @@ -2,6 +2,12 @@ #include "FprimeHal.hpp" +#include +#include +#include + +#include "MyComponent.hpp" + FprimeHal::FprimeHal(Components::MyComponent* component) : RadioLibHal(0, 0, 0, 0, 0, 0), m_component(component) {} void FprimeHal::init() {} diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp index 1fb6916b..686b5989 100644 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp @@ -3,8 +3,6 @@ #include -#include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" - namespace Components { class MyComponent; } // namespace Components diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index cecf5b64..7475c607 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -12,6 +12,8 @@ #include +#include "FprimeHal.hpp" + #define OP_SET_MODULATION_PARAMS 0x8B #define OP_SET_TX_PARAMS 0x8E #define OP_SET_CONTINUOUS_PREAMBLE 0xD2 @@ -27,7 +29,11 @@ namespace Components { // Component construction and destruction // ---------------------------------------------------------------------- -MyComponent ::MyComponent(const char* const compName) : MyComponentComponentBase(compName) {} +MyComponent ::MyComponent(const char* const compName) + : MyComponentComponentBase(compName), + m_rlb_hal(this), + m_rlb_module(&m_rlb_hal, 0, 0, 0), + m_rlb_radio(&m_rlb_module) {} MyComponent ::~MyComponent() {} @@ -44,28 +50,25 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { // ---------------------------------------------------------------------- void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - FprimeHal hal(this); - Module m(&hal, 0, 0, 0); - SX1280 radio(&m); - int state = radio.begin(); + int state = this->m_rlb_radio.begin(); FW_ASSERT(state == RADIOLIB_ERR_NONE); - state = radio.setOutputPower(13); // 13dB is max + state = this->m_rlb_radio.setOutputPower(13); // 13dB is max FW_ASSERT(state == RADIOLIB_ERR_NONE); // Match modulation parameters to CircuitPython defaults - state = radio.setSpreadingFactor(7); + state = this->m_rlb_radio.setSpreadingFactor(7); FW_ASSERT(state == RADIOLIB_ERR_NONE); - state = radio.setBandwidth(406.25); + state = this->m_rlb_radio.setBandwidth(406.25); FW_ASSERT(state == RADIOLIB_ERR_NONE); - state = radio.setCodingRate(5); + state = this->m_rlb_radio.setCodingRate(5); FW_ASSERT(state == RADIOLIB_ERR_NONE); - state = radio.setPacketParamsLoRa(12, RADIOLIB_SX128X_LORA_HEADER_EXPLICIT, 255, RADIOLIB_SX128X_LORA_CRC_ON, - RADIOLIB_SX128X_LORA_IQ_STANDARD); + state = this->m_rlb_radio.setPacketParamsLoRa(12, RADIOLIB_SX128X_LORA_HEADER_EXPLICIT, 255, + RADIOLIB_SX128X_LORA_CRC_ON, RADIOLIB_SX128X_LORA_IQ_STANDARD); FW_ASSERT(state == RADIOLIB_ERR_NONE); char s[] = "Hello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " "world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " "world!\nHello, world!\nHello, world!\nHello, world!\n"; - state = radio.transmit(s, sizeof(s)); + state = this->m_rlb_radio.transmit(s, sizeof(s)); if (state == RADIOLIB_ERR_NONE) { Fw::Logger::log("radio.transmit() success!\n"); } else { diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index d765646f..f8d3de3e 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -69,6 +69,11 @@ class MyComponent final : public MyComponentComponentBase { void spiSetTx(); void spiSetStandby(); U8 spiGetStatus(); + + private: + FprimeHal m_rlb_hal; //!< RadioLib HAL instance + Module m_rlb_module; //!< RadioLib Module instance + SX1280 m_rlb_radio; //!< RadioLib SX1280 radio instance }; } // namespace Components From 12493c8b3ad6396fea1fc41cddbe12b54dffd928 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Tue, 25 Nov 2025 08:52:58 -0800 Subject: [PATCH 046/101] Move radio config into `configure_radio` --- .../Components/MyComponent/MyComponent.cpp | 46 +++++++++++++------ .../Components/MyComponent/MyComponent.hpp | 3 ++ 2 files changed, 36 insertions(+), 13 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 7475c607..803c9d2b 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -50,19 +50,7 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { // ---------------------------------------------------------------------- void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - int state = this->m_rlb_radio.begin(); - FW_ASSERT(state == RADIOLIB_ERR_NONE); - state = this->m_rlb_radio.setOutputPower(13); // 13dB is max - FW_ASSERT(state == RADIOLIB_ERR_NONE); - // Match modulation parameters to CircuitPython defaults - state = this->m_rlb_radio.setSpreadingFactor(7); - FW_ASSERT(state == RADIOLIB_ERR_NONE); - state = this->m_rlb_radio.setBandwidth(406.25); - FW_ASSERT(state == RADIOLIB_ERR_NONE); - state = this->m_rlb_radio.setCodingRate(5); - FW_ASSERT(state == RADIOLIB_ERR_NONE); - state = this->m_rlb_radio.setPacketParamsLoRa(12, RADIOLIB_SX128X_LORA_HEADER_EXPLICIT, 255, - RADIOLIB_SX128X_LORA_CRC_ON, RADIOLIB_SX128X_LORA_IQ_STANDARD); + int state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); char s[] = "Hello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " @@ -78,6 +66,38 @@ void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } +int MyComponent ::configure_radio() { + int state = this->m_rlb_radio.begin(); + if (state != RADIOLIB_ERR_NONE) { + return state; + } + + state = this->m_rlb_radio.setOutputPower(13); // 13dB is max + if (state != RADIOLIB_ERR_NONE) { + return state; + } + + // Match modulation parameters to CircuitPython defaults + state = this->m_rlb_radio.setSpreadingFactor(7); + if (state != RADIOLIB_ERR_NONE) { + return state; + } + + state = this->m_rlb_radio.setBandwidth(406.25); + if (state != RADIOLIB_ERR_NONE) { + return state; + } + + state = this->m_rlb_radio.setCodingRate(5); + if (state != RADIOLIB_ERR_NONE) { + return state; + } + + state = this->m_rlb_radio.setPacketParamsLoRa(12, RADIOLIB_SX128X_LORA_HEADER_EXPLICIT, 255, + RADIOLIB_SX128X_LORA_CRC_ON, RADIOLIB_SX128X_LORA_IQ_STANDARD); + return state; +} + void MyComponent ::RESET_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { // BROKEN this->resetSend_out(0, Fw::Logic::HIGH); diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index f8d3de3e..238f319f 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -70,6 +70,9 @@ class MyComponent final : public MyComponentComponentBase { void spiSetStandby(); U8 spiGetStatus(); + // Configure the SX1280 radio (setup and parameter tuning) + int configure_radio(); + private: FprimeHal m_rlb_hal; //!< RadioLib HAL instance Module m_rlb_module; //!< RadioLib Module instance From 0ad7a609357d95c8786a3f759b230265fe4e8a80 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Tue, 25 Nov 2025 09:02:19 -0800 Subject: [PATCH 047/101] Rename command FOO -> TRANSMIT --- .../Components/MyComponent/MyComponent.cpp | 2 +- .../Components/MyComponent/MyComponent.fpp | 4 ++-- .../Components/MyComponent/MyComponent.hpp | 10 +++++----- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 803c9d2b..981cd2a3 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -49,7 +49,7 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { // Handler implementations for commands // ---------------------------------------------------------------------- -void MyComponent ::FOO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { +void MyComponent ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { int state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); char s[] = diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp index b8201289..abc6f08f 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -14,8 +14,8 @@ module Components { # @ Import Communication Interface # import Svc.Com - @ Command for testing - sync command FOO() + @ Command to transmit data + sync command TRANSMIT() @ Reset Radio Module sync command RESET() diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 238f319f..4861ac6a 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -45,12 +45,12 @@ class MyComponent final : public MyComponentComponentBase { // Handler implementations for commands // ---------------------------------------------------------------------- - //! Handler implementation for command FOO + //! Handler implementation for command TRANSMIT //! - //! Command for testing - void FOO_cmdHandler(FwOpcodeType opCode, //!< The opcode - U32 cmdSeq //!< The command sequence number - ) override; + //! Command to transmit data + void TRANSMIT_cmdHandler(FwOpcodeType opCode, //!< The opcode + U32 cmdSeq //!< The command sequence number + ) override; //! Handler implementation for command RESET //! From 140fa63ef1bb52f99d37df60ae9587cda46da3c0 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Tue, 25 Nov 2025 09:21:18 -0800 Subject: [PATCH 048/101] Connect tx_en to MyComponent --- FprimeZephyrReference/Components/MyComponent/MyComponent.fpp | 3 +++ FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp | 1 + 2 files changed, 4 insertions(+) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp index abc6f08f..12fe2b48 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -26,6 +26,9 @@ module Components { @ Radio Module Reset GPIO output port resetSend: Drv.GpioWrite + @ S-Band TX Enable GPIO (separate from reset) + output port txEnable: Drv.GpioWrite + ############################################################################### # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # ############################################################################### diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 7cdf62af..287c74c5 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -214,6 +214,7 @@ module ReferenceDeployment { connections MyConnectionGraph { mycomp.spiSend -> spiDriver.SpiReadWrite mycomp.resetSend -> gpioSbandNrst.gpioWrite + mycomp.txEnable -> gpioSbandTxEn.gpioWrite } connections ComCcsds_FileHandling { From 55347dddedb4dc8056cc2fcfceb37c3ba8d5b563 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Tue, 25 Nov 2025 10:19:37 -0800 Subject: [PATCH 049/101] Toggle tx_en on transmit --- FprimeZephyrReference/Components/MyComponent/MyComponent.cpp | 4 ++++ .../proves_flight_control_board_v5.dtsi | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 981cd2a3..9409280b 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -56,7 +56,11 @@ void MyComponent ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { "Hello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " "world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " "world!\nHello, world!\nHello, world!\nHello, world!\n"; + this->txEnable_out(0, Fw::Logic::HIGH); state = this->m_rlb_radio.transmit(s, sizeof(s)); + // TODO replace with proper TX done interrupt handling + Os::Task::delay(Fw::TimeInterval(0, 1000)); + this->txEnable_out(0, Fw::Logic::LOW); if (state == RADIOLIB_ERR_NONE) { Fw::Logger::log("radio.transmit() success!\n"); } else { diff --git a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi index 75f678d8..3d80e8d8 100644 --- a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi +++ b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi @@ -313,10 +313,10 @@ zephyr_udc0: &usbd { }; sband_tx_en: sband-tx-en { - gpios = <&gpio0 22 (GPIO_PULL_UP | GPIO_ACTIVE_LOW)>; + gpios = <&gpio0 22 (GPIO_PULL_UP | GPIO_ACTIVE_HIGH)>; label = "SBAND_TX_EN"; }; - }; + }; // Define GPIO inputs gpio_inputs { From d76b88c449eb78ab2b2a72860822aac64819e60f Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Tue, 25 Nov 2025 12:05:24 -0800 Subject: [PATCH 050/101] connect MyComponent to rx_enable GPIO --- FprimeZephyrReference/Components/MyComponent/MyComponent.fpp | 3 +++ FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp | 1 + 2 files changed, 4 insertions(+) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp index 12fe2b48..6255e25a 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -29,6 +29,9 @@ module Components { @ S-Band TX Enable GPIO (separate from reset) output port txEnable: Drv.GpioWrite + @ S-Band RX Enable GPIO + output port rxEnable: Drv.GpioWrite + ############################################################################### # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # ############################################################################### diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 287c74c5..ffb581db 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -215,6 +215,7 @@ module ReferenceDeployment { mycomp.spiSend -> spiDriver.SpiReadWrite mycomp.resetSend -> gpioSbandNrst.gpioWrite mycomp.txEnable -> gpioSbandTxEn.gpioWrite + mycomp.rxEnable -> gpioSbandRxEn.gpioWrite } connections ComCcsds_FileHandling { From 7a93743b219ccf7145218db834a9a900448d3a11 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Tue, 25 Nov 2025 13:35:12 -0800 Subject: [PATCH 051/101] Add RECEIVE + READ_DATA commands --- .../Components/MyComponent/MyComponent.cpp | 44 +++++++++++++++++++ .../Components/MyComponent/MyComponent.fpp | 6 +++ .../Components/MyComponent/MyComponent.hpp | 14 ++++++ 3 files changed, 64 insertions(+) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 9409280b..8c66886d 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -70,6 +70,50 @@ void MyComponent ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } +void MyComponent ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { + this->rxEnable_out(0, Fw::Logic::HIGH); + + int state = this->configure_radio(); + FW_ASSERT(state == RADIOLIB_ERR_NONE); + + state = this->m_rlb_radio.startReceive(); + if (state == RADIOLIB_ERR_NONE) { + Fw::Logger::log("radio.startReceive() success!\n"); + } else { + Fw::Logger::log("radio.startReceive() failed!\n"); + Fw::Logger::log("state: %i\n", state); + } + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); +} + +void MyComponent ::READ_DATA_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { + // Allocate a buffer large enough for the maximum packet size + constexpr size_t MAX_PKT = RADIOLIB_SX128X_MAX_PACKET_LENGTH + 1; + uint8_t buf[MAX_PKT] = {0}; + + int16_t state = this->m_rlb_radio.readData(buf, MAX_PKT); + if (state == RADIOLIB_ERR_NONE) { + Fw::Logger::log("radio.readData() success!\n"); + } else { + Fw::Logger::log("radio.readData() failed!\n"); + Fw::Logger::log("state: %i\n", state); + } + + // Log the entire receive buffer so user can observe any changes + Fw::Logger::log("readData() buffer (full %u bytes):\n", (unsigned)MAX_PKT); + + char msg[sizeof(buf) * 3 + 1]; + + for (size_t i = 0; i < MAX_PKT; ++i) { + sprintf(msg + i * 3, "%02X ", buf[i]); // NOLINT(runtime/printf) + } + msg[sizeof(buf) * 3] = '\0'; + + Fw::Logger::log("%s\n", msg); + + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); +} + int MyComponent ::configure_radio() { int state = this->m_rlb_radio.begin(); if (state != RADIOLIB_ERR_NONE) { diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp index 6255e25a..58feaee5 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -17,6 +17,12 @@ module Components { @ Command to transmit data sync command TRANSMIT() + @ Command to begin receive + sync command RECEIVE() + + @ Command to read recv data buffer + sync command READ_DATA() + @ Reset Radio Module sync command RESET() diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 4861ac6a..d86df08e 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -52,6 +52,20 @@ class MyComponent final : public MyComponentComponentBase { U32 cmdSeq //!< The command sequence number ) override; + //! Handler implementation for command RECEIVE + //! + //! Command to begin receive + void RECEIVE_cmdHandler(FwOpcodeType opCode, //!< The opcode + U32 cmdSeq //!< The command sequence number + ) override; + + //! Handler implementation for command READ_DATA + //! + //! Command to read recv data buffer + void READ_DATA_cmdHandler(FwOpcodeType opCode, //!< The opcode + U32 cmdSeq //!< The command sequence number + ) override; + //! Handler implementation for command RESET //! //! Reset Radio Module From 6e3bff3a791f40fe611be6a3dba32bdc665a9463 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 26 Nov 2025 09:35:50 -0800 Subject: [PATCH 052/101] cleanup in READ_DATA --- .../Components/MyComponent/MyComponent.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 8c66886d..e36f2c47 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -87,11 +87,9 @@ void MyComponent ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { } void MyComponent ::READ_DATA_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - // Allocate a buffer large enough for the maximum packet size - constexpr size_t MAX_PKT = RADIOLIB_SX128X_MAX_PACKET_LENGTH + 1; - uint8_t buf[MAX_PKT] = {0}; + uint8_t buf[256] = {0}; - int16_t state = this->m_rlb_radio.readData(buf, MAX_PKT); + int16_t state = this->m_rlb_radio.readData(buf, sizeof(buf)); if (state == RADIOLIB_ERR_NONE) { Fw::Logger::log("radio.readData() success!\n"); } else { @@ -99,12 +97,11 @@ void MyComponent ::READ_DATA_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { Fw::Logger::log("state: %i\n", state); } - // Log the entire receive buffer so user can observe any changes - Fw::Logger::log("readData() buffer (full %u bytes):\n", (unsigned)MAX_PKT); + Fw::Logger::log("readData() buffer:\n"); char msg[sizeof(buf) * 3 + 1]; - for (size_t i = 0; i < MAX_PKT; ++i) { + for (size_t i = 0; i < sizeof(buf); ++i) { sprintf(msg + i * 3, "%02X ", buf[i]); // NOLINT(runtime/printf) } msg[sizeof(buf) * 3] = '\0'; From d842839db722bc0295e1e94251a2eddce7c66b85 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 26 Nov 2025 10:58:12 -0800 Subject: [PATCH 053/101] Add untested RF2 IRQLine read capability --- .../Components/MyComponent/FprimeHal.cpp | 9 +++++++++ .../Components/MyComponent/MyComponent.cpp | 2 +- .../Components/MyComponent/MyComponent.fpp | 3 +++ .../Components/MyComponent/MyComponent.hpp | 1 + .../Top/ReferenceDeploymentTopology.cpp | 2 ++ .../ReferenceDeployment/Top/instances.fpp | 2 ++ .../ReferenceDeployment/Top/topology.fpp | 2 ++ 7 files changed, 20 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp index 1ea86223..f487f9f0 100644 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp @@ -19,6 +19,15 @@ void FprimeHal::pinMode(uint32_t pin, uint32_t mode) {} void FprimeHal::digitalWrite(uint32_t pin, uint32_t value) {} uint32_t FprimeHal::digitalRead(uint32_t pin) { + if (pin == 5) { + Fw::Logic irqState; + Drv::GpioStatus state = this->m_component->getIRQLine_out(0, irqState); + FW_ASSERT(state == Drv::GpioStatus::OP_OK); + if (irqState == Fw::Logic::HIGH) + return 1; + else + return 0; + } return 0; } diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index e36f2c47..192658df 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -32,7 +32,7 @@ namespace Components { MyComponent ::MyComponent(const char* const compName) : MyComponentComponentBase(compName), m_rlb_hal(this), - m_rlb_module(&m_rlb_hal, 0, 0, 0), + m_rlb_module(&m_rlb_hal, 0, 5, 0), m_rlb_radio(&m_rlb_module) {} MyComponent ::~MyComponent() {} diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp index 58feaee5..ccb0f360 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -38,6 +38,9 @@ module Components { @ S-Band RX Enable GPIO output port rxEnable: Drv.GpioWrite + @ S-Band IRQ Line + output port getIRQLine: Drv.GpioRead + ############################################################################### # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # ############################################################################### diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index d86df08e..043fa26f 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -25,6 +25,7 @@ class MyComponent final : public MyComponentComponentBase { //! Destroy MyComponent object ~MyComponent(); + using MyComponentComponentBase::getIRQLine_out; using MyComponentComponentBase::getTime; using MyComponentComponentBase::spiSend_out; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index c15bcf35..e5342b60 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -29,6 +29,7 @@ static const struct gpio_dt_spec payloadBatteryLoadSwitchGpio = static const struct gpio_dt_spec sbandNrstGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(sband_nrst), gpios); static const struct gpio_dt_spec sbandRxEnGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(sband_rx_en), gpios); static const struct gpio_dt_spec sbandTxEnGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(sband_tx_en), gpios); +static const struct gpio_dt_spec sbandTxEnIRQ = GPIO_DT_SPEC_GET(DT_NODELABEL(rf2_io1), gpios); // Allows easy reference to objects in FPP/autocoder required namespaces using namespace ReferenceDeployment; @@ -86,6 +87,7 @@ void configureTopology() { gpioSbandNrst.open(sbandNrstGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); gpioSbandRxEn.open(sbandRxEnGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); gpioSbandTxEn.open(sbandTxEnGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); + gpioSbandIRQ.open(sbandTxEnIRQ, Zephyr::ZephyrGpioDriver::GpioConfiguration::IN); } // Public functions for use in main program are namespaced with deployment name ReferenceDeployment diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 01c64a8e..e4f8a47e 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -103,6 +103,8 @@ module ReferenceDeployment { instance gpioSbandTxEn: Zephyr.ZephyrGpioDriver base id 0x10044000 + instance gpioSbandIRQ: Zephyr.ZephyrGpioDriver base id 0x10045000 + instance gpioface4LS: Zephyr.ZephyrGpioDriver base id 0x1002A000 instance gpioface0LS: Zephyr.ZephyrGpioDriver base id 0x1002B000 diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index ffb581db..06e1b68b 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -56,6 +56,7 @@ module ReferenceDeployment { instance gpioSbandNrst instance gpioSbandRxEn instance gpioSbandTxEn + instance gpioSbandIRQ instance face4LoadSwitch instance face0LoadSwitch instance face1LoadSwitch @@ -216,6 +217,7 @@ module ReferenceDeployment { mycomp.resetSend -> gpioSbandNrst.gpioWrite mycomp.txEnable -> gpioSbandTxEn.gpioWrite mycomp.rxEnable -> gpioSbandRxEn.gpioWrite + mycomp.getIRQLine -> gpioSbandRxEn.gpioRead } connections ComCcsds_FileHandling { From 2b6b3af1ae3ef0d9f829d04eb1f25589cf410270 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 26 Nov 2025 13:24:07 -0800 Subject: [PATCH 054/101] work around RtcManager bug --- FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp index f487f9f0..0e8c13df 100644 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp @@ -44,13 +44,11 @@ void FprimeHal::delayMicroseconds(unsigned long us) { } unsigned long FprimeHal::millis() { - Fw::Time time = this->m_component->getTime(); - return time.getSeconds() * 1000 + time.getUSeconds() / 1000; + return k_uptime_get(); } unsigned long FprimeHal::micros() { - Fw::Time time = this->m_component->getTime(); - return time.getSeconds() * 1000000 + time.getUSeconds(); + return k_uptime_get() * 1000; } long FprimeHal::pulseIn(uint32_t pin, uint32_t state, unsigned long timeout) { From fdb0fafa9e04ca03b13e3be1deecbc7c4885b51e Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 26 Nov 2025 13:53:27 -0800 Subject: [PATCH 055/101] add missing header --- FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp index 0e8c13df..d429ff71 100644 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp +++ b/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp @@ -7,6 +7,7 @@ #include #include "MyComponent.hpp" +#include FprimeHal::FprimeHal(Components::MyComponent* component) : RadioLibHal(0, 0, 0, 0, 0, 0), m_component(component) {} From 9a99eb34eda6b7ee6e29e1eccc34c7522528c62e Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 28 Nov 2025 11:23:31 -0800 Subject: [PATCH 056/101] fix irqLine connection --- FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 06e1b68b..156f4fae 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -217,7 +217,7 @@ module ReferenceDeployment { mycomp.resetSend -> gpioSbandNrst.gpioWrite mycomp.txEnable -> gpioSbandTxEn.gpioWrite mycomp.rxEnable -> gpioSbandRxEn.gpioWrite - mycomp.getIRQLine -> gpioSbandRxEn.gpioRead + mycomp.getIRQLine -> gpioSbandIRQ.gpioRead } connections ComCcsds_FileHandling { From 5e77358f2c1d2134ea3a78585b65b1f14c2b60c7 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 28 Nov 2025 12:11:01 -0800 Subject: [PATCH 057/101] Change RECEIVE command to do whole receive --- .../Components/MyComponent/MyComponent.cpp | 21 ++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 192658df..866ec858 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -76,13 +76,28 @@ void MyComponent ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { int state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); - state = this->m_rlb_radio.startReceive(); + uint8_t buf[256] = {0}; + + // cannot specify timeout greater than 2^16 * 15.625us = 1024 ms as timeout + // is internally resolved to 16-bit representation of 15.625us step count + state = this->m_rlb_radio.receive(buf, sizeof(buf), RadioLibTime_t(1024 * 1000)); if (state == RADIOLIB_ERR_NONE) { - Fw::Logger::log("radio.startReceive() success!\n"); + Fw::Logger::log("radio.receive() success!\n"); } else { - Fw::Logger::log("radio.startReceive() failed!\n"); + Fw::Logger::log("radio.receive() failed!\n"); Fw::Logger::log("state: %i\n", state); } + + Fw::Logger::log("RESULTING BUFFER:\n"); + + char msg[sizeof(buf) * 3 + 1]; + + for (size_t i = 0; i < sizeof(buf); ++i) { + sprintf(msg + i * 3, "%02X ", buf[i]); // NOLINT(runtime/printf) + } + msg[sizeof(buf) * 3] = '\0'; + + Fw::Logger::log("%s\n", msg); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } From 2dcbc125487f45f168e5fb648b7550ab1e2f841b Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 28 Nov 2025 13:26:27 -0800 Subject: [PATCH 058/101] remove manual driver commands --- .../Components/MyComponent/MyComponent.cpp | 88 ------------------- .../Components/MyComponent/MyComponent.hpp | 11 --- 2 files changed, 99 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 866ec858..a59a60bf 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -14,15 +14,6 @@ #include "FprimeHal.hpp" -#define OP_SET_MODULATION_PARAMS 0x8B -#define OP_SET_TX_PARAMS 0x8E -#define OP_SET_CONTINUOUS_PREAMBLE 0xD2 -#define OP_SET_CONTINUOUS_WAVE 0xD1 -#define OP_SET_RF_FREQUENCY 0x86 -#define OP_SET_TX 0x83 -#define OP_SET_STANDBY 0x80 -#define OP_GET_STATUS 0xC0 - namespace Components { // ---------------------------------------------------------------------- @@ -166,83 +157,4 @@ void MyComponent ::RESET_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } -// SPI Commands - -void MyComponent ::spiSetModulationParams() { - // Bitrate+Bandwidth, Modulation Index, Shaping - U8 write_data[] = {OP_SET_MODULATION_PARAMS, 0x4C, 0x0F, 0x00}; - U8 read_data[sizeof(write_data)]; - Fw::Buffer writeBuffer(write_data, sizeof(write_data)); - Fw::Buffer readBuffer(read_data, sizeof(read_data)); - this->spiSend_out(0, writeBuffer, readBuffer); -} - -void MyComponent ::spiSetTxParams() { - // power, rampTime - U8 write_data[] = {OP_SET_TX_PARAMS, 0x1F, 0x80}; - U8 read_data[sizeof(write_data)]; - Fw::Buffer writeBuffer(write_data, sizeof(write_data)); - Fw::Buffer readBuffer(read_data, sizeof(read_data)); - this->spiSend_out(0, writeBuffer, readBuffer); -} - -void MyComponent ::spiSetTxContinuousPreamble() { - U8 write_data[] = {OP_SET_CONTINUOUS_PREAMBLE}; - U8 read_data[sizeof(write_data)]; - Fw::Buffer writeBuffer(write_data, sizeof(write_data)); - Fw::Buffer readBuffer(read_data, sizeof(read_data)); - this->spiSend_out(0, writeBuffer, readBuffer); -} - -void MyComponent ::spiSetTxContinuousWave() { - U8 write_data[] = {OP_SET_CONTINUOUS_WAVE}; - U8 read_data[sizeof(write_data)]; - Fw::Buffer writeBuffer(write_data, sizeof(write_data)); - Fw::Buffer readBuffer(read_data, sizeof(read_data)); - this->spiSend_out(0, writeBuffer, readBuffer); -} - -void MyComponent ::spiSetRfFrequency() { - constexpr long double target_freq = 2405000000; // 2.405 GHz - constexpr long double step = 52000000.0L / static_cast(1ULL << 18); // per datasheet - constexpr U32 steps = static_cast(target_freq / step); - static_assert(steps <= 0xFFFFFF, "Result must fit in 24 bits (<= 0xFFFFFF)."); - uint8_t b0 = (uint8_t)(steps & 0xFFu); // least significant byte - uint8_t b1 = (uint8_t)((steps >> 8) & 0xFFu); - uint8_t b2 = (uint8_t)((steps >> 16) & 0xFFu); - - U8 write_data[] = {OP_SET_RF_FREQUENCY, b2, b1, b0}; - U8 read_data[sizeof(write_data)]; - Fw::Buffer writeBuffer(write_data, sizeof(write_data)); - Fw::Buffer readBuffer(read_data, sizeof(read_data)); - this->spiSend_out(0, writeBuffer, readBuffer); -} - -void MyComponent ::spiSetTx() { - // periodBase (1ms), periodBaseCount[15:8], periodBaseCount[7:0] - U8 write_data[] = {OP_SET_TX, 0x02, 0x4, 0x00}; - U8 read_data[sizeof(write_data)]; - Fw::Buffer writeBuffer(write_data, sizeof(write_data)); - Fw::Buffer readBuffer(read_data, sizeof(read_data)); - this->spiSend_out(0, writeBuffer, readBuffer); -} - -void MyComponent ::spiSetStandby() { - // config - U8 write_data[] = {OP_SET_STANDBY, 0x00}; - U8 read_data[sizeof(write_data)]; - Fw::Buffer writeBuffer(write_data, sizeof(write_data)); - Fw::Buffer readBuffer(read_data, sizeof(read_data)); - this->spiSend_out(0, writeBuffer, readBuffer); -} - -U8 MyComponent ::spiGetStatus() { - U8 write_data[] = {OP_GET_STATUS, 0x00}; - U8 read_data[sizeof(write_data)]; - Fw::Buffer writeBuffer(write_data, sizeof(write_data)); - Fw::Buffer readBuffer(read_data, sizeof(read_data)); - this->spiSend_out(0, writeBuffer, readBuffer); - return read_data[sizeof(read_data) - 1]; -} - } // namespace Components diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 043fa26f..11417e61 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -74,17 +74,6 @@ class MyComponent final : public MyComponentComponentBase { U32 cmdSeq //!< The command sequence number ) override; - //! SX1280 SPI Commands - - void spiSetModulationParams(); - void spiSetTxParams(); - void spiSetTxContinuousPreamble(); - void spiSetTxContinuousWave(); - void spiSetRfFrequency(); - void spiSetTx(); - void spiSetStandby(); - U8 spiGetStatus(); - // Configure the SX1280 radio (setup and parameter tuning) int configure_radio(); From 6ef3ff6cc8288113bce6f603fcaf30f57e8d7628 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 28 Nov 2025 15:54:24 -0800 Subject: [PATCH 059/101] remove unnecessary delay (now that we're not relying on broken RTC) --- FprimeZephyrReference/Components/MyComponent/MyComponent.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index a59a60bf..63a2db33 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -49,8 +49,6 @@ void MyComponent ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { "world!\nHello, world!\nHello, world!\nHello, world!\n"; this->txEnable_out(0, Fw::Logic::HIGH); state = this->m_rlb_radio.transmit(s, sizeof(s)); - // TODO replace with proper TX done interrupt handling - Os::Task::delay(Fw::TimeInterval(0, 1000)); this->txEnable_out(0, Fw::Logic::LOW); if (state == RADIOLIB_ERR_NONE) { Fw::Logger::log("radio.transmit() success!\n"); From 9f3d8f061c601aa6fb1a4057aa06597b8c129acb Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 28 Nov 2025 16:08:36 -0800 Subject: [PATCH 060/101] change configure_radio return type --- FprimeZephyrReference/Components/MyComponent/MyComponent.cpp | 2 +- FprimeZephyrReference/Components/MyComponent/MyComponent.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 63a2db33..d0dd2abe 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -115,7 +115,7 @@ void MyComponent ::READ_DATA_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } -int MyComponent ::configure_radio() { +int16_t MyComponent ::configure_radio() { int state = this->m_rlb_radio.begin(); if (state != RADIOLIB_ERR_NONE) { return state; diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 11417e61..609bafdc 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -75,7 +75,7 @@ class MyComponent final : public MyComponentComponentBase { ) override; // Configure the SX1280 radio (setup and parameter tuning) - int configure_radio(); + int16_t configure_radio(); private: FprimeHal m_rlb_hal; //!< RadioLib HAL instance From deb8e469da236cbbae9f91ff2fc6f8a047a1baea Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 28 Nov 2025 16:46:20 -0800 Subject: [PATCH 061/101] Move to polled receive via rate group --- .../Components/MyComponent/MyComponent.cpp | 48 +++++++++++-------- .../Components/MyComponent/MyComponent.hpp | 1 + .../ReferenceDeployment/Top/topology.fpp | 1 + 3 files changed, 29 insertions(+), 21 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index d0dd2abe..926b6503 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -33,7 +33,23 @@ MyComponent ::~MyComponent() {} // ---------------------------------------------------------------------- void MyComponent ::run_handler(FwIndexType portNum, U32 context) { - // TODO + if (this->wait_for_rx_fin) { + uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); + if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { + this->wait_for_rx_fin = false; + SX1280* radio = &this->m_rlb_radio; + uint8_t data[256] = {0}; + size_t len = radio->getPacketLength(); + radio->readData(data, len); + + Fw::Logger::log("MESSAGE RECEIVED:\n"); + char msg[sizeof(data) * 3 + 1]; + for (size_t i = 0; i < len; ++i) + sprintf(msg + i * 3, "%02X ", data[i]); // NOLINT(runtime/printf) + msg[len * 3] = '\0'; + Fw::Logger::log("%s\n", msg); + } + } } // ---------------------------------------------------------------------- @@ -43,6 +59,9 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { void MyComponent ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { int state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); + + this->wait_for_rx_fin = false; + char s[] = "Hello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " "world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " @@ -62,31 +81,18 @@ void MyComponent ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { void MyComponent ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->rxEnable_out(0, Fw::Logic::HIGH); - int state = this->configure_radio(); + int16_t state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); - uint8_t buf[256] = {0}; - - // cannot specify timeout greater than 2^16 * 15.625us = 1024 ms as timeout - // is internally resolved to 16-bit representation of 15.625us step count - state = this->m_rlb_radio.receive(buf, sizeof(buf), RadioLibTime_t(1024 * 1000)); - if (state == RADIOLIB_ERR_NONE) { - Fw::Logger::log("radio.receive() success!\n"); - } else { - Fw::Logger::log("radio.receive() failed!\n"); - Fw::Logger::log("state: %i\n", state); - } + SX1280* radio = &this->m_rlb_radio; - Fw::Logger::log("RESULTING BUFFER:\n"); + state = radio->standby(); + FW_ASSERT(state == RADIOLIB_ERR_NONE); + state = radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); + FW_ASSERT(state == RADIOLIB_ERR_NONE); - char msg[sizeof(buf) * 3 + 1]; + this->wait_for_rx_fin = true; - for (size_t i = 0; i < sizeof(buf); ++i) { - sprintf(msg + i * 3, "%02X ", buf[i]); // NOLINT(runtime/printf) - } - msg[sizeof(buf) * 3] = '\0'; - - Fw::Logger::log("%s\n", msg); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index 609bafdc..fa7f2fb2 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -81,6 +81,7 @@ class MyComponent final : public MyComponentComponentBase { FprimeHal m_rlb_hal; //!< RadioLib HAL instance Module m_rlb_module; //!< RadioLib Module instance SX1280 m_rlb_radio; //!< RadioLib SX1280 radio instance + bool wait_for_rx_fin = false; }; } // namespace Components diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 156f4fae..7f77c99d 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -176,6 +176,7 @@ module ReferenceDeployment { rateGroup1Hz.RateGroupMemberOut[10] -> FileHandling.fileDownlink.Run rateGroup1Hz.RateGroupMemberOut[11] -> startupManager.run rateGroup1Hz.RateGroupMemberOut[12] -> powerMonitor.run + rateGroup1Hz.RateGroupMemberOut[13] -> mycomp.run } From 28e90626291b30341b1d2ceebe48655b722b79d9 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 28 Nov 2025 16:57:19 -0800 Subject: [PATCH 062/101] remove READ_DATA and RESET commands --- .../Components/MyComponent/MyComponent.cpp | 33 ------------------- .../Components/MyComponent/MyComponent.fpp | 6 ---- .../Components/MyComponent/MyComponent.hpp | 14 -------- 3 files changed, 53 deletions(-) diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp index 926b6503..e9b0b0f3 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp @@ -96,31 +96,6 @@ void MyComponent ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } -void MyComponent ::READ_DATA_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - uint8_t buf[256] = {0}; - - int16_t state = this->m_rlb_radio.readData(buf, sizeof(buf)); - if (state == RADIOLIB_ERR_NONE) { - Fw::Logger::log("radio.readData() success!\n"); - } else { - Fw::Logger::log("radio.readData() failed!\n"); - Fw::Logger::log("state: %i\n", state); - } - - Fw::Logger::log("readData() buffer:\n"); - - char msg[sizeof(buf) * 3 + 1]; - - for (size_t i = 0; i < sizeof(buf); ++i) { - sprintf(msg + i * 3, "%02X ", buf[i]); // NOLINT(runtime/printf) - } - msg[sizeof(buf) * 3] = '\0'; - - Fw::Logger::log("%s\n", msg); - - this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); -} - int16_t MyComponent ::configure_radio() { int state = this->m_rlb_radio.begin(); if (state != RADIOLIB_ERR_NONE) { @@ -153,12 +128,4 @@ int16_t MyComponent ::configure_radio() { return state; } -void MyComponent ::RESET_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - // BROKEN - this->resetSend_out(0, Fw::Logic::HIGH); - Os::Task::delay(Fw::TimeInterval(0, 1000)); - this->resetSend_out(0, Fw::Logic::LOW); - this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); -} - } // namespace Components diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp index ccb0f360..6bf07a15 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp @@ -20,12 +20,6 @@ module Components { @ Command to begin receive sync command RECEIVE() - @ Command to read recv data buffer - sync command READ_DATA() - - @ Reset Radio Module - sync command RESET() - @ SPI Output Port output port spiSend: Drv.SpiReadWrite diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp index fa7f2fb2..4301e66a 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp @@ -60,20 +60,6 @@ class MyComponent final : public MyComponentComponentBase { U32 cmdSeq //!< The command sequence number ) override; - //! Handler implementation for command READ_DATA - //! - //! Command to read recv data buffer - void READ_DATA_cmdHandler(FwOpcodeType opCode, //!< The opcode - U32 cmdSeq //!< The command sequence number - ) override; - - //! Handler implementation for command RESET - //! - //! Reset Radio Module - void RESET_cmdHandler(FwOpcodeType opCode, //!< The opcode - U32 cmdSeq //!< The command sequence number - ) override; - // Configure the SX1280 radio (setup and parameter tuning) int16_t configure_radio(); From 4f0769c9abd13d8a49743de5b285d4aa2fec9edc Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Fri, 28 Nov 2025 17:33:23 -0800 Subject: [PATCH 063/101] rename `MyComponent` -> `LoRa2` --- .../Components/CMakeLists.txt | 2 +- .../{MyComponent => LoRa2}/CMakeLists.txt | 10 +++---- .../{MyComponent => LoRa2}/FprimeHal.cpp | 4 +-- .../{MyComponent => LoRa2}/FprimeHal.hpp | 6 ++--- .../MyComponent.cpp => LoRa2/LoRa2.cpp} | 23 +++++++--------- .../MyComponent.fpp => LoRa2/LoRa2.fpp} | 2 +- .../MyComponent.hpp => LoRa2/LoRa2.hpp} | 26 +++++++++---------- .../{MyComponent => LoRa2}/docs/sdd.md | 2 +- .../ReferenceDeployment/Top/instances.fpp | 2 +- 9 files changed, 37 insertions(+), 40 deletions(-) rename FprimeZephyrReference/Components/{MyComponent => LoRa2}/CMakeLists.txt (76%) rename FprimeZephyrReference/Components/{MyComponent => LoRa2}/FprimeHal.cpp (92%) rename FprimeZephyrReference/Components/{MyComponent => LoRa2}/FprimeHal.hpp (91%) rename FprimeZephyrReference/Components/{MyComponent/MyComponent.cpp => LoRa2/LoRa2.cpp} (85%) rename FprimeZephyrReference/Components/{MyComponent/MyComponent.fpp => LoRa2/LoRa2.fpp} (98%) rename FprimeZephyrReference/Components/{MyComponent/MyComponent.hpp => LoRa2/LoRa2.hpp} (76%) rename FprimeZephyrReference/Components/{MyComponent => LoRa2}/docs/sdd.md (96%) diff --git a/FprimeZephyrReference/Components/CMakeLists.txt b/FprimeZephyrReference/Components/CMakeLists.txt index 5f57f6e6..cd1f8254 100644 --- a/FprimeZephyrReference/Components/CMakeLists.txt +++ b/FprimeZephyrReference/Components/CMakeLists.txt @@ -1,7 +1,7 @@ # Include project-wide components here add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/AntennaDeployer/") -add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/MyComponent/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/LoRa2/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/BootloaderTrigger/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Burnwire/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ComDelay/") diff --git a/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt b/FprimeZephyrReference/Components/LoRa2/CMakeLists.txt similarity index 76% rename from FprimeZephyrReference/Components/MyComponent/CMakeLists.txt rename to FprimeZephyrReference/Components/LoRa2/CMakeLists.txt index c0b39406..00dbd36d 100644 --- a/FprimeZephyrReference/Components/MyComponent/CMakeLists.txt +++ b/FprimeZephyrReference/Components/LoRa2/CMakeLists.txt @@ -16,9 +16,9 @@ register_fprime_library( AUTOCODER_INPUTS - "${CMAKE_CURRENT_LIST_DIR}/MyComponent.fpp" + "${CMAKE_CURRENT_LIST_DIR}/LoRa2.fpp" SOURCES - "${CMAKE_CURRENT_LIST_DIR}/MyComponent.cpp" + "${CMAKE_CURRENT_LIST_DIR}/LoRa2.cpp" "${CMAKE_CURRENT_LIST_DIR}/FprimeHal.cpp" DEPENDS RadioLib @@ -29,10 +29,10 @@ add_subdirectory("${FPRIME_PROJECT_ROOT}/lib/RadioLib" "${CMAKE_BINARY_DIR}/Radi ### Unit Tests ### # register_fprime_ut( # AUTOCODER_INPUTS -# "${CMAKE_CURRENT_LIST_DIR}/MyComponent.fpp" +# "${CMAKE_CURRENT_LIST_DIR}/LoRa2.fpp" # SOURCES -# "${CMAKE_CURRENT_LIST_DIR}/test/ut/MyComponentTestMain.cpp" -# "${CMAKE_CURRENT_LIST_DIR}/test/ut/MyComponentTester.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/LoRa2TestMain.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/LoRa2Tester.cpp" # DEPENDS # STest # For rules-based testing # UT_AUTO_HELPERS diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp b/FprimeZephyrReference/Components/LoRa2/FprimeHal.cpp similarity index 92% rename from FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp rename to FprimeZephyrReference/Components/LoRa2/FprimeHal.cpp index d429ff71..5ddcf4b0 100644 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.cpp +++ b/FprimeZephyrReference/Components/LoRa2/FprimeHal.cpp @@ -6,10 +6,10 @@ #include #include -#include "MyComponent.hpp" +#include "LoRa2.hpp" #include -FprimeHal::FprimeHal(Components::MyComponent* component) : RadioLibHal(0, 0, 0, 0, 0, 0), m_component(component) {} +FprimeHal::FprimeHal(Components::LoRa2* component) : RadioLibHal(0, 0, 0, 0, 0, 0), m_component(component) {} void FprimeHal::init() {} diff --git a/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp b/FprimeZephyrReference/Components/LoRa2/FprimeHal.hpp similarity index 91% rename from FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp rename to FprimeZephyrReference/Components/LoRa2/FprimeHal.hpp index 686b5989..a1650a55 100644 --- a/FprimeZephyrReference/Components/MyComponent/FprimeHal.hpp +++ b/FprimeZephyrReference/Components/LoRa2/FprimeHal.hpp @@ -4,14 +4,14 @@ #include namespace Components { -class MyComponent; +class LoRa2; } // namespace Components // the HAL must inherit from the base RadioLibHal class // and implement all of its virtual methods class FprimeHal : public RadioLibHal { public: - explicit FprimeHal(Components::MyComponent* component); + explicit FprimeHal(Components::LoRa2* component); void init() override; @@ -54,7 +54,7 @@ class FprimeHal : public RadioLibHal { void spiEnd(); private: - Components::MyComponent* m_component; + Components::LoRa2* m_component; }; #endif diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp b/FprimeZephyrReference/Components/LoRa2/LoRa2.cpp similarity index 85% rename from FprimeZephyrReference/Components/MyComponent/MyComponent.cpp rename to FprimeZephyrReference/Components/LoRa2/LoRa2.cpp index e9b0b0f3..7fd7878e 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.cpp +++ b/FprimeZephyrReference/Components/LoRa2/LoRa2.cpp @@ -1,12 +1,12 @@ // ====================================================================== -// \title MyComponent.cpp +// \title LoRa2.cpp // \author jrpear -// \brief cpp file for MyComponent component implementation class +// \brief cpp file for LoRa2 component implementation class // ====================================================================== #define RADIOLIB_LOW_LEVEL 1 -#include "FprimeZephyrReference/Components/MyComponent/MyComponent.hpp" +#include "FprimeZephyrReference/Components/LoRa2/LoRa2.hpp" #include @@ -20,19 +20,16 @@ namespace Components { // Component construction and destruction // ---------------------------------------------------------------------- -MyComponent ::MyComponent(const char* const compName) - : MyComponentComponentBase(compName), - m_rlb_hal(this), - m_rlb_module(&m_rlb_hal, 0, 5, 0), - m_rlb_radio(&m_rlb_module) {} +LoRa2 ::LoRa2(const char* const compName) + : LoRa2ComponentBase(compName), m_rlb_hal(this), m_rlb_module(&m_rlb_hal, 0, 5, 0), m_rlb_radio(&m_rlb_module) {} -MyComponent ::~MyComponent() {} +LoRa2 ::~LoRa2() {} // ---------------------------------------------------------------------- // Handler implementations for typed input ports // ---------------------------------------------------------------------- -void MyComponent ::run_handler(FwIndexType portNum, U32 context) { +void LoRa2 ::run_handler(FwIndexType portNum, U32 context) { if (this->wait_for_rx_fin) { uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { @@ -56,7 +53,7 @@ void MyComponent ::run_handler(FwIndexType portNum, U32 context) { // Handler implementations for commands // ---------------------------------------------------------------------- -void MyComponent ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { +void LoRa2 ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { int state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); @@ -78,7 +75,7 @@ void MyComponent ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } -void MyComponent ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { +void LoRa2 ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->rxEnable_out(0, Fw::Logic::HIGH); int16_t state = this->configure_radio(); @@ -96,7 +93,7 @@ void MyComponent ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } -int16_t MyComponent ::configure_radio() { +int16_t LoRa2 ::configure_radio() { int state = this->m_rlb_radio.begin(); if (state != RADIOLIB_ERR_NONE) { return state; diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp b/FprimeZephyrReference/Components/LoRa2/LoRa2.fpp similarity index 98% rename from FprimeZephyrReference/Components/MyComponent/MyComponent.fpp rename to FprimeZephyrReference/Components/LoRa2/LoRa2.fpp index 6bf07a15..77d62c27 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.fpp +++ b/FprimeZephyrReference/Components/LoRa2/LoRa2.fpp @@ -2,7 +2,7 @@ module Components { @ Component for F Prime FSW framework. - passive component MyComponent { + passive component LoRa2 { ############################################################################## #### Uncomment the following examples to start customizing your component #### diff --git a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp b/FprimeZephyrReference/Components/LoRa2/LoRa2.hpp similarity index 76% rename from FprimeZephyrReference/Components/MyComponent/MyComponent.hpp rename to FprimeZephyrReference/Components/LoRa2/LoRa2.hpp index 4301e66a..e141a62d 100644 --- a/FprimeZephyrReference/Components/MyComponent/MyComponent.hpp +++ b/FprimeZephyrReference/Components/LoRa2/LoRa2.hpp @@ -1,33 +1,33 @@ // ====================================================================== -// \title MyComponent.hpp +// \title LoRa2.hpp // \author jrpear -// \brief hpp file for MyComponent component implementation class +// \brief hpp file for LoRa2 component implementation class // ====================================================================== -#ifndef Components_MyComponent_HPP -#define Components_MyComponent_HPP +#ifndef Components_LoRa2_HPP +#define Components_LoRa2_HPP #include "FprimeHal.hpp" -#include "FprimeZephyrReference/Components/MyComponent/MyComponentComponentAc.hpp" +#include "FprimeZephyrReference/Components/LoRa2/LoRa2ComponentAc.hpp" namespace Components { -class MyComponent final : public MyComponentComponentBase { +class LoRa2 final : public LoRa2ComponentBase { public: // ---------------------------------------------------------------------- // Component construction and destruction // ---------------------------------------------------------------------- - //! Construct MyComponent object - MyComponent(const char* const compName //!< The component name + //! Construct LoRa2 object + LoRa2(const char* const compName //!< The component name ); - //! Destroy MyComponent object - ~MyComponent(); + //! Destroy LoRa2 object + ~LoRa2(); - using MyComponentComponentBase::getIRQLine_out; - using MyComponentComponentBase::getTime; - using MyComponentComponentBase::spiSend_out; + using LoRa2ComponentBase::getIRQLine_out; + using LoRa2ComponentBase::getTime; + using LoRa2ComponentBase::spiSend_out; private: // ---------------------------------------------------------------------- diff --git a/FprimeZephyrReference/Components/MyComponent/docs/sdd.md b/FprimeZephyrReference/Components/LoRa2/docs/sdd.md similarity index 96% rename from FprimeZephyrReference/Components/MyComponent/docs/sdd.md rename to FprimeZephyrReference/Components/LoRa2/docs/sdd.md index 2cc9562c..d58f96fe 100644 --- a/FprimeZephyrReference/Components/MyComponent/docs/sdd.md +++ b/FprimeZephyrReference/Components/LoRa2/docs/sdd.md @@ -1,4 +1,4 @@ -# FprimeZephyrReference::MyComponent +# FprimeZephyrReference::LoRa2 Component for F Prime FSW framework. diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index e4f8a47e..35147274 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -95,7 +95,7 @@ module ReferenceDeployment { instance spiDriver: Zephyr.ZephyrSpiDriver base id 0x10040000 - instance mycomp: Components.MyComponent base id 0x10041000 + instance mycomp: Components.LoRa2 base id 0x10041000 instance gpioSbandNrst: Zephyr.ZephyrGpioDriver base id 0x10042000 From 53e8e24049cb92cfdb480934b213fb07cb0c6d1b Mon Sep 17 00:00:00 2001 From: ineskhou Date: Fri, 28 Nov 2025 22:41:38 -0800 Subject: [PATCH 064/101] Updated Names --- .../Components/CMakeLists.txt | 2 +- .../{LoRa2 => SBand}/CMakeLists.txt | 10 +++---- .../Components/{LoRa2 => SBand}/FprimeHal.cpp | 4 +-- .../Components/{LoRa2 => SBand}/FprimeHal.hpp | 6 ++--- .../{LoRa2/LoRa2.cpp => SBand/SBand.cpp} | 20 +++++++------- .../{LoRa2/LoRa2.fpp => SBand/SBand.fpp} | 2 +- .../{LoRa2/LoRa2.hpp => SBand/SBand.hpp} | 26 +++++++++---------- .../Components/{LoRa2 => SBand}/docs/sdd.md | 2 +- .../ReferenceDeployment/Top/instances.fpp | 2 +- 9 files changed, 37 insertions(+), 37 deletions(-) rename FprimeZephyrReference/Components/{LoRa2 => SBand}/CMakeLists.txt (78%) rename FprimeZephyrReference/Components/{LoRa2 => SBand}/FprimeHal.cpp (95%) rename FprimeZephyrReference/Components/{LoRa2 => SBand}/FprimeHal.hpp (92%) rename FprimeZephyrReference/Components/{LoRa2/LoRa2.cpp => SBand/SBand.cpp} (88%) rename FprimeZephyrReference/Components/{LoRa2/LoRa2.fpp => SBand/SBand.fpp} (98%) rename FprimeZephyrReference/Components/{LoRa2/LoRa2.hpp => SBand/SBand.hpp} (79%) rename FprimeZephyrReference/Components/{LoRa2 => SBand}/docs/sdd.md (96%) diff --git a/FprimeZephyrReference/Components/CMakeLists.txt b/FprimeZephyrReference/Components/CMakeLists.txt index cd1f8254..a1db7f21 100644 --- a/FprimeZephyrReference/Components/CMakeLists.txt +++ b/FprimeZephyrReference/Components/CMakeLists.txt @@ -1,7 +1,7 @@ # Include project-wide components here add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/AntennaDeployer/") -add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/LoRa2/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/SBand/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/BootloaderTrigger/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Burnwire/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ComDelay/") diff --git a/FprimeZephyrReference/Components/LoRa2/CMakeLists.txt b/FprimeZephyrReference/Components/SBand/CMakeLists.txt similarity index 78% rename from FprimeZephyrReference/Components/LoRa2/CMakeLists.txt rename to FprimeZephyrReference/Components/SBand/CMakeLists.txt index 00dbd36d..7e725b9d 100644 --- a/FprimeZephyrReference/Components/LoRa2/CMakeLists.txt +++ b/FprimeZephyrReference/Components/SBand/CMakeLists.txt @@ -16,9 +16,9 @@ register_fprime_library( AUTOCODER_INPUTS - "${CMAKE_CURRENT_LIST_DIR}/LoRa2.fpp" + "${CMAKE_CURRENT_LIST_DIR}/SBand.fpp" SOURCES - "${CMAKE_CURRENT_LIST_DIR}/LoRa2.cpp" + "${CMAKE_CURRENT_LIST_DIR}/SBand.cpp" "${CMAKE_CURRENT_LIST_DIR}/FprimeHal.cpp" DEPENDS RadioLib @@ -29,10 +29,10 @@ add_subdirectory("${FPRIME_PROJECT_ROOT}/lib/RadioLib" "${CMAKE_BINARY_DIR}/Radi ### Unit Tests ### # register_fprime_ut( # AUTOCODER_INPUTS -# "${CMAKE_CURRENT_LIST_DIR}/LoRa2.fpp" +# "${CMAKE_CURRENT_LIST_DIR}/SBand.fpp" # SOURCES -# "${CMAKE_CURRENT_LIST_DIR}/test/ut/LoRa2TestMain.cpp" -# "${CMAKE_CURRENT_LIST_DIR}/test/ut/LoRa2Tester.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/SBandTestMain.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/SBandTester.cpp" # DEPENDS # STest # For rules-based testing # UT_AUTO_HELPERS diff --git a/FprimeZephyrReference/Components/LoRa2/FprimeHal.cpp b/FprimeZephyrReference/Components/SBand/FprimeHal.cpp similarity index 95% rename from FprimeZephyrReference/Components/LoRa2/FprimeHal.cpp rename to FprimeZephyrReference/Components/SBand/FprimeHal.cpp index 5ddcf4b0..516e484c 100644 --- a/FprimeZephyrReference/Components/LoRa2/FprimeHal.cpp +++ b/FprimeZephyrReference/Components/SBand/FprimeHal.cpp @@ -6,10 +6,10 @@ #include #include -#include "LoRa2.hpp" +#include "SBand.hpp" #include -FprimeHal::FprimeHal(Components::LoRa2* component) : RadioLibHal(0, 0, 0, 0, 0, 0), m_component(component) {} +FprimeHal::FprimeHal(Components::SBand* component) : RadioLibHal(0, 0, 0, 0, 0, 0), m_component(component) {} void FprimeHal::init() {} diff --git a/FprimeZephyrReference/Components/LoRa2/FprimeHal.hpp b/FprimeZephyrReference/Components/SBand/FprimeHal.hpp similarity index 92% rename from FprimeZephyrReference/Components/LoRa2/FprimeHal.hpp rename to FprimeZephyrReference/Components/SBand/FprimeHal.hpp index a1650a55..a071e485 100644 --- a/FprimeZephyrReference/Components/LoRa2/FprimeHal.hpp +++ b/FprimeZephyrReference/Components/SBand/FprimeHal.hpp @@ -4,14 +4,14 @@ #include namespace Components { -class LoRa2; +class SBand; } // namespace Components // the HAL must inherit from the base RadioLibHal class // and implement all of its virtual methods class FprimeHal : public RadioLibHal { public: - explicit FprimeHal(Components::LoRa2* component); + explicit FprimeHal(Components::SBand* component); void init() override; @@ -54,7 +54,7 @@ class FprimeHal : public RadioLibHal { void spiEnd(); private: - Components::LoRa2* m_component; + Components::SBand* m_component; }; #endif diff --git a/FprimeZephyrReference/Components/LoRa2/LoRa2.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp similarity index 88% rename from FprimeZephyrReference/Components/LoRa2/LoRa2.cpp rename to FprimeZephyrReference/Components/SBand/SBand.cpp index 7fd7878e..3abbdf3c 100644 --- a/FprimeZephyrReference/Components/LoRa2/LoRa2.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -1,12 +1,12 @@ // ====================================================================== -// \title LoRa2.cpp +// \title SBand.cpp // \author jrpear -// \brief cpp file for LoRa2 component implementation class +// \brief cpp file for SBand component implementation class // ====================================================================== #define RADIOLIB_LOW_LEVEL 1 -#include "FprimeZephyrReference/Components/LoRa2/LoRa2.hpp" +#include "FprimeZephyrReference/Components/SBand/SBand.hpp" #include @@ -20,16 +20,16 @@ namespace Components { // Component construction and destruction // ---------------------------------------------------------------------- -LoRa2 ::LoRa2(const char* const compName) - : LoRa2ComponentBase(compName), m_rlb_hal(this), m_rlb_module(&m_rlb_hal, 0, 5, 0), m_rlb_radio(&m_rlb_module) {} +SBand ::SBand(const char* const compName) + : SBandComponentBase(compName), m_rlb_hal(this), m_rlb_module(&m_rlb_hal, 0, 5, 0), m_rlb_radio(&m_rlb_module) {} -LoRa2 ::~LoRa2() {} +SBand ::~SBand() {} // ---------------------------------------------------------------------- // Handler implementations for typed input ports // ---------------------------------------------------------------------- -void LoRa2 ::run_handler(FwIndexType portNum, U32 context) { +void SBand ::run_handler(FwIndexType portNum, U32 context) { if (this->wait_for_rx_fin) { uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { @@ -53,7 +53,7 @@ void LoRa2 ::run_handler(FwIndexType portNum, U32 context) { // Handler implementations for commands // ---------------------------------------------------------------------- -void LoRa2 ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { +void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { int state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); @@ -75,7 +75,7 @@ void LoRa2 ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } -void LoRa2 ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { +void SBand ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->rxEnable_out(0, Fw::Logic::HIGH); int16_t state = this->configure_radio(); @@ -93,7 +93,7 @@ void LoRa2 ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } -int16_t LoRa2 ::configure_radio() { +int16_t SBand ::configure_radio() { int state = this->m_rlb_radio.begin(); if (state != RADIOLIB_ERR_NONE) { return state; diff --git a/FprimeZephyrReference/Components/LoRa2/LoRa2.fpp b/FprimeZephyrReference/Components/SBand/SBand.fpp similarity index 98% rename from FprimeZephyrReference/Components/LoRa2/LoRa2.fpp rename to FprimeZephyrReference/Components/SBand/SBand.fpp index 77d62c27..373808ef 100644 --- a/FprimeZephyrReference/Components/LoRa2/LoRa2.fpp +++ b/FprimeZephyrReference/Components/SBand/SBand.fpp @@ -2,7 +2,7 @@ module Components { @ Component for F Prime FSW framework. - passive component LoRa2 { + passive component SBand { ############################################################################## #### Uncomment the following examples to start customizing your component #### diff --git a/FprimeZephyrReference/Components/LoRa2/LoRa2.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp similarity index 79% rename from FprimeZephyrReference/Components/LoRa2/LoRa2.hpp rename to FprimeZephyrReference/Components/SBand/SBand.hpp index e141a62d..2bfc92f0 100644 --- a/FprimeZephyrReference/Components/LoRa2/LoRa2.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -1,33 +1,33 @@ // ====================================================================== -// \title LoRa2.hpp +// \title SBand.hpp // \author jrpear -// \brief hpp file for LoRa2 component implementation class +// \brief hpp file for SBand component implementation class // ====================================================================== -#ifndef Components_LoRa2_HPP -#define Components_LoRa2_HPP +#ifndef Components_SBand_HPP +#define Components_SBand_HPP #include "FprimeHal.hpp" -#include "FprimeZephyrReference/Components/LoRa2/LoRa2ComponentAc.hpp" +#include "FprimeZephyrReference/Components/SBand/SBandComponentAc.hpp" namespace Components { -class LoRa2 final : public LoRa2ComponentBase { +class SBand final : public SBandComponentBase { public: // ---------------------------------------------------------------------- // Component construction and destruction // ---------------------------------------------------------------------- - //! Construct LoRa2 object - LoRa2(const char* const compName //!< The component name + //! Construct SBand object + SBand(const char* const compName //!< The component name ); - //! Destroy LoRa2 object - ~LoRa2(); + //! Destroy SBand object + ~SBand(); - using LoRa2ComponentBase::getIRQLine_out; - using LoRa2ComponentBase::getTime; - using LoRa2ComponentBase::spiSend_out; + using SBandComponentBase::getIRQLine_out; + using SBandComponentBase::getTime; + using SBandComponentBase::spiSend_out; private: // ---------------------------------------------------------------------- diff --git a/FprimeZephyrReference/Components/LoRa2/docs/sdd.md b/FprimeZephyrReference/Components/SBand/docs/sdd.md similarity index 96% rename from FprimeZephyrReference/Components/LoRa2/docs/sdd.md rename to FprimeZephyrReference/Components/SBand/docs/sdd.md index d58f96fe..86af828c 100644 --- a/FprimeZephyrReference/Components/LoRa2/docs/sdd.md +++ b/FprimeZephyrReference/Components/SBand/docs/sdd.md @@ -1,4 +1,4 @@ -# FprimeZephyrReference::LoRa2 +# FprimeZephyrReference::SBand Component for F Prime FSW framework. diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 35147274..12110afb 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -95,7 +95,7 @@ module ReferenceDeployment { instance spiDriver: Zephyr.ZephyrSpiDriver base id 0x10040000 - instance mycomp: Components.LoRa2 base id 0x10041000 + instance mycomp: Components.SBand base id 0x10041000 instance gpioSbandNrst: Zephyr.ZephyrGpioDriver base id 0x10042000 From 688c16af06f4c438a9e1e043e5a99abb8a304f9d Mon Sep 17 00:00:00 2001 From: ineskhou Date: Fri, 28 Nov 2025 22:59:44 -0800 Subject: [PATCH 065/101] topology naming --- .../ReferenceDeployment/Top/instances.fpp | 2 +- .../ReferenceDeployment/Top/topology.fpp | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 12110afb..6b49bb3a 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -95,7 +95,7 @@ module ReferenceDeployment { instance spiDriver: Zephyr.ZephyrSpiDriver base id 0x10040000 - instance mycomp: Components.SBand base id 0x10041000 + instance sband : Components.SBand base id 0x10041000 instance gpioSbandNrst: Zephyr.ZephyrGpioDriver base id 0x10042000 diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 7f77c99d..eeb072aa 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -52,7 +52,7 @@ module ReferenceDeployment { # For UART sideband communication instance comDriver instance spiDriver - instance mycomp + instance sband instance gpioSbandNrst instance gpioSbandRxEn instance gpioSbandTxEn @@ -176,7 +176,7 @@ module ReferenceDeployment { rateGroup1Hz.RateGroupMemberOut[10] -> FileHandling.fileDownlink.Run rateGroup1Hz.RateGroupMemberOut[11] -> startupManager.run rateGroup1Hz.RateGroupMemberOut[12] -> powerMonitor.run - rateGroup1Hz.RateGroupMemberOut[13] -> mycomp.run + rateGroup1Hz.RateGroupMemberOut[13] -> sband.run } @@ -214,11 +214,11 @@ module ReferenceDeployment { } connections MyConnectionGraph { - mycomp.spiSend -> spiDriver.SpiReadWrite - mycomp.resetSend -> gpioSbandNrst.gpioWrite - mycomp.txEnable -> gpioSbandTxEn.gpioWrite - mycomp.rxEnable -> gpioSbandRxEn.gpioWrite - mycomp.getIRQLine -> gpioSbandIRQ.gpioRead + sband.spiSend -> spiDriver.SpiReadWrite + sband.resetSend -> gpioSbandNrst.gpioWrite + sband.txEnable -> gpioSbandTxEn.gpioWrite + sband.rxEnable -> gpioSbandRxEn.gpioWrite + sband.getIRQLine -> gpioSbandIRQ.gpioRead } connections ComCcsds_FileHandling { From 11b360e97897b26a1bb5177f00a2cd2f56698256 Mon Sep 17 00:00:00 2001 From: ineskhou Date: Fri, 28 Nov 2025 23:17:22 -0800 Subject: [PATCH 066/101] adding the CommCcds for the Sband --- .../ComCcsdsSband/CMakeLists.txt | 12 ++ .../ComCcsdsSband/ComCcsdsSband.fpp | 203 ++++++++++++++++++ .../ComCcsdsSband/PingEntries.hpp | 7 + .../ComCcsdsSband/SubtopologyTopologyDefs.hpp | 21 ++ .../ComCcsdsSband/docs/sdd.md | 5 + 5 files changed, 248 insertions(+) create mode 100644 FprimeZephyrReference/ComCcsdsSband/CMakeLists.txt create mode 100644 FprimeZephyrReference/ComCcsdsSband/ComCcsdsSband.fpp create mode 100644 FprimeZephyrReference/ComCcsdsSband/PingEntries.hpp create mode 100644 FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp create mode 100644 FprimeZephyrReference/ComCcsdsSband/docs/sdd.md diff --git a/FprimeZephyrReference/ComCcsdsSband/CMakeLists.txt b/FprimeZephyrReference/ComCcsdsSband/CMakeLists.txt new file mode 100644 index 00000000..a95400b6 --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsSband/CMakeLists.txt @@ -0,0 +1,12 @@ + +register_fprime_module( + EXCLUDE_FROM_ALL + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/ComCcsds.fpp" + HEADERS + "${CMAKE_CURRENT_LIST_DIR}/SubtopologyTopologyDefs.hpp" + "${CMAKE_CURRENT_LIST_DIR}/PingEntries.hpp" + DEPENDS + Svc_Subtopologies_ComCcsds_ComCcsdsConfig + INTERFACE +) diff --git a/FprimeZephyrReference/ComCcsdsSband/ComCcsdsSband.fpp b/FprimeZephyrReference/ComCcsdsSband/ComCcsdsSband.fpp new file mode 100644 index 00000000..293f6309 --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsSband/ComCcsdsSband.fpp @@ -0,0 +1,203 @@ +module ComCcsdsUart { + + # ComPacket Queue enum for queue types + enum Ports_ComPacketQueue { + EVENTS, + TELEMETRY + } + + enum Ports_ComBufferQueue { + FILE + } + + # ---------------------------------------------------------------------- + # Active Components + # ---------------------------------------------------------------------- + instance comQueue: Svc.ComQueue base id ComCcsdsConfig.BASE_ID_UART + 0x00000 \ + queue size ComCcsdsConfig.QueueSizes.comQueue \ + stack size ComCcsdsConfig.StackSizes.comQueue \ + priority ComCcsdsConfig.Priorities.comQueue \ + { + phase Fpp.ToCpp.Phases.configComponents """ + using namespace ComCcsdsUart; + Svc::ComQueue::QueueConfigurationTable configurationTableUart; + + // Events (highest-priority) + configurationTableUart.entries[Ports_ComPacketQueue::EVENTS].depth = ComCcsdsConfig::QueueDepths::events; + configurationTableUart.entries[Ports_ComPacketQueue::EVENTS].priority = ComCcsdsConfig::QueuePriorities::events; + + // Telemetry + configurationTableUart.entries[Ports_ComPacketQueue::TELEMETRY].depth = ComCcsdsConfig::QueueDepths::tlm; + configurationTableUart.entries[Ports_ComPacketQueue::TELEMETRY].priority = ComCcsdsConfig::QueuePriorities::tlm; + + // File Downlink Queue (buffer queue using NUM_CONSTANTS offset) + configurationTableUart.entries[Ports_ComPacketQueue::NUM_CONSTANTS + Ports_ComBufferQueue::FILE].depth = ComCcsdsConfig::QueueDepths::file; + configurationTableUart.entries[Ports_ComPacketQueue::NUM_CONSTANTS + Ports_ComBufferQueue::FILE].priority = ComCcsdsConfig::QueuePriorities::file; + + // Allocation identifier is 0 as the MallocAllocator discards it + ComCcsdsUart::comQueue.configure(configurationTableUart, 0, ComCcsds::Allocation::memAllocator); + """ + phase Fpp.ToCpp.Phases.tearDownComponents """ + ComCcsdsUart::comQueue.cleanup(); + """ + } + + instance aggregator: Svc.ComAggregator base id ComCcsdsConfig.BASE_ID_UART + 0x06000 \ + queue size ComCcsdsConfig.QueueSizes.aggregator \ + stack size ComCcsdsConfig.StackSizes.aggregator + + # ---------------------------------------------------------------------- + # Passive Components + # ---------------------------------------------------------------------- + instance frameAccumulator: Svc.FrameAccumulator base id ComCcsdsConfig.BASE_ID_UART + 0x01000 \ + { + + phase Fpp.ToCpp.Phases.configObjects """ + Svc::FrameDetectors::CcsdsTcFrameDetector frameDetector; + """ + phase Fpp.ToCpp.Phases.configComponents """ + ComCcsdsUart::frameAccumulator.configure( + ConfigObjects::ComCcsdsUart_frameAccumulator::frameDetector, + 1, + ComCcsds::Allocation::memAllocator, + ComCcsdsConfig::BuffMgr::frameAccumulatorSize + ); + """ + + phase Fpp.ToCpp.Phases.tearDownComponents """ + ComCcsdsUart::frameAccumulator.cleanup(); + """ + } + + instance commsBufferManager: Svc.BufferManager base id ComCcsdsConfig.BASE_ID_UART + 0x02000 \ + { + phase Fpp.ToCpp.Phases.configObjects """ + Svc::BufferManager::BufferBins bins; + """ + + phase Fpp.ToCpp.Phases.configComponents """ + memset(&ConfigObjects::ComCcsdsUart_commsBufferManager::bins, 0, sizeof(ConfigObjects::ComCcsdsUart_commsBufferManager::bins)); + ConfigObjects::ComCcsdsUart_commsBufferManager::bins.bins[0].bufferSize = ComCcsdsConfig::BuffMgr::commsBuffSize; + ConfigObjects::ComCcsdsUart_commsBufferManager::bins.bins[0].numBuffers = ComCcsdsConfig::BuffMgr::commsBuffCount; + ConfigObjects::ComCcsdsUart_commsBufferManager::bins.bins[1].bufferSize = ComCcsdsConfig::BuffMgr::commsFileBuffSize; + ConfigObjects::ComCcsdsUart_commsBufferManager::bins.bins[1].numBuffers = ComCcsdsConfig::BuffMgr::commsFileBuffCount; + ComCcsdsUart::commsBufferManager.setup( + ComCcsdsConfig::BuffMgr::commsBuffMgrId, + 0, + ComCcsds::Allocation::memAllocator, + ConfigObjects::ComCcsdsUart_commsBufferManager::bins + ); + """ + + phase Fpp.ToCpp.Phases.tearDownComponents """ + ComCcsdsUart::commsBufferManager.cleanup(); + """ + } + + instance fprimeRouter: Svc.FprimeRouter base id ComCcsdsConfig.BASE_ID_UART + 0x03000 + + instance tcDeframer: Svc.Ccsds.TcDeframer base id ComCcsdsConfig.BASE_ID_UART + 0x04000 + + instance spacePacketDeframer: Svc.Ccsds.SpacePacketDeframer base id ComCcsdsConfig.BASE_ID_UART + 0x05000 + + # NOTE: name 'framer' is used for the framer that connects to the Com Adapter Interface for better subtopology interoperability + instance framer: Svc.Ccsds.TmFramer base id ComCcsdsConfig.BASE_ID_UART + 0x07000 + + instance spacePacketFramer: Svc.Ccsds.SpacePacketFramer base id ComCcsdsConfig.BASE_ID_UART + 0x08000 + + instance apidManager: Svc.Ccsds.ApidManager base id ComCcsdsConfig.BASE_ID_UART + 0x09000 + + instance comStub: Svc.ComStub base id ComCcsdsConfig.BASE_ID_UART + 0x0A000 + + topology FramingSubtopology { + # Usage Note: + # + # When importing this subtopology, users shall establish 5 port connections with a component implementing + # the Svc.Com (Svc/Interfaces/Com.fpp) interface. They are as follows: + # + # 1) Outputs: + # - ComCcsds.framer.dataOut -> [Svc.Com].dataIn + # - ComCcsds.frameAccumulator.dataReturnOut -> [Svc.Com].dataReturnIn + # 2) Inputs: + # - [Svc.Com].dataReturnOut -> ComCcsds.framer.dataReturnIn + # - [Svc.Com].comStatusOut -> ComCcsds.framer.comStatusIn + # - [Svc.Com].dataOut -> ComCcsds.frameAccumulator.dataIn + + + # Active Components + instance comQueue + + # Passive Components + instance commsBufferManager + instance frameAccumulator + instance fprimeRouter + instance tcDeframer + instance spacePacketDeframer + instance framer + instance spacePacketFramer + instance apidManager + instance aggregator + + connections Downlink { + # ComQueue <-> SpacePacketFramer + comQueue.dataOut -> spacePacketFramer.dataIn + spacePacketFramer.dataReturnOut -> comQueue.dataReturnIn + # SpacePacketFramer buffer and APID management + spacePacketFramer.bufferAllocate -> commsBufferManager.bufferGetCallee + spacePacketFramer.bufferDeallocate -> commsBufferManager.bufferSendIn + spacePacketFramer.getApidSeqCount -> apidManager.getApidSeqCountIn + # SpacePacketFramer <-> TmFramer + spacePacketFramer.dataOut -> aggregator.dataIn + aggregator.dataOut -> framer.dataIn + + framer.dataReturnOut -> aggregator.dataReturnIn + aggregator.dataReturnOut -> spacePacketFramer.dataReturnIn + + # ComStatus + framer.comStatusOut -> aggregator.comStatusIn + aggregator.comStatusOut -> spacePacketFramer.comStatusIn + spacePacketFramer.comStatusOut -> comQueue.comStatusIn + # (Outgoing) Framer <-> ComInterface connections shall be established by the user + } + + connections Uplink { + # (Incoming) ComInterface <-> FrameAccumulator connections shall be established by the user + # FrameAccumulator buffer allocations + frameAccumulator.bufferDeallocate -> commsBufferManager.bufferSendIn + frameAccumulator.bufferAllocate -> commsBufferManager.bufferGetCallee + # FrameAccumulator <-> TcDeframer + frameAccumulator.dataOut -> tcDeframer.dataIn + tcDeframer.dataReturnOut -> frameAccumulator.dataReturnIn + # TcDeframer <-> SpacePacketDeframer + tcDeframer.dataOut -> spacePacketDeframer.dataIn + spacePacketDeframer.dataReturnOut -> tcDeframer.dataReturnIn + # SpacePacketDeframer APID validation + spacePacketDeframer.validateApidSeqCount -> apidManager.validateApidSeqCountIn + # SpacePacketDeframer <-> Router + spacePacketDeframer.dataOut -> fprimeRouter.dataIn + fprimeRouter.dataReturnOut -> spacePacketDeframer.dataReturnIn + # Router buffer allocations + fprimeRouter.bufferAllocate -> commsBufferManager.bufferGetCallee + fprimeRouter.bufferDeallocate -> commsBufferManager.bufferSendIn + } + } # end FramingSubtopology + + # This subtopology uses FramingSubtopology with a ComStub component for Com Interface + topology Subtopology { + import FramingSubtopology + + instance comStub + + connections ComStub { + # Framer <-> ComStub (Downlink) + framer.dataOut -> comStub.dataIn + comStub.dataReturnOut -> framer.dataReturnIn + comStub.comStatusOut -> framer.comStatusIn + + # ComStub <-> FrameAccumulator (Uplink) + comStub.dataOut -> frameAccumulator.dataIn + frameAccumulator.dataReturnOut -> comStub.dataReturnIn + } + } # end Subtopology + +} # end ComCcsds diff --git a/FprimeZephyrReference/ComCcsdsSband/PingEntries.hpp b/FprimeZephyrReference/ComCcsdsSband/PingEntries.hpp new file mode 100644 index 00000000..dd033856 --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsSband/PingEntries.hpp @@ -0,0 +1,7 @@ +#ifndef COMCCSDS_SBAND_PINGENTRIES_HPP +#define COMCCSDS_SBAND_PINGENTRIES_HPP +namespace PingEntries { +// No ping-enabled components in ComCcsds S-band subtopology +} + +#endif diff --git a/FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp b/FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp new file mode 100644 index 00000000..daa5e746 --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp @@ -0,0 +1,21 @@ +#ifndef COMCCSDSSUBTOPOLOGY_DEFS_HPP +#define COMCCSDSSUBTOPOLOGY_DEFS_HPP + +#include +#include +#include + +#include "ComCcsdsConfig/ComCcsdsSubtopologyConfig.hpp" +#include "Svc/Subtopologies/ComCcsds/ComCcsdsConfig/FppConstantsAc.hpp" + +namespace ComCcsds { +struct SubtopologyState { + // Empty - no external state needed for ComCcsds subtopology +}; + +struct TopologyState { + SubtopologyState comCcsds; +}; +} // namespace ComCcsds + +#endif diff --git a/FprimeZephyrReference/ComCcsdsSband/docs/sdd.md b/FprimeZephyrReference/ComCcsdsSband/docs/sdd.md new file mode 100644 index 00000000..9ebc9bef --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsSband/docs/sdd.md @@ -0,0 +1,5 @@ +# ComCcsdsUart + +This is a clone with a renamed module of the F Prime's Svc::ComCcsds. + +See: [Svc::ComCcsds](../../../lib/fprime/Svc/Subtopologies/ComCcsds/docs/sdd.md) From 43c906f2fb6fa6f80268dc50720583eb7b640124 Mon Sep 17 00:00:00 2001 From: ineskhou Date: Fri, 28 Nov 2025 23:18:29 -0800 Subject: [PATCH 067/101] name change --- FprimeZephyrReference/ComCcsdsSband/docs/sdd.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FprimeZephyrReference/ComCcsdsSband/docs/sdd.md b/FprimeZephyrReference/ComCcsdsSband/docs/sdd.md index 9ebc9bef..9e000e31 100644 --- a/FprimeZephyrReference/ComCcsdsSband/docs/sdd.md +++ b/FprimeZephyrReference/ComCcsdsSband/docs/sdd.md @@ -1,4 +1,4 @@ -# ComCcsdsUart +# ComCcsdsSband This is a clone with a renamed module of the F Prime's Svc::ComCcsds. From 910c46c75d800d3328c844a8ca93786c1cb15fd9 Mon Sep 17 00:00:00 2001 From: ineskhou Date: Sat, 29 Nov 2025 01:01:13 -0800 Subject: [PATCH 068/101] updated the configuration --- FprimeZephyrReference/project/config/AcConstants.fpp | 2 +- .../project/config/ComCcsdsConfig.fpp | 1 + .../ComCcsdsConfig/CMakeLists.txt\342\200\216" | 12 ++++++++++++ .../ComCcsdsConfig/ComCcsdsSubtopologyConfig.cpp | 9 +++++++++ .../ComCcsdsConfig/ComCcsdsSubtopologyConfig.hpp | 12 ++++++++++++ 5 files changed, 35 insertions(+), 1 deletion(-) create mode 100644 "FprimeZephyrReference/project/config/ComCcsdsConfig/CMakeLists.txt\342\200\216" create mode 100644 FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.cpp create mode 100644 FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.hpp diff --git a/FprimeZephyrReference/project/config/AcConstants.fpp b/FprimeZephyrReference/project/config/AcConstants.fpp index 8eeefb4b..c88be742 100644 --- a/FprimeZephyrReference/project/config/AcConstants.fpp +++ b/FprimeZephyrReference/project/config/AcConstants.fpp @@ -13,7 +13,7 @@ constant PassiveRateGroupOutputPorts = 10 constant RateGroupDriverRateGroupPorts = 3 @ Used for command and registration ports -constant CmdDispatcherComponentCommandPorts = 30 +constant CmdDispatcherComponentCommandPorts = 35 @ Used for uplink/sequencer buffer/response ports constant CmdDispatcherSequencePorts = 5 diff --git a/FprimeZephyrReference/project/config/ComCcsdsConfig.fpp b/FprimeZephyrReference/project/config/ComCcsdsConfig.fpp index c39d3778..829ab0b3 100644 --- a/FprimeZephyrReference/project/config/ComCcsdsConfig.fpp +++ b/FprimeZephyrReference/project/config/ComCcsdsConfig.fpp @@ -2,6 +2,7 @@ module ComCcsdsConfig { #Base ID for the ComCcsds Subtopology, all components are offsets from this base ID constant BASE_ID = 0x02000000 constant BASE_ID_UART = 0x21000000 + constant BASE_ID_SBAND = 0x23000000 module QueueSizes { constant comQueue = 5 diff --git "a/FprimeZephyrReference/project/config/ComCcsdsConfig/CMakeLists.txt\342\200\216" "b/FprimeZephyrReference/project/config/ComCcsdsConfig/CMakeLists.txt\342\200\216" new file mode 100644 index 00000000..aad62bfe --- /dev/null +++ "b/FprimeZephyrReference/project/config/ComCcsdsConfig/CMakeLists.txt\342\200\216" @@ -0,0 +1,12 @@ +register_fprime_module( + EXCLUDE_FROM_ALL + SOURCES + "${CMAKE_CURRENT_LIST_DIR}/ComCcsdsSubtopologyConfig.cpp" + HEADERS + "${CMAKE_CURRENT_LIST_DIR}/ComCcsdsSubtopologyConfig.hpp" + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/../ComCcsdsConfig.fpp" + DEPENDS + Fw_Types + INTERFACE +) diff --git a/FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.cpp b/FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.cpp new file mode 100644 index 00000000..c94d1689 --- /dev/null +++ b/FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.cpp @@ -0,0 +1,9 @@ +#include "ComCcsdsSubtopologyConfig.hpp" + +namespace ComCcsdsSband { +namespace Allocation { +// This instance can be changed to use a different allocator in the ComCcsdsSband Subtopology +Fw::MallocAllocator mallocatorInstance; +Fw::MemAllocator& memAllocator = mallocatorInstance; +} // namespace Allocation +} // namespace ComCcsdsSband diff --git a/FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.hpp b/FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.hpp new file mode 100644 index 00000000..e249853d --- /dev/null +++ b/FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.hpp @@ -0,0 +1,12 @@ +#ifndef COMCCSDSSUBTOPOLOGY_CONFIG_HPP +#define COMCCSDSSUBTOPOLOGY_CONFIG_HPP + +#include "Fw/Types/MallocAllocator.hpp" + +namespace ComCcsdsSband { +namespace Allocation { +extern Fw::MemAllocator& memAllocator; +} +} // namespace ComCcsdsSband + +#endif From ce62336a5add96de8d4f1f6957288c4f1d573799 Mon Sep 17 00:00:00 2001 From: ineskhou Date: Sat, 29 Nov 2025 02:51:13 -0800 Subject: [PATCH 069/101] added topology connections and ports to SBand --- FprimeZephyrReference/CMakeLists.txt | 1 + .../ComCcsdsSband/CMakeLists.txt | 2 +- .../ComCcsdsSband/ComCcsdsSband.fpp | 68 +++++++++---------- .../ComCcsdsSband/SubtopologyTopologyDefs.hpp | 12 ++-- .../Components/SBand/SBand.cpp | 28 ++++++++ .../Components/SBand/SBand.fpp | 7 +- .../Components/SBand/SBand.hpp | 14 ++++ .../Top/ReferenceDeploymentPackets.fppi | 7 ++ .../Top/ReferenceDeploymentTopologyDefs.hpp | 27 ++++---- .../ReferenceDeployment/Top/topology.fpp | 35 +++++++++- .../project/config/AcConstants.fpp | 2 +- 11 files changed, 146 insertions(+), 57 deletions(-) diff --git a/FprimeZephyrReference/CMakeLists.txt b/FprimeZephyrReference/CMakeLists.txt index 95ee3b94..b9d1d0f2 100644 --- a/FprimeZephyrReference/CMakeLists.txt +++ b/FprimeZephyrReference/CMakeLists.txt @@ -5,4 +5,5 @@ add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/project/config") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Components") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ComCcsdsUart/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ComCcsdsSband/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ReferenceDeployment/") diff --git a/FprimeZephyrReference/ComCcsdsSband/CMakeLists.txt b/FprimeZephyrReference/ComCcsdsSband/CMakeLists.txt index a95400b6..686067e9 100644 --- a/FprimeZephyrReference/ComCcsdsSband/CMakeLists.txt +++ b/FprimeZephyrReference/ComCcsdsSband/CMakeLists.txt @@ -2,7 +2,7 @@ register_fprime_module( EXCLUDE_FROM_ALL AUTOCODER_INPUTS - "${CMAKE_CURRENT_LIST_DIR}/ComCcsds.fpp" + "${CMAKE_CURRENT_LIST_DIR}/ComCcsdsSband.fpp" HEADERS "${CMAKE_CURRENT_LIST_DIR}/SubtopologyTopologyDefs.hpp" "${CMAKE_CURRENT_LIST_DIR}/PingEntries.hpp" diff --git a/FprimeZephyrReference/ComCcsdsSband/ComCcsdsSband.fpp b/FprimeZephyrReference/ComCcsdsSband/ComCcsdsSband.fpp index 293f6309..6c1d8704 100644 --- a/FprimeZephyrReference/ComCcsdsSband/ComCcsdsSband.fpp +++ b/FprimeZephyrReference/ComCcsdsSband/ComCcsdsSband.fpp @@ -1,4 +1,4 @@ -module ComCcsdsUart { +module ComCcsdsSband { # ComPacket Queue enum for queue types enum Ports_ComPacketQueue { @@ -13,51 +13,51 @@ module ComCcsdsUart { # ---------------------------------------------------------------------- # Active Components # ---------------------------------------------------------------------- - instance comQueue: Svc.ComQueue base id ComCcsdsConfig.BASE_ID_UART + 0x00000 \ + instance comQueue: Svc.ComQueue base id ComCcsdsConfig.BASE_ID_SBAND + 0x00000 \ queue size ComCcsdsConfig.QueueSizes.comQueue \ stack size ComCcsdsConfig.StackSizes.comQueue \ priority ComCcsdsConfig.Priorities.comQueue \ { phase Fpp.ToCpp.Phases.configComponents """ - using namespace ComCcsdsUart; - Svc::ComQueue::QueueConfigurationTable configurationTableUart; + using namespace ComCcsdsSband; + Svc::ComQueue::QueueConfigurationTable configurationTableSband; // Events (highest-priority) - configurationTableUart.entries[Ports_ComPacketQueue::EVENTS].depth = ComCcsdsConfig::QueueDepths::events; - configurationTableUart.entries[Ports_ComPacketQueue::EVENTS].priority = ComCcsdsConfig::QueuePriorities::events; + configurationTableSband.entries[Ports_ComPacketQueue::EVENTS].depth = ComCcsdsConfig::QueueDepths::events; + configurationTableSband.entries[Ports_ComPacketQueue::EVENTS].priority = ComCcsdsConfig::QueuePriorities::events; // Telemetry - configurationTableUart.entries[Ports_ComPacketQueue::TELEMETRY].depth = ComCcsdsConfig::QueueDepths::tlm; - configurationTableUart.entries[Ports_ComPacketQueue::TELEMETRY].priority = ComCcsdsConfig::QueuePriorities::tlm; + configurationTableSband.entries[Ports_ComPacketQueue::TELEMETRY].depth = ComCcsdsConfig::QueueDepths::tlm; + configurationTableSband.entries[Ports_ComPacketQueue::TELEMETRY].priority = ComCcsdsConfig::QueuePriorities::tlm; // File Downlink Queue (buffer queue using NUM_CONSTANTS offset) - configurationTableUart.entries[Ports_ComPacketQueue::NUM_CONSTANTS + Ports_ComBufferQueue::FILE].depth = ComCcsdsConfig::QueueDepths::file; - configurationTableUart.entries[Ports_ComPacketQueue::NUM_CONSTANTS + Ports_ComBufferQueue::FILE].priority = ComCcsdsConfig::QueuePriorities::file; + configurationTableSband.entries[Ports_ComPacketQueue::NUM_CONSTANTS + Ports_ComBufferQueue::FILE].depth = ComCcsdsConfig::QueueDepths::file; + configurationTableSband.entries[Ports_ComPacketQueue::NUM_CONSTANTS + Ports_ComBufferQueue::FILE].priority = ComCcsdsConfig::QueuePriorities::file; // Allocation identifier is 0 as the MallocAllocator discards it - ComCcsdsUart::comQueue.configure(configurationTableUart, 0, ComCcsds::Allocation::memAllocator); + ComCcsdsSband::comQueue.configure(configurationTableSband, 0, ComCcsds::Allocation::memAllocator); """ phase Fpp.ToCpp.Phases.tearDownComponents """ - ComCcsdsUart::comQueue.cleanup(); + ComCcsdsSband::comQueue.cleanup(); """ } - instance aggregator: Svc.ComAggregator base id ComCcsdsConfig.BASE_ID_UART + 0x06000 \ + instance aggregator: Svc.ComAggregator base id ComCcsdsConfig.BASE_ID_SBAND + 0x06000 \ queue size ComCcsdsConfig.QueueSizes.aggregator \ stack size ComCcsdsConfig.StackSizes.aggregator # ---------------------------------------------------------------------- # Passive Components # ---------------------------------------------------------------------- - instance frameAccumulator: Svc.FrameAccumulator base id ComCcsdsConfig.BASE_ID_UART + 0x01000 \ + instance frameAccumulator: Svc.FrameAccumulator base id ComCcsdsConfig.BASE_ID_SBAND + 0x01000 \ { phase Fpp.ToCpp.Phases.configObjects """ Svc::FrameDetectors::CcsdsTcFrameDetector frameDetector; """ phase Fpp.ToCpp.Phases.configComponents """ - ComCcsdsUart::frameAccumulator.configure( - ConfigObjects::ComCcsdsUart_frameAccumulator::frameDetector, + ComCcsdsSband::frameAccumulator.configure( + ConfigObjects::ComCcsdsSband_frameAccumulator::frameDetector, 1, ComCcsds::Allocation::memAllocator, ComCcsdsConfig::BuffMgr::frameAccumulatorSize @@ -65,49 +65,49 @@ module ComCcsdsUart { """ phase Fpp.ToCpp.Phases.tearDownComponents """ - ComCcsdsUart::frameAccumulator.cleanup(); + ComCcsdsSband::frameAccumulator.cleanup(); """ } - instance commsBufferManager: Svc.BufferManager base id ComCcsdsConfig.BASE_ID_UART + 0x02000 \ + instance commsBufferManager: Svc.BufferManager base id ComCcsdsConfig.BASE_ID_SBAND + 0x02000 \ { phase Fpp.ToCpp.Phases.configObjects """ Svc::BufferManager::BufferBins bins; """ phase Fpp.ToCpp.Phases.configComponents """ - memset(&ConfigObjects::ComCcsdsUart_commsBufferManager::bins, 0, sizeof(ConfigObjects::ComCcsdsUart_commsBufferManager::bins)); - ConfigObjects::ComCcsdsUart_commsBufferManager::bins.bins[0].bufferSize = ComCcsdsConfig::BuffMgr::commsBuffSize; - ConfigObjects::ComCcsdsUart_commsBufferManager::bins.bins[0].numBuffers = ComCcsdsConfig::BuffMgr::commsBuffCount; - ConfigObjects::ComCcsdsUart_commsBufferManager::bins.bins[1].bufferSize = ComCcsdsConfig::BuffMgr::commsFileBuffSize; - ConfigObjects::ComCcsdsUart_commsBufferManager::bins.bins[1].numBuffers = ComCcsdsConfig::BuffMgr::commsFileBuffCount; - ComCcsdsUart::commsBufferManager.setup( + memset(&ConfigObjects::ComCcsdsSband_commsBufferManager::bins, 0, sizeof(ConfigObjects::ComCcsdsSband_commsBufferManager::bins)); + ConfigObjects::ComCcsdsSband_commsBufferManager::bins.bins[0].bufferSize = ComCcsdsConfig::BuffMgr::commsBuffSize; + ConfigObjects::ComCcsdsSband_commsBufferManager::bins.bins[0].numBuffers = ComCcsdsConfig::BuffMgr::commsBuffCount; + ConfigObjects::ComCcsdsSband_commsBufferManager::bins.bins[1].bufferSize = ComCcsdsConfig::BuffMgr::commsFileBuffSize; + ConfigObjects::ComCcsdsSband_commsBufferManager::bins.bins[1].numBuffers = ComCcsdsConfig::BuffMgr::commsFileBuffCount; + ComCcsdsSband::commsBufferManager.setup( ComCcsdsConfig::BuffMgr::commsBuffMgrId, 0, ComCcsds::Allocation::memAllocator, - ConfigObjects::ComCcsdsUart_commsBufferManager::bins + ConfigObjects::ComCcsdsSband_commsBufferManager::bins ); """ phase Fpp.ToCpp.Phases.tearDownComponents """ - ComCcsdsUart::commsBufferManager.cleanup(); + ComCcsdsSband::commsBufferManager.cleanup(); """ } - instance fprimeRouter: Svc.FprimeRouter base id ComCcsdsConfig.BASE_ID_UART + 0x03000 + instance fprimeRouter: Svc.FprimeRouter base id ComCcsdsConfig.BASE_ID_SBAND + 0x03000 - instance tcDeframer: Svc.Ccsds.TcDeframer base id ComCcsdsConfig.BASE_ID_UART + 0x04000 + instance tcDeframer: Svc.Ccsds.TcDeframer base id ComCcsdsConfig.BASE_ID_SBAND + 0x04000 - instance spacePacketDeframer: Svc.Ccsds.SpacePacketDeframer base id ComCcsdsConfig.BASE_ID_UART + 0x05000 + instance spacePacketDeframer: Svc.Ccsds.SpacePacketDeframer base id ComCcsdsConfig.BASE_ID_SBAND + 0x05000 # NOTE: name 'framer' is used for the framer that connects to the Com Adapter Interface for better subtopology interoperability - instance framer: Svc.Ccsds.TmFramer base id ComCcsdsConfig.BASE_ID_UART + 0x07000 + instance framer: Svc.Ccsds.TmFramer base id ComCcsdsConfig.BASE_ID_SBAND + 0x07000 - instance spacePacketFramer: Svc.Ccsds.SpacePacketFramer base id ComCcsdsConfig.BASE_ID_UART + 0x08000 + instance spacePacketFramer: Svc.Ccsds.SpacePacketFramer base id ComCcsdsConfig.BASE_ID_SBAND + 0x08000 - instance apidManager: Svc.Ccsds.ApidManager base id ComCcsdsConfig.BASE_ID_UART + 0x09000 + instance apidManager: Svc.Ccsds.ApidManager base id ComCcsdsConfig.BASE_ID_SBAND + 0x09000 - instance comStub: Svc.ComStub base id ComCcsdsConfig.BASE_ID_UART + 0x0A000 + instance comStub: Svc.ComStub base id ComCcsdsConfig.BASE_ID_SBAND + 0x0A000 topology FramingSubtopology { # Usage Note: @@ -200,4 +200,4 @@ module ComCcsdsUart { } } # end Subtopology -} # end ComCcsds +} # end ComCcsdsSband diff --git a/FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp b/FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp index daa5e746..1b116f17 100644 --- a/FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp +++ b/FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp @@ -1,5 +1,5 @@ -#ifndef COMCCSDSSUBTOPOLOGY_DEFS_HPP -#define COMCCSDSSUBTOPOLOGY_DEFS_HPP +#ifndef COMCCSDSSBANDSUBDEFS_HPP +#define COMCCSDSSBANDSUBDEFS_HPP #include #include @@ -8,14 +8,14 @@ #include "ComCcsdsConfig/ComCcsdsSubtopologyConfig.hpp" #include "Svc/Subtopologies/ComCcsds/ComCcsdsConfig/FppConstantsAc.hpp" -namespace ComCcsds { +namespace ComCcsdsSband { struct SubtopologyState { - // Empty - no external state needed for ComCcsds subtopology + // Empty - no external state needed for ComCcsdsSband subtopology }; struct TopologyState { - SubtopologyState comCcsds; + SubtopologyState comCcsdsSband; }; -} // namespace ComCcsds +} // namespace ComCcsdsSband #endif diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index 3abbdf3c..b8573876 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -10,6 +10,7 @@ #include +#include #include #include "FprimeHal.hpp" @@ -45,10 +46,37 @@ void SBand ::run_handler(FwIndexType portNum, U32 context) { sprintf(msg + i * 3, "%02X ", data[i]); // NOLINT(runtime/printf) msg[len * 3] = '\0'; Fw::Logger::log("%s\n", msg); + + // Allocate buffer and send received data to F' system + // This goes to ComCcsdsSband.frameAccumulator.dataIn, which processes the uplink frames + // @ jack may want to check here for any edits for how the s band actually processes it + Fw::Buffer buffer = this->allocate_out(0, static_cast(len)); + if (buffer.isValid()) { + (void)::memcpy(buffer.getData(), data, len); + ComCfg::FrameContext frameContext; + this->dataOut_out(0, buffer, frameContext); + } } } } +// ---------------------------------------------------------------------- +// Handler implementations for Com interface +// ---------------------------------------------------------------------- + +void SBand ::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { + // TODO: Implement data transmission + // For now, just return the buffer and indicate success + Fw::Success returnStatus = Fw::Success::SUCCESS; + this->dataReturnOut_out(0, data, context); + this->comStatusOut_out(0, returnStatus); +} + +void SBand ::dataReturnIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { + // Deallocate the buffer + this->deallocate_out(0, data); +} + // ---------------------------------------------------------------------- // Handler implementations for commands // ---------------------------------------------------------------------- diff --git a/FprimeZephyrReference/Components/SBand/SBand.fpp b/FprimeZephyrReference/Components/SBand/SBand.fpp index 373808ef..cc4b6e06 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.fpp +++ b/FprimeZephyrReference/Components/SBand/SBand.fpp @@ -11,8 +11,11 @@ module Components { @ Port receiving calls from the rate group sync input port run: Svc.Sched - # @ Import Communication Interface - # import Svc.Com + @ Import Communication Interface + import Svc.Com + + @ Import the allocation interface + import Svc.BufferAllocation @ Command to transmit data sync command TRANSMIT() diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index 2bfc92f0..77d1c9ae 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -41,6 +41,20 @@ class SBand final : public SBandComponentBase { U32 context //!< The call order ) override; + //! Handler implementation for dataIn + //! + //! Data to be sent on the wire (coming in to the component) + void dataIn_handler(FwIndexType portNum, //!< The port number + Fw::Buffer& data, + const ComCfg::FrameContext& context) override; + + //! Handler implementation for dataReturnIn + //! + //! Port receiving back ownership of buffer sent out on dataOut + void dataReturnIn_handler(FwIndexType portNum, //!< The port number + Fw::Buffer& data, + const ComCfg::FrameContext& context) override; + private: // ---------------------------------------------------------------------- // Handler implementations for commands diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi index 9b21a34f..37047ed1 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi @@ -5,6 +5,8 @@ telemetry packets ReferenceDeploymentPackets { CdhCore.cmdDisp.CommandsDropped ComCcsds.comQueue.comQueueDepth ComCcsds.commsBufferManager.HiBuffs + ComCcsdsSband.comQueue.comQueueDepth + ComCcsdsSband.commsBufferManager.HiBuffs ComCcsdsUart.comQueue.comQueueDepth ComCcsdsUart.commsBufferManager.HiBuffs ReferenceDeployment.rateGroup10Hz.RgMaxTime @@ -21,6 +23,8 @@ telemetry packets ReferenceDeploymentPackets { ComCcsdsUart.commsBufferManager.EmptyBuffs ReferenceDeployment.rateGroup10Hz.RgCycleSlips ReferenceDeployment.rateGroup1Hz.RgCycleSlips + ComCcsdsSband.commsBufferManager.NoBuffs + ComCcsdsSband.commsBufferManager.EmptyBuffs } @@ -32,6 +36,9 @@ telemetry packets ReferenceDeploymentPackets { ComCcsdsUart.commsBufferManager.CurrBuffs ComCcsdsUart.comQueue.buffQueueDepth CdhCore.tlmSend.SendLevel + ComCcsdsSband.commsBufferManager.TotalBuffs + ComCcsdsSband.commsBufferManager.CurrBuffs + ComCcsdsSband.comQueue.buffQueueDepth } packet Version id 4 group 3 { diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp index 95bae1bd..724a93d7 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp @@ -7,12 +7,14 @@ #define REFERENCEDEPLOYMENT_REFERENCEDEPLOYMENTTOPOLOGYDEFS_HPP // Subtopology PingEntries includes +#include "FprimeZephyrReference/ComCcsdsSband/PingEntries.hpp" #include "Svc/Subtopologies/CdhCore/PingEntries.hpp" #include "Svc/Subtopologies/ComCcsds/PingEntries.hpp" #include "Svc/Subtopologies/DataProducts/PingEntries.hpp" #include "Svc/Subtopologies/FileHandling/PingEntries.hpp" // SubtopologyTopologyDefs includes +#include "FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp" #include "Svc/Subtopologies/CdhCore/SubtopologyTopologyDefs.hpp" #include "Svc/Subtopologies/ComCcsds/SubtopologyTopologyDefs.hpp" #include "Svc/Subtopologies/FileHandling/SubtopologyTopologyDefs.hpp" @@ -69,18 +71,19 @@ namespace ReferenceDeployment { * autocoder. The contents are entirely up to the definition of the project. This deployment uses subtopologies. */ struct TopologyState { - const device* uartDevice; //!< UART device path for communication - const device* loraDevice; //!< LoRa device path for communication - const device* spi0Device; //!< Spi device path for s-band LoRa module - U32 baudRate; //!< Baud rate for UART communication - CdhCore::SubtopologyState cdhCore; //!< Subtopology state for CdhCore - ComCcsds::SubtopologyState comCcsds; //!< Subtopology state for ComCcsds - FileHandling::SubtopologyState fileHandling; //!< Subtopology state for FileHandling - const device* ina219SysDevice; //!< device path for battery board ina219 - const device* ina219SolDevice; //!< device path for solar panel ina219 - const device* lsm6dsoDevice; //!< LSM6DSO device path for accelerometer/gyroscope - const device* lis2mdlDevice; //!< LIS2MDL device path for magnetometer - const device* rtcDevice; //!< RTC device path + const device* uartDevice; //!< UART device path for communication + const device* loraDevice; //!< LoRa device path for communication + const device* spi0Device; //!< Spi device path for s-band LoRa module + U32 baudRate; //!< Baud rate for UART communication + CdhCore::SubtopologyState cdhCore; //!< Subtopology state for CdhCore + ComCcsds::SubtopologyState comCcsds; //!< Subtopology state for ComCcsds + FileHandling::SubtopologyState fileHandling; //!< Subtopology state for FileHandling + ComCcsdsSband::SubtopologyState comCcsdsSband; //!< Subtopology state for ComCcsdsSband + const device* ina219SysDevice; //!< device path for battery board ina219 + const device* ina219SolDevice; //!< device path for solar panel ina219 + const device* lsm6dsoDevice; //!< LSM6DSO device path for accelerometer/gyroscope + const device* lis2mdlDevice; //!< LIS2MDL device path for magnetometer + const device* rtcDevice; //!< RTC device path }; namespace PingEntries = ::PingEntries; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index eeb072aa..0401eced 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -18,6 +18,7 @@ module ReferenceDeployment { import ComCcsds.FramingSubtopology import ComCcsdsUart.Subtopology import FileHandling.Subtopology + import ComCcsdsSband.FramingSubtopology # ---------------------------------------------------------------------- # Instances used in the topology @@ -115,6 +116,31 @@ module ReferenceDeployment { cmdSeq.comCmdOut -> CdhCore.cmdDisp.seqCmdBuff CdhCore.cmdDisp.seqCmdStatus -> cmdSeq.cmdResponseIn + + + # Sband connections + comSplitterEvents.comOut-> ComCcsdsSband.comQueue.comPacketQueueIn[ComCcsds.Ports_ComPacketQueue.EVENTS] + comSplitterTelemetry.comOut -> ComCcsdsSband.comQueue.comPacketQueueIn[ComCcsds.Ports_ComPacketQueue.TELEMETRY] + + ComCcsdsSband.fprimeRouter.commandOut -> CdhCore.cmdDisp.seqCmdBuff + CdhCore.cmdDisp.seqCmdStatus -> ComCcsdsSband.fprimeRouter.cmdResponseIn + + } + + + connections CommunicationsSBandRadio { + sband.allocate -> ComCcsdsSband.commsBufferManager.bufferGetCallee + sband.deallocate -> ComCcsdsSband.commsBufferManager.bufferSendIn + + # ComDriver <-> ComStub (Uplink) + sband.dataOut -> ComCcsdsSband.frameAccumulator.dataIn + ComCcsdsSband.frameAccumulator.dataReturnOut -> sband.dataReturnIn + + # ComStub <-> ComDriver (Downlink) + ComCcsdsSband.framer.dataOut -> sband.dataIn + sband.dataReturnOut -> ComCcsdsSband.framer.dataReturnIn + sband.comStatusOut -> ComCcsdsSband.framer.comStatusIn + } connections CommunicationsRadio { @@ -160,6 +186,7 @@ module ReferenceDeployment { rateGroup10Hz.RateGroupMemberOut[2] -> ComCcsds.aggregator.timeout rateGroup10Hz.RateGroupMemberOut[3] -> FileHandling.fileManager.schedIn rateGroup10Hz.RateGroupMemberOut[4] -> cmdSeq.schedIn + rateGroup10Hz.RateGroupMemberOut[5] -> ComCcsdsSband.aggregator.timeout # Slow rate (1Hz) rate group rateGroupDriver.CycleOut[Ports_RateGroups.rateGroup1Hz] -> rateGroup1Hz.CycleIn @@ -177,7 +204,8 @@ module ReferenceDeployment { rateGroup1Hz.RateGroupMemberOut[11] -> startupManager.run rateGroup1Hz.RateGroupMemberOut[12] -> powerMonitor.run rateGroup1Hz.RateGroupMemberOut[13] -> sband.run - + rateGroup1Hz.RateGroupMemberOut[14] -> ComCcsdsSband.comQueue.run + rateGroup1Hz.RateGroupMemberOut[15] -> ComCcsdsSband.commsBufferManager.schedIn } @@ -229,9 +257,14 @@ module ReferenceDeployment { # Router <-> FileUplink ComCcsdsUart.fprimeRouter.fileOut -> FileHandling.fileUplink.bufferSendIn FileHandling.fileUplink.bufferSendOut -> ComCcsdsUart.fprimeRouter.fileBufferReturnIn + + # Router <-> FileUplink (S-band input only - file downlink uses UART only) + ComCcsdsSband.fprimeRouter.fileOut -> FileHandling.fileUplink.bufferSendIn } + + connections sysPowerMonitor { powerMonitor.sysVoltageGet -> ina219SysManager.voltageGet powerMonitor.sysCurrentGet -> ina219SysManager.currentGet diff --git a/FprimeZephyrReference/project/config/AcConstants.fpp b/FprimeZephyrReference/project/config/AcConstants.fpp index c88be742..2c84297d 100644 --- a/FprimeZephyrReference/project/config/AcConstants.fpp +++ b/FprimeZephyrReference/project/config/AcConstants.fpp @@ -4,7 +4,7 @@ # ====================================================================== @ Number of rate group member output ports for ActiveRateGroup -constant ActiveRateGroupOutputPorts = 15 +constant ActiveRateGroupOutputPorts = 16 @ Number of rate group member output ports for PassiveRateGroup constant PassiveRateGroupOutputPorts = 10 From e8ce67b3f36a9aef13567208c3b34cc2c36ae811 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 09:59:59 -0800 Subject: [PATCH 070/101] increase TLMPACKETIZER_HASH_BUCKETS --- FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp b/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp index 68e52a32..7236d231 100644 --- a/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp +++ b/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp @@ -24,8 +24,9 @@ static const FwChanIdType TLMPACKETIZER_HASH_MOD_VALUE = 999; // !< The modulo value of the hashing function. // Should be set to a little below the ID gaps to spread the entries around -static const FwChanIdType TLMPACKETIZER_HASH_BUCKETS = 90; // !< Buckets assignable to a hash slot. - // Buckets must be >= number of telemetry channels in system +static const FwChanIdType TLMPACKETIZER_HASH_BUCKETS = + 100; // !< Buckets assignable to a hash slot. + // Buckets must be >= number of telemetry channels in system static const FwChanIdType TLMPACKETIZER_MAX_MISSING_TLM_CHECK = 25; // !< Maximum number of missing telemetry channel checks From cc7e964f7bb96f10bfa35c14fd314953d2ac395e Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 10:33:19 -0800 Subject: [PATCH 071/101] Disable compile warnings for RadioLib --- FprimeZephyrReference/Components/SBand/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/FprimeZephyrReference/Components/SBand/CMakeLists.txt b/FprimeZephyrReference/Components/SBand/CMakeLists.txt index 7e725b9d..24968493 100644 --- a/FprimeZephyrReference/Components/SBand/CMakeLists.txt +++ b/FprimeZephyrReference/Components/SBand/CMakeLists.txt @@ -26,6 +26,9 @@ register_fprime_library( add_subdirectory("${FPRIME_PROJECT_ROOT}/lib/RadioLib" "${CMAKE_BINARY_DIR}/RadioLib") +# Disable compile warnings for RadioLib as they pollute the log and we can't do anything about them. +target_compile_options(RadioLib PRIVATE -w) + ### Unit Tests ### # register_fprime_ut( # AUTOCODER_INPUTS From 35d109c7e9e386c84f1f44eb912f83b7c7534abe Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 10:37:39 -0800 Subject: [PATCH 072/101] change spi config to use Zephyr 4.3 interface --- .../ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index e5342b60..63c8779b 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -123,6 +123,7 @@ void setupTopology(const TopologyState& state) { static struct spi_cs_control cs_ctrl = { .gpio = GPIO_DT_SPEC_GET_BY_IDX(DT_NODELABEL(spi0), cs_gpios, 0), .delay = 0U, /* us to wait after asserting CS before transfer */ + .cs_is_gpio = true, }; struct spi_config cfg = { @@ -130,7 +131,7 @@ void setupTopology(const TopologyState& state) { .operation = SPI_WORD_SET(8), .slave = 0, .cs = cs_ctrl, - + .word_delay = 0, }; spiDriver.configure(state.spi0Device, cfg); lsm6dsoManager.configure(state.lsm6dsoDevice); From 59c36bf6fcc22116cd91c0fff79e3397c2a165de Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 13:04:56 -0800 Subject: [PATCH 073/101] fix adafruit rfm module import --- circuit-python-lora-passthrough-feather/code.py | 4 ++-- circuit-python-lora-passthrough/code.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/circuit-python-lora-passthrough-feather/code.py b/circuit-python-lora-passthrough-feather/code.py index 7f394ab9..37376774 100644 --- a/circuit-python-lora-passthrough-feather/code.py +++ b/circuit-python-lora-passthrough-feather/code.py @@ -7,7 +7,7 @@ # import time -import adafruit_rfm9x +import adafruit_rfm.rfm9x import board import digitalio import usb_cdc @@ -17,7 +17,7 @@ CS = digitalio.DigitalInOut(board.RFM9X_CS) RESET = digitalio.DigitalInOut(board.RFM9X_RST) -rfm95 = adafruit_rfm9x.RFM9x(board.SPI(), CS, RESET, RADIO_FREQ_MHZ) +rfm95 = adafruit_rfm.rfm9x.RFM9x(board.SPI(), CS, RESET, RADIO_FREQ_MHZ) rfm95.spreading_factor = 8 rfm95.signal_bandwidth = 125000 rfm95.coding_rate = 5 diff --git a/circuit-python-lora-passthrough/code.py b/circuit-python-lora-passthrough/code.py index 4efd89f5..3554798f 100644 --- a/circuit-python-lora-passthrough/code.py +++ b/circuit-python-lora-passthrough/code.py @@ -7,7 +7,7 @@ import time -import adafruit_rfm9x +import adafruit_rfm.rfm9x import board import digitalio import usb_cdc @@ -17,7 +17,7 @@ CS = digitalio.DigitalInOut(board.SPI0_CS0) RESET = digitalio.DigitalInOut(board.RF1_RST) -rfm95 = adafruit_rfm9x.RFM9x(board.SPI(), CS, RESET, RADIO_FREQ_MHZ) +rfm95 = adafruit_rfm.rfm9x.RFM9x(board.SPI(), CS, RESET, RADIO_FREQ_MHZ) rfm95.spreading_factor = 8 rfm95.signal_bandwidth = 125000 rfm95.coding_rate = 5 From 6eb14e1f090a83f4b5542df7253c43f627edf3be Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 14:31:00 -0800 Subject: [PATCH 074/101] add `circuit-python-passthrough` --- circuit-python-sband-passthrough/boot.py | 12 + circuit-python-sband-passthrough/code.py | 356 +++++++++++++++++++++++ 2 files changed, 368 insertions(+) create mode 100644 circuit-python-sband-passthrough/boot.py create mode 100644 circuit-python-sband-passthrough/code.py diff --git a/circuit-python-sband-passthrough/boot.py b/circuit-python-sband-passthrough/boot.py new file mode 100644 index 00000000..8b54a07d --- /dev/null +++ b/circuit-python-sband-passthrough/boot.py @@ -0,0 +1,12 @@ +""" +boot.py + +This file helps to use an RP2350 CircuitPython environment as a passthrough for +interacting with the radio and F Prime GDS. The usb_cdc.enable() function creates a +USB serial endpoint that the GDS can connect to to receive all the data that is +streamed out. The function has to be called in boot.py before the USB device is enabled. +""" + +import usb_cdc + +usb_cdc.enable(console=True, data=True) diff --git a/circuit-python-sband-passthrough/code.py b/circuit-python-sband-passthrough/code.py new file mode 100644 index 00000000..c7985f1b --- /dev/null +++ b/circuit-python-sband-passthrough/code.py @@ -0,0 +1,356 @@ +""" +CircuitPython Feather RP2350 LoRa Radio forwarder + +This code will forward any received LoRa packets to the serial console (sys.stdout). It cycles through neo pixel colors +to indicate packet reception. +""" + +import os +import time + +import board +import digitalio +import usb_cdc # for passthrough at end of file +from lib.adafruit_mcp230xx.mcp23017 import ( + MCP23017, # This is Hacky V5a Devel Stuff### +) +from lib.adafruit_tca9548a import TCA9548A # This is Hacky V5a Devel Stuff### + +# from lib.pysquared.Big_Data import AllFaces ### This is Hacky V5a Devel Stuff### +from lib.pysquared.beacon import Beacon +from lib.pysquared.cdh import CommandDataHandler +from lib.pysquared.config.config import Config +from lib.pysquared.file_validation.manager.file_validation import FileValidationManager +from lib.pysquared.hardware.burnwire.manager.burnwire import BurnwireManager +from lib.pysquared.hardware.busio import _spi_init, initialize_i2c_bus +from lib.pysquared.hardware.digitalio import initialize_pin +from lib.pysquared.hardware.imu.manager.lsm6dsox import LSM6DSOXManager +from lib.pysquared.hardware.light_sensor.manager.veml7700 import VEML7700Manager +from lib.pysquared.hardware.load_switch.manager.loadswitch_manager import ( + LoadSwitchManager, +) +from lib.pysquared.hardware.magnetometer.manager.lis2mdl import LIS2MDLManager +from lib.pysquared.hardware.power_monitor.manager.ina219 import INA219Manager +from lib.pysquared.hardware.radio.manager.rfm9x import RFM9xManager +from lib.pysquared.hardware.radio.manager.sx1280 import SX1280Manager +from lib.pysquared.hardware.radio.packetizer.packet_manager import PacketManager +from lib.pysquared.hardware.temperature_sensor.manager.mcp9808 import MCP9808Manager +from lib.pysquared.logger import Logger +from lib.pysquared.nvm.counter import Counter +from lib.pysquared.protos.power_monitor import PowerMonitorProto +from lib.pysquared.rtc.manager.microcontroller import MicrocontrollerManager +from lib.pysquared.sleep_helper import SleepHelper +from lib.pysquared.watchdog import Watchdog +from version import __version__ + +rtc = MicrocontrollerManager() + + +logger: Logger = Logger( + error_counter=Counter(0), + colorized=False, +) + +logger.info( + "Booting", + hardware_version=os.uname().version, # type: ignore[attr-defined] + software_version=__version__, +) + + +def get_temp(sensor): + """ + Get temperature + """ + for i in range(1000): + print(sensor.get_temperature().value) + time.sleep(0.1) + + +watchdog = Watchdog(logger, board.WDT_WDI) +watchdog.pet() + +logger.debug("Initializing Config") +config: Config = Config("config.json") + +mux_reset = initialize_pin(logger, board.MUX_RESET, digitalio.Direction.OUTPUT, False) + +# TODO(nateinaction): fix spi init +spi0 = _spi_init( + logger, + board.SPI0_SCK, + board.SPI0_MOSI, + board.SPI0_MISO, +) + +spi1 = _spi_init( + logger, + board.SPI1_SCK, + board.SPI1_MOSI, + board.SPI1_MISO, +) + +sband_radio = SX1280Manager( + logger, + config.radio, + spi1, + initialize_pin(logger, board.SPI1_CS0, digitalio.Direction.OUTPUT, True), + initialize_pin(logger, board.RF2_RST, digitalio.Direction.OUTPUT, True), + initialize_pin(logger, board.RF2_IO0, digitalio.Direction.OUTPUT, True), + 2.4, + initialize_pin(logger, board.RF2_TX_EN, digitalio.Direction.OUTPUT, False), + initialize_pin(logger, board.RF2_RX_EN, digitalio.Direction.OUTPUT, False), +) + +i2c1 = initialize_i2c_bus( + logger, + board.SCL1, + board.SDA1, + 100000, +) + +i2c0 = initialize_i2c_bus( + logger, + board.SCL0, + board.SDA0, + 100000, +) + +sleep_helper = SleepHelper(logger, config, watchdog) + +uhf_radio = RFM9xManager( + logger, + config.radio, + spi0, + initialize_pin(logger, board.SPI0_CS0, digitalio.Direction.OUTPUT, True), + initialize_pin(logger, board.RF1_RST, digitalio.Direction.OUTPUT, True), +) + +magnetometer = LIS2MDLManager(logger, i2c1) + +imu = LSM6DSOXManager(logger, i2c1, 0x6B) + +uhf_packet_manager = PacketManager( + logger, + uhf_radio, + config.radio.license, + Counter(2), + 0.2, +) + +cdh = CommandDataHandler(logger, config, uhf_packet_manager) + +beacon = Beacon( + logger, + config.cubesat_name, + uhf_packet_manager, + time.monotonic(), + imu, + magnetometer, + uhf_radio, + sband_radio, +) + +## Initialize the MCP23017 GPIO Expander and its pins ## +GPIO_RESET = initialize_pin( + logger, board.GPIO_EXPANDER_RESET, digitalio.Direction.OUTPUT, True +) +mcp = MCP23017(i2c1) + +# This sets up all of the GPIO pins on the MCP23017 +FACE4_ENABLE = mcp.get_pin(8) +FACE0_ENABLE = mcp.get_pin(9) +FACE1_ENABLE = mcp.get_pin(10) +FACE2_ENABLE = mcp.get_pin(11) +FACE3_ENABLE = mcp.get_pin(12) +ENAB_RF = mcp.get_pin(13) +VBUS_RESET = mcp.get_pin(14) +SPI0_CS1 = mcp.get_pin(15) +ENABLE_HEATER = mcp.get_pin(0) +PAYLOAD_PWR_ENABLE = mcp.get_pin(1) +Z_GPIO0 = mcp.get_pin(2) +Z_GPIO1 = mcp.get_pin(3) +RF2_IO2 = mcp.get_pin(4) +RF2_IO1 = mcp.get_pin(5) + +# This defines the direction of the GPIO pins +FACE4_ENABLE.direction = digitalio.Direction.OUTPUT +FACE0_ENABLE.direction = digitalio.Direction.OUTPUT +FACE1_ENABLE.direction = digitalio.Direction.OUTPUT +FACE2_ENABLE.direction = digitalio.Direction.OUTPUT +FACE3_ENABLE.direction = digitalio.Direction.OUTPUT +ENAB_RF.direction = digitalio.Direction.OUTPUT +VBUS_RESET.direction = digitalio.Direction.OUTPUT +ENABLE_HEATER.direction = digitalio.Direction.OUTPUT +PAYLOAD_PWR_ENABLE.direction = digitalio.Direction.OUTPUT + + +load_switch_0 = LoadSwitchManager(FACE0_ENABLE, True) # type: ignore , upstream on mcp TODO +load_switch_1 = LoadSwitchManager(FACE1_ENABLE, True) # type: ignore , upstream on mcp TODO +load_switch_2 = LoadSwitchManager(FACE2_ENABLE, True) # type: ignore , upstream on mcp TODO +load_switch_3 = LoadSwitchManager(FACE3_ENABLE, True) # type: ignore , upstream on mcp TODO +load_switch_4 = LoadSwitchManager(FACE4_ENABLE, True) # type: ignore , upstream on mcp TODO + + +# Face Control Helper Functions +def all_faces_off(): + """ + This function turns off all of the faces. Note the load switches are disabled low. + """ + load_switch_0.disable_load() + load_switch_1.disable_load() + load_switch_2.disable_load() + load_switch_3.disable_load() + load_switch_4.disable_load() + + +def all_faces_on(): + """ + This function turns on all of the faces. Note the load switches are enabled high. + """ + load_switch_0.enable_load() + load_switch_1.enable_load() + load_switch_2.enable_load() + load_switch_3.enable_load() + load_switch_4.enable_load() + + +file_validator = FileValidationManager(logger) + +## Face Sensor Stuff ## + +# This is the TCA9548A I2C Multiplexer +all_faces_on() +mux_reset.value = True +tca = TCA9548A(i2c1, address=int(0x77)) + + +# Light Sensors +light_sensors = [] +try: + sensor = VEML7700Manager(logger, tca[0]) + light_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Light sensor 0 failed to initialize") + light_sensors.append(None) +try: + sensor = VEML7700Manager(logger, tca[1]) + light_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Light sensor 1 failed to initialize") + light_sensors.append(None) +try: + sensor = VEML7700Manager(logger, tca[2]) + light_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Light sensor 2 failed to initialize") + light_sensors.append(None) +try: + sensor = VEML7700Manager(logger, tca[3]) + light_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Light sensor 3 failed to initialize") + light_sensors.append(None) +try: + sensor = VEML7700Manager(logger, tca[4]) + light_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Light sensor 4 failed to initialize") + light_sensors.append(None) + + +# Onboard Temp Sensors +temp_sensors = [] + +# Direct I2C sensors +try: + temp_sensor5 = MCP9808Manager(logger, i2c0, addr=25) # Antenna Board +except Exception: + logger.debug("WARNING!!! Temp sensor 5 (Antenna Board) failed") + temp_sensor5 = None +temp_sensors.append(temp_sensor5) + +try: + temp_sensor6 = MCP9808Manager(logger, i2c1, addr=27) # Flight Controller Board +except Exception: + logger.debug("WARNING!!! Temp sensor 6 (Flight Controller Board) failed") + temp_sensor6 = None +temp_sensors.append(temp_sensor6) + +# TCA-connected temp sensors +try: + sensor = MCP9808Manager(logger, tca[0], addr=27) + temp_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Temp sensor (TCA[0]) failed") + temp_sensors.append(None) +try: + sensor = MCP9808Manager(logger, tca[1], addr=27) + temp_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Temp sensor 1]) failed") + temp_sensors.append(None) +try: + sensor = MCP9808Manager(logger, tca[2], addr=27) + temp_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Temp sensor 2 failed") + temp_sensors.append(None) +try: + sensor = MCP9808Manager(logger, tca[3], addr=27) + temp_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Temp sensor3 failed") + temp_sensors.append(None) +try: + sensor = MCP9808Manager(logger, tca[4], addr=27) + temp_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Temp sensor 4 failed") + temp_sensors.append(None) + +battery_power_monitor: PowerMonitorProto = INA219Manager(logger, i2c1, 0x40) +solar_power_monitor: PowerMonitorProto = INA219Manager(logger, i2c1, 0x44) + +## Init Misc Pins ## +burnwire_heater_enable = initialize_pin( + logger, board.FIRE_DEPLOY1_A, digitalio.Direction.OUTPUT, False +) +burnwire1_fire = initialize_pin( + logger, board.FIRE_DEPLOY1_B, digitalio.Direction.OUTPUT, False +) + +## Initializing the Burn Wire ## +antenna_deployment = BurnwireManager( + logger, burnwire_heater_enable, burnwire1_fire, enable_logic=True +) + +# END `repl.py` copy + +time_start = time.time() +packet_count = 0 +last_packet = None + +print("[INFO] LoRa Receiver receiving packets") +while True: + # Returns the last packet received if there is no new packet + packet = sband_radio._radio.receive() + + # Only forward `packet` if it is indeed a new packet + # - Every packet is unique due to a unique counter which is prepended to + # each packet + # - The logical packet never spans more than one physical packet as + # packet size is hard-coded to 252 bytes on the Fprime board + if packet != last_packet: + usb_cdc.data.write(packet) + packet_count += 1 + time_delta = time.time() - time_start + if time_delta > 10: + print(f"[INFO] Packets received: {packet_count}") + time_start = time.time() + # `usb_cdc.data` is None until board is hard reset (after reflashing `boot.py`) + data = usb_cdc.data.read(usb_cdc.data.in_waiting) + if len(data) > 0: + print("Sending packet") + sband_radio._radio.send(data, header=False) + time.sleep(1) From 02414a8dc5d22f044d7f70a63c6e4b1b1c917624 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 19:15:51 -0800 Subject: [PATCH 075/101] Move rx enable into enableRx --- FprimeZephyrReference/Components/SBand/SBand.cpp | 10 +++++++--- FprimeZephyrReference/Components/SBand/SBand.hpp | 1 + 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index b8573876..03065c0e 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -91,9 +91,9 @@ void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { "Hello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " "world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " "world!\nHello, world!\nHello, world!\nHello, world!\n"; + this->rxEnable_out(0, Fw::Logic::LOW); this->txEnable_out(0, Fw::Logic::HIGH); state = this->m_rlb_radio.transmit(s, sizeof(s)); - this->txEnable_out(0, Fw::Logic::LOW); if (state == RADIOLIB_ERR_NONE) { Fw::Logger::log("radio.transmit() success!\n"); } else { @@ -104,6 +104,12 @@ void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { } void SBand ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { + this->enableRx(); + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); +} + +void SBand ::enableRx() { + this->txEnable_out(0, Fw::Logic::LOW); this->rxEnable_out(0, Fw::Logic::HIGH); int16_t state = this->configure_radio(); @@ -117,8 +123,6 @@ void SBand ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { FW_ASSERT(state == RADIOLIB_ERR_NONE); this->wait_for_rx_fin = true; - - this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } int16_t SBand ::configure_radio() { diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index 77d1c9ae..278bc27c 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -76,6 +76,7 @@ class SBand final : public SBandComponentBase { // Configure the SX1280 radio (setup and parameter tuning) int16_t configure_radio(); + void enableRx(); private: FprimeHal m_rlb_hal; //!< RadioLib HAL instance From c705e830919e045e3aac6ac27f0c09462f971d1c Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 19:17:09 -0800 Subject: [PATCH 076/101] rename `wait_for_rx_fin` -> `rx_mode` --- FprimeZephyrReference/Components/SBand/SBand.cpp | 8 ++++---- FprimeZephyrReference/Components/SBand/SBand.hpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index 03065c0e..ef822f95 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -31,10 +31,10 @@ SBand ::~SBand() {} // ---------------------------------------------------------------------- void SBand ::run_handler(FwIndexType portNum, U32 context) { - if (this->wait_for_rx_fin) { + if (this->rx_mode) { uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { - this->wait_for_rx_fin = false; + this->rx_mode = false; SX1280* radio = &this->m_rlb_radio; uint8_t data[256] = {0}; size_t len = radio->getPacketLength(); @@ -85,7 +85,7 @@ void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { int state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); - this->wait_for_rx_fin = false; + this->rx_mode = false; char s[] = "Hello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " @@ -122,7 +122,7 @@ void SBand ::enableRx() { state = radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); FW_ASSERT(state == RADIOLIB_ERR_NONE); - this->wait_for_rx_fin = true; + this->rx_mode = true; } int16_t SBand ::configure_radio() { diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index 278bc27c..206aab73 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -82,7 +82,7 @@ class SBand final : public SBandComponentBase { FprimeHal m_rlb_hal; //!< RadioLib HAL instance Module m_rlb_module; //!< RadioLib Module instance SX1280 m_rlb_radio; //!< RadioLib SX1280 radio instance - bool wait_for_rx_fin = false; + bool rx_mode = false; }; } // namespace Components From 7d923fe4fce7834013465f316335435e9b83cdcc Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 19:25:58 -0800 Subject: [PATCH 077/101] add continuous receive --- FprimeZephyrReference/Components/SBand/SBand.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index ef822f95..76dd071a 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -34,11 +34,11 @@ void SBand ::run_handler(FwIndexType portNum, U32 context) { if (this->rx_mode) { uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { - this->rx_mode = false; SX1280* radio = &this->m_rlb_radio; uint8_t data[256] = {0}; size_t len = radio->getPacketLength(); radio->readData(data, len); + radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); Fw::Logger::log("MESSAGE RECEIVED:\n"); char msg[sizeof(data) * 3 + 1]; From befa32387fac0276743ccbb89cd7a78fa5f412f5 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 19:33:48 -0800 Subject: [PATCH 078/101] move radio configure into on-start operation --- FprimeZephyrReference/Components/SBand/SBand.cpp | 15 +++++++-------- FprimeZephyrReference/Components/SBand/SBand.hpp | 2 ++ .../Top/ReferenceDeploymentTopology.cpp | 1 + 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index 76dd071a..523c6b61 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -82,9 +82,6 @@ void SBand ::dataReturnIn_handler(FwIndexType portNum, Fw::Buffer& data, const C // ---------------------------------------------------------------------- void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - int state = this->configure_radio(); - FW_ASSERT(state == RADIOLIB_ERR_NONE); - this->rx_mode = false; char s[] = @@ -93,7 +90,7 @@ void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { "world!\nHello, world!\nHello, world!\nHello, world!\n"; this->rxEnable_out(0, Fw::Logic::LOW); this->txEnable_out(0, Fw::Logic::HIGH); - state = this->m_rlb_radio.transmit(s, sizeof(s)); + int16_t state = this->m_rlb_radio.transmit(s, sizeof(s)); if (state == RADIOLIB_ERR_NONE) { Fw::Logger::log("radio.transmit() success!\n"); } else { @@ -112,12 +109,9 @@ void SBand ::enableRx() { this->txEnable_out(0, Fw::Logic::LOW); this->rxEnable_out(0, Fw::Logic::HIGH); - int16_t state = this->configure_radio(); - FW_ASSERT(state == RADIOLIB_ERR_NONE); - SX1280* radio = &this->m_rlb_radio; - state = radio->standby(); + int16_t state = radio->standby(); FW_ASSERT(state == RADIOLIB_ERR_NONE); state = radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); FW_ASSERT(state == RADIOLIB_ERR_NONE); @@ -157,4 +151,9 @@ int16_t SBand ::configure_radio() { return state; } +void SBand ::start() { + int16_t state = this->configure_radio(); + FW_ASSERT(state == RADIOLIB_ERR_NONE); +} + } // namespace Components diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index 206aab73..ec46c870 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -25,6 +25,8 @@ class SBand final : public SBandComponentBase { //! Destroy SBand object ~SBand(); + void start(); + using SBandComponentBase::getIRQLine_out; using SBandComponentBase::getTime; using SBandComponentBase::spiSend_out; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index 63c8779b..e1209c4a 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -134,6 +134,7 @@ void setupTopology(const TopologyState& state) { .word_delay = 0, }; spiDriver.configure(state.spi0Device, cfg); + sband.start(); lsm6dsoManager.configure(state.lsm6dsoDevice); lis2mdlManager.configure(state.lis2mdlDevice); ina219SysManager.configure(state.ina219SysDevice); From 5bb58082931777f529131d0c74d79c759549c295 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 19:38:38 -0800 Subject: [PATCH 079/101] Enable rx on start --- FprimeZephyrReference/Components/SBand/SBand.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index 523c6b61..97240162 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -154,6 +154,8 @@ int16_t SBand ::configure_radio() { void SBand ::start() { int16_t state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); + + this->enableRx(); } } // namespace Components From 23af55003aeec2e2b86761686704848bac3116f1 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 19:43:34 -0800 Subject: [PATCH 080/101] automatically re-enable Rx after Tx --- FprimeZephyrReference/Components/SBand/SBand.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index 97240162..dca0fa8c 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -82,7 +82,7 @@ void SBand ::dataReturnIn_handler(FwIndexType portNum, Fw::Buffer& data, const C // ---------------------------------------------------------------------- void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - this->rx_mode = false; + this->rx_mode = false; // possible race condition with check in run_handler char s[] = "Hello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " @@ -97,6 +97,7 @@ void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { Fw::Logger::log("radio.transmit() failed!\n"); Fw::Logger::log("state: %i\n", state); } + this->enableRx(); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } From 7ea7dc400c402528800549f2456fd65651410e41 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 19:48:32 -0800 Subject: [PATCH 081/101] remove RECEIVE command --- FprimeZephyrReference/Components/SBand/SBand.cpp | 5 ----- FprimeZephyrReference/Components/SBand/SBand.fpp | 3 --- FprimeZephyrReference/Components/SBand/SBand.hpp | 7 ------- 3 files changed, 15 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index dca0fa8c..51bc00ca 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -101,11 +101,6 @@ void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } -void SBand ::RECEIVE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - this->enableRx(); - this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); -} - void SBand ::enableRx() { this->txEnable_out(0, Fw::Logic::LOW); this->rxEnable_out(0, Fw::Logic::HIGH); diff --git a/FprimeZephyrReference/Components/SBand/SBand.fpp b/FprimeZephyrReference/Components/SBand/SBand.fpp index cc4b6e06..9182a0af 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.fpp +++ b/FprimeZephyrReference/Components/SBand/SBand.fpp @@ -20,9 +20,6 @@ module Components { @ Command to transmit data sync command TRANSMIT() - @ Command to begin receive - sync command RECEIVE() - @ SPI Output Port output port spiSend: Drv.SpiReadWrite diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index ec46c870..554d3496 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -69,13 +69,6 @@ class SBand final : public SBandComponentBase { U32 cmdSeq //!< The command sequence number ) override; - //! Handler implementation for command RECEIVE - //! - //! Command to begin receive - void RECEIVE_cmdHandler(FwOpcodeType opCode, //!< The opcode - U32 cmdSeq //!< The command sequence number - ) override; - // Configure the SX1280 radio (setup and parameter tuning) int16_t configure_radio(); void enableRx(); From 7faa232534bcbede972bf36895fb6ba70138be99 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Sat, 29 Nov 2025 20:07:07 -0800 Subject: [PATCH 082/101] `Implement dataIn_handler` --- FprimeZephyrReference/Components/SBand/SBand.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index 51bc00ca..ecfd8421 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -65,8 +65,17 @@ void SBand ::run_handler(FwIndexType portNum, U32 context) { // ---------------------------------------------------------------------- void SBand ::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { - // TODO: Implement data transmission - // For now, just return the buffer and indicate success + this->rx_mode = false; // possible race condition with check in run_handler + + this->rxEnable_out(0, Fw::Logic::LOW); + this->txEnable_out(0, Fw::Logic::HIGH); + + int16_t state = this->m_rlb_radio.transmit(data.getData(), data.getSize()); + FW_ASSERT(state == RADIOLIB_ERR_NONE); + + Fw::Logger::log("got dataIn_handler\n"); + + this->enableRx(); Fw::Success returnStatus = Fw::Success::SUCCESS; this->dataReturnOut_out(0, data, context); this->comStatusOut_out(0, returnStatus); From a25d11cbecce7d2f3e773c2bd53caf1e3742f1cf Mon Sep 17 00:00:00 2001 From: ineskhou Date: Sat, 29 Nov 2025 23:03:42 -0800 Subject: [PATCH 083/101] added comdelay instance to sband topology --- FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp | 2 ++ FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp | 5 ++++- FprimeZephyrReference/project/config/AcConstants.fpp | 2 +- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 6b49bb3a..7ea54e86 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -85,6 +85,8 @@ module ReferenceDeployment { instance comDelay: Components.ComDelay base id 0x10025000 + instance comDelaySband: Components.ComDelay base id 0x10024000 + instance lora: Zephyr.LoRa base id 0x10026000 instance comSplitterEvents: Svc.ComSplitter base id 0x10027000 diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 0401eced..507aac06 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -46,6 +46,7 @@ module ReferenceDeployment { instance lsm6dsoManager instance bootloaderTrigger instance comDelay + instance comDelaySband instance burnwire instance antennaDeployer instance comSplitterEvents @@ -139,7 +140,8 @@ module ReferenceDeployment { # ComStub <-> ComDriver (Downlink) ComCcsdsSband.framer.dataOut -> sband.dataIn sband.dataReturnOut -> ComCcsdsSband.framer.dataReturnIn - sband.comStatusOut -> ComCcsdsSband.framer.comStatusIn + sband.comStatusOut -> comDelaySband.comStatusIn + comDelaySband.comStatusOut -> ComCcsdsSband.framer.comStatusIn } @@ -197,6 +199,7 @@ module ReferenceDeployment { rateGroup1Hz.RateGroupMemberOut[4] -> watchdog.run rateGroup1Hz.RateGroupMemberOut[5] -> imuManager.run rateGroup1Hz.RateGroupMemberOut[6] -> comDelay.run + rateGroup1Hz.RateGroupMemberOut[16] -> comDelaySband.run rateGroup1Hz.RateGroupMemberOut[7] -> burnwire.schedIn rateGroup1Hz.RateGroupMemberOut[8] -> antennaDeployer.schedIn rateGroup1Hz.RateGroupMemberOut[9] -> fsSpace.run diff --git a/FprimeZephyrReference/project/config/AcConstants.fpp b/FprimeZephyrReference/project/config/AcConstants.fpp index 2c84297d..267483c6 100644 --- a/FprimeZephyrReference/project/config/AcConstants.fpp +++ b/FprimeZephyrReference/project/config/AcConstants.fpp @@ -4,7 +4,7 @@ # ====================================================================== @ Number of rate group member output ports for ActiveRateGroup -constant ActiveRateGroupOutputPorts = 16 +constant ActiveRateGroupOutputPorts = 20 @ Number of rate group member output ports for PassiveRateGroup constant PassiveRateGroupOutputPorts = 10 From 5d685a4ab69a5f54d8de7709f542e5295c486bb2 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Mon, 1 Dec 2025 13:48:19 -0800 Subject: [PATCH 084/101] add initial comStatusOut_out emission so s band radio is routed packets. --- FprimeZephyrReference/Components/SBand/SBand.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index ecfd8421..64395bcc 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -73,8 +73,6 @@ void SBand ::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg: int16_t state = this->m_rlb_radio.transmit(data.getData(), data.getSize()); FW_ASSERT(state == RADIOLIB_ERR_NONE); - Fw::Logger::log("got dataIn_handler\n"); - this->enableRx(); Fw::Success returnStatus = Fw::Success::SUCCESS; this->dataReturnOut_out(0, data, context); @@ -160,6 +158,9 @@ void SBand ::start() { int16_t state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); + Fw::Success status = Fw::Success::SUCCESS; + this->comStatusOut_out(0, status); + this->enableRx(); } From b913c82341061db7403881711bc027291815aa67 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Tue, 2 Dec 2025 14:18:15 -0800 Subject: [PATCH 085/101] Mutex on SPI commands and component mode access --- FprimeZephyrReference/Components/SBand/SBand.cpp | 6 +++++- FprimeZephyrReference/Components/SBand/SBand.hpp | 2 ++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index 64395bcc..a0fd9c8e 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -31,6 +31,7 @@ SBand ::~SBand() {} // ---------------------------------------------------------------------- void SBand ::run_handler(FwIndexType portNum, U32 context) { + Os::ScopeLock lock(this->m_mutex); if (this->rx_mode) { uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { @@ -65,7 +66,8 @@ void SBand ::run_handler(FwIndexType portNum, U32 context) { // ---------------------------------------------------------------------- void SBand ::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { - this->rx_mode = false; // possible race condition with check in run_handler + Os::ScopeLock lock(this->m_mutex); + this->rx_mode = false; this->rxEnable_out(0, Fw::Logic::LOW); this->txEnable_out(0, Fw::Logic::HIGH); @@ -108,6 +110,7 @@ void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } +// must have mutex held to call this as it touches rx_mode void SBand ::enableRx() { this->txEnable_out(0, Fw::Logic::LOW); this->rxEnable_out(0, Fw::Logic::HIGH); @@ -155,6 +158,7 @@ int16_t SBand ::configure_radio() { } void SBand ::start() { + Os::ScopeLock lock(this->m_mutex); int16_t state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index 554d3496..7caa5496 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -9,6 +9,7 @@ #include "FprimeHal.hpp" #include "FprimeZephyrReference/Components/SBand/SBandComponentAc.hpp" +#include "Os/Mutex.hpp" namespace Components { @@ -78,6 +79,7 @@ class SBand final : public SBandComponentBase { Module m_rlb_module; //!< RadioLib Module instance SX1280 m_rlb_radio; //!< RadioLib SX1280 radio instance bool rx_mode = false; + Os::Mutex m_mutex; //!< Mutex for thread safety }; } // namespace Components From c4715641670faf1fbfd6ac1faffad26e8da109f5 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Tue, 2 Dec 2025 14:30:36 -0800 Subject: [PATCH 086/101] Revert "added comdelay instance to sband topology" This reverts commit a25d11cbecce7d2f3e773c2bd53caf1e3742f1cf. --- FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp | 2 -- FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp | 5 +---- FprimeZephyrReference/project/config/AcConstants.fpp | 2 +- 3 files changed, 2 insertions(+), 7 deletions(-) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 56331b3c..9bc25d34 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -90,8 +90,6 @@ module ReferenceDeployment { instance comDelay: Components.ComDelay base id 0x10025000 - instance comDelaySband: Components.ComDelay base id 0x10024000 - instance lora: Zephyr.LoRa base id 0x10026000 instance comSplitterEvents: Svc.ComSplitter base id 0x10027000 diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index b88c9aef..35431624 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -46,7 +46,6 @@ module ReferenceDeployment { instance lsm6dsoManager instance bootloaderTrigger instance comDelay - instance comDelaySband instance burnwire instance antennaDeployer instance comSplitterEvents @@ -141,8 +140,7 @@ module ReferenceDeployment { # ComStub <-> ComDriver (Downlink) ComCcsdsSband.framer.dataOut -> sband.dataIn sband.dataReturnOut -> ComCcsdsSband.framer.dataReturnIn - sband.comStatusOut -> comDelaySband.comStatusIn - comDelaySband.comStatusOut -> ComCcsdsSband.framer.comStatusIn + sband.comStatusOut -> ComCcsdsSband.framer.comStatusIn } @@ -201,7 +199,6 @@ module ReferenceDeployment { rateGroup1Hz.RateGroupMemberOut[4] -> watchdog.run rateGroup1Hz.RateGroupMemberOut[5] -> imuManager.run rateGroup1Hz.RateGroupMemberOut[6] -> comDelay.run - rateGroup1Hz.RateGroupMemberOut[16] -> comDelaySband.run rateGroup1Hz.RateGroupMemberOut[7] -> burnwire.schedIn rateGroup1Hz.RateGroupMemberOut[8] -> antennaDeployer.schedIn rateGroup1Hz.RateGroupMemberOut[9] -> fsSpace.run diff --git a/FprimeZephyrReference/project/config/AcConstants.fpp b/FprimeZephyrReference/project/config/AcConstants.fpp index 267483c6..2c84297d 100644 --- a/FprimeZephyrReference/project/config/AcConstants.fpp +++ b/FprimeZephyrReference/project/config/AcConstants.fpp @@ -4,7 +4,7 @@ # ====================================================================== @ Number of rate group member output ports for ActiveRateGroup -constant ActiveRateGroupOutputPorts = 20 +constant ActiveRateGroupOutputPorts = 16 @ Number of rate group member output ports for PassiveRateGroup constant PassiveRateGroupOutputPorts = 10 From eda8e60006df8d0c734f53304a720dc96976034b Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 3 Dec 2025 11:20:24 -0800 Subject: [PATCH 087/101] Revert "Mutex on SPI commands and component mode access" This reverts commit b913c82341061db7403881711bc027291815aa67. --- FprimeZephyrReference/Components/SBand/SBand.cpp | 6 +----- FprimeZephyrReference/Components/SBand/SBand.hpp | 2 -- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index a0fd9c8e..64395bcc 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -31,7 +31,6 @@ SBand ::~SBand() {} // ---------------------------------------------------------------------- void SBand ::run_handler(FwIndexType portNum, U32 context) { - Os::ScopeLock lock(this->m_mutex); if (this->rx_mode) { uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { @@ -66,8 +65,7 @@ void SBand ::run_handler(FwIndexType portNum, U32 context) { // ---------------------------------------------------------------------- void SBand ::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { - Os::ScopeLock lock(this->m_mutex); - this->rx_mode = false; + this->rx_mode = false; // possible race condition with check in run_handler this->rxEnable_out(0, Fw::Logic::LOW); this->txEnable_out(0, Fw::Logic::HIGH); @@ -110,7 +108,6 @@ void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } -// must have mutex held to call this as it touches rx_mode void SBand ::enableRx() { this->txEnable_out(0, Fw::Logic::LOW); this->rxEnable_out(0, Fw::Logic::HIGH); @@ -158,7 +155,6 @@ int16_t SBand ::configure_radio() { } void SBand ::start() { - Os::ScopeLock lock(this->m_mutex); int16_t state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index 7caa5496..554d3496 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -9,7 +9,6 @@ #include "FprimeHal.hpp" #include "FprimeZephyrReference/Components/SBand/SBandComponentAc.hpp" -#include "Os/Mutex.hpp" namespace Components { @@ -79,7 +78,6 @@ class SBand final : public SBandComponentBase { Module m_rlb_module; //!< RadioLib Module instance SX1280 m_rlb_radio; //!< RadioLib SX1280 radio instance bool rx_mode = false; - Os::Mutex m_mutex; //!< Mutex for thread safety }; } // namespace Components From c5c6795039ba88ccff76525204be504b579132e4 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 3 Dec 2025 11:36:37 -0800 Subject: [PATCH 088/101] fix packet duplicate check on passthrough --- circuit-python-sband-passthrough/code.py | 1 + 1 file changed, 1 insertion(+) diff --git a/circuit-python-sband-passthrough/code.py b/circuit-python-sband-passthrough/code.py index c7985f1b..3efd4801 100644 --- a/circuit-python-sband-passthrough/code.py +++ b/circuit-python-sband-passthrough/code.py @@ -344,6 +344,7 @@ def all_faces_on(): if packet != last_packet: usb_cdc.data.write(packet) packet_count += 1 + last_packet = packet time_delta = time.time() - time_start if time_delta > 10: print(f"[INFO] Packets received: {packet_count}") From 3259db218eebe9adeda5df885051239c5dfbb6ac Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 3 Dec 2025 11:43:37 -0800 Subject: [PATCH 089/101] Reapply "added comdelay instance to sband topology" This reverts commit c4715641670faf1fbfd6ac1faffad26e8da109f5. --- FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp | 2 ++ FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp | 5 ++++- FprimeZephyrReference/project/config/AcConstants.fpp | 2 +- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 9bc25d34..56331b3c 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -90,6 +90,8 @@ module ReferenceDeployment { instance comDelay: Components.ComDelay base id 0x10025000 + instance comDelaySband: Components.ComDelay base id 0x10024000 + instance lora: Zephyr.LoRa base id 0x10026000 instance comSplitterEvents: Svc.ComSplitter base id 0x10027000 diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 35431624..b88c9aef 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -46,6 +46,7 @@ module ReferenceDeployment { instance lsm6dsoManager instance bootloaderTrigger instance comDelay + instance comDelaySband instance burnwire instance antennaDeployer instance comSplitterEvents @@ -140,7 +141,8 @@ module ReferenceDeployment { # ComStub <-> ComDriver (Downlink) ComCcsdsSband.framer.dataOut -> sband.dataIn sband.dataReturnOut -> ComCcsdsSband.framer.dataReturnIn - sband.comStatusOut -> ComCcsdsSband.framer.comStatusIn + sband.comStatusOut -> comDelaySband.comStatusIn + comDelaySband.comStatusOut -> ComCcsdsSband.framer.comStatusIn } @@ -199,6 +201,7 @@ module ReferenceDeployment { rateGroup1Hz.RateGroupMemberOut[4] -> watchdog.run rateGroup1Hz.RateGroupMemberOut[5] -> imuManager.run rateGroup1Hz.RateGroupMemberOut[6] -> comDelay.run + rateGroup1Hz.RateGroupMemberOut[16] -> comDelaySband.run rateGroup1Hz.RateGroupMemberOut[7] -> burnwire.schedIn rateGroup1Hz.RateGroupMemberOut[8] -> antennaDeployer.schedIn rateGroup1Hz.RateGroupMemberOut[9] -> fsSpace.run diff --git a/FprimeZephyrReference/project/config/AcConstants.fpp b/FprimeZephyrReference/project/config/AcConstants.fpp index 2c84297d..267483c6 100644 --- a/FprimeZephyrReference/project/config/AcConstants.fpp +++ b/FprimeZephyrReference/project/config/AcConstants.fpp @@ -4,7 +4,7 @@ # ====================================================================== @ Number of rate group member output ports for ActiveRateGroup -constant ActiveRateGroupOutputPorts = 16 +constant ActiveRateGroupOutputPorts = 20 @ Number of rate group member output ports for PassiveRateGroup constant PassiveRateGroupOutputPorts = 10 From 7680a8492f2ba80a31244aa68e8a898a9dc6f68c Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 3 Dec 2025 11:51:40 -0800 Subject: [PATCH 090/101] Reapply "Mutex on SPI commands and component mode access" This reverts commit eda8e60006df8d0c734f53304a720dc96976034b. --- FprimeZephyrReference/Components/SBand/SBand.cpp | 6 +++++- FprimeZephyrReference/Components/SBand/SBand.hpp | 2 ++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index 64395bcc..a0fd9c8e 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -31,6 +31,7 @@ SBand ::~SBand() {} // ---------------------------------------------------------------------- void SBand ::run_handler(FwIndexType portNum, U32 context) { + Os::ScopeLock lock(this->m_mutex); if (this->rx_mode) { uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { @@ -65,7 +66,8 @@ void SBand ::run_handler(FwIndexType portNum, U32 context) { // ---------------------------------------------------------------------- void SBand ::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { - this->rx_mode = false; // possible race condition with check in run_handler + Os::ScopeLock lock(this->m_mutex); + this->rx_mode = false; this->rxEnable_out(0, Fw::Logic::LOW); this->txEnable_out(0, Fw::Logic::HIGH); @@ -108,6 +110,7 @@ void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } +// must have mutex held to call this as it touches rx_mode void SBand ::enableRx() { this->txEnable_out(0, Fw::Logic::LOW); this->rxEnable_out(0, Fw::Logic::HIGH); @@ -155,6 +158,7 @@ int16_t SBand ::configure_radio() { } void SBand ::start() { + Os::ScopeLock lock(this->m_mutex); int16_t state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index 554d3496..7caa5496 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -9,6 +9,7 @@ #include "FprimeHal.hpp" #include "FprimeZephyrReference/Components/SBand/SBandComponentAc.hpp" +#include "Os/Mutex.hpp" namespace Components { @@ -78,6 +79,7 @@ class SBand final : public SBandComponentBase { Module m_rlb_module; //!< RadioLib Module instance SX1280 m_rlb_radio; //!< RadioLib SX1280 radio instance bool rx_mode = false; + Os::Mutex m_mutex; //!< Mutex for thread safety }; } // namespace Components From d68fcb45b3b0560fed4b00828cdc6f24a66aa88e Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 3 Dec 2025 13:27:25 -0800 Subject: [PATCH 091/101] remove TRANSMIT command --- .../Components/SBand/SBand.cpp | 24 ------------------- .../Components/SBand/SBand.fpp | 3 --- .../Components/SBand/SBand.hpp | 11 --------- 3 files changed, 38 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index a0fd9c8e..31fd03b0 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -86,30 +86,6 @@ void SBand ::dataReturnIn_handler(FwIndexType portNum, Fw::Buffer& data, const C this->deallocate_out(0, data); } -// ---------------------------------------------------------------------- -// Handler implementations for commands -// ---------------------------------------------------------------------- - -void SBand ::TRANSMIT_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - this->rx_mode = false; // possible race condition with check in run_handler - - char s[] = - "Hello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " - "world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, world!\nHello, " - "world!\nHello, world!\nHello, world!\nHello, world!\n"; - this->rxEnable_out(0, Fw::Logic::LOW); - this->txEnable_out(0, Fw::Logic::HIGH); - int16_t state = this->m_rlb_radio.transmit(s, sizeof(s)); - if (state == RADIOLIB_ERR_NONE) { - Fw::Logger::log("radio.transmit() success!\n"); - } else { - Fw::Logger::log("radio.transmit() failed!\n"); - Fw::Logger::log("state: %i\n", state); - } - this->enableRx(); - this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); -} - // must have mutex held to call this as it touches rx_mode void SBand ::enableRx() { this->txEnable_out(0, Fw::Logic::LOW); diff --git a/FprimeZephyrReference/Components/SBand/SBand.fpp b/FprimeZephyrReference/Components/SBand/SBand.fpp index 9182a0af..0774017e 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.fpp +++ b/FprimeZephyrReference/Components/SBand/SBand.fpp @@ -17,9 +17,6 @@ module Components { @ Import the allocation interface import Svc.BufferAllocation - @ Command to transmit data - sync command TRANSMIT() - @ SPI Output Port output port spiSend: Drv.SpiReadWrite diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index 7caa5496..6b549f9b 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -59,17 +59,6 @@ class SBand final : public SBandComponentBase { const ComCfg::FrameContext& context) override; private: - // ---------------------------------------------------------------------- - // Handler implementations for commands - // ---------------------------------------------------------------------- - - //! Handler implementation for command TRANSMIT - //! - //! Command to transmit data - void TRANSMIT_cmdHandler(FwOpcodeType opCode, //!< The opcode - U32 cmdSeq //!< The command sequence number - ) override; - // Configure the SX1280 radio (setup and parameter tuning) int16_t configure_radio(); void enableRx(); From 995aba1a515fd1450656b693563a6e43860cb2fb Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 3 Dec 2025 13:34:58 -0800 Subject: [PATCH 092/101] remove logging in `run_handler` --- FprimeZephyrReference/Components/SBand/SBand.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index 31fd03b0..d51c7e72 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -41,16 +41,6 @@ void SBand ::run_handler(FwIndexType portNum, U32 context) { radio->readData(data, len); radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); - Fw::Logger::log("MESSAGE RECEIVED:\n"); - char msg[sizeof(data) * 3 + 1]; - for (size_t i = 0; i < len; ++i) - sprintf(msg + i * 3, "%02X ", data[i]); // NOLINT(runtime/printf) - msg[len * 3] = '\0'; - Fw::Logger::log("%s\n", msg); - - // Allocate buffer and send received data to F' system - // This goes to ComCcsdsSband.frameAccumulator.dataIn, which processes the uplink frames - // @ jack may want to check here for any edits for how the s band actually processes it Fw::Buffer buffer = this->allocate_out(0, static_cast(len)); if (buffer.isValid()) { (void)::memcpy(buffer.getData(), data, len); From 401b4a99e0a8b0eaecccf5d731d8fbc53134af4e Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 3 Dec 2025 16:13:52 -0800 Subject: [PATCH 093/101] defer receive handling to prevent rate group slips --- .../Components/SBand/SBand.cpp | 40 +++++++++++------- .../Components/SBand/SBand.fpp | 5 ++- .../Components/SBand/SBand.hpp | 41 ++++++++++++++++++- .../Top/ReferenceDeploymentTopology.cpp | 2 +- .../ReferenceDeployment/Top/instances.fpp | 5 ++- 5 files changed, 73 insertions(+), 20 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index d51c7e72..d632c814 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -31,24 +31,34 @@ SBand ::~SBand() {} // ---------------------------------------------------------------------- void SBand ::run_handler(FwIndexType portNum, U32 context) { + uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); + + if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { + if (m_irqPending.trySetPending()) { + this->deferredRxHandler_internalInterfaceInvoke(); + } + } +} + +void SBand ::deferredRxHandler_internalInterfaceHandler() { Os::ScopeLock lock(this->m_mutex); + if (this->rx_mode) { - uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); - if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { - SX1280* radio = &this->m_rlb_radio; - uint8_t data[256] = {0}; - size_t len = radio->getPacketLength(); - radio->readData(data, len); - radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); - - Fw::Buffer buffer = this->allocate_out(0, static_cast(len)); - if (buffer.isValid()) { - (void)::memcpy(buffer.getData(), data, len); - ComCfg::FrameContext frameContext; - this->dataOut_out(0, buffer, frameContext); - } + SX1280* radio = &this->m_rlb_radio; + uint8_t data[256] = {0}; + size_t len = radio->getPacketLength(); + radio->readData(data, len); + radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); + + Fw::Buffer buffer = this->allocate_out(0, static_cast(len)); + if (buffer.isValid()) { + (void)::memcpy(buffer.getData(), data, len); + ComCfg::FrameContext frameContext; + this->dataOut_out(0, buffer, frameContext); } } + + m_irqPending.clearPending(); } // ---------------------------------------------------------------------- @@ -123,7 +133,7 @@ int16_t SBand ::configure_radio() { return state; } -void SBand ::start() { +void SBand ::configureRadio() { Os::ScopeLock lock(this->m_mutex); int16_t state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); diff --git a/FprimeZephyrReference/Components/SBand/SBand.fpp b/FprimeZephyrReference/Components/SBand/SBand.fpp index 0774017e..1622bf8b 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.fpp +++ b/FprimeZephyrReference/Components/SBand/SBand.fpp @@ -2,7 +2,7 @@ module Components { @ Component for F Prime FSW framework. - passive component SBand { + active component SBand { ############################################################################## #### Uncomment the following examples to start customizing your component #### @@ -11,6 +11,9 @@ module Components { @ Port receiving calls from the rate group sync input port run: Svc.Sched + @ Internal port for deferred RX processing + internal port deferredRxHandler() priority 10 + @ Import Communication Interface import Svc.Com diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index 6b549f9b..a3fd3f5c 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -14,6 +14,37 @@ namespace Components { class SBand final : public SBandComponentBase { + private: + //! Thread-safe monitor for IRQ pending flag + class IrqPendingMonitor { + public: + IrqPendingMonitor() : m_pending(false) {} + + void setPending() { + Os::ScopeLock lock(m_mutex); + m_pending = true; + } + + void clearPending() { + Os::ScopeLock lock(m_mutex); + m_pending = false; + } + + //! Atomic test-and-set: returns true if flag was clear, false if already set + bool trySetPending() { + Os::ScopeLock lock(m_mutex); + if (m_pending) { + return false; + } + m_pending = true; + return true; + } + + private: + Os::Mutex m_mutex; + bool m_pending; + }; + public: // ---------------------------------------------------------------------- // Component construction and destruction @@ -26,7 +57,7 @@ class SBand final : public SBandComponentBase { //! Destroy SBand object ~SBand(); - void start(); + void configureRadio(); using SBandComponentBase::getIRQLine_out; using SBandComponentBase::getTime; @@ -58,6 +89,11 @@ class SBand final : public SBandComponentBase { Fw::Buffer& data, const ComCfg::FrameContext& context) override; + //! Handler implementation for deferredRxHandler + //! + //! Internal async handler for processing received data + void deferredRxHandler_internalInterfaceHandler() override; + private: // Configure the SX1280 radio (setup and parameter tuning) int16_t configure_radio(); @@ -68,7 +104,8 @@ class SBand final : public SBandComponentBase { Module m_rlb_module; //!< RadioLib Module instance SX1280 m_rlb_radio; //!< RadioLib SX1280 radio instance bool rx_mode = false; - Os::Mutex m_mutex; //!< Mutex for thread safety + Os::Mutex m_mutex; //!< Mutex for thread safety + IrqPendingMonitor m_irqPending; //!< Monitor for deferred handler state }; } // namespace Components diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index e1209c4a..ed60e279 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -134,7 +134,7 @@ void setupTopology(const TopologyState& state) { .word_delay = 0, }; spiDriver.configure(state.spi0Device, cfg); - sband.start(); + sband.configureRadio(); lsm6dsoManager.configure(state.lsm6dsoDevice); lis2mdlManager.configure(state.lis2mdlDevice); ina219SysManager.configure(state.ina219SysDevice); diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 56331b3c..d5fac09c 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -102,7 +102,10 @@ module ReferenceDeployment { instance spiDriver: Zephyr.ZephyrSpiDriver base id 0x10050000 - instance sband : Components.SBand base id 0x10051000 + instance sband : Components.SBand base id 0x10051000 \ + queue size Default.QUEUE_SIZE \ + stack size Default.STACK_SIZE \ + priority 10 instance gpioSbandNrst: Zephyr.ZephyrGpioDriver base id 0x10052000 From db2f1205558a5c27c8b55fdfd332ac689cc75fe3 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 3 Dec 2025 18:25:11 -0800 Subject: [PATCH 094/101] Defer transmit as well to prevent mutual access issues; wait until configure is called to do anything. --- .../Components/SBand/SBand.cpp | 51 +++++++++++++------ .../Components/SBand/SBand.fpp | 3 ++ .../Components/SBand/SBand.hpp | 48 ++++------------- 3 files changed, 49 insertions(+), 53 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index d632c814..2f5c8984 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -31,19 +31,25 @@ SBand ::~SBand() {} // ---------------------------------------------------------------------- void SBand ::run_handler(FwIndexType portNum, U32 context) { - uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); + // Only process if radio is configured + if (!m_configured) { + return; + } - if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { - if (m_irqPending.trySetPending()) { - this->deferredRxHandler_internalInterfaceInvoke(); - } + // Queue RX handler only if not already queued + if (!m_rxHandlerQueued) { + m_rxHandlerQueued = true; + this->deferredRxHandler_internalInterfaceInvoke(); } } void SBand ::deferredRxHandler_internalInterfaceHandler() { - Os::ScopeLock lock(this->m_mutex); + // Check IRQ status + uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); - if (this->rx_mode) { + // Only process if RX_DONE + if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { + // Process received data SX1280* radio = &this->m_rlb_radio; uint8_t data[256] = {0}; size_t len = radio->getPacketLength(); @@ -58,7 +64,13 @@ void SBand ::deferredRxHandler_internalInterfaceHandler() { } } - m_irqPending.clearPending(); + // Clear the queued flag + m_rxHandlerQueued = false; +} + +void SBand ::deferredTxHandler_internalInterfaceHandler() { + // Re-enable RX mode after transmission + this->enableRx(); } // ---------------------------------------------------------------------- @@ -66,19 +78,29 @@ void SBand ::deferredRxHandler_internalInterfaceHandler() { // ---------------------------------------------------------------------- void SBand ::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { - Os::ScopeLock lock(this->m_mutex); - this->rx_mode = false; + // Only process if radio is configured + if (!m_configured) { + Fw::Success failureStatus = Fw::Success::FAILURE; + this->dataReturnOut_out(0, data, context); + this->comStatusOut_out(0, failureStatus); + return; + } + // Switch to TX mode this->rxEnable_out(0, Fw::Logic::LOW); this->txEnable_out(0, Fw::Logic::HIGH); + // Transmit data int16_t state = this->m_rlb_radio.transmit(data.getData(), data.getSize()); FW_ASSERT(state == RADIOLIB_ERR_NONE); - this->enableRx(); + // Return buffer and status Fw::Success returnStatus = Fw::Success::SUCCESS; this->dataReturnOut_out(0, data, context); this->comStatusOut_out(0, returnStatus); + + // Queue deferred handler to re-enable RX + this->deferredTxHandler_internalInterfaceInvoke(); } void SBand ::dataReturnIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { @@ -86,7 +108,6 @@ void SBand ::dataReturnIn_handler(FwIndexType portNum, Fw::Buffer& data, const C this->deallocate_out(0, data); } -// must have mutex held to call this as it touches rx_mode void SBand ::enableRx() { this->txEnable_out(0, Fw::Logic::LOW); this->rxEnable_out(0, Fw::Logic::HIGH); @@ -97,8 +118,6 @@ void SBand ::enableRx() { FW_ASSERT(state == RADIOLIB_ERR_NONE); state = radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); FW_ASSERT(state == RADIOLIB_ERR_NONE); - - this->rx_mode = true; } int16_t SBand ::configure_radio() { @@ -134,10 +153,12 @@ int16_t SBand ::configure_radio() { } void SBand ::configureRadio() { - Os::ScopeLock lock(this->m_mutex); int16_t state = this->configure_radio(); FW_ASSERT(state == RADIOLIB_ERR_NONE); + // Mark as configured + m_configured = true; + Fw::Success status = Fw::Success::SUCCESS; this->comStatusOut_out(0, status); diff --git a/FprimeZephyrReference/Components/SBand/SBand.fpp b/FprimeZephyrReference/Components/SBand/SBand.fpp index 1622bf8b..db8f65bc 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.fpp +++ b/FprimeZephyrReference/Components/SBand/SBand.fpp @@ -14,6 +14,9 @@ module Components { @ Internal port for deferred RX processing internal port deferredRxHandler() priority 10 + @ Internal port for deferred TX processing + internal port deferredTxHandler() priority 10 + @ Import Communication Interface import Svc.Com diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index a3fd3f5c..ec310b65 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -9,42 +9,10 @@ #include "FprimeHal.hpp" #include "FprimeZephyrReference/Components/SBand/SBandComponentAc.hpp" -#include "Os/Mutex.hpp" namespace Components { class SBand final : public SBandComponentBase { - private: - //! Thread-safe monitor for IRQ pending flag - class IrqPendingMonitor { - public: - IrqPendingMonitor() : m_pending(false) {} - - void setPending() { - Os::ScopeLock lock(m_mutex); - m_pending = true; - } - - void clearPending() { - Os::ScopeLock lock(m_mutex); - m_pending = false; - } - - //! Atomic test-and-set: returns true if flag was clear, false if already set - bool trySetPending() { - Os::ScopeLock lock(m_mutex); - if (m_pending) { - return false; - } - m_pending = true; - return true; - } - - private: - Os::Mutex m_mutex; - bool m_pending; - }; - public: // ---------------------------------------------------------------------- // Component construction and destruction @@ -94,18 +62,22 @@ class SBand final : public SBandComponentBase { //! Internal async handler for processing received data void deferredRxHandler_internalInterfaceHandler() override; + //! Handler implementation for deferredTxHandler + //! + //! Internal async handler for processing transmitted data + void deferredTxHandler_internalInterfaceHandler() override; + private: // Configure the SX1280 radio (setup and parameter tuning) int16_t configure_radio(); void enableRx(); private: - FprimeHal m_rlb_hal; //!< RadioLib HAL instance - Module m_rlb_module; //!< RadioLib Module instance - SX1280 m_rlb_radio; //!< RadioLib SX1280 radio instance - bool rx_mode = false; - Os::Mutex m_mutex; //!< Mutex for thread safety - IrqPendingMonitor m_irqPending; //!< Monitor for deferred handler state + FprimeHal m_rlb_hal; //!< RadioLib HAL instance + Module m_rlb_module; //!< RadioLib Module instance + SX1280 m_rlb_radio; //!< RadioLib SX1280 radio instance + bool m_configured = false; //!< Flag indicating radio is configured + bool m_rxHandlerQueued = false; //!< Flag indicating RX handler is queued }; } // namespace Components From 6715e376fa5a829c461541e49eb0626a069763e1 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 3 Dec 2025 18:51:49 -0800 Subject: [PATCH 095/101] really defer tx --- .../Components/SBand/SBand.cpp | 33 ++++++++++--------- .../Components/SBand/SBand.fpp | 5 ++- .../Components/SBand/SBand.hpp | 3 +- 3 files changed, 23 insertions(+), 18 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index 2f5c8984..8381020d 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -68,7 +68,21 @@ void SBand ::deferredRxHandler_internalInterfaceHandler() { m_rxHandlerQueued = false; } -void SBand ::deferredTxHandler_internalInterfaceHandler() { +void SBand ::deferredTxHandler_internalInterfaceHandler(const Fw::Buffer& data, const ComCfg::FrameContext& context) { + // Switch to TX mode + this->rxEnable_out(0, Fw::Logic::LOW); + this->txEnable_out(0, Fw::Logic::HIGH); + + // Transmit data + int16_t state = this->m_rlb_radio.transmit(data.getData(), data.getSize()); + FW_ASSERT(state == RADIOLIB_ERR_NONE); + + // Return buffer and status (need mutable copy for dataReturnOut_out) + Fw::Buffer mutableData = data; + Fw::Success returnStatus = Fw::Success::SUCCESS; + this->dataReturnOut_out(0, mutableData, context); + this->comStatusOut_out(0, returnStatus); + // Re-enable RX mode after transmission this->enableRx(); } @@ -86,21 +100,8 @@ void SBand ::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg: return; } - // Switch to TX mode - this->rxEnable_out(0, Fw::Logic::LOW); - this->txEnable_out(0, Fw::Logic::HIGH); - - // Transmit data - int16_t state = this->m_rlb_radio.transmit(data.getData(), data.getSize()); - FW_ASSERT(state == RADIOLIB_ERR_NONE); - - // Return buffer and status - Fw::Success returnStatus = Fw::Success::SUCCESS; - this->dataReturnOut_out(0, data, context); - this->comStatusOut_out(0, returnStatus); - - // Queue deferred handler to re-enable RX - this->deferredTxHandler_internalInterfaceInvoke(); + // Queue deferred handler to perform transmission + this->deferredTxHandler_internalInterfaceInvoke(data, context); } void SBand ::dataReturnIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { diff --git a/FprimeZephyrReference/Components/SBand/SBand.fpp b/FprimeZephyrReference/Components/SBand/SBand.fpp index db8f65bc..a48e344a 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.fpp +++ b/FprimeZephyrReference/Components/SBand/SBand.fpp @@ -15,7 +15,10 @@ module Components { internal port deferredRxHandler() priority 10 @ Internal port for deferred TX processing - internal port deferredTxHandler() priority 10 + internal port deferredTxHandler( + data: Fw.Buffer + context: ComCfg.FrameContext + ) priority 10 @ Import Communication Interface import Svc.Com diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index ec310b65..d5cb7415 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -65,7 +65,8 @@ class SBand final : public SBandComponentBase { //! Handler implementation for deferredTxHandler //! //! Internal async handler for processing transmitted data - void deferredTxHandler_internalInterfaceHandler() override; + void deferredTxHandler_internalInterfaceHandler(const Fw::Buffer& data, + const ComCfg::FrameContext& context) override; private: // Configure the SX1280 radio (setup and parameter tuning) From 901a84009e38abed23acf610aef97de846d424a4 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Wed, 3 Dec 2025 20:46:55 -0800 Subject: [PATCH 096/101] add proper error logging --- .../Components/SBand/SBand.cpp | 131 +++++++++++++----- .../Components/SBand/SBand.fpp | 18 +++ .../Components/SBand/SBand.hpp | 15 +- 3 files changed, 129 insertions(+), 35 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index 8381020d..bd2d3585 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -53,14 +53,28 @@ void SBand ::deferredRxHandler_internalInterfaceHandler() { SX1280* radio = &this->m_rlb_radio; uint8_t data[256] = {0}; size_t len = radio->getPacketLength(); - radio->readData(data, len); - radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); - - Fw::Buffer buffer = this->allocate_out(0, static_cast(len)); - if (buffer.isValid()) { - (void)::memcpy(buffer.getData(), data, len); - ComCfg::FrameContext frameContext; - this->dataOut_out(0, buffer, frameContext); + int16_t state = radio->readData(data, len); + + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + } else { + Fw::Buffer buffer = this->allocate_out(0, static_cast(len)); + if (buffer.isValid()) { + (void)::memcpy(buffer.getData(), data, len); + ComCfg::FrameContext frameContext; + this->dataOut_out(0, buffer, frameContext); + // Clear throttled warnings on success + this->log_WARNING_HI_RadioLibFailed_ThrottleClear(); + this->log_WARNING_HI_AllocationFailed_ThrottleClear(); + } else { + this->log_WARNING_HI_AllocationFailed(static_cast(len)); + } + } + + // Re-enable receive mode + state = radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); } } @@ -69,22 +83,31 @@ void SBand ::deferredRxHandler_internalInterfaceHandler() { } void SBand ::deferredTxHandler_internalInterfaceHandler(const Fw::Buffer& data, const ComCfg::FrameContext& context) { - // Switch to TX mode - this->rxEnable_out(0, Fw::Logic::LOW); - this->txEnable_out(0, Fw::Logic::HIGH); - - // Transmit data - int16_t state = this->m_rlb_radio.transmit(data.getData(), data.getSize()); - FW_ASSERT(state == RADIOLIB_ERR_NONE); + Fw::Success returnStatus = Fw::Success::FAILURE; + + // Enable transmit mode + Status status = this->enableTx(); + if (status == Status::SUCCESS) { + // Transmit data + int16_t state = this->m_rlb_radio.transmit(data.getData(), data.getSize()); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + returnStatus = Fw::Success::FAILURE; + } else { + returnStatus = Fw::Success::SUCCESS; + // Clear throttled warnings on success + this->log_WARNING_HI_RadioLibFailed_ThrottleClear(); + } + } // Return buffer and status (need mutable copy for dataReturnOut_out) Fw::Buffer mutableData = data; - Fw::Success returnStatus = Fw::Success::SUCCESS; this->dataReturnOut_out(0, mutableData, context); this->comStatusOut_out(0, returnStatus); // Re-enable RX mode after transmission - this->enableRx(); + status = this->enableRx(); + // enableRx will log RadioLibFailed internally if it fails } // ---------------------------------------------------------------------- @@ -94,6 +117,7 @@ void SBand ::deferredTxHandler_internalInterfaceHandler(const Fw::Buffer& data, void SBand ::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { // Only process if radio is configured if (!m_configured) { + this->log_WARNING_HI_RadioNotConfigured(); Fw::Success failureStatus = Fw::Success::FAILURE; this->dataReturnOut_out(0, data, context); this->comStatusOut_out(0, failureStatus); @@ -109,61 +133,104 @@ void SBand ::dataReturnIn_handler(FwIndexType portNum, Fw::Buffer& data, const C this->deallocate_out(0, data); } -void SBand ::enableRx() { +SBand::Status SBand ::enableRx() { this->txEnable_out(0, Fw::Logic::LOW); this->rxEnable_out(0, Fw::Logic::HIGH); SX1280* radio = &this->m_rlb_radio; int16_t state = radio->standby(); - FW_ASSERT(state == RADIOLIB_ERR_NONE); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + state = radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); - FW_ASSERT(state == RADIOLIB_ERR_NONE); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + return Status::SUCCESS; +} + +SBand::Status SBand ::enableTx() { + this->rxEnable_out(0, Fw::Logic::LOW); + this->txEnable_out(0, Fw::Logic::HIGH); + + SX1280* radio = &this->m_rlb_radio; + + int16_t state = radio->standby(); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + return Status::SUCCESS; } -int16_t SBand ::configure_radio() { - int state = this->m_rlb_radio.begin(); +SBand::Status SBand ::configure_radio() { + int16_t state = this->m_rlb_radio.begin(); if (state != RADIOLIB_ERR_NONE) { - return state; + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; } state = this->m_rlb_radio.setOutputPower(13); // 13dB is max if (state != RADIOLIB_ERR_NONE) { - return state; + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; } // Match modulation parameters to CircuitPython defaults state = this->m_rlb_radio.setSpreadingFactor(7); if (state != RADIOLIB_ERR_NONE) { - return state; + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; } state = this->m_rlb_radio.setBandwidth(406.25); if (state != RADIOLIB_ERR_NONE) { - return state; + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; } state = this->m_rlb_radio.setCodingRate(5); if (state != RADIOLIB_ERR_NONE) { - return state; + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; } state = this->m_rlb_radio.setPacketParamsLoRa(12, RADIOLIB_SX128X_LORA_HEADER_EXPLICIT, 255, RADIOLIB_SX128X_LORA_CRC_ON, RADIOLIB_SX128X_LORA_IQ_STANDARD); - return state; + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + + return Status::SUCCESS; } -void SBand ::configureRadio() { - int16_t state = this->configure_radio(); - FW_ASSERT(state == RADIOLIB_ERR_NONE); +SBand::Status SBand ::configureRadio() { + Status config_status = this->configure_radio(); + if (config_status != Status::SUCCESS) { + // configure_radio logs RadioLibFailed internally + return Status::ERROR; + } // Mark as configured m_configured = true; + // Enable RX mode + Status rx_status = this->enableRx(); + if (rx_status != Status::SUCCESS) { + // enableRx logs RadioLibFailed internally + return Status::ERROR; + } + + // Send success status Fw::Success status = Fw::Success::SUCCESS; this->comStatusOut_out(0, status); - this->enableRx(); + return Status::SUCCESS; } } // namespace Components diff --git a/FprimeZephyrReference/Components/SBand/SBand.fpp b/FprimeZephyrReference/Components/SBand/SBand.fpp index a48e344a..74d30db6 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.fpp +++ b/FprimeZephyrReference/Components/SBand/SBand.fpp @@ -41,6 +41,24 @@ module Components { @ S-Band IRQ Line output port getIRQLine: Drv.GpioRead + @ Event to indicate RadioLib call failure + event RadioLibFailed(error: I16) severity warning high \ + format "SBand RadioLib call failed, error: {}" throttle 2 + + @ Event to indicate allocation failure + event AllocationFailed(allocation_size: FwSizeType) severity warning high \ + format "Failed to allocate buffer of: {} bytes" throttle 2 + + @ Event to indicate radio not configured + event RadioNotConfigured() severity warning high \ + format "Radio not configured, operation ignored" throttle 3 + + @ Last received RSSI (if available) + telemetry LastRssi: I16 update on change + + @ Last received SNR (if available) + telemetry LastSnr: I8 update on change + ############################################################################### # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # ############################################################################### diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp index d5cb7415..168e845d 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.hpp +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -14,6 +14,9 @@ namespace Components { class SBand final : public SBandComponentBase { public: + // Status returned from various SBand operations + enum Status { ERROR, SUCCESS }; + // ---------------------------------------------------------------------- // Component construction and destruction // ---------------------------------------------------------------------- @@ -25,7 +28,8 @@ class SBand final : public SBandComponentBase { //! Destroy SBand object ~SBand(); - void configureRadio(); + //! Configure the radio and start operation + Status configureRadio(); using SBandComponentBase::getIRQLine_out; using SBandComponentBase::getTime; @@ -70,8 +74,13 @@ class SBand final : public SBandComponentBase { private: // Configure the SX1280 radio (setup and parameter tuning) - int16_t configure_radio(); - void enableRx(); + Status configure_radio(); + + //! Enable receive mode + Status enableRx(); + + //! Enable transmit mode + Status enableTx(); private: FprimeHal m_rlb_hal; //!< RadioLib HAL instance From 4e9853639165709778af54033b4c9092057794d4 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 4 Dec 2025 09:38:45 -0800 Subject: [PATCH 097/101] Remove unimplemented telemetry --- FprimeZephyrReference/Components/SBand/SBand.fpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.fpp b/FprimeZephyrReference/Components/SBand/SBand.fpp index 74d30db6..bf899a01 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.fpp +++ b/FprimeZephyrReference/Components/SBand/SBand.fpp @@ -53,11 +53,7 @@ module Components { event RadioNotConfigured() severity warning high \ format "Radio not configured, operation ignored" throttle 3 - @ Last received RSSI (if available) - telemetry LastRssi: I16 update on change - - @ Last received SNR (if available) - telemetry LastSnr: I8 update on change + # TODO add RSSI and SNR telemetry ############################################################################### # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # From 81af10566c5e721a6abb2f1d45c077dec68fc414 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 4 Dec 2025 10:59:15 -0800 Subject: [PATCH 098/101] implement RSSI and SNR telemetry --- FprimeZephyrReference/Components/SBand/SBand.cpp | 7 +++++++ FprimeZephyrReference/Components/SBand/SBand.fpp | 6 +++++- .../Top/ReferenceDeploymentPackets.fppi | 4 ++++ FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp | 2 +- 4 files changed, 17 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp index bd2d3585..e980615d 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.cpp +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -63,6 +63,13 @@ void SBand ::deferredRxHandler_internalInterfaceHandler() { (void)::memcpy(buffer.getData(), data, len); ComCfg::FrameContext frameContext; this->dataOut_out(0, buffer, frameContext); + + // Log RSSI and SNR for received packet + float rssi = radio->getRSSI(); + float snr = radio->getSNR(); + this->tlmWrite_LastRssi(rssi); + this->tlmWrite_LastSnr(snr); + // Clear throttled warnings on success this->log_WARNING_HI_RadioLibFailed_ThrottleClear(); this->log_WARNING_HI_AllocationFailed_ThrottleClear(); diff --git a/FprimeZephyrReference/Components/SBand/SBand.fpp b/FprimeZephyrReference/Components/SBand/SBand.fpp index bf899a01..e699f2a2 100644 --- a/FprimeZephyrReference/Components/SBand/SBand.fpp +++ b/FprimeZephyrReference/Components/SBand/SBand.fpp @@ -53,7 +53,11 @@ module Components { event RadioNotConfigured() severity warning high \ format "Radio not configured, operation ignored" throttle 3 - # TODO add RSSI and SNR telemetry + @ Last received RSSI (if available) + telemetry LastRssi: F32 update on change + + @ Last received SNR (if available) + telemetry LastSnr: F32 update on change ############################################################################### # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi index 7963a1a9..5691105b 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi @@ -98,7 +98,11 @@ telemetry packets ReferenceDeploymentPackets { ComCcsdsUart.authenticate.AuthenticatedPacketsCount ComCcsdsUart.authenticate.RejectedPacketsCount ComCcsdsUart.authenticate.CurrentSequenceNumber + } + packet SBand id 12 group 4 { + sband.LastRssi + sband.LastSnr } } omit { diff --git a/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp b/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp index 99a1c693..a26f7709 100644 --- a/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp +++ b/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp @@ -29,7 +29,7 @@ static const FwChanIdType TLMPACKETIZER_HASH_MOD_VALUE = // are added (e.g., ModeManager: CurrentMode, SafeModeEntryCount), this sizing is // intentionally over-provisioned to support planned expansion and to ensure efficient hash table usage. static const FwChanIdType TLMPACKETIZER_HASH_BUCKETS = - 100; // !< Buckets assignable to a hash slot. + 128; // !< Buckets assignable to a hash slot. // Buckets must be >= number of telemetry channels in system static const FwChanIdType TLMPACKETIZER_MAX_MISSING_TLM_CHECK = 25; // !< Maximum number of missing telemetry channel checks From 58244d5256852ce1e79b311e55f456e1fab67686 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 4 Dec 2025 11:43:23 -0800 Subject: [PATCH 099/101] update fprime-zephyr with log statement removal --- lib/fprime-zephyr | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/fprime-zephyr b/lib/fprime-zephyr index 66ea1412..3e8c4d7a 160000 --- a/lib/fprime-zephyr +++ b/lib/fprime-zephyr @@ -1 +1 @@ -Subproject commit 66ea141240a6cb0f4abab2815a8e41bc4b144b81 +Subproject commit 3e8c4d7afc9106aeef0917cb7e3bd1cf94bc9628 From dc28de293a1d7bca10428cd9531b4cf77436b1d1 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 4 Dec 2025 11:51:21 -0800 Subject: [PATCH 100/101] move lib/fprime-reference to branch with starch's changes and the log removal --- lib/fprime-zephyr | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/fprime-zephyr b/lib/fprime-zephyr index 3e8c4d7a..d511ccaf 160000 --- a/lib/fprime-zephyr +++ b/lib/fprime-zephyr @@ -1 +1 @@ -Subproject commit 3e8c4d7afc9106aeef0917cb7e3bd1cf94bc9628 +Subproject commit d511ccaf7ffd41413e595a74f22f7424cab5dbb5 From 74338e0cb92db426f485e7606cf98543abdf9285 Mon Sep 17 00:00:00 2001 From: Jack Pearson Date: Thu, 4 Dec 2025 12:20:09 -0800 Subject: [PATCH 101/101] write SDD for S band radio --- .../Components/SBand/docs/sdd.md | 42 +++++++++---------- 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/FprimeZephyrReference/Components/SBand/docs/sdd.md b/FprimeZephyrReference/Components/SBand/docs/sdd.md index 86af828c..071c3bcd 100644 --- a/FprimeZephyrReference/Components/SBand/docs/sdd.md +++ b/FprimeZephyrReference/Components/SBand/docs/sdd.md @@ -1,32 +1,24 @@ -# FprimeZephyrReference::SBand +# Components::SBand -Component for F Prime FSW framework. - -## Usage Examples -Add usage examples here - -### Diagrams -Add diagrams here +Wrapper for the [RadioLib SX1280 driver](https://github.com/jgromes/RadioLib). This component integrates into the F Prime communication stack. ### Typical Usage -And the typical usage of the component here -## Class Diagram -Add a class diagram here +The component should be connected in accordance with the [F Prime communication adapter interface](../../../../lib/fprime/docs/reference/communication-adapter-interface.md). + ## Port Descriptions -| Name | Description | -|---|---| -|---|---| -## Component States -Add component states in the chart below | Name | Description | |---|---| -|---|---| - -## Sequence Diagrams -Add sequence diagrams here +| run | Scheduler port called by rate group to check for received data | +| Svc.Com | Standard communication interface (dataIn, dataOut, dataReturnIn, dataReturnOut, comStatusIn, comStatusOut) | +| Svc.BufferAllocation | Buffer allocation interface (allocate, deallocate) | +| spiSend | SPI communication with the SX1280 radio | +| resetSend | GPIO control for radio module reset | +| txEnable | GPIO control for S-Band TX enable | +| rxEnable | GPIO control for S-Band RX enable | +| getIRQLine | GPIO read for S-Band IRQ line status | ## Parameters | Name | Description | @@ -41,15 +33,19 @@ Add sequence diagrams here ## Events | Name | Description | |---|---| -|---|---| +| RadioLibFailed | RadioLib call failed with error code (throttled: 2) | +| AllocationFailed | Failed to allocate buffer for received data (throttled: 2) | +| RadioNotConfigured | Radio not configured, operation ignored (throttled: 3) | ## Telemetry | Name | Description | |---|---| -|---|---| +| LastRssi | RSSI (Received Signal Strength Indicator) of last received packet in dBm | +| LastSnr | SNR (Signal-to-Noise Ratio) of last received packet in dB | + ## Unit Tests -Add unit test descriptions in the chart below + | Name | Description | Output | Coverage | |---|---|---|---| |---|---|---|---|