add ppm and rcmapper

firmware/lib/PPM/PPMController.cpp

@@ -0,0 +1,36 @@
+#include "pico/stdlib.h"
+
+#include "PPMController.hpp"
+#include "hardware/pio.h"
+#include "ppm.pio.h"
+
+PPMController::PPMController() {
+    // Constructor implementation
+}
+
+PPMController::~PPMController() {
+    // Destructor implementation
+}
+
+void PPMController::initialize(const uint& pin_number) {
+    ppm_program_init(pio1, pin_number);
+}
+
+void PPMController::setPPMValues(const ControlSignals& signals) {
+    // Convert and set PPM values for each channel
+    ppm_set_value(1, convertToPPM(signals.yaw));
+    ppm_set_value(2, convertToPPM(signals.pitch));
+    ppm_set_value(3, convertToPPM(signals.roll));
+    ppm_set_value(4, convertToPPM(signals.throttle));
+
+    // Not setting the additional channels for now
+    // for (int i = 0; i < 4; ++i) {
+    //     ppm_set_value(4 + i, convertToPPM(signals.additional_channels[i]));
+    // }
+}
+
+uint16_t PPMController::convertToPPM(uint16_t signal) {
+    // Assuming the signal is already in the 1000 to 2000 range, we directly return it.
+    // If further scaling or conversion is needed, it can be added here.
+    return signal;
+}

firmware/lib/PPM/PPMController.hpp

@@ -0,0 +1,24 @@
+#pragma once
+
+#ifndef PPMCONTROLLER_H
+#define PPMCONTROLLER_H
+
+#include "../RCMapper/rcmapper.hpp"
+
+class PPMController {
+public:
+    PPMController();
+    ~PPMController();
+
+    // Initialize the PPM controller
+    void initialize(const uint& pin_number);
+
+    // Function to set the PPM values based on control signals
+    void setPPMValues(const ControlSignals& signals);
+
+private:
+    // Function to convert a control signal to PPM value
+    uint16_t convertToPPM(uint16_t signal);
+};
+
+#endif // PPMCONTROLLER_H

firmware/lib/PPM/ppm.pio

@@ -0,0 +1,83 @@
+;
+; Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+;
+; SPDX-License-Identifier: BSD-3-Clause
+;
+
+.program ppm
+.side_set 1
+
+    set pindirs, 1  side 0; Set pin to output
+.wrap_target
+    out x, 16 side 0
+delay0:
+    jmp x-- delay0 side 0
+    out x, 16 side 1
+delay1:
+    jmp x-- delay1 side 1
+    .wrap
+
+% c-sdk {
+#include "hardware/pio.h"
+#include "hardware/clocks.h"
+#include "ppm.pio.h"
+
+
+#define PPM_CHANNELS (8)
+struct {
+    volatile PIO pio;
+    // 16 MSB - strobe length in microsec
+    // 16 LSB - gap length in microsec
+    volatile uint32_t ch_values[PPM_CHANNELS + 1];
+    volatile uint value_idx;
+} ppm_data;
+
+void ppm_handler() {
+    while (!pio_sm_is_tx_fifo_full(ppm_data.pio, 0)) {
+        pio_sm_put(ppm_data.pio, 0, ppm_data.ch_values[ppm_data.value_idx]);
+        ppm_data.value_idx = (ppm_data.value_idx + 1) % (PPM_CHANNELS + 1);
+    }
+}
+
+// 500 microseconds strobe + gap = value (microseconds)
+#define BUILD_CH_VALUE(value_us) (((500 - 1) << 16) + (value_us - 500 - 1))
+
+static inline void ppm_program_init(PIO pio, uint pin) {
+    ppm_data.pio = pio == NULL ? pio0 : pio;
+    ppm_data.ch_values[0] = BUILD_CH_VALUE(5500);
+    // Neutral position (1500us) by default
+    for (int i = 1; i <= PPM_CHANNELS; ++i)
+        ppm_data.ch_values[i] = BUILD_CH_VALUE(1500);
+    ppm_data.value_idx = 0;
+    uint offset = pio_add_program(ppm_data.pio, &ppm_program);
+    pio_gpio_init(ppm_data.pio, pin);
+    pio_sm_set_consecutive_pindirs(pio, 0, pin, 1, true);
+    pio_sm_config pio_conf = ppm_program_get_default_config(offset);
+    sm_config_set_sideset_pins(&pio_conf, pin);
+    sm_config_set_set_pins(&pio_conf, 0, 1);
+    sm_config_set_out_shift(&pio_conf, true, true, 0);
+    sm_config_set_fifo_join(&pio_conf, PIO_FIFO_JOIN_TX);
+    pio_sm_init(ppm_data.pio, 0, offset, &pio_conf);
+    pio_sm_set_clkdiv(ppm_data.pio, 0, (float) clock_get_hz(clk_sys) / 1000000); //1MHz
+    pio_sm_clear_fifos(ppm_data.pio, 0);
+    pio_sm_restart(ppm_data.pio, 0);
+
+    pio_set_irq0_source_enabled(ppm_data.pio, pis_sm0_tx_fifo_not_full, true);
+    irq_set_exclusive_handler(PIO0_IRQ_0, ppm_handler);
+    irq_set_enabled(PIO0_IRQ_0, true);
+    pio_sm_set_enabled(ppm_data.pio, 0, true);
+}
+
+/** Sets channel value
+ *
+ * @channel_number RC channel number [1..PPM_CHANNELS]
+ * @value_usec channel vaue [1000, 2000]
+ */
+static inline void ppm_set_value(uint channel_number, uint value_usec) {
+    valid_params_if(PIO, channel_number <= PPM_CHANNELS);
+    valid_params_if(PIO, channel_number >= 1);
+    valid_params_if(PIO, value_usec <= 2000);
+    valid_params_if(PIO, value_usec >= 1000);
+    ppm_data.ch_values[channel_number] = BUILD_CH_VALUE(value_usec);
+}
+%}

firmware/lib/PPM/ppm.pio.h

@@ -0,0 +1,98 @@
+// -------------------------------------------------- //
+// This file is autogenerated by pioasm; do not edit! //
+// -------------------------------------------------- //
+
+#pragma once
+
+#if !PICO_NO_HARDWARE
+#include "hardware/pio.h"
+#endif
+
+// --- //
+// ppm //
+// --- //
+
+#define ppm_wrap_target 1
+#define ppm_wrap 4
+
+static const uint16_t ppm_program_instructions[] = {
+    0xe081, //  0: set    pindirs, 1      side 0     
+            //     .wrap_target
+    0x6030, //  1: out    x, 16           side 0     
+    0x0042, //  2: jmp    x--, 2          side 0     
+    0x7030, //  3: out    x, 16           side 1     
+    0x1044, //  4: jmp    x--, 4          side 1     
+            //     .wrap
+};
+
+#if !PICO_NO_HARDWARE
+static const struct pio_program ppm_program = {
+    .instructions = ppm_program_instructions,
+    .length = 5,
+    .origin = -1,
+};
+
+static inline pio_sm_config ppm_program_get_default_config(uint offset) {
+    pio_sm_config c = pio_get_default_sm_config();
+    sm_config_set_wrap(&c, offset + ppm_wrap_target, offset + ppm_wrap);
+    sm_config_set_sideset(&c, 1, false, false);
+    return c;
+}
+
+#include "hardware/pio.h"
+#include "hardware/clocks.h"
+#include "ppm.pio.h"
+#define PPM_CHANNELS (8)
+struct {
+    volatile PIO pio;
+    // 16 MSB - strobe length in microsec
+    // 16 LSB - gap length in microsec
+    volatile uint32_t ch_values[PPM_CHANNELS + 1];
+    volatile uint value_idx;
+} ppm_data;
+void ppm_handler() {
+    while (!pio_sm_is_tx_fifo_full(ppm_data.pio, 0)) {
+        pio_sm_put(ppm_data.pio, 0, ppm_data.ch_values[ppm_data.value_idx]);
+        ppm_data.value_idx = (ppm_data.value_idx + 1) % (PPM_CHANNELS + 1);
+    }
+}
+// 500 microseconds strobe + gap = value (microseconds)
+#define BUILD_CH_VALUE(value_us) (((500 - 1) << 16) + (value_us - 500 - 1))
+static inline void ppm_program_init(PIO pio, uint pin) {
+    ppm_data.pio = pio == NULL ? pio0 : pio;
+    ppm_data.ch_values[0] = BUILD_CH_VALUE(5500);
+    // Neutral position (1500us) by default
+    for (int i = 1; i <= PPM_CHANNELS; ++i)
+        ppm_data.ch_values[i] = BUILD_CH_VALUE(1500);
+    ppm_data.value_idx = 0;
+    uint offset = pio_add_program(ppm_data.pio, &ppm_program);
+    pio_gpio_init(ppm_data.pio, pin);
+    pio_sm_set_consecutive_pindirs(pio, 0, pin, 1, true);
+    pio_sm_config pio_conf = ppm_program_get_default_config(offset);
+    sm_config_set_sideset_pins(&pio_conf, pin);
+    sm_config_set_set_pins(&pio_conf, 0, 1);
+    sm_config_set_out_shift(&pio_conf, true, true, 0);
+    sm_config_set_fifo_join(&pio_conf, PIO_FIFO_JOIN_TX);
+    pio_sm_init(ppm_data.pio, 0, offset, &pio_conf);
+    pio_sm_set_clkdiv(ppm_data.pio, 0, (float) clock_get_hz(clk_sys) / 1000000); //1MHz
+    pio_sm_clear_fifos(ppm_data.pio, 0);
+    pio_sm_restart(ppm_data.pio, 0);
+    pio_set_irq0_source_enabled(ppm_data.pio, pis_sm0_tx_fifo_not_full, true);
+    irq_set_exclusive_handler(PIO1_IRQ_0, ppm_handler);
+    irq_set_enabled(PIO1_IRQ_0, true);
+    pio_sm_set_enabled(ppm_data.pio, 0, true);
+}
+/** Sets channel value
+ *
+ * @channel_number RC channel number [1..PPM_CHANNELS]
+ * @value_usec channel vaue [1000, 2000]
+ */
+static inline void ppm_set_value(uint channel_number, uint value_usec) {
+    valid_params_if(PIO, channel_number <= PPM_CHANNELS);
+    valid_params_if(PIO, channel_number >= 1);
+    valid_params_if(PIO, value_usec <= 2000);
+    valid_params_if(PIO, value_usec >= 1000);
+    ppm_data.ch_values[channel_number] = BUILD_CH_VALUE(value_usec);
+}
+
+#endif

firmware/lib/RCMapper/rcmapper.cpp

@@ -0,0 +1,43 @@
+#include "rcmapper.hpp"
+#include <Arduino.h>
+#include <cmath> // For mathematical operations
+#include <algorithm> // For std::clamp
+
+RCMapper::RCMapper() {
+    // Constructor implementation
+}
+
+RCMapper::~RCMapper() {
+    // Destructor implementation
+}
+
+ControlSignals RCMapper::mapValues(inputFrameControlSignals input_frame) {
+    ControlSignals signals{};
+
+    // Clamping and mapping control angles (yaw, pitch, roll)
+    signals.yaw = mapSingleAngle(input_frame.control_angles(2));   // Yaw
+    signals.pitch = mapSingleAngle(input_frame.control_angles(1)); // Pitch
+    signals.roll = mapSingleAngle(input_frame.control_angles(0));  // Roll
+
+    // Mapping throttle
+    signals.throttle = std::clamp(input_frame.throttle, static_cast<uint16_t>(0), static_cast<uint16_t>(65535));
+
+    // // Mapping additional channels
+    // for (size_t i = 0; i < 4; ++i) {
+    //     signals.additional_channels[i] = std::clamp(input_frame.additional_channels[i], static_cast<uint16_t>(0), static_cast<uint16_t>(65535));
+    // }
+
+    return signals;
+}
+
+uint16_t RCMapper::mapSingleAngle(float angle) {
+    float angle_degrees = degrees(angle);
+    // Clamp the angle between MIN_ANGLE and MAX_ANGLE
+    float clamped_angle = std::clamp(angle_degrees, MIN_ANGLE, MAX_ANGLE);
+
+    // Map the clamped angle to the 1000 to 2000 range
+    float normalized_angle = (clamped_angle - MIN_ANGLE) / (MAX_ANGLE - MIN_ANGLE);
+    uint16_t mapped_signal = static_cast<uint16_t>(MIN_SIGNAL + normalized_angle * (MAX_SIGNAL - MIN_SIGNAL));
+
+    return mapped_signal;
+}

firmware/lib/RCMapper/rcmapper.hpp

@@ -0,0 +1,40 @@
+#pragma once
+
+#ifndef RCMAPPER_H
+#define RCMAPPER_H
+
+#include "../matrix/matrix/math.hpp"
+
+#define MIN_ANGLE -30.0f
+#define MAX_ANGLE 30.0f
+#define MIN_SIGNAL 1000
+#define MAX_SIGNAL 2000
+
+struct ControlSignals {
+    uint16_t yaw;
+    uint16_t pitch;
+    uint16_t roll;
+    uint16_t throttle;
+    uint16_t additional_channels[4];
+};
+
+struct inputFrameControlSignals {
+    matrix::Eulerf control_angles;
+    uint16_t throttle;
+    uint16_t additional_channels[4];
+};
+
+class RCMapper {
+public:
+    RCMapper();
+    ~RCMapper();
+
+    // Function to map input angles and values to output values
+    ControlSignals mapValues(inputFrameControlSignals input_frame);
+
+private:
+    // Map a single angle to the 1000 to 2000 range
+    uint16_t mapSingleAngle(float angle);
+};
+
+#endif // RCMAPPER_HPP