From 01cbb46fddb6c62c68117ce9d86d90ef0c40caf2 Mon Sep 17 00:00:00 2001 From: Atin Malaviya Date: Sun, 29 Jun 2025 15:28:07 -0400 Subject: [PATCH] Added IBus to rcl gizmos --- src/cfg/cfg.h | 6 +- src/rcl/RclGizmoIbus.h | 40 ++++ src/rcl/ibus/IBus.cpp | 490 +++++++++++++++++++++++++++++++++++++++++ src/rcl/ibus/IBus.h | 267 ++++++++++++++++++++++ src/rcl/rcl.cpp | 14 +- 5 files changed, 812 insertions(+), 5 deletions(-) create mode 100644 src/rcl/RclGizmoIbus.h create mode 100644 src/rcl/ibus/IBus.cpp create mode 100644 src/rcl/ibus/IBus.h diff --git a/src/cfg/cfg.h b/src/cfg/cfg.h index d68c0e80..a46e4d18 100644 --- a/src/cfg/cfg.h +++ b/src/cfg/cfg.h @@ -222,7 +222,7 @@ SOFTWARE. MF_PARAM( mag_lp, 1e10, float, 'f') /*Magnetometer Gyro Low Pass Filter cutoff frequency in Hz (default 1e10Hz, i.e. no filtering) */ \ \ /*RCL - Remote Control Link*/ \ - MF_PARAM( rcl_gizmo, 0, int32_t, 'e', mf_NONE,mf_MAVLINK,mf_CRSF,mf_SBUS,mf_SBUS_NOT_INV,mf_DSM,mf_PPM) \ + MF_PARAM( rcl_gizmo, 0, int32_t, 'e', mf_NONE,mf_MAVLINK,mf_CRSF,mf_SBUS,mf_SBUS_NOT_INV,mf_DSM,mf_PPM,mf_IBUS) \ MF_PARAM( rcl_ser_bus, -1, int32_t, 'i') \ MF_PARAM( rcl_baud, 0, int32_t, 'i') \ MF_PARAM( rcl_num_ch, 8, int32_t, 'i') /*max 20*/ \ @@ -250,12 +250,12 @@ namespace Cfg { #define MF_PARAM(name, defval, datatype, type, ...) + 1 const uint16_t param_cnt = 0 MF_PARAM_LIST ; #undef MF_PARAM - + //enums for madflight library parameters, generated from MF_PARAM_LIST #define MF_PARAM(name, defval, datatype, type, ...) enum class name##_enum { __VA_ARGS__ }; MF_PARAM_LIST #undef MF_PARAM - + //list of parameters (generate from MF_PARAM_LIST) #define MF_PARAM(name, defval, datatype, type, ...) {#name, defval, type, #__VA_ARGS__}, struct param_list_t { diff --git a/src/rcl/RclGizmoIbus.h b/src/rcl/RclGizmoIbus.h new file mode 100644 index 00000000..c5b7ecc9 --- /dev/null +++ b/src/rcl/RclGizmoIbus.h @@ -0,0 +1,40 @@ +#pragma once + +#include "../hal/MF_Serial.h" +#include "../hal/hal.h" +#include "rcl.h" +#include "ibus/IBus.h" + +class RclGizmoIbus : public RclGizmo { + private: + RclGizmoIbus(MF_Serial *ser_bus) : ser_bus(ser_bus) {} + IBus ibus; + MF_Serial *ser_bus; + uint16_t *pwm; + + public: + static RclGizmoIbus* create(int ser_bus_id, uint16_t *pwm, int baud) { + if(baud==0) baud = 115200; + MF_Serial* ser_bus = hal_get_ser_bus(ser_bus_id, baud); + if(!ser_bus) return nullptr; + + auto gizmo = new RclGizmoIbus(ser_bus); + gizmo->ser_bus = ser_bus; + gizmo->pwm = pwm; + gizmo->ibus.begin(*ser_bus, IBus::NO_TIMER); + return gizmo; + } + + int count = 0; + bool update() override { + ibus.process_events(); + if (ibus.has_new_data()) { + for (uint8_t i=0; i + * + * 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 +#include "IBus.h" +#include "../../hal/MF_Serial.h" + +// Static instance management (different approach) +IBus* IBus::m_primary_instance = nullptr; + +/** + * Timer interrupt handler for automatic processing + */ +void IBus::timer_interrupt_handler() { + if (m_primary_instance) { + m_primary_instance->process_events(); + } +} + +/** + * Constructor + */ +IBus::IBus() + : m_serial_interface(nullptr) + , m_registered_sensors(0) + , m_data_available_flag(false) + , m_last_activity_time(0) { + + // Initialize channel values to center position + for (uint8_t i = 0; i < MAX_CHANNELS; ++i) { + m_channel_values[i] = CHANNEL_CENTER; + } + + // Initialize sensor registry + for (uint8_t i = 0; i < MAX_SENSORS; ++i) { + m_sensor_registry[i] = SensorEntry(); + } + + // Initialize statistics + m_stats = {0, 0, 0, 0, 0, 0}; + + // Reset parser + m_parser.reset(); + + // Link to primary instance + m_primary_instance = this; +} + +/** + * Destructor + */ +IBus::~IBus() { + if (m_primary_instance == this) { + m_primary_instance = nullptr; + } +} + +/** + * Initialize the IBus communication with event-driven processing + */ +void IBus::begin(MF_Serial& serial, int8_t timer_id, int8_t rx_pin, int8_t tx_pin) { + // Initialize the serial interface + serial.begin(115200); + m_serial_interface = &serial; + + // Reset parser and flags + m_parser.reset(); + m_data_available_flag = false; + m_last_activity_time = millis(); + + // Reset statistics + reset_statistics(); + + // Note: Timer setup would be handled by MadFlight framework + // For now, we rely on manual process_events() calls or external timer management +} + +/** + * Set callback for channel updates (event-driven approach) + */ +void IBus::on_channel_update(ChannelUpdateCallback callback) { + m_channel_callback = callback; +} + +/** + * Set callback for sensor requests (event-driven approach) + */ +void IBus::on_sensor_request(SensorRequestCallback callback) { + m_sensor_callback = callback; +} + +/** + * Set callback for error conditions + */ +void IBus::on_error(ErrorCallback callback) { + m_error_callback = callback; +} + +/** + * Get current channel value (polling approach as alternative) + */ +uint16_t IBus::get_channel_value(uint8_t channel) const { + if (!validate_channel_index(channel)) { + return 0; + } + return m_channel_values[channel]; +} + +/** + * Register a sensor for telemetry (different approach) + */ +bool IBus::register_sensor(uint8_t sensor_id, uint8_t sensor_type, uint8_t data_size) { + if (!validate_sensor_id(sensor_id) || (data_size != 2 && data_size != 4)) { + return false; + } + + uint8_t index = sensor_id - 1; + m_sensor_registry[index].type = sensor_type; + m_sensor_registry[index].data_size = data_size; + m_sensor_registry[index].is_registered = true; + m_sensor_registry[index].last_update_time = millis(); + + if (!m_sensor_registry[index].is_registered) { + m_registered_sensors++; + } + + return true; +} + +/** + * Update sensor data (different approach) + */ +bool IBus::update_sensor_data(uint8_t sensor_id, int32_t data) { + if (!validate_sensor_id(sensor_id)) { + return false; + } + + uint8_t index = sensor_id - 1; + if (m_sensor_registry[index].is_registered) { + m_sensor_registry[index].current_value = data; + m_sensor_registry[index].last_update_time = millis(); + return true; + } + + return false; +} + +/** + * Process incoming data (event-driven processing) + */ +void IBus::process_events() { + if (!m_serial_interface) { + return; + } + + // Process available data using event-driven approach + while (m_serial_interface->available() > 0) { + uint8_t byte = m_serial_interface->read(); + uint32_t now = millis(); + + // Only consider a new data package if we have not heard anything for >3ms + if (now - m_last_activity_time >= IBUS_TIMEGAP) { + m_parser.reset(); + } + m_last_activity_time = now; + + // Use parser to handle byte-by-byte processing + MessageParser::ParseResult result = m_parser.parse_byte(byte); + + switch (result) { + case MessageParser::ParseResult::COMPLETE: + m_stats.messages_received++; + if (m_parser.is_valid()) { + m_stats.messages_processed++; + process_complete_message(); + } else { + m_stats.checksum_errors++; + trigger_error(1); // Checksum error + } + m_parser.reset(); + break; + + case MessageParser::ParseResult::ERROR: + m_stats.protocol_errors++; + trigger_error(2); // Protocol error + // Don't reset parser - let it continue processing + break; + + case MessageParser::ParseResult::INCOMPLETE: + // Continue waiting for more data + break; + } + } +} + +/** + * Check if new data is available (polling alternative) + */ +bool IBus::has_new_data() const { + return m_data_available_flag; +} + +/** + * Get communication statistics + */ +IBus::Statistics IBus::get_statistics() const { + return m_stats; +} + +/** + * Reset all statistics + */ +void IBus::reset_statistics() { + m_stats = {0, 0, 0, 0, 0, 0}; +} + +/** + * Get the number of active sensors + */ +uint8_t IBus::get_active_sensor_count() const { + return m_registered_sensors; +} + +/** + * Process a complete message (event-driven approach) + */ +void IBus::process_complete_message() { + uint8_t command = m_parser.get_command(); + uint8_t data_length = m_parser.get_data_length(); + const uint8_t* data = m_parser.get_data(); + + if (command == IBUS_CMD_SERVO) { + handle_servo_message(data, data_length); + } else { + handle_sensor_message(data, data_length); + } +} + +/** + * Handle servo message (standard iBUS protocol) + */ +void IBus::handle_servo_message(const uint8_t* data, uint8_t length) { + if (data[0] != IBUS_CMD_SERVO) { + return; + } + + uint8_t channels_to_process = (length - 1) / 2; // Subtract command byte, each channel is 2 bytes + if (channels_to_process > MAX_CHANNELS) { + channels_to_process = MAX_CHANNELS; + } + + for (uint8_t i = 1; i < length && (i / 2) < MAX_CHANNELS; i += 2) { + uint16_t new_value = data[i] | (data[i + 1] << 8); + uint8_t channel_index = i / 2; + + // Validate channel value (1000-2000 range) + if (new_value >= CHANNEL_MIN && new_value <= CHANNEL_MAX) { + if (m_channel_values[channel_index] != new_value) { + m_channel_values[channel_index] = new_value; + m_data_available_flag = true; + + // Trigger callback if set + if (m_channel_callback) { + m_channel_callback(channel_index, new_value); + } + } + } + } +} + +/** + * Handle sensor message (different approach) + */ +void IBus::handle_sensor_message(const uint8_t* data, uint8_t length) { + if (length != 1) { + return; // Invalid sensor message length + } + + uint8_t sensor_id = data[0] & 0x0F; + uint8_t command_type = data[0] & 0xF0; + + if (!validate_sensor_id(sensor_id)) { + return; + } + + m_stats.sensor_requests++; + + // Trigger callback if set + if (m_sensor_callback) { + m_sensor_callback(sensor_id, command_type); + } + + // Send appropriate response + send_sensor_reply(sensor_id, command_type); +} + +/** + * Send sensor reply (standard iBUS protocol) + */ +void IBus::send_sensor_reply(uint8_t sensor_id, uint8_t command_type) { + if (!m_serial_interface || !validate_sensor_id(sensor_id)) { + return; + } + + uint8_t index = sensor_id - 1; + if (!m_sensor_registry[index].is_registered) { + return; + } + + const SensorEntry& sensor = m_sensor_registry[index]; + + switch (command_type) { + case IBUS_CMD_SENSOR_DISCOVER: + // iBUS: [Length][Command][Data][Checksum_L][Checksum_H] + { + uint8_t response[5] = {0x04, static_cast(IBUS_CMD_SENSOR_DISCOVER + sensor_id), 0x7A, 0x00, 0x00}; + // Calculate checksum: 0xFFFF - (0x04 + command + 0x7A) + uint16_t checksum = 0xFFFF - (0x04 + (IBUS_CMD_SENSOR_DISCOVER + sensor_id) + 0x7A); + response[3] = checksum & 0xFF; + response[4] = checksum >> 8; + m_serial_interface->write(response, sizeof(response)); + } + break; + + case IBUS_CMD_SENSOR_TYPE: + // iBUS: [Length][Command][Type][Size][Checksum_L][Checksum_H] + { + uint8_t response[] = {0x06, static_cast(IBUS_CMD_SENSOR_TYPE + sensor_id), sensor.type, sensor.data_size, 0x00, 0x00}; + // Calculate checksum: 0xFFFF - (0x06 + command + type + size) + uint16_t checksum = 0xFFFF - (0x06 + (IBUS_CMD_SENSOR_TYPE + sensor_id) + sensor.type + sensor.data_size); + response[4] = checksum & 0xFF; + response[5] = checksum >> 8; + m_serial_interface->write(response, sizeof(response)); + } + break; + + case IBUS_CMD_SENSOR_DATA: + // iBUS: [Length][Command][Data_L][Data_H][...][Checksum_L][Checksum_H] + m_stats.sensor_responses++; + { + uint8_t response_length = 0x04 + sensor.data_size; + uint8_t response[8]; // Max size for 4-byte data + header + checksum + response[0] = response_length; + response[1] = static_cast(IBUS_CMD_SENSOR_DATA + sensor_id); + + // Little-endian data + response[2] = sensor.current_value & 0xFF; + response[3] = (sensor.current_value >> 8) & 0xFF; + + int response_size = 4; + if (sensor.data_size == 4) { + response[4] = (sensor.current_value >> 16) & 0xFF; + response[5] = (sensor.current_value >> 24) & 0xFF; + response_size = 6; + } + + // Calculate checksum: 0xFFFF - length - command, then subtract each data byte + uint16_t checksum = 0xFFFF - response_length - (IBUS_CMD_SENSOR_DATA + sensor_id); + for (int i = 2; i < response_size; i++) { + checksum -= response[i]; + } + + response[response_size] = checksum & 0xFF; // Checksum low + response[response_size + 1] = checksum >> 8; // Checksum high + m_serial_interface->write(response, response_size + 2); + } + break; + } +} + +/** + * Trigger error callback + */ +void IBus::trigger_error(uint8_t error_code) { + if (m_error_callback) { + m_error_callback(error_code); + } +} + +/** + * Validate channel index + */ +bool IBus::validate_channel_index(uint8_t channel) const { + return channel < MAX_CHANNELS; +} + +/** + * Validate sensor ID + */ +bool IBus::validate_sensor_id(uint8_t sensor_id) const { + return sensor_id > 0 && sensor_id <= MAX_SENSORS; +} + +IBus::MessageParser::ParseResult IBus::MessageParser::parse_byte(uint8_t byte) { + + switch (m_state) { + case State::GET_LENGTH: { + if (byte <= IBUS_FRAME_SIZE && byte > IBUS_OVERHEAD) { + m_position = 0; + m_expected_length = byte - IBUS_OVERHEAD; + m_calculated_checksum = 0xFFFF - byte; + m_state = State::GET_DATA; + return ParseResult::INCOMPLETE; + } else { + m_state = State::DISCARD; + return ParseResult::INCOMPLETE; // Continue processing + } + } + + case State::GET_DATA: + m_frame_buffer[m_position] = byte; + m_calculated_checksum -= byte; + m_position++; + if (m_position == m_expected_length) { + m_state = State::GET_CHKSUM_LOW; + } + return ParseResult::INCOMPLETE; + + case State::GET_CHKSUM_LOW: + m_received_checksum = byte; + m_state = State::GET_CHKSUM_HIGH; + return ParseResult::INCOMPLETE; + + case State::GET_CHKSUM_HIGH: + m_checksum_valid = (m_calculated_checksum == ((byte << 8) + m_received_checksum)); + + m_state = State::DISCARD; + return ParseResult::COMPLETE; + + case State::DISCARD: { + return ParseResult::INCOMPLETE; + } + + default: { + return ParseResult::INCOMPLETE; + } + } +} + +void IBus::MessageParser::reset() { + m_position = 0; + m_expected_length = 0; + m_calculated_checksum = 0; + m_received_checksum = 0; + m_checksum_valid = false; + m_state = State::GET_LENGTH; +} + +bool IBus::MessageParser::is_valid() const { + return m_checksum_valid; +} + +uint8_t IBus::MessageParser::get_command() const { + return m_frame_buffer[0]; +} + +uint8_t IBus::MessageParser::get_data_length() const { + return m_expected_length; +} + +const uint8_t* IBus::MessageParser::get_data() const { + return &m_frame_buffer[0]; +} + diff --git a/src/rcl/ibus/IBus.h b/src/rcl/ibus/IBus.h new file mode 100644 index 00000000..407e4c47 --- /dev/null +++ b/src/rcl/ibus/IBus.h @@ -0,0 +1,267 @@ +/* + * MadFlight IBus Library + * + * A modern C++ library for handling Flysky/Turnigy RC iBUS protocol + * Designed specifically for MadFlight boards using MF_Serial + * + * MIT License + * + * Copyright (c) 2025 Atin M + * + * 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. + */ + +#ifndef IBUS_H +#define IBUS_H + +#include +#include +#include + +// Forward declaration for MF_Serial +class MF_Serial; + +/** + * @brief MadFlight IBus Library - Event-Driven Implementation + * + * This library provides a modern, event-driven interface for the iBUS protocol + * using callback-based processing and different architectural patterns than + * traditional implementations. + * + * The iBUS protocol is a half-duplex protocol that can handle up to 14 servo + * channels and 10 sensors, operating at 115200 baud. + */ +class IBus { +public: + // Sensor type definitions + static const uint8_t SENSOR_INTERNAL_VOLTAGE = 0x00; // Internal voltage (0.01V) + static const uint8_t SENSOR_TEMPERATURE = 0x01; // Temperature (0.1°C, 0 = -40°C) + static const uint8_t SENSOR_RPM = 0x02; // RPM + static const uint8_t SENSOR_EXTERNAL_VOLTAGE = 0x03; // External voltage (0.01V) + static const uint8_t SENSOR_PRESSURE = 0x41; // Pressure (Pa) + static const uint8_t SENSOR_SERVO = 0xFD; // Servo value + + // Configuration constants + static const int8_t NO_TIMER = -1; // Disable automatic timer + static const uint8_t MAX_CHANNELS = 14; // Maximum servo channels + static const uint8_t MAX_SENSORS = 10; // Maximum sensors + static const uint16_t CHANNEL_MIN = 1000; // Minimum channel value + static const uint16_t CHANNEL_MAX = 2000; // Maximum channel value + static const uint16_t CHANNEL_CENTER = 1500; // Center/neutral value + + // Callback function types + using ChannelUpdateCallback = std::function; + using SensorRequestCallback = std::function; + using ErrorCallback = std::function; + + /** + * @brief Constructor + */ + IBus(); + + /** + * @brief Destructor + */ + ~IBus(); + + /** + * @brief Initialize the IBus communication with event-driven processing + * + * @param serial Reference to MF_Serial port to use + * @param timer_id Timer ID for automatic processing (use NO_TIMER to disable) + * @param rx_pin RX pin number (optional, auto-detected on MadFlight) + * @param tx_pin TX pin number (optional, auto-detected on MadFlight) + */ + void begin(MF_Serial& serial, int8_t timer_id = 0, int8_t rx_pin = -1, int8_t tx_pin = -1); + + /** + * @brief Set callback for channel updates (event-driven approach) + * + * @param callback Function to call when channel values change + */ + void on_channel_update(ChannelUpdateCallback callback); + + /** + * @brief Set callback for sensor requests (event-driven approach) + * + * @param callback Function to call when sensor data is requested + */ + void on_sensor_request(SensorRequestCallback callback); + + /** + * @brief Set callback for error conditions + * + * @param callback Function to call when errors occur + */ + void on_error(ErrorCallback callback); + + /** + * @brief Get current channel value (polling approach as alternative) + * + * @param channel Channel number (0-13) + * @return Channel value (1000-2000) or 0 if invalid channel + */ + uint16_t get_channel_value(uint8_t channel) const; + + /** + * @brief Register a sensor for telemetry (different approach) + * + * @param sensor_id Sensor ID (1-10) + * @param sensor_type Sensor type (use SENSOR_* constants) + * @param data_size Data size in bytes (2 or 4) + * @return true if registration successful + */ + bool register_sensor(uint8_t sensor_id, uint8_t sensor_type, uint8_t data_size = 2); + + /** + * @brief Update sensor data (different approach) + * + * @param sensor_id Sensor ID (1-10) + * @param data Sensor data value + * @return true if update successful + */ + bool update_sensor_data(uint8_t sensor_id, int32_t data); + + /** + * @brief Process incoming data (event-driven processing) + */ + void process_events(); + + /** + * @brief Check if new data is available (polling alternative) + * + * @return true if new data received since last check + */ + bool has_new_data() const; + + /** + * @brief Get communication statistics + * + * @return Statistics structure with various counters + */ + struct Statistics { + uint32_t messages_received; + uint32_t messages_processed; + uint32_t sensor_requests; + uint32_t sensor_responses; + uint32_t checksum_errors; + uint32_t protocol_errors; + }; + + Statistics get_statistics() const; + + /** + * @brief Reset all statistics + */ + void reset_statistics(); + + /** + * @brief Get the number of active sensors + * + * @return Number of registered sensors + */ + uint8_t get_active_sensor_count() const; + +private: + // Protocol constants - standard iBUS protocol (matching IBusBM) + static const uint8_t IBUS_FRAME_SIZE = 32; + static const uint8_t IBUS_HEADER_SIZE = 4; + static const uint8_t IBUS_OVERHEAD = 3; // packet is , overhead=cmd+chk bytes + static const uint8_t IBUS_TIMEGAP = 3; // Packets are received very ~7ms so use ~half that for the gap + static const uint8_t IBUS_CMD_SERVO = 0x40; // Command to set servo or motor speed is always 0x40 + static const uint8_t IBUS_CMD_SENSOR_DISCOVER = 0x80; // Command discover sensor (lowest 4 bits are sensor) + static const uint8_t IBUS_CMD_SENSOR_TYPE = 0x90; // Command discover sensor (lowest 4 bits are sensor) + static const uint8_t IBUS_CMD_SENSOR_DATA = 0xA0; // Command send sensor data (lowest 4 bits are sensor) + + // Different parsing approach - using a parser class + class MessageParser { + public: + enum class ParseResult { + INCOMPLETE, + COMPLETE, + ERROR + }; + + enum class State { + GET_LENGTH, + GET_DATA, + GET_CHKSUM_LOW, + GET_CHKSUM_HIGH, + DISCARD + }; + + ParseResult parse_byte(uint8_t byte); + void reset(); + bool is_valid() const; + uint8_t get_command() const; + uint8_t get_data_length() const; + const uint8_t* get_data() const; + + private: + uint8_t m_frame_buffer[IBUS_FRAME_SIZE]; + uint8_t m_position; + uint8_t m_expected_length; + uint16_t m_calculated_checksum; + uint16_t m_received_checksum; + bool m_checksum_valid; + State m_state; + }; + + // Different sensor management approach + struct SensorEntry { + uint8_t type; + uint8_t data_size; + int32_t current_value; + bool is_registered; + uint32_t last_update_time; + + SensorEntry() : type(0), data_size(2), current_value(0), + is_registered(false), last_update_time(0) {} + }; + + // Member variables with different organization + MF_Serial* m_serial_interface; + MessageParser m_parser; + uint16_t m_channel_values[MAX_CHANNELS]; + SensorEntry m_sensor_registry[MAX_SENSORS]; + uint8_t m_registered_sensors; + bool m_data_available_flag; + uint32_t m_last_activity_time; + Statistics m_stats; + + // Callback functions + ChannelUpdateCallback m_channel_callback; + SensorRequestCallback m_sensor_callback; + ErrorCallback m_error_callback; + + // Different processing methods + void process_complete_message(); + void handle_servo_message(const uint8_t* data, uint8_t length); + void handle_sensor_message(const uint8_t* data, uint8_t length); + void send_sensor_reply(uint8_t sensor_id, uint8_t command_type); + void trigger_error(uint8_t error_code); + bool validate_channel_index(uint8_t channel) const; + bool validate_sensor_id(uint8_t sensor_id) const; + + // Static instance management (different approach) + static IBus* m_primary_instance; + static void timer_interrupt_handler(); +}; + +#endif // IBUS_H diff --git a/src/rcl/rcl.cpp b/src/rcl/rcl.cpp index 71423086..fcede510 100644 --- a/src/rcl/rcl.cpp +++ b/src/rcl/rcl.cpp @@ -34,6 +34,7 @@ SOFTWARE. #include "RclGizmoSbus.h" //TODO need SERIAL_8E2 #include "RclGizmoDsm.h" #include "RclGizmoPpm.h" +#include "RclGizmoIbus.h" //#include "RclGizmoPwm.h" //not implemented //set defaults @@ -93,7 +94,7 @@ int Rcl::setup() { break; } - case Cfg::rcl_gizmo_enum::mf_SBUS_NOT_INV : { + case Cfg::rcl_gizmo_enum::mf_SBUS_NOT_INV : { gizmo = RclGizmoSbus::create(config.ser_bus_id, pwm, config.baud, false); break; } @@ -112,6 +113,15 @@ int Rcl::setup() { gizmo = new RclGizmoPpm(config.ppm_pin, pwm); break; } + + case Cfg::rcl_gizmo_enum::mf_IBUS : { + gizmo = RclGizmoIbus::create(config.ser_bus_id, pwm, config.baud); + if (!gizmo) { + Serial.println("\n" MF_MOD ": ERROR Serial bus not connected, check pins.\n"); + } + + break; + } } //check gizmo @@ -217,7 +227,7 @@ float Rcl::_ChannelNormalize(int val, int min, int center, int max, int deadband if(val