diff --git a/.gitignore b/.gitignore index be91b21147e8842750043ac7982987bb86bfb2d4..0c1482597615bca1daaa4fe5261dc01c874d9a1c 100644 --- a/.gitignore +++ b/.gitignore @@ -11,9 +11,9 @@ bin/* .*.sw? .*.pyc -.project -.cproject -.settings +.project +.cproject +.settings STM32F429zi_Discovery.xml ._* @@ -25,5 +25,5 @@ build .vscode/* store.json -scripts/homeone/event_header_generator/generated/ +*/generated/ core diff --git a/sbs.conf b/sbs.conf index cd6dccc8ea50a1d6603845d59454bfc3c1516cac..26cfb6ac6e53bc86336095d6a2a6241073aa696b 100644 --- a/sbs.conf +++ b/sbs.conf @@ -475,6 +475,7 @@ Include: %shared %spi Defines: Main: drivers/test-l3gd20 +<<<<<<< HEAD [test-lsm9ds1] Type: test BoardId: stm32f407vg_stm32f4discovery @@ -490,6 +491,15 @@ BinName: test-lsm9ds1-class Include: %shared %spi Defines: -DDEBUG Main: drivers/test-lsm9ds1-class +======= +[test-l3gd20-fifo] +Type: test +BoardId: stm32f429zi_stm32f4discovery +BinName: test-l3gd20-fifo +Include: %shared %spi +Defines: +Main: drivers/test-l3gd20-fifo +>>>>>>> origin/testing [test-rls] Type: test diff --git a/scripts/eventgen/EventFunctions.cpp.template b/scripts/eventgen/EventFunctions.cpp.template new file mode 100644 index 0000000000000000000000000000000000000000..1611abafb11180cdadeafae5294f45acc4f85f78 --- /dev/null +++ b/scripts/eventgen/EventFunctions.cpp.template @@ -0,0 +1,53 @@ +/* Copyright (c) 2018-2020 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alvise de' Faveri Tron + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +/* + ****************************************************************************** + * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * + ****************************************************************************** + */ + +// Autogen date: {date} + + +#include "Events.h" +#include "Topics.h" + +#include <map> + +string getEventString(uint8_t event) +{{ + static const map<uint8_t, string> event_string_map {{ +{event_map_data} + }}; + auto it = event_string_map.find(event); + return it == event_string_map.end() ? "EV_UNKNOWN" : it->second; +}} + +string getTopicString(uint8_t topic) +{{ + static const map<uint8_t, string> topic_string_map{{ +{topic_map_data} + }}; + auto it = topic_string_map.find(topic); + return it == topic_string_map.end() ? "TOPIC_UNKNOWN" : it->second; +}} \ No newline at end of file diff --git a/scripts/eventgen/Events.h.template b/scripts/eventgen/Events.h.template new file mode 100644 index 0000000000000000000000000000000000000000..bd9cc25367ec63690cfc588a877778f110f09791 --- /dev/null +++ b/scripts/eventgen/Events.h.template @@ -0,0 +1,57 @@ +/* Copyright (c) 2018-2020 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alvise de' Faveri Tron + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +/* + ****************************************************************************** + * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * + ****************************************************************************** + */ + +// Autogen date: {date} + +#pragma once + +#include <cstdint> +#include <string> + +#include "events/Event.h" +#include "events/EventBroker.h" +#include "EventClasses.h" +#include "Topics.h" + +using std::string; +using std::map; + +/** + * Definition of all events signals. +enum Events : uint8_t +{{ +{enum_data} +}}; + +/** + * @brief Returns the name of the provided event + * + * @param event + * @return string + */ +string getEventString(uint8_t event); \ No newline at end of file diff --git a/scripts/eventgen/README.md b/scripts/eventgen/README.md new file mode 100644 index 0000000000000000000000000000000000000000..7bfb1c9b0625ffdfc86794475246b92e6251e586 --- /dev/null +++ b/scripts/eventgen/README.md @@ -0,0 +1,10 @@ +# Events Generator Script + +This script generates Events.h, Topics.h and EventFunctions.cpp from a list of +SCXML files representing the state machines of a project. + +To execute the script: + +`./eventgen.py <LIST_OF_SCXML_FILES>` + +The files will be generated in the generated/ folder. \ No newline at end of file diff --git a/scripts/eventgen/Topics.h.template b/scripts/eventgen/Topics.h.template new file mode 100644 index 0000000000000000000000000000000000000000..2ce2c2c536a6ba099b7c72ab2df987ccf9449fd3 --- /dev/null +++ b/scripts/eventgen/Topics.h.template @@ -0,0 +1,54 @@ +/* Copyright (c) 2018-2020 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alvise de' Faveri Tron + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +/* + ****************************************************************************** + * THIS FILE IS AUTOGENERATED. DO NOT EDIT. * + ****************************************************************************** + */ + +// Autogen date: {date} + +#pragma once + +#include <stdint.h> +#include <string> + +using std::map; +using std::string; + +/** + * Definition of various event topics to use in the EventBroker + */ +enum Topics : uint8_t +{{ +{enum_data} +}}; + +/** + * @brief Returns the name of the provided event + * + * @param event + * @return string + */ +string getTopicString(uint8_t topic); + diff --git a/scripts/eventgen/eventgen.py b/scripts/eventgen/eventgen.py new file mode 100755 index 0000000000000000000000000000000000000000..9c36223a3266a22a65961776074efcfc0bd14656 --- /dev/null +++ b/scripts/eventgen/eventgen.py @@ -0,0 +1,200 @@ +#!/usr/bin/python3 + +# Copyright (c) 2018-2020 Skyward Experimental Rocketry +# Authors: Luca Erbetta, Alvise de' Faveri Tron +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +# +import re +import datetime +from os.path import join +import os + +import xml.etree.ElementTree as ET +import sys +from collections import OrderedDict + +OUTPUT_FOLDER = "generated" + +# +# ASCII art banner +# +def printBanner(): + print("+--------------------------------------------------------+") + print("| ____ _ _ |") + print("| / ___|| | ___ ___ ____ _ _ __ __| | |") + print("| \\___ \\| |/ / | | \\ \\ /\\ / / _` | '__/ _` | |") + print("| ___) | <| |_| |\\ V V / (_| | | | (_| | |") + print("| |____/|_|\\_\\\\__, | \\_/\\_/ \\__,_|_| \\__,_| |") + print("| _____|___/ _ |") + print("| | ____|_ _____ _ __ | |_ __ _ ___ _ __ |") + print("| | _| \\ \\ / / _ \\ '_ \\| __/ _` |/ _ \\ '_ \\ |") + print("| | |___ \\ V / __/ | | | || (_| | __/ | | | |") + print("| |_____| \\_/ \\___|_| |_|\\__\\__, |\\___|_| |_| |") + print("| |___/ |") + print("+--------------------------------------------------------+") + +# +# Parse SCXML file(s) +# +def parse_scxml(files): + events = [] + topics = [] + + for file in files: + print("parsing " + str(file)) + tree = ET.parse(str(file)) + root = tree.getroot() + + # find all transitions and related events + first = 0 + for child in root: + for tran in child.iter('{http://www.w3.org/2005/07/scxml}transition'): + if(tran.attrib['target'] != 'final'): + ev = tran.attrib['event'] + + try: + topics.append("TOPIC_" + ev.split('.')[0]) + events.append(ev.split('.')[1]) + + except: + print("Cannot parse the event name... Forgot the topic?") + print(tran.tag, tran.attrib) + + # remove duplicates + events = list(OrderedDict.fromkeys(events)) + topics = list(OrderedDict.fromkeys(topics)) + events.sort() + topics.sort() + + print("{} events loaded.".format(len(events))) + print("{} topics loaded.".format(len(topics))) + + return events, topics + +# +# Generate Events.h +# +def generate_events(events, date): + enum_str = "" + event_map_str = "" + + # generate string + for e in events: + endl = ",\n" if e != events[-1] else "" + enum_str += " " + e + \ + (" = EV_FIRST_SIGNAL" if e == events[0] else "") + endl + event_map_str += " {{ {event}, {string} }}{endl}".format( + event=e, string='"' + e + '"', endl=endl) + + # read template + with open('Events.h.template', 'r') as template_file: + template = template_file.read() + + # write template + template = template.format(date=date, enum_data=enum_str) + + with open(join(OUTPUT_FOLDER, 'Events.h'), 'w') as header_file: + header_file.write(template) + + return event_map_str + +# +# Generate Topics.h +# +def generate_topics(topics, date): + enum_str = "" + topic_map_str = "" + + for t in topics: + endl = ",\n" if t != topics[-1] else "" + enum_str += " " + t + endl + topic_map_str += " {{ {topics}, {string} }}{endl}".format( + topics=t, string='"' + t + '"', endl=endl) + + with open('Topics.h.template', 'r') as template_file: + template = template_file.read() + + template = template.format(date=date, enum_data=enum_str) + + with open(join(OUTPUT_FOLDER, 'Topics.h'), 'w') as header_file: + header_file.write(template) + + return topic_map_str + +# +# Generate EventFunctions.cpp +# +def generate_functions(event_map_str, topic_map_str, date): + with open('EventFunctions.cpp.template', 'r') as cpp_template_file: + cpp = cpp_template_file.read() + + cpp = cpp.format(date=date, event_map_data=event_map_str, topic_map_data=topic_map_str) + + with open(join(OUTPUT_FOLDER, 'EventFunctions.cpp'), 'w') as cpp_file: + cpp_file.write(cpp) + +# +# Main +# +printBanner() +print() +print("Skyward SCXML event generator") +print() + +if(len(sys.argv) < 2): + print("Error: no SCXML files provided.\nUsage: eventgen.py <SCXML_FILES>") + sys.exit(-1) + +if not os.path.exists(OUTPUT_FOLDER): + os.mkdir(OUTPUT_FOLDER) + +date = datetime.datetime.now() + +print("-"*30) +print("Parsing SCXML files...") +events, topics = parse_scxml(sys.argv[1:]) + +print("-"*30) +print("Generating Events.h...") +event_map_str = generate_events(events, date) +print("OK\n") + +print("-"*30) +print("Generating Topics.h...") +topic_map_str = generate_topics(topics, date) +print("OK\n") + +print("-"*30) +print("Generating EventFunctions.cpp...") +generate_functions(event_map_str, topic_map_str, date) +print("OK\n") + + +# print +print("-"*30) +print("Events:\n") +for e in events: + print(e) +print("\n\nTopics:\n") +for t in topics: + print(t) + +print("-"*30) +print("... Done.") diff --git a/scripts/homeone/mavlink_client/Makefile b/scripts/mavlink_client/Makefile similarity index 100% rename from scripts/homeone/mavlink_client/Makefile rename to scripts/mavlink_client/Makefile diff --git a/scripts/mavlink_client/README.md b/scripts/mavlink_client/README.md new file mode 100644 index 0000000000000000000000000000000000000000..4a3c225d75fc187df6b0ac5550cb8bad7f174c9b --- /dev/null +++ b/scripts/mavlink_client/README.md @@ -0,0 +1,14 @@ +# Mavlink Clinet + +These scripts can be used to transmit and receive mavlink data from a PC, either +connected via serial port to a transceiver or directly connected to one of the boards. + +`make` compiles the 3 `.c` files: + +* continuous_tx: sends a message of raw bytes with a given length at a certain +frequency. This was used to test the LoRa modules. +* mavlink_demo_tx: sends a message whenever it receives a char on stdin +* mavlink_demo_rx: receives and prints packets, eventually sending back ana ACK. + +`plot.py`: reads value on stdin and plots them. Can be used piping in the output +of mavlink_demo_rx \ No newline at end of file diff --git a/scripts/homeone/mavlink_client/continuous_tx.c b/scripts/mavlink_client/continuous_tx.c similarity index 100% rename from scripts/homeone/mavlink_client/continuous_tx.c rename to scripts/mavlink_client/continuous_tx.c diff --git a/scripts/homeone/mavlink_client/mavlink_demo_rx.c b/scripts/mavlink_client/mavlink_demo_rx.c similarity index 98% rename from scripts/homeone/mavlink_client/mavlink_demo_rx.c rename to scripts/mavlink_client/mavlink_demo_rx.c index e33145dfb2ba3eb00163ba5c0329326d3e5bdfc5..95c49aa44d440abf09822dd0889435438775b51b 100644 --- a/scripts/homeone/mavlink_client/mavlink_demo_rx.c +++ b/scripts/mavlink_client/mavlink_demo_rx.c @@ -1,6 +1,6 @@ /************************************************** -Ssimple demo that receives mavlink +Simple demo that receives mavlink messages on the serial port and prints them on the screen. **************************************************/ diff --git a/scripts/homeone/mavlink_client/mavlink_demo_tx.c b/scripts/mavlink_client/mavlink_demo_tx.c similarity index 100% rename from scripts/homeone/mavlink_client/mavlink_demo_tx.c rename to scripts/mavlink_client/mavlink_demo_tx.c diff --git a/scripts/homeone/mavlink_client/plot.py b/scripts/mavlink_client/plot.py similarity index 100% rename from scripts/homeone/mavlink_client/plot.py rename to scripts/mavlink_client/plot.py diff --git a/scripts/homeone/mavlink_client/rs232.c b/scripts/mavlink_client/rs232.c similarity index 100% rename from scripts/homeone/mavlink_client/rs232.c rename to scripts/mavlink_client/rs232.c diff --git a/scripts/homeone/mavlink_client/rs232.h b/scripts/mavlink_client/rs232.h similarity index 100% rename from scripts/homeone/mavlink_client/rs232.h rename to scripts/mavlink_client/rs232.h diff --git a/scripts/anakin.sh b/scripts/old/anakin.sh similarity index 100% rename from scripts/anakin.sh rename to scripts/old/anakin.sh diff --git a/scripts/anakin_client_curses/.gitignore b/scripts/old/anakin_client_curses/.gitignore similarity index 100% rename from scripts/anakin_client_curses/.gitignore rename to scripts/old/anakin_client_curses/.gitignore diff --git a/scripts/anakin_client_curses/Makefile b/scripts/old/anakin_client_curses/Makefile similarity index 100% rename from scripts/anakin_client_curses/Makefile rename to scripts/old/anakin_client_curses/Makefile diff --git a/scripts/anakin_client_curses/client.c b/scripts/old/anakin_client_curses/client.c similarity index 100% rename from scripts/anakin_client_curses/client.c rename to scripts/old/anakin_client_curses/client.c diff --git a/scripts/homeone/event_header_generator/EventFunctions.cpp.template b/scripts/old/excel_eventgen/EventFunctions.cpp.template similarity index 100% rename from scripts/homeone/event_header_generator/EventFunctions.cpp.template rename to scripts/old/excel_eventgen/EventFunctions.cpp.template diff --git a/scripts/homeone/event_header_generator/Events.h.template b/scripts/old/excel_eventgen/Events.h.template similarity index 100% rename from scripts/homeone/event_header_generator/Events.h.template rename to scripts/old/excel_eventgen/Events.h.template diff --git a/scripts/homeone/event_header_generator/README.md b/scripts/old/excel_eventgen/README.md similarity index 100% rename from scripts/homeone/event_header_generator/README.md rename to scripts/old/excel_eventgen/README.md diff --git a/scripts/homeone/event_header_generator/Topics.h.template b/scripts/old/excel_eventgen/Topics.h.template similarity index 100% rename from scripts/homeone/event_header_generator/Topics.h.template rename to scripts/old/excel_eventgen/Topics.h.template diff --git a/scripts/homeone/event_header_generator/credentials.json b/scripts/old/excel_eventgen/credentials.json similarity index 100% rename from scripts/homeone/event_header_generator/credentials.json rename to scripts/old/excel_eventgen/credentials.json diff --git a/scripts/homeone/event_header_generator/event_generator.py b/scripts/old/excel_eventgen/event_generator.py similarity index 100% rename from scripts/homeone/event_header_generator/event_generator.py rename to scripts/old/excel_eventgen/event_generator.py diff --git a/scripts/gen_fault_headers.py b/scripts/old/gen_fault_headers.py similarity index 100% rename from scripts/gen_fault_headers.py rename to scripts/old/gen_fault_headers.py diff --git a/src/shared/drivers/BusTemplate.h b/src/shared/drivers/BusTemplate.h index 0652a840bf017fa009fbc6883e85947ac5454318..0a9ebfbc6b2fab8322900d69f2bd86067938703e 100644 --- a/src/shared/drivers/BusTemplate.h +++ b/src/shared/drivers/BusTemplate.h @@ -110,6 +110,9 @@ private: inline void _write(uint8_t byte) const { + // Wait until the peripheral is ready to transmit + while ((getSPIAddr(N)->SR & SPI_SR_TXE) == 0) + ; getSPIAddr(N)->DR = byte; while ((getSPIAddr(N)->SR & SPI_SR_RXNE) == 0) ; @@ -127,6 +130,9 @@ private: inline uint8_t _read() const { + // Wait until the peripheral is ready to transmit + while ((getSPIAddr(N)->SR & SPI_SR_TXE) == 0) + ; getSPIAddr(N)->DR = 0; while ((getSPIAddr(N)->SR & SPI_SR_RXNE) == 0) ; @@ -157,6 +163,9 @@ private: */ inline uint8_t _transfer(uint8_t* byte) { + // Wait until the peripheral is ready to transmit + while ((getSPIAddr(N)->SR & SPI_SR_TXE) == 0) + ; getSPIAddr(N)->DR = *byte; while ((getSPIAddr(N)->SR & SPI_SR_RXNE) == 0) ; diff --git a/src/shared/drivers/HardwareTimer.h b/src/shared/drivers/HardwareTimer.h index cb82f77b63815471b8cd60cb673be7ec53b65afa..267014839b9ace07595246a649e8eb1a38daa2ec 100644 --- a/src/shared/drivers/HardwareTimer.h +++ b/src/shared/drivers/HardwareTimer.h @@ -40,22 +40,22 @@ public: * @brief returns the timer clock frequency before the prescaler. * Function borrowed from the SyncronizedServo class in Miosix. * - * @return unsigned int Prescaler input frequency + * @return Prescaler input frequency */ - static unsigned int getPrescalerInputFrequency(InputClock input_clock) + static uint32_t getPrescalerInputFrequency(InputClock input_clock) { // The global variable SystemCoreClock from ARM's CMSIS allows to // know // the CPU frequency. - unsigned int freq = SystemCoreClock; + uint32_t freq = SystemCoreClock; // The position of the PPRE1 bit in RCC->CFGR is different in some stm32 #ifdef _ARCH_CORTEXM3_STM32 - const unsigned int ppre1 = 8; + const uint32_t ppre1 = 8; #error "Architecture not currently supported" #else // stm32f2 and f4 - const unsigned int ppre1 = 10; - const unsigned int ppre2 = 13; + const uint32_t ppre1 = 10; + const uint32_t ppre2 = 13; #endif // The timer frequency may however be a submultiple of the CPU // frequency, @@ -104,7 +104,7 @@ public: * @param psc_input_freq Input frequency of the timer's prescaler, see * TimerUtils::getPrescalerInputFrequency() */ - HardwareTimer(TIM_TypeDef* timer, unsigned int psc_input_freq) + HardwareTimer(TIM_TypeDef* timer, uint32_t psc_input_freq) : tim(timer), prescaler_freq(psc_input_freq) { } @@ -137,6 +137,8 @@ public: */ float toMicroSeconds(Type ticks); + uint64_t toIntMicroSeconds(Type ticks); + /** * @brief Converts from ticks to milliseconds, using the current prescaler * setting @@ -185,9 +187,9 @@ public: private: TIM_TypeDef* tim; - unsigned prescaler_freq; + uint32_t prescaler_freq; - bool ticking = false; + bool ticking = false; uint16_t prescaler = 0; Type auto_reload = static_cast<Type>(-1); // Max value of Type (Type is unsigned) @@ -253,6 +255,13 @@ float HardwareTimer<Type>::toMicroSeconds(Type ticks) return (1.0f * ticks * 1000000 * (1 + prescaler)) / prescaler_freq; } +template <typename Type> +uint64_t HardwareTimer<Type>::toIntMicroSeconds(Type ticks) +{ + return ((uint64_t)ticks * 1000000 * (uint64_t)(1 + prescaler)) / + (uint64_t)prescaler_freq; +} + template <typename Type> float HardwareTimer<Type>::toMilliSeconds(Type ticks) { diff --git a/src/shared/drivers/Xbee/Xbee.h b/src/shared/drivers/Xbee/Xbee.h index d269b1e27052a71a807ac8571cc865dd792cad03..ea5625585ead196ab292591194e8f5e2942fcf47 100644 --- a/src/shared/drivers/Xbee/Xbee.h +++ b/src/shared/drivers/Xbee/Xbee.h @@ -119,7 +119,7 @@ public: unsigned int send_timeout = 1000) : send_timeout(send_timeout), spi_xbee(bus, cs), attn(attn), rst(rst) { - spi_xbee.config.br = SPIBaudRate::DIV_128; + spi_xbee.config.clock_div = SPIClockDivider::DIV128; // No need to configure before each transaction, we are the only device // on the bus. diff --git a/src/shared/drivers/spi/SPIBus.cpp b/src/shared/drivers/spi/SPIBus.cpp index c7dc7c803a4972a0150f1b9ac6d916f63ec6af67..afe76d97724c74860d7ed6cafb4423ed24d07707 100644 --- a/src/shared/drivers/spi/SPIBus.cpp +++ b/src/shared/drivers/spi/SPIBus.cpp @@ -34,17 +34,22 @@ void SPIBus::configure(SPIBusConfig new_config) first_config_applied = true; config = new_config; - // Clean CR1 + // Wait until the peripheral is done before changing configuration + while ((spi->SR & SPI_SR_TXE) == 0) + ; + while ((spi->SR & SPI_SR_BSY) == 1) + ; + spi->CR1 = 0; - // Configure clock division (BR bits) - spi->CR1 |= (static_cast<uint16_t>(config.br) & 0x0007) << 3; // Configure CPOL & CPHA bits - spi->CR1 |= ((uint16_t)config.cpol & 0x0001) << 1 | - ((uint16_t)config.cpha & 0x0001); + spi->CR1 |= static_cast<uint32_t>(config.mode); + + // Configure clock division (BR bits) + spi->CR1 |= static_cast<uint32_t>(config.clock_div); // Configure LSBFIRST bit - spi->CR1 |= (uint16_t)config.lsb_first << 7; + spi->CR1 |= static_cast<uint32_t>(config.bit_order); spi->CR1 |= SPI_CR1_SSI | SPI_CR1_SSM // Use software chip-select | SPI_CR1_MSTR // Master mode diff --git a/src/shared/drivers/spi/SPIInterface.h b/src/shared/drivers/spi/SPIInterface.h index e7d9955e9a5c516e250aa65fdfcd9be740b8fd29..7a3eaa1fd3497e99dc517492833fe82d69badd81 100644 --- a/src/shared/drivers/spi/SPIInterface.h +++ b/src/shared/drivers/spi/SPIInterface.h @@ -32,24 +32,48 @@ using miosix::delayUs; using miosix::GpioPin; /** - * @brief SPI baud rate selection parameter. - * SPI clock frequency will be equal to the SPI peripheral bus clock speed - * divided by the value specified in this enum. + * @brief SPI Clock divider. + * SPI clock frequency will be equal to the SPI peripheral bus clock speed (see + * datasheet) divided by the value specified in this enum. * * Eg: DIV_2 --> spi clock freq = f_PCLK / 2 * - * See SPI->CR1 on the datasheet for further information. + * See register CR1 of the SPI peripheral on the reference manual for further + * information. */ -enum class SPIBaudRate +enum class SPIClockDivider : uint8_t { - DIV_2 = 0, - DIV_4 = 1, - DIV_8 = 2, - DIV_16 = 3, - DIV_32 = 4, - DIV_64 = 5, - DIV_128 = 6, - DIV_256 = 7, + DIV2 = 0x00, + DIV4 = 0x08, + DIV8 = 0x10, + DIV16 = 0x18, + DIV32 = 0x20, + DIV64 = 0x28, + DIV128 = 0x30, + DIV256 = 0x38, +}; + +/** + * @brief SPI Mode. + * See slave device datasheet for information on which one to use. + */ +enum class SPIMode : uint8_t +{ + MODE0 = 0, ///> CPOL = 0, CPHA = 0 + MODE1 = 1, ///> CPOL = 0, CPHA = 1 + MODE2 = 2, ///> CPOL = 1, CPHA = 0 + MODE3 = 3 ///> CPOL = 1, CPHA = 1 +}; + +/** + * @brief SPI Bit Order + * See register CR1 of the SPI peripheral on the reference manual for further + * information. + */ +enum class SPIBitOrder : uint8_t +{ + MSB_FIRST = 0, + LSB_FIRST = 0x80 }; /** @@ -58,10 +82,10 @@ enum class SPIBaudRate */ struct SPIBusConfig { - SPIBaudRate br = SPIBaudRate::DIV_2; ///> Peripheral clock division - uint8_t cpol = 0; ///> Clock polarity (0 - 1) - uint8_t cpha = 0; ///> Clock phase (0 - 1) - bool lsb_first = false; ///> MSB or LSB first + SPIClockDivider clock_div = + SPIClockDivider::DIV2; ///> Peripheral clock division + SPIMode mode = SPIMode::MODE0; ///> Clock polarity (0 - 1) + SPIBitOrder bit_order = SPIBitOrder::MSB_FIRST; ///> MSB or LSB first unsigned int cs_setup_time_us = 0; ///> How long to wait before starting a ///> a trasmission after CS is set (us) @@ -73,10 +97,9 @@ struct SPIBusConfig { // Compare member-by-member // clang-format off - return br == other.br - && cpol == other.cpol - && cpha == other.cpha - && lsb_first == other.lsb_first + return clock_div == other.clock_div + && mode == other.mode + && bit_order == other.bit_order && cs_setup_time_us == other.cs_setup_time_us && cs_setup_time_us == other.cs_hold_time_us; // clang-format on diff --git a/src/shared/drivers/spi/SPITransaction.cpp b/src/shared/drivers/spi/SPITransaction.cpp index 8e5746907547e82f25cd8deeaf7f3e9b17a16ca4..a6f026c6b821325aa8e4e836cbd8fbdb924aa4b3 100644 --- a/src/shared/drivers/spi/SPITransaction.cpp +++ b/src/shared/drivers/spi/SPITransaction.cpp @@ -77,12 +77,11 @@ uint8_t SPITransaction::read(uint8_t reg, bool set_read_bit) if (set_read_bit) reg = reg | 0x80; - uint8_t out; bus.select(cs); bus.write(®, 1); - bus.read(&out, 1); + bus.read(®, 1); bus.deselect(cs); - return out; + return reg; } void SPITransaction::read(uint8_t reg, uint8_t* data, size_t size, diff --git a/src/shared/sensors/L3GD20.h b/src/shared/sensors/L3GD20.h index 9133d5b5090e3fd47ba9257ef8c295c0f5afd1f3..69c60af52f763be70850af28aceb376d2bad9046 100644 --- a/src/shared/sensors/L3GD20.h +++ b/src/shared/sensors/L3GD20.h @@ -1,3 +1,4 @@ + /** * Copyright (c) 2019 Skyward Experimental Rocketry * Authors: Luca Erbetta @@ -32,6 +33,12 @@ using miosix::GpioPin; using std::array; +struct L3GD20Data +{ + uint64_t timestamp; + Vec3 gyro; +}; + class L3GD20 : public GyroSensor { public: @@ -44,11 +51,12 @@ public: enum class OutPutDataRate { - ODR_95 = 0x00, - ODR_190 = 0x01, - ODR_380 = 0x02, - ODR_760 = 0x03 + ODR_95 = 95, + ODR_190 = 190, + ODR_380 = 380, + ODR_760 = 760 }; + /** * @brief Creates an instance of an L3GD20 sensor * @@ -57,21 +65,57 @@ public: * @param range Full Scale Range (See datasheet) * @param odr Output Data Rate (See datasheet) * @param cutoff_freq Low pass filter cutoff frequency (See datasheet) - * @param fifo_enabled Fifo enabled - * @param fifo_watermark FIFO watermark level in range [1,32] (used for - * interrupt generation, see datasheet). */ L3GD20(SPIBusInterface& bus, GpioPin cs, FullScaleRange range = FullScaleRange::FS_250, OutPutDataRate odr = OutPutDataRate::ODR_95, - uint8_t cutoff_freq = 0x03, bool fifo_enabled = false, - unsigned int fifo_watermark = 24) - : fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark), - spislave(bus, cs), fs(range), odr(odr), cutoff_freq(cutoff_freq) + uint8_t cutoff_freq = 0x03) + : L3GD20(bus, cs, {}, range, odr, cutoff_freq) { // Configure SPI - spislave.config.br = SPIBaudRate::DIV_64; - // memset(last_fifo, 0, sizeof(Vec3) * 32); + spislave.config.clock_div = SPIClockDivider::DIV32; + } + + /** + * @brief Creates an instance of an L3GD20 sensor + * + * @param bus SPI bus the sensor is connected to + * @param cs Chip Select GPIO + * @param cfg Custom SPI bus configuration + * @param range Full Scale Range (See datasheet) + * @param odr Output Data Rate (See datasheet) + * @param cutoff_freq Low pass filter cutoff selector (See datasheet) + */ + L3GD20(SPIBusInterface& bus, GpioPin cs, SPIBusConfig cfg, + FullScaleRange range = FullScaleRange::FS_250, + OutPutDataRate odr = OutPutDataRate::ODR_95, + uint8_t cutoff_freq = 0x03) + : spislave(bus, cs, cfg), fs(range), odr(odr), cutoff_freq(cutoff_freq) + { + switch (fs) + { + case FullScaleRange::FS_250: + sensitivity = SENSITIVITY_250; + break; + case FullScaleRange::FS_500: + sensitivity = SENSITIVITY_500; + break; + case FullScaleRange::FS_2000: + sensitivity = SENSITIVITY_2000; + break; + } + } + + /** + * @brief Enables storing samples in a FIFO, must be called before init(). + * + * @param fifo_watermark How many samples in the FIFO when the fifo + * watermark input is generated on INT2. + */ + void enableFifo(unsigned int fifo_watermark) + { + fifo_enabled = true; + this->fifo_watermark = fifo_watermark; } bool init() @@ -87,8 +131,6 @@ public: return false; } - // uint8_t ctrl4 = spi.read(REG_CTRL4); - switch (fs) { case FullScaleRange::FS_250: @@ -103,6 +145,7 @@ public: default: break; } + if (fifo_enabled) { // Enable fifo @@ -111,23 +154,42 @@ public: // Set watermark level to fifo_watermark samples uint8_t fifo_ctrl = fifo_watermark; - // Set fifo to FIFO mode + // Set fifo to STREAM mode fifo_ctrl |= 0x02 << 5; spi.write(REG_FIFO_CTRL, fifo_ctrl); - // Enable FIFO watermark interrupt + // Enable FIFO watermark interrupt on INT2 spi.write(REG_CTRL3, 0x04); } + else + { + // Enable DRDY interrupt on INT2 + spi.write(REG_CTRL3, 0x08); + } + // Enter normal mode, enable output uint8_t ctrl1 = 0x0F; - // Configure ODR - ctrl1 |= static_cast<uint8_t>(odr) << 6; - // Configure cutoff frequency ctrl1 |= (cutoff_freq & 0x03) << 4; + // Configure ODR + switch (odr) + { + case OutPutDataRate::ODR_95: + break; + case OutPutDataRate::ODR_190: + ctrl1 |= 1 << 6; + break; + case OutPutDataRate::ODR_380: + ctrl1 |= 2 << 6; + break; + case OutPutDataRate::ODR_760: + ctrl1 |= 3 << 6; + break; + } + spi.write(REG_CTRL1, ctrl1); return true; @@ -137,75 +199,123 @@ public: bool onSimpleUpdate() { - float scale = static_cast<int>(fs); - - if (!fifo_enabled) + if (!fifo_enabled) // FIFO not enabled { - uint8_t data[6]; // Read output data registers (X, Y, Z) { SPITransaction spi(spislave); - spi.read(REG_OUT_X_L | 0x40, data, 6); + spi.read(REG_OUT_X_L | 0x40, buf, 6); } - int16_t x = data[0] | data[1] << 8; - int16_t y = data[2] | data[3] << 8; - int16_t z = data[4] | data[5] << 8; + int16_t x = buf[0] | buf[1] << 8; + int16_t y = buf[2] | buf[3] << 8; + int16_t z = buf[4] | buf[5] << 8; // printf("%02X,%02X,%02X\n", x, y, z); - mLastGyro = - Vec3(x * scale / 65535, y * scale / 65535, z * scale / 65535); + last_fifo[0] = {interrupt_us, toRadiansPerSecond(x, y, z)}; } + else // FIFO is enabled { - uint8_t buf[192]; - SPITransaction spi(spislave); // Read last fifo level - uint8_t fifo_src = spi.read(REG_FIFO_SRC); - uint8_t ovr = (fifo_src & 0x40); - last_fifo_level = fifo_src & 0x1F; - - // if ovr --> fifo is full --> level = level + 1 - if (ovr) - { - ++last_fifo_level; - } + uint8_t fifo_src = spi.read(REG_FIFO_SRC); + uint8_t ovr = (fifo_src & 0x40) >> 7; // Overrun bit + uint8_t fifo_level = (fifo_src & 0x1F) + ovr; // Read fifo - spi.read(REG_OUT_X_L | 0x40, buf, last_fifo_level * 6); - - // Convert units & store the FIFO content - for (uint8_t i = 0; i < last_fifo_level; i++) - { - int16_t x = buf[i * 6] | buf[i * 6 + 1] << 8; - int16_t y = buf[i * 6 + 2] | buf[i * 6 + 3] << 8; - int16_t z = buf[i * 6 + 4] | buf[i * 6 + 5] << 8; + spi.read(REG_OUT_X_L | 0x40, buf, fifo_level * 6); - Vec3 t = Vec3(x * scale / 65535, y * scale / 65535, - z * scale / 65535); + uint64_t dt = dt_interrupt / last_fifo_level; - last_fifo[i] = t; + uint8_t duplicates = 0; + for (uint8_t i = 0; i < fifo_level; ++i) + { + bool rmv; + // Check for duplicates: there seems to be a bug where the + // sensor occasionaly shifts out the same sample two times in a + // row: discard one + if (i < fifo_level - 1) + { + rmv = true; + for (uint8_t j = 0; j < 6; ++j) + { + if (buf[i * 6 + j] != buf[(i + 1) * 6 + j]) + { + rmv = false; + break; + } + } + } + else + { + rmv = false; + } + + if (rmv) + { + // Skip this sample; + ++duplicates; + continue; + } + + // Assign a timestamp to each sample in the FIFO + // Samples before the watermark are older, after the + // watermark are younger. + last_fifo[i - duplicates].timestamp = + interrupt_us + + ((int)i - (int)fifo_watermark - (int)duplicates) * dt; + last_fifo[i - duplicates].gyro = + toRadiansPerSecond(buf[i * 6] | buf[i * 6 + 1] << 8, + buf[i * 6 + 2] | buf[i * 6 + 3] << 8, + buf[i * 6 + 4] | buf[i * 6 + 5] << 8); } + + last_fifo_level = fifo_level - duplicates; } return true; } - const array<Vec3, 32>& getLastFifo() const { return last_fifo; } + L3GD20Data getLastSample() { return last_fifo[last_fifo_level - 1]; } + + void IRQupdateTimestamp(uint64_t ts) + { + dt_interrupt = ts - interrupt_us; + interrupt_us = ts; + } + + const array<L3GD20Data, 32>& getLastFifo() const { return last_fifo; } uint8_t getLastFifoSize() const { return last_fifo_level; } private: - bool fifo_enabled = false; - unsigned int fifo_watermark; + Vec3 toRadiansPerSecond(int16_t x, int16_t y, int16_t z) + { + return Vec3(x * sensitivity, y * sensitivity, z * sensitivity); + } - array<Vec3, 32> last_fifo; - uint8_t last_fifo_level = 0; + static constexpr float SENSITIVITY_250 = 0.00875f; + static constexpr float SENSITIVITY_500 = 0.0175f; + static constexpr float SENSITIVITY_2000 = 0.070f; SPISlave spislave; - FullScaleRange fs; - OutPutDataRate odr; - uint8_t cutoff_freq; + + FullScaleRange fs = FullScaleRange::FS_250; + OutPutDataRate odr = OutPutDataRate::ODR_760; + uint8_t cutoff_freq = 0x03; + + bool fifo_enabled = false; + unsigned int fifo_watermark = 24; + + float sensitivity = SENSITIVITY_250; + + uint64_t interrupt_us = 0; + uint64_t dt_interrupt = 0; + + array<L3GD20Data, 32> last_fifo; + uint8_t last_fifo_level = 1; + + uint8_t buf[192]; constexpr static uint8_t WHO_AM_I_VAL = 212; diff --git a/src/shared/sensors/MS580301BA07/MS580301BA07.h b/src/shared/sensors/MS580301BA07/MS580301BA07.h index 5131e32496e1b7e79068ffd5118a72957949daab..5f8a02d55e805dd9510cc9f9f529834f7154ba20 100644 --- a/src/shared/sensors/MS580301BA07/MS580301BA07.h +++ b/src/shared/sensors/MS580301BA07/MS580301BA07.h @@ -37,7 +37,7 @@ public: MS580301BA07(SPIBusInterface& bus, GpioPin cs) : MS580301BA07(bus, cs, SPIBusConfig{}) { - spi_ms5803.config.br = SPIBaudRate::DIV_128; + spi_ms5803.config.clock_div = SPIClockDivider::DIV128; } MS580301BA07(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config) diff --git a/src/tests/catch/spidriver/FakeSPIBus.h b/src/tests/catch/spidriver/FakeSPIBus.h index 2a395552b1fe4d9698a7854154b3d543a270b623..94087b088fa4957db9d30b9520a4f3edc7374dde 100644 --- a/src/tests/catch/spidriver/FakeSPIBus.h +++ b/src/tests/catch/spidriver/FakeSPIBus.h @@ -151,14 +151,14 @@ void FakeSPIBus::configure(SPIBusConfig new_config) // Clean CR1 spi->CR1 = 0; - // Configure clock division (BR bits) - spi->CR1 |= (static_cast<uint16_t>(config.br) & 0x0007) << 3; // Configure CPOL & CPHA bits - spi->CR1 |= ((uint16_t)config.cpol & 0x0001) << 1 | - ((uint16_t)config.cpha & 0x0001); + spi->CR1 |= static_cast<uint32_t>(config.mode); + // Configure clock division (BR bits) + spi->CR1 |= static_cast<uint32_t>(config.clock_div); + // Configure LSBFIRST bit - spi->CR1 |= (uint16_t)config.lsb_first << 7; + spi->CR1 |= static_cast<uint32_t>(config.bit_order); spi->CR1 |= SPI_CR1_SSI | SPI_CR1_SSM // Use software chip-select | SPI_CR1_MSTR // Master mode diff --git a/src/tests/catch/spidriver/test-spidriver.cpp b/src/tests/catch/spidriver/test-spidriver.cpp index 5438d812fa9bbff845e426a997a30dfe08c9e993..8b2f47f7edea4fedcaeab4c58f4cd9874d97a343 100644 --- a/src/tests/catch/spidriver/test-spidriver.cpp +++ b/src/tests/catch/spidriver/test-spidriver.cpp @@ -56,80 +56,102 @@ TEST_CASE("SPIBus - Bus Configuration") SECTION("Configure & check CR1") { SPIBusConfig config; - config.br = SPIBaudRate::DIV_16; - - config.cpha = 1; - config.cpol = 1; - config.lsb_first = true; - - uint32_t expected_CR1 = 0x03DF; - - bus.configure(config); - REQUIRE(spi.CR1 == expected_CR1); - - // Change config - config.br = SPIBaudRate::DIV_256; - - config.cpha = 0; - config.cpol = 0; - config.lsb_first = false; - - expected_CR1 = 0x037C; - - bus.configure(config); - REQUIRE(spi.CR1 == expected_CR1); - - config.cpha = 0; - config.cpol = 1; - config.lsb_first = false; - - expected_CR1 = 0x037E; - - bus.configure(config); - REQUIRE(spi.CR1 == expected_CR1); + REQUIRE(spi.CR1 == 0); - config.cpha = 1; - config.cpol = 0; - config.lsb_first = false; + SECTION("Mode") + { + config.mode = SPIMode::MODE0; + uint32_t expected_CR1 = 0x0344; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + + config.mode = SPIMode::MODE1; + expected_CR1 = 0x0345; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + + config.mode = SPIMode::MODE2; + expected_CR1 = 0x0346; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + + config.mode = SPIMode::MODE3; + expected_CR1 = 0x0347; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); - expected_CR1 = 0x037D; + } - bus.configure(config); - REQUIRE(spi.CR1 == expected_CR1); + SECTION("Clock Divider") + { + config.clock_div = SPIClockDivider::DIV2; + uint32_t expected_CR1 = 0x0344; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + + config.clock_div = SPIClockDivider::DIV4; + expected_CR1 = 0x034C; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + + config.clock_div = SPIClockDivider::DIV8; + expected_CR1 = 0x0354; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + + config.clock_div = SPIClockDivider::DIV16; + expected_CR1 = 0x035C; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + + config.clock_div = SPIClockDivider::DIV32; + expected_CR1 = 0x0364; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + + config.clock_div = SPIClockDivider::DIV64; + expected_CR1 = 0x036C; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + + config.clock_div = SPIClockDivider::DIV128; + expected_CR1 = 0x0374; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + + config.clock_div = SPIClockDivider::DIV256; + expected_CR1 = 0x037C; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + } - // Reapply same config - bus.configure(config); - REQUIRE(spi.CR1 == expected_CR1); + SECTION("Bit order") + { + config.bit_order = SPIBitOrder::MSB_FIRST; + uint32_t expected_CR1 = 0x0344; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + + config.bit_order = SPIBitOrder::LSB_FIRST; + expected_CR1 = 0x03C4; + bus.configure(config); + REQUIRE(spi.CR1 == expected_CR1); + } + } SECTION("Disable configuration") { SPIBusConfig config; - config.br = SPIBaudRate::DIV_16; + config.clock_div = SPIClockDivider::DIV16; - config.cpha = 1; - config.cpol = 1; - config.lsb_first = true; + config.mode = SPIMode::MODE3; + config.bit_order = SPIBitOrder::LSB_FIRST; bus.enableSlaveConfiguration(false); bus.configure(config); REQUIRE(spi.CR1 == 0); } - - SECTION("Wrong parameters") - { - SPIBusConfig config; - config.br = SPIBaudRate::DIV_16; - - config.cpha = 8; - config.cpol = 9; - config.lsb_first = true; - - uint32_t expected_CR1 = 0x03DE; - - bus.configure(config); - REQUIRE(spi.CR1 == expected_CR1); - } } TEST_CASE("SPIBus - Chip select") @@ -157,11 +179,10 @@ TEST_CASE("SPIBus - One byte operations") FakeSPIBus bus{&spi}; SPIBusConfig config; - config.br = SPIBaudRate::DIV_16; + config.clock_div = SPIClockDivider::DIV16; - config.cpha = 1; - config.cpol = 1; - config.lsb_first = true; + config.mode = SPIMode::MODE3; + config.bit_order = SPIBitOrder::LSB_FIRST; bus.configure(config); bus.select(spi.cs); @@ -209,11 +230,10 @@ TEST_CASE("SPIBus - Multi byte operations") FakeSPIBus bus{&spi}; SPIBusConfig config; - config.br = SPIBaudRate::DIV_16; + config.clock_div = SPIClockDivider::DIV16; - config.cpha = 1; - config.cpol = 1; - config.lsb_first = true; + config.mode = SPIMode::MODE3; + config.bit_order = SPIBitOrder::LSB_FIRST; bus.configure(config); bus.select(spi.cs); @@ -264,8 +284,8 @@ TEST_CASE("SPITransaction - writes") MockSPIBus bus{}; SPIBusConfig config1{}; - config1.cpha = 1; - config1.br = SPIBaudRate::DIV_32; + config1.mode = SPIMode::MODE1; + config1.clock_div = SPIClockDivider::DIV32; bus.expected_config = config1; @@ -360,8 +380,8 @@ TEST_CASE("SPITransaction - reads") SPIBusConfig config1; - config1.cpha = 1; - config1.br = SPIBaudRate::DIV_32; + config1.mode = SPIMode::MODE1; + config1.clock_div = SPIClockDivider::DIV32; bus.expected_config = config1; @@ -450,8 +470,8 @@ TEST_CASE("SPITransaction - transfer") SPIBusConfig config1; - config1.cpha = 1; - config1.br = SPIBaudRate::DIV_32; + config1.mode = SPIMode::MODE1; + config1.clock_div = SPIClockDivider::DIV32; bus.expected_config = config1; @@ -460,7 +480,7 @@ TEST_CASE("SPITransaction - transfer") SPISlave slave(bus, GpioPin(GPIOA_BASE, 1), config1); SPITransaction spi(slave); - uint8_t buf[4] = {4, 3, 2, 1}; + uint8_t buf[4] = {4, 3, 2, 1}; uint8_t cmp[4] = {4, 3, 2, 1}; spi.transfer(buf, 0); diff --git a/src/tests/drivers/test-l3gd20-fifo.cpp b/src/tests/drivers/test-l3gd20-fifo.cpp new file mode 100644 index 0000000000000000000000000000000000000000..31beec225602bd435c864e1183b02f73946d1cc2 --- /dev/null +++ b/src/tests/drivers/test-l3gd20-fifo.cpp @@ -0,0 +1,246 @@ +/** + * Copyright (c) 2019 Skyward Experimental Rocketry + * Authors: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <array> + +#include "diagnostic/CpuMeter.h" +#include "drivers/HardwareTimer.h" +#include "drivers/spi/SPIDriver.h" +#include "sensors/L3GD20.h" + +using namespace miosix; +using std::array; + +typedef Gpio<GPIOF_BASE, 7> GpioSck; +typedef Gpio<GPIOF_BASE, 8> GpioMiso; +typedef Gpio<GPIOF_BASE, 9> GpioMosi; + +typedef Gpio<GPIOA_BASE, 2> GpioINT2; + +static constexpr unsigned int FIFO_WATERMARK = 12; + +// Expected frequency from the datasheet is 760 Hz, but due to clock +// misalignment / temperature errors and other factors, the observed clock (and +// output data rate) is the following: +static constexpr float SAMPLE_FREQUENCY = 782.3f; +static constexpr int NUM_SAMPLES = SAMPLE_FREQUENCY * 120; + +// FD +void enableInterrupt(); +void configure(); + +struct GyroSample +{ + int fifo_num; + L3GD20Data gyro; + int level; + uint64_t wtm_delta; + float cpu; + uint64_t update; +}; + +// GyroSample data[NUM_SAMPLES]; +GyroSample data[NUM_SAMPLES]; +int data_counter = 0; + +// High resolution hardware timer using TIM5 +HardwareTimer<uint32_t> hrclock{ + TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)}; + +// Last interrupt received timer tick +volatile uint32_t last_watermark_tick; // Stores the high-res tick of the last + // interrupt (L3GD20 watermark event) +volatile uint32_t watermark_delta; // Tick delta between the last 2 watermark + // events + +// L3GD20 SPI +SPIBus bus(SPI5); +GpioPin cs(GPIOC_BASE, 1); + +L3GD20* gyro = nullptr; + +// Interrupt handlers +void __attribute__((naked)) EXTI2_IRQHandler() +{ + saveContext(); + asm volatile("bl _Z20EXTI2_IRQHandlerImplv"); + restoreContext(); +} + +void __attribute__((used)) EXTI2_IRQHandlerImpl() +{ + // Current high resolution tick + uint32_t tick = hrclock.tick(); + watermark_delta = tick - last_watermark_tick; + last_watermark_tick = tick; + // Pass tick microseconds to sensor + if (gyro != nullptr) + { + gyro->IRQupdateTimestamp(hrclock.toIntMicroSeconds(tick)); + } + + EXTI->PR |= EXTI_PR_PR2; // Reset pending register +} + +int main() +{ + Thread::sleep(5000); + + configure(); + + // Setup sensor + gyro = new L3GD20(bus, cs, L3GD20::FullScaleRange::FS_250, + L3GD20::OutPutDataRate::ODR_760, 0x03); + + gyro->enableFifo(FIFO_WATERMARK); + + // Init + while (!gyro->init()) + { + } + + // Sample NUM_SAMPLES data + int fifo_num = 0; + while (data_counter < NUM_SAMPLES) + { + long last_tick = miosix::getTick(); + + // Read the fifo + uint32_t update = hrclock.tick(); + gyro->onSimpleUpdate(); + update = hrclock.tick() - update; + + uint8_t level = + gyro->getLastFifoSize(); // Number of samples in the FIFO + + // Get the FIFO + const array<L3GD20Data, 32>& fifo = gyro->getLastFifo(); + + // Store everything in the data buffer + for (int i = 0; i < level; i++) + { + // data[data_counter++] = fifo[i]; + data[data_counter++] = {fifo_num, + fifo[i], + level, + hrclock.toIntMicroSeconds(watermark_delta), + averageCpuUtilization(), + hrclock.toIntMicroSeconds(update)}; + + // Stop if we have enough data + if (data_counter >= NUM_SAMPLES) + { + break; + } + } + ++fifo_num; + + // Wait until fifo has about 25 samples + Thread::sleepUntil(last_tick + 25.5 * 1000 / SAMPLE_FREQUENCY); + } + + // Dump buffer content as CSV on the serial (might take a while) + // printf("t,x,y,z\n"); + printf("FIFO_num,timestamp,int_delta,read_time,sample_delta,x,y,z,cpu\n"); + + for (int i = 1; i < data_counter; i++) + { + // clang-format off + printf("%d,%llu,%llu,%llu,%llu,%f,%f,%f,%.2f\n", + data[i].fifo_num, + data[i].gyro.timestamp, + data[i].wtm_delta, + data[i].update, + (data[i].gyro.timestamp - data[i - 1].gyro.timestamp), + data[i].gyro.gyro.getX(), + data[i].gyro.gyro.getY(), + data[i].gyro.gyro.getZ(), + data[i].cpu); + // clang-format on + } + + printf("\n\n\nend.\n"); + for (;;) + { + Thread::sleep(1000); + } +} + +void configure() +{ + { + FastInterruptDisableLock dLock; + + RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; + RCC->APB2ENR |= RCC_APB2ENR_SPI5EN; + + GpioSck::mode(Mode::ALTERNATE); + GpioMiso::mode(Mode::ALTERNATE); + GpioMosi::mode(Mode::ALTERNATE); + + GpioSck::alternateFunction(5); + GpioMiso::alternateFunction(5); + GpioMosi::alternateFunction(5); + + // Interrupt + GpioINT2::mode(Mode::INPUT_PULL_DOWN); + + cs.mode(Mode::OUTPUT); + } + + cs.high(); + + enableInterrupt(); + + // High resolution clock configuration + hrclock.setPrescaler(382); + hrclock.start(); +} + +void enableInterrupt() +{ + { + FastInterruptDisableLock l; + RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN; + } + // Refer to the datasheet for a detailed description on the procedure and + // interrupt registers + + // Clear the mask on the wanted line + EXTI->IMR |= EXTI_IMR_MR2; + + // Trigger the interrupt on a falling edge + // EXTI->FTSR |= EXTI_FTSR_TR2; + + // Trigger the interrupt on a rising edge + EXTI->RTSR |= EXTI_RTSR_TR2; + + EXTI->PR |= EXTI_PR_PR2; // Reset pending register + + // Enable interrupt on PA2 in SYSCFG + SYSCFG->EXTICR[0] &= 0xFFFFF0FF; + + // // Enable the interrput in the interrupt controller + NVIC_EnableIRQ(EXTI2_IRQn); + NVIC_SetPriority(EXTI2_IRQn, 15); +} \ No newline at end of file diff --git a/src/tests/drivers/test-l3gd20.cpp b/src/tests/drivers/test-l3gd20.cpp index f5a63718ccb9427fc4f07f0d0f18afe5fc78ddd0..227dd44f16fc632840792cb352717cbc636b42fd 100644 --- a/src/tests/drivers/test-l3gd20.cpp +++ b/src/tests/drivers/test-l3gd20.cpp @@ -37,28 +37,20 @@ typedef Gpio<GPIOF_BASE, 9> GpioMosi; typedef Gpio<GPIOA_BASE, 2> GpioINT2; -static constexpr bool FIFO_ENABLED = true; -static constexpr unsigned int FIFO_WATERMARK = 24; -// Expected frequency from the datasheet is 760 Hz, but due to clock -// misalignment / temperature errors and other factors, the observed clock (and -// output data rate) is the following: -static constexpr float SAMPLE_FREQUENCY = 782.3f; -static constexpr int NUM_SAMPLES = 10000; +static constexpr int NUM_SAMPLES = 500 * 5; // 120 seconds of data void enableInterrupt(); +void configure(); struct GyroSample { - int fifo_num; - float timestamp; + uint64_t timestamp; + uint64_t sample_delta; Vec3 data; - int level; - float wtm_delta; float cpu; }; -// 364 KB buffer to store up to 30 seconds of data @ 760 Hz -GyroSample data[22800]; +GyroSample data[NUM_SAMPLES]; int data_counter = 0; // High resolution hardware timer using TIM5 @@ -66,16 +58,16 @@ HardwareTimer<uint32_t> hrclock{ TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)}; // Last interrupt received timer tick -volatile uint32_t last_watermark_tick; // Stores the high-res tick of the last - // interrupt (L3GD20 watermark event) -volatile uint32_t watermark_delta; // Tick delta between the last 2 watermark - // events +volatile uint32_t last_sample_tick; // Stores the high-res tick of the last + // interrupt (L3GD20 watermark event) +volatile uint32_t sample_delta; // Tick delta between the last 2 watermark + // events // L3GD20 SPI SPIBus bus(SPI5); GpioPin cs(GPIOC_BASE, 1); -SPIBusConfig cfg; -uint8_t buf[192]; + +L3GD20* gyro; // Interrupt handlers void __attribute__((naked)) EXTI2_IRQHandler() @@ -87,121 +79,64 @@ void __attribute__((naked)) EXTI2_IRQHandler() void __attribute__((used)) EXTI2_IRQHandlerImpl() { - uint32_t tick = hrclock.tick(); - watermark_delta = tick - last_watermark_tick; - last_watermark_tick = tick; + // Current high resolution tick + uint32_t tick = hrclock.tick(); + sample_delta = tick - last_sample_tick; + last_sample_tick = tick; + + // Pass tick microseconds to sensor for timestamp estimation + if (gyro != nullptr) + gyro->IRQupdateTimestamp(hrclock.toIntMicroSeconds(tick)); EXTI->PR |= EXTI_PR_PR2; // Reset pending register } int main() { - cfg.br = SPIBaudRate::DIV_64; - - { - FastInterruptDisableLock dLock; - - RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; - RCC->APB2ENR |= RCC_APB2ENR_SPI5EN; + Thread::sleep(5000); - GpioSck::mode(Mode::ALTERNATE); - GpioMiso::mode(Mode::ALTERNATE); - GpioMosi::mode(Mode::ALTERNATE); + configure(); - GpioSck::alternateFunction(5); - GpioMiso::alternateFunction(5); - GpioMosi::alternateFunction(5); + gyro = new L3GD20(bus, cs, L3GD20::FullScaleRange::FS_250, + L3GD20::OutPutDataRate::ODR_760, 0x03); - // Interrupt - GpioINT2::mode(Mode::INPUT_PULL_DOWN); + // gyro->setRealSampleInterval( + // static_cast<uint64_t>(1000000 / SAMPLE_FREQUENCY)); - cs.mode(Mode::OUTPUT); - } - // High resolution clock configuration - // 1.8 hours run time, 1.5 us resolution - hrclock.setPrescaler(127); - hrclock.start(); - - enableInterrupt(); - cs.high(); - - L3GD20 gyro(bus, cs, L3GD20::FullScaleRange::FS_250, - L3GD20::OutPutDataRate::ODR_760, 0x03, FIFO_ENABLED, - FIFO_WATERMARK); - - while (!gyro.init()) + while (!gyro->init()) { } Thread::sleep(500); - long long first_tick = miosix::getTick(); - - // Sample NUM_SAMPLES data - int fifo_num = 0; while (data_counter < NUM_SAMPLES) { long last_tick = miosix::getTick(); - if (FIFO_ENABLED) - { - // Precise timestamp of the last sample in the FIFO - float wtm_timestamp = hrclock.toSeconds(last_watermark_tick); - float wtm_delta = hrclock.toMilliSeconds(watermark_delta); - - // Read the fifo - gyro.onSimpleUpdate(); - - uint8_t level = - gyro.getLastFifoSize(); // Number of samples in the FIFO - - const array<Vec3, 32>& fifo = gyro.getLastFifo(); - - for (int i = 0; i < level; i++) - { - // Assign a timestamp to each sample in the FIFO - // Samples before the watermark are older, after the watermark - // are younger. Time delta between samples is - // (1 / SAMPLE_FREQUENCY) seconds - float ts = wtm_timestamp + - (i - (int)FIFO_WATERMARK) / SAMPLE_FREQUENCY; - - data[data_counter++] = {fifo_num, ts, - fifo[i], level, - wtm_delta, averageCpuUtilization()}; - if (data_counter >= NUM_SAMPLES) - { - break; - } - } - ++fifo_num; - - // Wait until fifo has about 25 samples - Thread::sleepUntil(last_tick + 34); - } - else - { - // Sample sensor @ 500 Hz - gyro.onSimpleUpdate(); - - data[data_counter++] = {0, - (last_tick - first_tick) / 1000.0f, - *(gyro.gyroDataPtr()), - 0, - 0, - averageCpuUtilization()}; - Thread::sleepUntil(last_tick + 2); - } + // Sample sensor @ 500 Hz + gyro->onSimpleUpdate(); + L3GD20Data d = gyro->getLastSample(); + + data[data_counter++] = {d.timestamp, sample_delta, d.gyro, + averageCpuUtilization()}; + + Thread::sleepUntil(last_tick + 1); } // Dump buffer content as CSV on the serial (might take a while) - printf("FIFO_num,timestamp,watermark_delta,sample_delta,x,y,z,level,cpu"); + printf("FIFO_num,timestamp,int_delta,sample_delta,x,y,z,cpu\n"); for (int i = 1; i < data_counter; i++) { - printf("%d,%.3f,%.3f,%.3f,%f,%f,%f,%d,%.2f\n", data[i].fifo_num, - data[i].timestamp * 1000, data[i].wtm_delta, - (data[i].timestamp - data[i - 1].timestamp) * 1000, - data[i].data.getX(), data[i].data.getY(), data[i].data.getZ(), - data[i].level, data[i].cpu); + // clang-format off + printf("%d,%llu,%llu,%llu,%f,%f,%f,%.2f\n", + 0, + data[i].timestamp, + hrclock.toIntMicroSeconds(data[i].sample_delta), + (data[i].timestamp - data[i - 1].timestamp), + data[i].data.getX(), + data[i].data.getY(), + data[i].data.getZ(), + data[i].cpu); + // clang-format on } printf("\n\n\nend.\n"); @@ -211,6 +146,38 @@ int main() } } +void configure() +{ + { + FastInterruptDisableLock dLock; + + RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; + RCC->APB2ENR |= RCC_APB2ENR_SPI5EN; + + GpioSck::mode(Mode::ALTERNATE); + GpioMiso::mode(Mode::ALTERNATE); + GpioMosi::mode(Mode::ALTERNATE); + + GpioSck::alternateFunction(5); + GpioMiso::alternateFunction(5); + GpioMosi::alternateFunction(5); + + // Interrupt + GpioINT2::mode(Mode::INPUT_PULL_DOWN); + + cs.mode(Mode::OUTPUT); + } + + cs.high(); + + enableInterrupt(); + + // High resolution clock configuration + // 1.8 hours run time, 1.5 us resolution + hrclock.setPrescaler(127); + hrclock.start(); +} + void enableInterrupt() { {