Skip to content
Snippets Groups Projects
Commit 8db8151d authored by Jakob Faltisek's avatar Jakob Faltisek
Browse files

Adding source code

parents
No related branches found
No related tags found
No related merge requests found
Showing
with 1060 additions and 0 deletions
# Kiwi: Elevating Avionics Reliability for the Aerospace Team Graz
This repository contains the source code for the "Kiwi: Elevating Avionics Reliability for the Aerospace Team Graz" master thesis, by Jakob Faltisek.
It is split into two parts:
- [Kiwi Interface Software](./kiwi-interface-software/): The software running on the Kiwi Interface PCB.
- [Kiwi Mission Control Software](./kiwi-mission-control-software/): The software running on the Kiwi mission control, containing only files that were changed of the ASTG mission control software.
\ No newline at end of file
This diff is collapsed.
This diff is collapsed.
.metadata/
RemoteSystemsTempFiles/
.settings/
Debug/
*.launch
# STM32 git ignore
*.mxproject
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>kiwi-interface-software</name>
<comment></comment>
<projects>
</projects>
<buildSpec>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
<triggers>clean,full,incremental,</triggers>
<arguments>
</arguments>
</buildCommand>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
<triggers>full,incremental,</triggers>
<arguments>
</arguments>
</buildCommand>
</buildSpec>
<natures>
<nature>com.st.stm32cube.ide.mcu.MCUProjectNature</nature>
<nature>com.st.stm32cube.ide.mcu.MCUCubeProjectNature</nature>
<nature>org.eclipse.cdt.core.cnature</nature>
<nature>org.eclipse.cdt.core.ccnature</nature>
<nature>com.st.stm32cube.ide.mcu.MCUCubeIdeServicesRevAev2ProjectNature</nature>
<nature>com.st.stm32cube.ide.mcu.MCUAdvancedStructureProjectNature</nature>
<nature>com.st.stm32cube.ide.mcu.MCUSingleCpuProjectNature</nature>
<nature>com.st.stm32cube.ide.mcu.MCURootProjectNature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
</natures>
</projectDescription>
/* USER CODE BEGIN Header */
/*
* FreeRTOS Kernel V10.3.1
* Portion Copyright (C) 2017 Amazon.com, Inc. or its affiliates. All Rights Reserved.
* Portion Copyright (C) 2019 StMicroelectronics, Inc. All Rights Reserved.
*
* 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.
*
* http://www.FreeRTOS.org
* http://aws.amazon.com/freertos
*
* 1 tab == 4 spaces!
*/
/* USER CODE END Header */
#ifndef FREERTOS_CONFIG_H
#define FREERTOS_CONFIG_H
/*-----------------------------------------------------------
* Application specific definitions.
*
* These definitions should be adjusted for your particular hardware and
* application requirements.
*
* These parameters and more are described within the 'configuration' section of the
* FreeRTOS API documentation available on the FreeRTOS.org web site.
*
* See http://www.freertos.org/a00110.html
*----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* Section where include file can be added */
/* USER CODE END Includes */
/* Ensure definitions are only used by the compiler, and not by the assembler. */
#if defined(__ICCARM__) || defined(__CC_ARM) || defined(__GNUC__)
#include <stdint.h>
extern uint32_t SystemCoreClock;
#endif
#ifndef CMSIS_device_header
#define CMSIS_device_header "stm32h7xx.h"
#endif /* CMSIS_device_header */
#define configENABLE_FPU 0
#define configENABLE_MPU 0
#define configUSE_PREEMPTION 1
#define configSUPPORT_STATIC_ALLOCATION 1
#define configSUPPORT_DYNAMIC_ALLOCATION 1
#define configUSE_IDLE_HOOK 0
#define configUSE_TICK_HOOK 0
#define configCPU_CLOCK_HZ ( SystemCoreClock )
#define configTICK_RATE_HZ ((TickType_t)1000)
#define configMAX_PRIORITIES ( 56 )
#define configMINIMAL_STACK_SIZE ((uint16_t)128)
#define configTOTAL_HEAP_SIZE ((size_t)15360)
#define configMAX_TASK_NAME_LEN ( 16 )
#define configUSE_TRACE_FACILITY 1
#define configUSE_16_BIT_TICKS 0
#define configUSE_MUTEXES 1
#define configQUEUE_REGISTRY_SIZE 8
#define configUSE_RECURSIVE_MUTEXES 1
#define configUSE_COUNTING_SEMAPHORES 1
#define configUSE_PORT_OPTIMISED_TASK_SELECTION 0
/* USER CODE BEGIN MESSAGE_BUFFER_LENGTH_TYPE */
/* Defaults to size_t for backward compatibility, but can be changed
if lengths will always be less than the number of bytes in a size_t. */
#define configMESSAGE_BUFFER_LENGTH_TYPE size_t
/* USER CODE END MESSAGE_BUFFER_LENGTH_TYPE */
/* Co-routine definitions. */
#define configUSE_CO_ROUTINES 0
#define configMAX_CO_ROUTINE_PRIORITIES ( 2 )
/* Software timer definitions. */
#define configUSE_TIMERS 1
#define configTIMER_TASK_PRIORITY ( 2 )
#define configTIMER_QUEUE_LENGTH 10
#define configTIMER_TASK_STACK_DEPTH 256
/* The following flag must be enabled only when using newlib */
#define configUSE_NEWLIB_REENTRANT 1
/* CMSIS-RTOS V2 flags */
#define configUSE_OS2_THREAD_SUSPEND_RESUME 1
#define configUSE_OS2_THREAD_ENUMERATE 1
#define configUSE_OS2_EVENTFLAGS_FROM_ISR 1
#define configUSE_OS2_THREAD_FLAGS 1
#define configUSE_OS2_TIMER 1
#define configUSE_OS2_MUTEX 1
/* Set the following definitions to 1 to include the API function, or zero
to exclude the API function. */
#define INCLUDE_vTaskPrioritySet 1
#define INCLUDE_uxTaskPriorityGet 1
#define INCLUDE_vTaskDelete 1
#define INCLUDE_vTaskCleanUpResources 0
#define INCLUDE_vTaskSuspend 1
#define INCLUDE_vTaskDelayUntil 1
#define INCLUDE_vTaskDelay 1
#define INCLUDE_xTaskGetSchedulerState 1
#define INCLUDE_xTimerPendFunctionCall 1
#define INCLUDE_xQueueGetMutexHolder 1
#define INCLUDE_uxTaskGetStackHighWaterMark 1
#define INCLUDE_xTaskGetCurrentTaskHandle 1
#define INCLUDE_eTaskGetState 1
/*
* The CMSIS-RTOS V2 FreeRTOS wrapper is dependent on the heap implementation used
* by the application thus the correct define need to be enabled below
*/
#define USE_FreeRTOS_HEAP_4
/* Cortex-M specific definitions. */
#ifdef __NVIC_PRIO_BITS
/* __BVIC_PRIO_BITS will be specified when CMSIS is being used. */
#define configPRIO_BITS __NVIC_PRIO_BITS
#else
#define configPRIO_BITS 4
#endif
/* The lowest interrupt priority that can be used in a call to a "set priority"
function. */
#define configLIBRARY_LOWEST_INTERRUPT_PRIORITY 15
/* The highest interrupt priority that can be used by any interrupt service
routine that makes calls to interrupt safe FreeRTOS API functions. DO NOT CALL
INTERRUPT SAFE FREERTOS API FUNCTIONS FROM ANY INTERRUPT THAT HAS A HIGHER
PRIORITY THAN THIS! (higher priorities are lower numeric values. */
#define configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY 5
/* Interrupt priorities used by the kernel port layer itself. These are generic
to all Cortex-M ports, and do not rely on any particular library functions. */
#define configKERNEL_INTERRUPT_PRIORITY ( configLIBRARY_LOWEST_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) )
/* !!!! configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to zero !!!!
See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html. */
#define configMAX_SYSCALL_INTERRUPT_PRIORITY ( configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) )
/* Normal assert() semantics without relying on the provision of an assert.h
header file. */
/* USER CODE BEGIN 1 */
#define configASSERT( x ) if ((x) == 0) {taskDISABLE_INTERRUPTS(); for( ;; );}
/* USER CODE END 1 */
/* Definitions that map the FreeRTOS port interrupt handlers to their CMSIS
standard names. */
#define vPortSVCHandler SVC_Handler
#define xPortPendSVHandler PendSV_Handler
/* IMPORTANT: After 10.3.1 update, Systick_Handler comes from NVIC (if SYS timebase = systick), otherwise from cmsis_os2.c */
#define USE_CUSTOM_SYSTICK_HANDLER_IMPLEMENTATION 0
/* USER CODE BEGIN Defines */
/* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */
/* USER CODE END Defines */
#endif /* FREERTOS_CONFIG_H */
#pragma once
#include <handler.hpp>
struct AnalogInput {
uint8_t rank_;
volatile uint16_t *buffer_;
float factor_;
};
class AnalogInputHandler : public Handler {
public:
AnalogInputHandler(ATPBodyKiwiSystem &system);
void init() override;
void run() override;
static constexpr uint32_t ADC1_CHANNEL_COUNT = 8;
static constexpr uint32_t ADC2_CHANNEL_COUNT = 4;
static constexpr uint32_t ADC3_CHANNEL_COUNT = 2;
private:
AnalogInput voltage_srad_fd_1_;
AnalogInput voltage_srad_fd_2_;
AnalogInput voltage_srad_sd_1_;
AnalogInput voltage_srad_sd_2_;
AnalogInput voltage_cots_fd_1_;
AnalogInput voltage_cots_fd_2_;
AnalogInput voltage_cots_sd_1_;
AnalogInput voltage_cots_sd_2_;
AnalogInput mb_3v3_;
AnalogInput bp_battery_;
AnalogInput bp_5v_;
AnalogInput bp_12v_;
AnalogInput reference_voltage_;
AnalogInput mcu_temperature_;
static constexpr uint16_t REFERENCE_VOLTAGE_DEFAULT_MV = 3300;
static constexpr uint16_t REFERENCE_VOLTAGE_LOWER_THRESHOLD_MV = 500;
static constexpr uint16_t REFERENCE_VOLTAGE_UPPER_THRESHOLD_MV = 4000;
uint16_t reference_voltage_mv_;
void updateReferenceVoltage();
uint8_t readTemperature();
uint16_t readVoltage(AnalogInput &input);
};
#pragma once
#include "byte_ring_buffer.hpp"
#include "cmsis_os.h"
#include "handler.hpp"
#include "sensors_handler.hpp"
#include "stm32h7xx_hal.h"
#include "usbd_cdc_if.h"
extern "C" {
#include "atp.h"
}
class CommunicationHandler : public Handler {
public:
CommunicationHandler(ATPBodyKiwiSystem &system,
SensorSamples &sensor_samples);
void init() override;
void run() override;
static uint8_t usbReceive(uint8_t *buf, uint32_t *len);
static constexpr uint32_t RING_BUFFER_SIZE_ = APP_RX_DATA_SIZE * 16;
private:
uint32_t transmit_counter_;
static bool received_flag_;
static util::ByteRingBuffer ring_buffer_;
SensorSamples &sensor_samples_;
static void handlePacket(ATPInterface interface, ATPHeader *header,
ATPBody *body, ATPFooter *footer, uint8_t buffer[],
size_t buffer_size);
static void handlePing(ATPHeader *header, ATPBodyPing *ping);
static void handlePong(ATPHeader *header, ATPBodyPong *pong);
};
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
uint8_t usbReceive(uint8_t *buf, uint32_t *len);
#ifdef __cplusplus
}
#endif
#pragma once
#include "handler.hpp"
#include "main.h"
#include <vector>
struct GpioInput {
uint16_t pin_;
GPIO_TypeDef *port_;
};
class DigitalInputHandler : public Handler {
public:
DigitalInputHandler(ATPBodyKiwiSystem &system);
void init() override;
void run() override;
private:
GpioInput rec_camera_;
GpioInput mb_debug_io_0_;
GpioInput mb_debug_io_1_;
GpioInput mb_debug_io_2_;
GpioInput mb_debug_io_3_;
GpioInput mb_debug_io_4_;
GpioInput mb_debug_io_5_;
GpioInput mb_debug_io_6_;
GpioInput mb_debug_io_7_;
GpioInput bp_camera_1_;
GpioInput bp_camera_2_;
GpioInput bp_fan_;
GpioInput canary_rst_;
GpioInput mb_buzzer_;
void readGpioInputs();
};
#pragma once
#include "packet.h"
class Handler {
public:
Handler(ATPBodyKiwiSystem &system) : system_(system) {}
virtual void init() = 0;
virtual void run() = 0;
virtual ~Handler() = default;
protected:
ATPBodyKiwiSystem &system_;
};
#pragma once
#include <handler.hpp>
class LedHandler : public Handler {
public:
LedHandler(ATPBodyKiwiSystem &system) : Handler(system), counter_{0} {}
void init() override;
void run() override;
static constexpr auto LED_BLINK_PERIOD_MS = 1000;
private:
uint32_t counter_;
};
#pragma once
#include "bmi088_sensor.hpp"
#include "bmp390_sensor.hpp"
#include "ms5607_sensor.hpp"
#include "sensor_sample.hpp"
#include <handler.hpp>
#include <ring_buffer.hpp>
extern "C" {
#include <packet.h>
}
#include <map>
#include <utility>
using sensor_sim::BMI088Sensor;
using sensor_sim::BMP390Sensor;
using sensor_sim::MS5607Sensor;
using sensor_sim::Sensor;
using sensor_sim::SensorSample;
extern I2C_HandleTypeDef hi2c4;
extern I2C_HandleTypeDef hi2c5;
// Assuming hi2c is a pointer to some I2C handler type, e.g., I2C_HandleTypeDef*
// hi2c;
using I2CAddressKey = std::pair<I2C_HandleTypeDef *, uint8_t>;
class SensorsHandler : public Handler {
public:
SensorsHandler(ATPBodyKiwiSystem &system, SensorSamples &sensor_samples);
void init() override;
void run() override;
private:
SensorSamples &sensor_samples_;
SensorSample current_sample_;
std::map<I2CAddressKey, Sensor *>
sensors_; // Array holding pointers to all sensor objects
// Sensor instances
BMI088Sensor bmi088_gyro_1;
BMI088Sensor bmi088_accel_1;
BMI088Sensor bmi088_gyro_2;
BMI088Sensor bmi088_accel_2;
BMP390Sensor bmp390_1;
BMP390Sensor bmp390_2;
MS5607Sensor ms5607_1;
MS5607Sensor ms5607_2;
void loadNextSample(); // Load the next sample from the ring buffer to sensors
};
#include "util.hpp"
#include <atomic>
extern "C" {
#include <packet.h>
}
struct AtomicSensorSample {
std::atomic<uint32_t> bmp390_pressure_1;
std::atomic<uint32_t> bmp390_pressure_2;
std::atomic<uint32_t> bmp390_temperature_1;
std::atomic<uint32_t> bmp390_temperature_2;
std::atomic<uint32_t> ms5609_pressure_1;
std::atomic<uint32_t> ms5609_pressure_2;
std::atomic<uint32_t> ms5609_temperature_1;
std::atomic<uint32_t> ms5609_temperature_2;
std::atomic<uint16_t> bmi088_acceleration_x_imu_1;
std::atomic<uint16_t> bmi088_acceleration_y_imu_1;
std::atomic<uint16_t> bmi088_acceleration_z_imu_1;
std::atomic<uint16_t> bmi088_acceleration_x_imu_2;
std::atomic<uint16_t> bmi088_acceleration_y_imu_2;
std::atomic<uint16_t> bmi088_acceleration_z_imu_2;
std::atomic<uint16_t> bmi088_gyro_x_imu_1;
std::atomic<uint16_t> bmi088_gyro_y_imu_1;
std::atomic<uint16_t> bmi088_gyro_z_imu_1;
std::atomic<uint16_t> bmi088_gyro_x_imu_2;
std::atomic<uint16_t> bmi088_gyro_y_imu_2;
std::atomic<uint16_t> bmi088_gyro_z_imu_2;
// Method to load values from the original struct
void loadFrom(const ATPBodyKiwiSensorSample &source) {
bmp390_pressure_1.store(
util::reverseEndiannessUint24(source.bmp390_pressure_1),
std::memory_order_relaxed);
bmp390_pressure_2.store(
util::reverseEndiannessUint24(source.bmp390_pressure_2),
std::memory_order_relaxed);
bmp390_temperature_1.store(
util::reverseEndiannessUint24(source.bmp390_temperature_1),
std::memory_order_relaxed);
bmp390_temperature_2.store(
util::reverseEndiannessUint24(source.bmp390_temperature_2),
std::memory_order_relaxed);
ms5609_pressure_1.store(
util::reverseEndiannessUint24(source.ms5609_pressure_1),
std::memory_order_relaxed);
ms5609_pressure_2.store(
util::reverseEndiannessUint24(source.ms5609_pressure_2),
std::memory_order_relaxed);
ms5609_temperature_1.store(
util::reverseEndiannessUint24(source.ms5609_temperature_1),
std::memory_order_relaxed);
ms5609_temperature_2.store(
util::reverseEndiannessUint24(source.ms5609_temperature_2),
std::memory_order_relaxed);
bmi088_acceleration_x_imu_1.store(
util::reverseEndianness(source.bmi088_acceleration_x_imu_1),
std::memory_order_relaxed);
bmi088_acceleration_y_imu_1.store(
util::reverseEndianness(source.bmi088_acceleration_y_imu_1),
std::memory_order_relaxed);
bmi088_acceleration_z_imu_1.store(
util::reverseEndianness(source.bmi088_acceleration_z_imu_1),
std::memory_order_relaxed);
bmi088_acceleration_x_imu_2.store(
util::reverseEndianness(source.bmi088_acceleration_x_imu_2),
std::memory_order_relaxed);
bmi088_acceleration_y_imu_2.store(
util::reverseEndianness(source.bmi088_acceleration_y_imu_2),
std::memory_order_relaxed);
bmi088_acceleration_z_imu_2.store(
util::reverseEndianness(source.bmi088_acceleration_z_imu_2),
std::memory_order_relaxed);
bmi088_gyro_x_imu_1.store(
util::reverseEndianness(source.bmi088_gyro_x_imu_1),
std::memory_order_relaxed);
bmi088_gyro_y_imu_1.store(
util::reverseEndianness(source.bmi088_gyro_y_imu_1),
std::memory_order_relaxed);
bmi088_gyro_z_imu_1.store(
util::reverseEndianness(source.bmi088_gyro_z_imu_1),
std::memory_order_relaxed);
bmi088_gyro_x_imu_2.store(
util::reverseEndianness(source.bmi088_gyro_x_imu_2),
std::memory_order_relaxed);
bmi088_gyro_y_imu_2.store(
util::reverseEndianness(source.bmi088_gyro_y_imu_2),
std::memory_order_relaxed);
bmi088_gyro_z_imu_2.store(
util::reverseEndianness(source.bmi088_gyro_z_imu_2),
std::memory_order_relaxed);
}
// Method to copy values from another AtomicSensorSample
void copyFrom(const AtomicSensorSample &source) {
bmp390_pressure_1.store(
source.bmp390_pressure_1.load(std::memory_order_relaxed),
std::memory_order_release);
bmp390_pressure_2.store(
source.bmp390_pressure_2.load(std::memory_order_relaxed),
std::memory_order_release);
bmp390_temperature_1.store(
source.bmp390_temperature_1.load(std::memory_order_relaxed),
std::memory_order_release);
bmp390_temperature_2.store(
source.bmp390_temperature_2.load(std::memory_order_relaxed),
std::memory_order_release);
ms5609_pressure_1.store(
source.ms5609_pressure_1.load(std::memory_order_relaxed),
std::memory_order_release);
ms5609_pressure_2.store(
source.ms5609_pressure_2.load(std::memory_order_relaxed),
std::memory_order_release);
ms5609_temperature_1.store(
source.ms5609_temperature_1.load(std::memory_order_relaxed),
std::memory_order_release);
ms5609_temperature_2.store(
source.ms5609_temperature_2.load(std::memory_order_relaxed),
std::memory_order_release);
bmi088_acceleration_x_imu_1.store(
source.bmi088_acceleration_x_imu_1.load(std::memory_order_relaxed),
std::memory_order_release);
bmi088_acceleration_y_imu_1.store(
source.bmi088_acceleration_y_imu_1.load(std::memory_order_relaxed),
std::memory_order_release);
bmi088_acceleration_z_imu_1.store(
source.bmi088_acceleration_z_imu_1.load(std::memory_order_relaxed),
std::memory_order_release);
bmi088_acceleration_x_imu_2.store(
source.bmi088_acceleration_x_imu_2.load(std::memory_order_relaxed),
std::memory_order_release);
bmi088_acceleration_y_imu_2.store(
source.bmi088_acceleration_y_imu_2.load(std::memory_order_relaxed),
std::memory_order_release);
bmi088_acceleration_z_imu_2.store(
source.bmi088_acceleration_z_imu_2.load(std::memory_order_relaxed),
std::memory_order_release);
bmi088_gyro_x_imu_1.store(
source.bmi088_gyro_x_imu_1.load(std::memory_order_relaxed),
std::memory_order_release);
bmi088_gyro_y_imu_1.store(
source.bmi088_gyro_y_imu_1.load(std::memory_order_relaxed),
std::memory_order_release);
bmi088_gyro_z_imu_1.store(
source.bmi088_gyro_z_imu_1.load(std::memory_order_relaxed),
std::memory_order_release);
bmi088_gyro_x_imu_2.store(
source.bmi088_gyro_x_imu_2.load(std::memory_order_relaxed),
std::memory_order_release);
bmi088_gyro_y_imu_2.store(
source.bmi088_gyro_y_imu_2.load(std::memory_order_relaxed),
std::memory_order_release);
bmi088_gyro_z_imu_2.store(
source.bmi088_gyro_z_imu_2.load(std::memory_order_relaxed),
std::memory_order_release);
}
};
#pragma once
#include "sensor.hpp"
#include <array>
#include <cstdint>
namespace sensor_sim {
// BMI088 Register Addresses
namespace BMI088Registers {
// Accelerometer Registers
constexpr uint8_t ACCEL_CHIP_ID = 0x00;
constexpr uint8_t ACCEL_PWR_CONF = 0x7C;
constexpr uint8_t ACCEL_PWR_CTRL = 0x7D;
constexpr uint8_t ACCEL_CONF = 0x40;
constexpr uint8_t ACCEL_RANGE = 0x41;
constexpr uint8_t ACCEL_DATA_START =
0x12; // Start address for accel data (X LSB)
constexpr uint8_t ACCEL_DATA_LENGTH = 6; // Length of accel data block
constexpr uint8_t ACCEL_TEMPERATURE_START = 0x22;
// Gyroscope Registers
constexpr uint8_t GYRO_CHIP_ID = 0x00;
constexpr uint8_t GYRO_LPM1 = 0x11;
constexpr uint8_t GYRO_LPM2 = 0x12;
constexpr uint8_t GYRO_BANDWIDTH = 0x10;
constexpr uint8_t GYRO_DATA_START = 0x02; // Start address for gyro data (X LSB)
constexpr uint8_t GYRO_DATA_LENGTH = 6; // Length of gyro data block
} // namespace BMI088Registers
// BMI088 Chip IDs
namespace BMI088ChipIDs {
constexpr uint8_t ACCEL_CHIP_ID_VALUE = 0x1E;
constexpr uint8_t GYRO_CHIP_ID_VALUE = 0x0F;
} // namespace BMI088ChipIDs
// BMI088 I2C addresses
namespace BMI088Addr {
constexpr uint8_t ACCEL_PRIMARY = 0x18;
constexpr uint8_t ACCEL_SECONDARY = 0x19;
constexpr uint8_t GYRO_PRIMARY = 0x68;
constexpr uint8_t GYRO_SECONDARY = 0x69;
} // namespace BMI088Addr
class BMI088Sensor : public Sensor {
public:
BMI088Sensor(SensorSample &sample, I2C_HandleTypeDef *hi2c, uint8_t address,
SensorId sensor_id);
virtual void handleRegAddr(uint8_t reg) override;
virtual void handleReadRequest(uint8_t reg) override;
virtual void handleWriteRequest(uint8_t reg, uint8_t data) override;
};
} // namespace sensor_sim
#pragma once
#include "sensor.hpp"
#include <array>
#include <cstdint>
namespace sensor_sim {
// BMP390 Register Addresses
namespace BMP390Registers {
constexpr uint8_t CHIP_ID = 0x00;
constexpr uint8_t ERR = 0x02;
constexpr uint8_t SENS_STATUS = 0x03;
constexpr uint8_t DATA_START =
0x04; // Start address for pressure/temperature data
constexpr uint8_t EVENT = 0x10;
constexpr uint8_t INT_STATUS = 0x11;
constexpr uint8_t CALIB_DATA = 0x31; // Start address for calibration data
constexpr uint8_t PWR_CTRL = 0x1B;
constexpr uint8_t OSR = 0x1C;
constexpr uint8_t ODR = 0x1D;
constexpr uint8_t CMD = 0x7E;
} // namespace BMP390Registers
// BMP390 Commands
namespace BMP390Commands {
constexpr uint8_t SOFT_RESET = 0xB6;
}
// BMP390 Chip ID
namespace BMP390ChipID {
constexpr uint8_t CHIP_ID_VALUE = 0x60;
}
// Calibration Data Array
constexpr std::array<uint8_t, 21> CALIB_DATA_RESPONSE = {
0x00, 0x00, // NVM_PAR_T1 = 0
0x00, 0x40, // NVM_PAR_T2 = 2^14 = 0x4000
0x00, // NVM_PAR_T3 = 0
0x00, 0x60, // NVM_PAR_P1 = 2^ 14 + 2^13 = 0x6000
0x00, 0x40, // NVM_PAR_P2 = 2^14 = 0x4000
0x00, // NVM_PAR_P3 = 0
0x00, // NVM_PAR_P4 = 0
0x00, 0x00, // NVM_PAR_P5 = 0
0x00, 0x00, // NVM_PAR_P6 = 0
0x00, // NVM_PAR_P7 = 0
0x00, // NVM_PAR_P8 = 0
0x00, // NVM_PAR_P9 = 0
0x00, // NVM_PAR_P10 = 0
0x00 // NVM_PAR_P11 = 0
};
constexpr uint8_t INT_STATUS_RESPONSE = 1 << 3;
constexpr uint8_t SENS_STATUS_RESPONSE = (1 << 4) | (1 << 6);
constexpr uint8_t EVENT_RESPONSE = 0x00;
constexpr uint8_t ERR_RESPONSE = 0x00;
constexpr uint8_t PWR_CTRL_RESPONSE = 0x00;
constexpr std::array<uint8_t, 4> OSR_RESPONSE = {0x00, 0x00, 0x00, 0x00};
constexpr uint8_t ODR_RESPONSE = 0x00;
constexpr uint8_t DEFAULT_RESPONSE = 0x00;
class BMP390Sensor : public Sensor {
public:
BMP390Sensor(SensorSample &sample, I2C_HandleTypeDef *hi2c, uint8_t address,
SensorId sensor_id);
virtual void handleRegAddr(uint8_t reg) override;
virtual void handleReadRequest(uint8_t reg) override;
virtual void handleWriteRequest(uint8_t reg, uint8_t data) override;
private:
std::array<uint8_t, 21> calib_data_ = CALIB_DATA_RESPONSE;
};
} // namespace sensor_sim
#pragma once
#include "sensor.hpp"
#include <array>
#include <cstdint>
namespace sensor_sim {
// MS5607 Register Addresses and Commands
namespace MS5607Registers {
constexpr uint8_t PROM_START =
0xA0; // Start address for calibration coefficients
constexpr uint8_t ADC_READ = 0x00; // Address for reading ADC results
constexpr uint8_t RESET = 0x1E; // Reset command
constexpr uint8_t ADC_CONV = 0x40; // Start ADC conversion
constexpr uint8_t ADC_PRESSURE = 0x00; // Pressure conversion
constexpr uint8_t ADC_TEMPERATURE = 0x10; // Temperature conversion
} // namespace MS5607Registers
// Calibration Data Array
constexpr std::array<uint16_t, 8> PROM_DATA_RESPONSE = {
0x0000, // Factory data and setup
0x2000, // C1
0x0000, // C2
0x0000, // C3
0x0000, // C4
0x5DC0, // C5
0x2000, // C6
0x0000 // Serial code and CRC
};
class MS5607Sensor : public Sensor {
public:
MS5607Sensor(SensorSample &sample, I2C_HandleTypeDef *hi2c, uint8_t address,
SensorId sensor_id);
virtual void handleRegAddr(uint8_t reg) override;
virtual void handleReadRequest(uint8_t reg) override;
virtual void handleWriteRequest(uint8_t reg, uint8_t data) override;
private:
constexpr uint8_t isBitSet(uint8_t byte, uint8_t bitmask) {
return (byte & bitmask) == bitmask;
};
uint8_t trigger_cmd_ = 0;
};
} // namespace sensor_sim
#pragma once
#include "ring_buffer.hpp"
#include <cstdint>
#include <cstring>
extern "C" {
#include <packet.h>
}
#include "main.h"
#include "sensor_sample.hpp"
namespace sensor_sim {
enum class SensorId {
ID_1 = 1,
ID_2 = 2,
};
class Sensor {
public:
Sensor(SensorSample &sample, I2C_HandleTypeDef *hi2c, uint8_t address,
SensorId sensor_id);
bool handleRxEvent();
bool handleTxEvent();
void handleAddrEvent(uint8_t transfer_direction);
bool handleErrorEvent();
inline I2C_HandleTypeDef *getI2C() { return hi2c_; }
inline uint8_t getAddress() { return address_; }
protected:
SensorSample &sample_;
I2C_HandleTypeDef *hi2c_;
uint8_t address_;
SensorId sensor_id_;
static constexpr size_t RX_BUFFER_SIZE = 2;
static constexpr size_t TX_BUFFER_SIZE = 6;
std::array<uint8_t, RX_BUFFER_SIZE> rx_buffer_;
std::array<uint8_t, TX_BUFFER_SIZE> tx_buffer_;
uint8_t register_address_;
bool operation_running_;
bool address_received_;
size_t addr_event_while_operation_running_;
virtual void handleRegAddr(uint8_t reg) = 0;
virtual void handleReadRequest(uint8_t reg) = 0;
virtual void handleWriteRequest(uint8_t reg, uint8_t data) = 0;
void resetState();
};
;
} // namespace sensor_sim
#pragma once
#include "atomic_sensor_sample.hpp"
#include "ring_buffer.hpp"
#include "util.hpp"
#include <array>
#include <atomic>
#include <tuple>
extern "C" {
#include <packet.h>
}
constexpr uint32_t SENSOR_SAMPLES = 255;
typedef util::RingBuffer<ATPBodyKiwiSensorSample, SENSOR_SAMPLES> SensorSamples;
namespace sensor_sim {
class SensorSample {
public:
enum class SensorSampleIndex {
BMI088_ACCEL_IMU_1,
BMI088_GYRO_IMU_1,
BMI088_ACCEL_IMU_2,
BMI088_GYRO_IMU_2,
BMP390_PRESSURE_1,
BMP390_TEMPERATURE_1,
BMP390_PRESSURE_2,
BMP390_TEMPERATURE_2,
MS5609_PRESSURE_1,
MS5609_TEMPERATURE_1,
MS5609_PRESSURE_2,
MS5609_TEMPERATURE_2
};
inline SensorSample(SensorSamples &ring_buffer)
: ring_buffer_(ring_buffer), current_flags_({false}),
next_flags_({false}), current_sample_{0}, next_sample_{0} {}
void prepareSensorData() {
bool shouldUpdate = false;
for (size_t i = 0; i < next_flags_.size(); ++i) {
if (next_flags_[i].load(std::memory_order_acquire)) {
shouldUpdate = true;
break;
}
}
if (shouldUpdate) {
// Copy next_sample_ to current_sample_
current_sample_.copyFrom(next_sample_);
for (size_t i = 0; i < next_flags_.size(); ++i) {
current_flags_[i].store(next_flags_[i].load(std::memory_order_relaxed),
std::memory_order_release);
next_flags_[i].store(false, std::memory_order_release);
}
// Load the next sample into next_sample_
loadNewSample();
}
}
inline std::tuple<uint16_t, uint16_t, uint16_t> getBMI088AccelIMU1() {
return getSensorData(SensorSampleIndex::BMI088_ACCEL_IMU_1,
current_sample_.bmi088_acceleration_x_imu_1,
current_sample_.bmi088_acceleration_y_imu_1,
current_sample_.bmi088_acceleration_z_imu_1,
next_sample_.bmi088_acceleration_x_imu_1,
next_sample_.bmi088_acceleration_y_imu_1,
next_sample_.bmi088_acceleration_z_imu_1);
}
inline std::tuple<uint16_t, uint16_t, uint16_t> getBMI088GyroIMU1() {
return getSensorData(
SensorSampleIndex::BMI088_GYRO_IMU_1,
current_sample_.bmi088_gyro_x_imu_1,
current_sample_.bmi088_gyro_y_imu_1,
current_sample_.bmi088_gyro_z_imu_1, next_sample_.bmi088_gyro_x_imu_1,
next_sample_.bmi088_gyro_y_imu_1, next_sample_.bmi088_gyro_z_imu_1);
}
inline std::tuple<uint16_t, uint16_t, uint16_t> getBMI088AccelIMU2() {
return getSensorData(SensorSampleIndex::BMI088_ACCEL_IMU_2,
current_sample_.bmi088_acceleration_x_imu_2,
current_sample_.bmi088_acceleration_y_imu_2,
current_sample_.bmi088_acceleration_z_imu_2,
next_sample_.bmi088_acceleration_x_imu_2,
next_sample_.bmi088_acceleration_y_imu_2,
next_sample_.bmi088_acceleration_z_imu_2);
}
inline std::tuple<uint16_t, uint16_t, uint16_t> getBMI088GyroIMU2() {
return getSensorData(
SensorSampleIndex::BMI088_GYRO_IMU_2,
current_sample_.bmi088_gyro_x_imu_2,
current_sample_.bmi088_gyro_y_imu_2,
current_sample_.bmi088_gyro_z_imu_2, next_sample_.bmi088_gyro_x_imu_2,
next_sample_.bmi088_gyro_y_imu_2, next_sample_.bmi088_gyro_z_imu_2);
}
inline uint32_t getBMP390Pressure1() {
return getSensorData(SensorSampleIndex::BMP390_PRESSURE_1,
current_sample_.bmp390_pressure_1,
next_sample_.bmp390_pressure_1);
}
inline uint32_t getBMP390Temperature1() {
return getSensorData(SensorSampleIndex::BMP390_TEMPERATURE_1,
current_sample_.bmp390_temperature_1,
next_sample_.bmp390_temperature_1);
}
inline uint32_t getBMP390Pressure2() {
return getSensorData(SensorSampleIndex::BMP390_PRESSURE_2,
current_sample_.bmp390_pressure_2,
next_sample_.bmp390_pressure_2);
}
inline uint32_t getBMP390Temperature2() {
return getSensorData(SensorSampleIndex::BMP390_TEMPERATURE_2,
current_sample_.bmp390_temperature_2,
next_sample_.bmp390_temperature_2);
}
inline uint32_t getMS5609Pressure1() {
return getSensorData(SensorSampleIndex::MS5609_PRESSURE_1,
current_sample_.ms5609_pressure_1,
next_sample_.ms5609_pressure_1);
}
inline uint32_t getMS5609Temperature1() {
return getSensorData(SensorSampleIndex::MS5609_TEMPERATURE_1,
current_sample_.ms5609_temperature_1,
next_sample_.ms5609_temperature_1);
}
inline uint32_t getMS5609Pressure2() {
return getSensorData(SensorSampleIndex::MS5609_PRESSURE_2,
current_sample_.ms5609_pressure_2,
next_sample_.ms5609_pressure_2);
}
inline uint32_t getMS5609Temperature2() {
return getSensorData(SensorSampleIndex::MS5609_TEMPERATURE_2,
current_sample_.ms5609_temperature_2,
next_sample_.ms5609_temperature_2);
}
private:
SensorSamples &ring_buffer_;
std::array<std::atomic<bool>, 12> current_flags_;
std::array<std::atomic<bool>, 12> next_flags_;
AtomicSensorSample current_sample_;
AtomicSensorSample next_sample_;
inline void loadNewSample() {
auto sample_opt = ring_buffer_.read();
if (sample_opt) {
next_sample_.loadFrom(sample_opt.value());
}
}
template <typename T>
inline T getSensorData(SensorSampleIndex index, std::atomic<T> &current_value,
std::atomic<T> &next_value) {
size_t idx = static_cast<size_t>(index);
if (current_flags_[idx].load(std::memory_order_acquire)) {
next_flags_[idx].store(true, std::memory_order_release);
return next_value.load(std::memory_order_acquire);
} else {
return current_value.load(std::memory_order_acquire);
}
}
template <typename T>
inline std::tuple<T, T, T>
getSensorData(SensorSampleIndex index, std::atomic<T> &current_x,
std::atomic<T> &current_y, std::atomic<T> &current_z,
std::atomic<T> &next_x, std::atomic<T> &next_y,
std::atomic<T> &next_z) {
size_t idx = static_cast<size_t>(index);
if (current_flags_[idx].load(std::memory_order_acquire)) {
next_flags_[idx].store(true, std::memory_order_release);
return {next_x.load(std::memory_order_acquire),
next_y.load(std::memory_order_acquire),
next_z.load(std::memory_order_acquire)};
} else {
current_flags_[idx].store(true, std::memory_order_release);
return {current_x.load(std::memory_order_acquire),
current_y.load(std::memory_order_acquire),
current_z.load(std::memory_order_acquire)};
}
}
};
} // namespace sensor_sim
#pragma once
#include "analog_input_handler.hpp"
#include "communication_handler.hpp"
#include "digital_input_handler.hpp"
#include "led_handler.hpp"
#include "packet.h"
#include "sensors_handler.hpp"
#include <array>
#include <handler.hpp>
#include <task.hpp>
class AppTask : public Task {
public:
AppTask();
static void StartAppTask(void *argument);
void init() override;
static constexpr auto NAME = "appTask";
static constexpr auto STACK_SIZE = 2048 * 4;
static constexpr auto PRIORITY = osPriorityNormal;
static constexpr TickType_t PERIOD_MS = 10;
static constexpr size_t NUM_HANDLERS = 8;
private:
void run();
ATPBodyKiwiSystem system_;
SensorSamples sensor_samples_;
// Static handler instances
DigitalInputHandler digital_input_handler_;
AnalogInputHandler analog_input_handler_;
LedHandler led_handler_;
SensorsHandler sensors_handler_;
CommunicationHandler communication_handler_;
std::array<Handler *, NUM_HANDLERS> handlers_;
};
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment