From ec5ce26b0b564284caff0b6281c916f8d1204e8f Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Thu, 4 Dec 2025 16:31:02 +0100 Subject: [PATCH 01/14] first implement of EKF --- data/config/config_zumo1.json | 2 +- lib/APPLineFollowerSensorFusion/src/App.cpp | 327 ++++++++++++++- lib/APPLineFollowerSensorFusion/src/App.h | 159 +++++-- .../src/SerialMuxChannels.h | 20 +- lib/Service/src/EKF.cpp | 389 ++++++++++++++++++ lib/Service/src/EKF.h | 295 +++++++++++++ platformio.ini | 3 + 7 files changed, 1128 insertions(+), 67 deletions(-) create mode 100644 lib/Service/src/EKF.cpp create mode 100644 lib/Service/src/EKF.h diff --git a/data/config/config_zumo1.json b/data/config/config_zumo1.json index 161a5bea..1f6e73b5 100644 --- a/data/config/config_zumo1.json +++ b/data/config/config_zumo1.json @@ -1,5 +1,5 @@ { - "robotName": "1", + "robotName": "0", "wifi": { "ssid": "", "pswd": "" diff --git a/lib/APPLineFollowerSensorFusion/src/App.cpp b/lib/APPLineFollowerSensorFusion/src/App.cpp index dfc31097..fdbbcd56 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.cpp +++ b/lib/APPLineFollowerSensorFusion/src/App.cpp @@ -69,10 +69,11 @@ /****************************************************************************** * Local Variables *****************************************************************************/ + /** Serial interface baudrate. */ static const uint32_t SERIAL_BAUDRATE = 115200U; -/** Serial log sink */ +/** Serial log sink. */ static LogSinkPrinter gLogSinkSerial("Serial", &Serial); /* MQTT topic name for birth messages. */ @@ -84,21 +85,31 @@ const char* App::TOPIC_NAME_WILL = "will"; /** MQTT topic name for status messages. */ const char* App::TOPIC_NAME_STATUS = "zumo/status"; -/** MQTT topic name for fusion Pose */ +/** MQTT topic name for fusion pose. */ const char* App::TOPIC_NAME_FUSION_POSE = "zumo/fusion"; -/** MQTT topic name for raw Sensor data */ +/** MQTT topic name for raw sensor data. */ const char* App::TOPIC_NAME_RAW_SENSORS = "zumo/sensors"; -/** MQTT topic name for receiving Space Ship Radar Pose. */ +/** MQTT topic name for receiving Space Ship Radar pose. */ const char* App::TOPIC_NAME_RADAR_POSE = "ssr"; -/** Buffer size for JSON serialization of birth / will message */ +/** Buffer size for JSON serialization of birth / will message. */ static const uint32_t JSON_BIRTHMESSAGE_MAX_SIZE = 64U; -/** Buffer size for JSON serialization of combined sensor snapshot */ +/** Buffer size for JSON serialization of combined sensor snapshot. */ static const uint32_t JSON_SENSOR_SNAPSHOT_MAX_SIZE = 256U; +/** Buffer size for JSON serialization of fusion pose. */ +static const uint32_t JSON_FUSION_POSE_MAX_SIZE = 128U; + +/* Convenience aliases for EKF types. */ +using ImuMeasVector = ExtendedKalmanFilter5D::ImuMeasVector; +using OdoMeasVector = ExtendedKalmanFilter5D::OdoMeasVector; +using CamMeasVector = ExtendedKalmanFilter5D::CamMeasVector; +using StateVector = ExtendedKalmanFilter5D::StateVector; +using StateMatrix = ExtendedKalmanFilter5D::StateMatrix; + /****************************************************************************** * Public Methods *****************************************************************************/ @@ -111,7 +122,17 @@ App::App() : m_lineSensors(m_serMuxChannelProvider), m_motors(m_serMuxChannelProvider), m_stateMachine(), - m_mqttClient() + m_mqttClient(), + m_ekf(), + m_lastEkfUpdateMs(0U), + m_lastVehicleData{}, + m_hasVehicleData(false), + m_lastSsrPose{}, + m_hasSsrPose(false), + m_odoOriginInitialized(false), + m_odoOriginX_mm(0.0F), + m_odoOriginY_mm(0.0F), + m_ekfInitializedFromSSR(false) { /* Inject dependencies into states. */ StartupState::getInstance().injectDependencies(m_serMuxChannelProvider, m_motors); @@ -152,7 +173,7 @@ void App::setup() NetworkSettings networkSettings = {settings.getWiFiSSID(), settings.getWiFiPassword(), settings.getRobotName(), ""}; - /* If the robot name is empty, use the wifi MAC address as robot name. */ + /* If the robot name is empty, use the WiFi MAC address as robot name. */ if (true == settings.getRobotName().isEmpty()) { String robotName = WiFi.macAddress(); @@ -175,13 +196,29 @@ void App::setup() { LOG_FATAL("MQTT connection could not be setup."); } + else if (false == m_ekf.init()) + { + LOG_FATAL("Extended Kalman Filter could not be initialized."); + } else { /* Log incoming vehicle data and corresponding time sync information. */ - m_serMuxChannelProvider.registerVehicleDataCallback([this](const VehicleData& data) - { publishVehicleAndSensorSnapshot(data); }); - - /* Start network time (NTP) against Host and Zumo serial ping-pong. */ + m_serMuxChannelProvider.registerVehicleDataCallback( + [this](const VehicleData& data) + { + publishVehicleAndSensorSnapshot(data); + + m_lastVehicleData = data; + m_hasVehicleData = true; + + /* Run sensor fusion whenever new vehicle data arrives. + * SSR data may or may not be available yet; fusion will + * only start after SSR-based EKF initialization. + */ + filterLocationData(data, m_lastSsrPose); + }); + + /* Start network time (NTP) against host and Zumo serial ping-pong. */ m_timeSync.begin(); m_statusTimer.start(1000U); isSuccessful = true; @@ -207,7 +244,7 @@ void App::loop() /* Process battery, device and network. */ Board::getInstance().process(); - /* Process MQTT Communication */ + /* Process MQTT communication. */ m_mqttClient.process(); /* Process serial multiplexer. */ @@ -216,7 +253,7 @@ void App::loop() /* Process time synchronization (NTP refresh + serial ping-pong). */ m_timeSync.process(); - /* Process statemachine. */ + /* Process state machine. */ m_stateMachine.process(); /* Send heartbeat status to Radon Ulzer controller periodically. */ @@ -244,6 +281,7 @@ void App::loop() /****************************************************************************** * Private Methods *****************************************************************************/ + bool App::setupMqtt(const String& clientId, const String& brokerAddr, uint16_t brokerPort) { bool isSuccessful = false; @@ -269,11 +307,11 @@ bool App::setupMqtt(const String& clientId, const String& brokerAddr, uint16_t b { LOG_FATAL("MQTT configuration could not be set."); } - /* Subscribe to Space Ship Radar Topic. */ + /* Subscribe to Space Ship Radar topic. */ else if (false == m_mqttClient.subscribe(ssrTopic, false, [this](const String& payload) { ssrTopicCallback(payload); })) { - LOG_FATAL("Could not subcribe to MQTT topic: %s.", TOPIC_NAME_RADAR_POSE); + LOG_FATAL("Could not subscribe to MQTT topic: %s.", TOPIC_NAME_RADAR_POSE); } else { @@ -293,7 +331,7 @@ void App::ssrTopicCallback(const String& payload) if (error != DeserializationError::Ok) { - LOG_ERROR("JSON Deserialization error %d.", error); + LOG_ERROR("JSON deserialization error %d.", error); } else { @@ -304,8 +342,77 @@ void App::ssrTopicCallback(const String& payload) JsonVariantConst angle_mrad = jsonPayload["angle"]; /* int : in mrad */ JsonVariantConst id = jsonPayload["identifier"]; /* int : unique id of the target */ - LOG_INFO("SSR pose: xPos=%dmm yPos=%dmm angle=%dmrad vx=%dmm/s vy=%dmm/s", xPos_mm, yPos_mm, angle_mrad, - xVel_mms, yVel_mms); + const int x_mm_i = xPos_mm.as(); + const int y_mm_i = yPos_mm.as(); + const int vx_mms_i = xVel_mms.as(); + const int vy_mms_i = yVel_mms.as(); + const int ang_mrad_i = angle_mrad.as(); + + LOG_INFO("SSR pose: xPos=%dmm yPos=%dmm angle=%dmrad vx=%dmm/s vy=%dmm/s", + x_mm_i, y_mm_i, ang_mrad_i, vx_mms_i, vy_mms_i); + + /* 1) Copy pose into struct. */ + SpaceShipRadarPose ssrPose; + ssrPose.timestamp = static_cast(m_timeSync.localNowMs()); + ssrPose.x = static_cast(x_mm_i); + ssrPose.y = static_cast(y_mm_i); + ssrPose.theta = static_cast(ang_mrad_i); + ssrPose.v_x = static_cast(vx_mms_i); + ssrPose.v_y = static_cast(vy_mms_i); + + /* 2) Initialize odometry origin from first SSR pose. */ + if (false == m_odoOriginInitialized) + { + m_odoOriginX_mm = ssrPose.x; + m_odoOriginY_mm = ssrPose.y; + m_odoOriginInitialized = true; + + LOG_INFO("Odometry origin set from SSR: x=%dmm y=%dmm", x_mm_i, y_mm_i); + } + + /* 3) Initialize EKF from first SSR pose. */ + if (false == m_ekfInitializedFromSSR) + { + StateVector x0; + x0.setZero(); + + /* Position and heading from SSR. */ + x0(0) = ssrPose.x; /* p_x [mm] */ + x0(1) = ssrPose.y; /* p_y [mm] */ + x0(2) = ssrPose.theta; /* theta [mrad] */ + + /* Velocity magnitude from vx, vy. */ + const float v_mms = std::sqrt(ssrPose.v_x * ssrPose.v_x + ssrPose.v_y * ssrPose.v_y); + x0(3) = v_mms; /* v [mm/s] */ + x0(4) = 0.0F; /* omega [mrad/s], unknown -> 0 */ + + StateMatrix P0 = StateMatrix::Identity(); + /* Position uncertainty ~ 50 mm. */ + P0(0,0) = 50.0F * 50.0F; + P0(1,1) = 50.0F * 50.0F; + /* Heading uncertainty ~ 200 mrad. */ + P0(2,2) = 200.0F * 200.0F; + /* Velocity and omega moderately uncertain. */ + P0(3,3) = 200.0F * 200.0F; + P0(4,4) = 200.0F * 200.0F; + + (void)m_ekf.init(x0, P0); + m_lastEkfUpdateMs = ssrPose.timestamp; + m_ekfInitializedFromSSR = true; + + LOG_INFO("EKF initialized from SSR: x=%.1fmm y=%.1fmm theta=%.1fmrad v=%.1fmm/s", + x0(0), x0(1), x0(2), x0(3)); + } + + /* 4) Store last SSR pose. */ + m_lastSsrPose = ssrPose; + m_hasSsrPose = true; + + /* 5) Run fusion if we already have at least one vehicle data sample. */ + if (true == m_hasVehicleData) + { + filterLocationData(m_lastVehicleData, m_lastSsrPose); + } } } @@ -319,6 +426,11 @@ void App::publishVehicleAndSensorSnapshot(const VehicleData& data) const bool zumoSynced = m_timeSync.isZumoSynced(); const bool rtcSynced = m_timeSync.isRtcSynced(); + (void)localNowMs; + (void)offsetMs; + (void)zumoSynced; + (void)rtcSynced; + /* Publish snapshot of vehicle + line sensor data. */ const uint16_t* lineSensorValues = m_lineSensors.getSensorValues(); JsonDocument payloadJson; @@ -335,7 +447,8 @@ void App::publishVehicleAndSensorSnapshot(const VehicleData& data) vehicleObj["vL_mms"] = static_cast(data.left); vehicleObj["vR_mms"] = static_cast(data.right); vehicleObj["vC_mms"] = static_cast(data.center); - vehicleObj["proximity"] = static_cast(data.proximity); + vehicleObj["accelX_digit"] = static_cast(data.accelerationX); + vehicleObj["turnRateZ_digit"] = static_cast(data.turnRateZ); JsonObject lineObj = payloadJson["line"].to(); JsonArray values = lineObj["values"].to(); @@ -352,6 +465,180 @@ void App::publishVehicleAndSensorSnapshot(const VehicleData& data) } } +void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRadarPose& ssrPose) +{ + /* Do not run fusion until EKF has been initialized from SSR. */ + if (false == m_ekfInitializedFromSSR) + { + return; + } + + const uint32_t zumoTs32 = static_cast(vehicleData.timestamp); + const uint32_t ssrTs32 = static_cast(ssrPose.timestamp); + + /* If EKF has no valid timestamp yet, initialize from the first available one. */ + if (0U == m_lastEkfUpdateMs) + { + if (0U != zumoTs32) + { + m_lastEkfUpdateMs = zumoTs32; + } + else if (0U != ssrTs32) + { + m_lastEkfUpdateMs = ssrTs32; + } + else + { + return; + } + } + + /* Determine which sensor provides the newest information. */ + enum class Source + { + None, + Vehicle, + SSR + }; + + Source newestSource = Source::None; + uint32_t newestTs = m_lastEkfUpdateMs; + + if (zumoTs32 > newestTs) + { + newestTs = zumoTs32; + newestSource = Source::Vehicle; + } + + if (ssrTs32 > newestTs) + { + newestTs = ssrTs32; + newestSource = Source::SSR; + } + + /* No newer data available -> nothing to do. */ + if (Source::None == newestSource) + { + return; + } + + /* Time difference in seconds for prediction. */ + const uint32_t dtMs = newestTs - m_lastEkfUpdateMs; + const float dt = static_cast(dtMs) / 1000.0F; + + /* Longitudinal acceleration as input to the process model (mm/s^2). */ + const float a_x = static_cast(vehicleData.accelerationX); + + /* EKF prediction step. */ + m_ekf.predict(a_x, dt); + + /* Measurement update depending on source. */ + if (Source::Vehicle == newestSource) + { + /* IMU update: only yaw rate (turnRateZ). */ + { + ImuMeasVector z_imu; + z_imu(0) = static_cast(vehicleData.turnRateZ); /* [mrad/s] */ + m_ekf.updateImu(z_imu); + } + + /* Odometry update in global frame. + * + * EKF odometryModel(x): + * z_odo(0) = p_x + * z_odo(1) = p_y + * z_odo(2) = v + * z_odo(3) = theta + */ + { + float xGlob_mm = 0.0F; + float yGlob_mm = 0.0F; + float thetaGlob_mrad = 0.0F; + + transformOdometryToGlobal(vehicleData, xGlob_mm, yGlob_mm, thetaGlob_mrad); + + OdoMeasVector z_odo; + z_odo(0) = xGlob_mm; /* global p_x [mm] */ + z_odo(1) = yGlob_mm; /* global p_y [mm] */ + z_odo(2) = static_cast(vehicleData.center); /* speed [mm/s] (v_center) */ + z_odo(3) = thetaGlob_mrad; /* global theta [mrad] */ + + m_ekf.updateOdometry(z_odo); + } + } + else if (Source::SSR == newestSource) + { + /* Camera / SpaceShipRadar update. + * + * EKF cameraModel(x): + * z_cam(0) = p_x + * z_cam(1) = p_y + * z_cam(2) = theta [mrad] + * z_cam(3) = v_x + * z_cam(4) = v_y + */ + CamMeasVector z_cam; + z_cam(0) = ssrPose.x; /* p_x [mm] */ + z_cam(1) = ssrPose.y; /* p_y [mm] */ + z_cam(2) = ssrPose.theta; /* theta [mrad] */ + z_cam(3) = ssrPose.v_x; /* v_x [mm/s] */ + z_cam(4) = ssrPose.v_y; /* v_y [mm/s] */ + + m_ekf.updateCamera(z_cam); + } + + /* Update timestamp of last EKF update. */ + m_lastEkfUpdateMs = newestTs; + + /* Publish fused pose. */ + publishFusionPose(newestTs); +} + +void App::transformOdometryToGlobal(const VehicleData& vehicleData, + float& xGlob_mm, + float& yGlob_mm, + float& thetaGlob_mrad) const +{ + /* Y axis and heading sign differ between local odometry frame and SSR frame. */ + constexpr float Y_SIGN = -1.0F; + constexpr float THETA_SIGN = -1.0F; + + const float xLocal_mm = static_cast(vehicleData.xPos); + const float yLocal_mm = static_cast(vehicleData.yPos); + const float thetaLocal_mrad = static_cast(vehicleData.orientation); + + const float originX = (true == m_odoOriginInitialized) ? m_odoOriginX_mm : 0.0F; + const float originY = (true == m_odoOriginInitialized) ? m_odoOriginY_mm : 0.0F; + + xGlob_mm = originX + xLocal_mm; + yGlob_mm = originY + (Y_SIGN * yLocal_mm); + thetaGlob_mrad = THETA_SIGN * thetaLocal_mrad; +} + +void App::publishFusionPose(uint32_t tsMs) +{ + /* EKF state: [p_x, p_y, theta, v, omega]. */ + const StateVector& state = m_ekf.getState(); + + JsonDocument payloadJson; + char payloadArray[JSON_FUSION_POSE_MAX_SIZE]; + + payloadJson["ts_ms"] = static_cast(tsMs); + payloadJson["x_mm"] = static_cast(state(0)); + payloadJson["y_mm"] = static_cast(state(1)); + payloadJson["theta_mrad"] = static_cast(state(2)); + payloadJson["v_mms"] = static_cast(state(3)); + payloadJson["omega_mradps"] = static_cast(state(4)); + + (void)serializeJson(payloadJson, payloadArray); + String payloadStr(payloadArray); + + if (false == m_mqttClient.publish(TOPIC_NAME_FUSION_POSE, true, payloadStr)) + { + LOG_WARNING("Publishing fusion pose via MQTT failed."); + } +} + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/APPLineFollowerSensorFusion/src/App.h b/lib/APPLineFollowerSensorFusion/src/App.h index 7a90941d..630b50be 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.h +++ b/lib/APPLineFollowerSensorFusion/src/App.h @@ -53,6 +53,7 @@ #include "Motors.h" #include #include "TimeSync.h" +#include "EKF.h" /****************************************************************************** * Macros @@ -62,29 +63,44 @@ * Types and Classes *****************************************************************************/ -/** The Remote Control application. */ +/** + * @brief Space Ship Radar pose in global (SSR) coordinate frame. + */ +struct SpaceShipRadarPose +{ + uint32_t timestamp; /**< Local timestamp in ms (ESP time base). */ + float x; /**< X position in mm. */ + float y; /**< Y position in mm. */ + float theta; /**< Heading in mrad. */ + float v_x; /**< Velocity in X direction in mm/s. */ + float v_y; /**< Velocity in Y direction in mm/s. */ +}; + +/** + * @brief Line follower Sensor Fusion application. + */ class App { public: /** - * Construct the Remote Control application. + * @brief Constructs the application. */ App(); /** - * Destroy the Remote Control application. + * @brief Destroys the application. */ ~App() { } /** - * Setup the application. + * @brief Setup the application. */ void setup(); /** - * Process the application periodically. + * @brief Process the application periodically. */ void loop(); @@ -98,91 +114,178 @@ class App /** MQTT topic name for status messages. */ static const char* TOPIC_NAME_STATUS; - /** MQTT topic name for fusion Pose */ + /** MQTT topic name for fusion pose. */ static const char* TOPIC_NAME_FUSION_POSE; - /** MQTT topic name for raw Sensor data */ + /** MQTT topic name for raw sensor data. */ static const char* TOPIC_NAME_RAW_SENSORS; - /** MQTT topic name for receiving Space Ship Radar Pose. */ + /** MQTT topic name for receiving Space Ship Radar pose. */ static const char* TOPIC_NAME_RADAR_POSE; /** - * Flag for setting initial data through SMP. + * @brief Flag for setting initial data through SMP. */ bool m_initialDataSent; /** - * Timer for sending system status to RU. + * @brief Timer for sending system status to Radon Ulzer. */ SimpleTimer m_statusTimer; /** - * SerialMux Channel Provider handler. + * @brief SerialMux channel provider handler. */ SerMuxChannelProvider m_serMuxChannelProvider; /** - * Time synchronization handler. + * @brief Time synchronization handler. */ TimeSync m_timeSync; /** - * Line sensors handler. + * @brief Line sensors handler. */ LineSensors m_lineSensors; /** - * Motors handler. + * @brief Motors handler. */ Motors m_motors; /** - * State machine for the application. + * @brief State machine for the application. */ StateMachine m_stateMachine; /** - * MQTT client handler. + * @brief MQTT client handler. */ MqttClient m_mqttClient; /** - * Setup the MQTT connection. + * @brief Extended Kalman Filter handler (5D state). + */ + ExtendedKalmanFilter5D m_ekf; + + /** + * @brief Last time the EKF was updated [ms] (local time base). + */ + uint32_t m_lastEkfUpdateMs; + + /** + * @brief Last received vehicle data (odometry + IMU) from Zumo. + */ + VehicleData m_lastVehicleData; + + /** + * @brief Flag indicating if at least one valid vehicle data has been received. + */ + bool m_hasVehicleData; + + /** + * @brief Last received Space Ship Radar pose. + */ + SpaceShipRadarPose m_lastSsrPose; + + /** + * @brief Flag indicating if at least one valid SSR pose has been received. + */ + bool m_hasSsrPose; + + /** + * @brief Flag indicating if the odometry origin in the global frame is initialized. + */ + bool m_odoOriginInitialized; + + /** + * @brief Odometry origin X coordinate in mm in global (SSR) frame. + */ + float m_odoOriginX_mm; + + /** + * @brief Odometry origin Y coordinate in mm in global (SSR) frame. + */ + float m_odoOriginY_mm; + + /** + * @brief Flag indicating if the EKF has been initialized from SSR. + */ + bool m_ekfInitializedFromSSR; + + /** + * @brief Setup the MQTT connection. * - * @param[in] clientId The MQTT client id. - * @param[in] brokerAddr The address of the MQTT broker. - * @param[in] brokerPort The port of the MQTT broker. + * @param[in] clientId The MQTT client id. + * @param[in] brokerAddr The address of the MQTT broker. + * @param[in] brokerPort The port of the MQTT broker. * * @return true if successful, otherwise false. */ bool setupMqtt(const String& clientId, const String& brokerAddr, uint16_t brokerPort); /** - * Callback for receiving Space Ship Radar Pose over MQTT topic. + * @brief Callback for receiving Space Ship Radar pose over MQTT topic. * - * @param[in] payload The topic payload. + * @param[in] payload The topic payload as JSON string. */ void ssrTopicCallback(const String& payload); /** - * Publish a combined snapshot of vehicle and line sensor data via MQTT. + * @brief Publish a combined snapshot of vehicle and line sensor data via MQTT. * - * @param[in] data Vehicle data received via SerialMux. + * @param[in] data Vehicle data received via SerialMux. */ void publishVehicleAndSensorSnapshot(const VehicleData& data); /** - * Copy construction of an instance. - * Not allowed. + * @brief Run sensor fusion for vehicle data and Space Ship Radar pose. + * + * This function: + * - selects the newest data source (vehicle or SSR), + * - performs the EKF prediction step, + * - applies the corresponding measurement update(s), + * - and publishes the fused pose via MQTT. + * + * @param[in] data Vehicle data received via SerialMux. + * @param[in] ssrPose Latest Space Ship Radar pose (global frame). + */ + void filterLocationData(const VehicleData& data, const SpaceShipRadarPose& ssrPose); + + /** + * @brief Transform odometry from local Zumo frame into global (SSR) frame. + * + * The transformation: + * - applies a configurable origin offset (initialized from SSR), + * - flips the Y axis to match the SSR convention, + * - flips the heading sign to match the SSR rotation direction. + * + * @param[in] vehicleData Raw odometry from Zumo. + * @param[out] xGlob_mm Global X position in mm. + * @param[out] yGlob_mm Global Y position in mm. + * @param[out] thetaGlob_mrad Global heading in mrad. + */ + void transformOdometryToGlobal(const VehicleData& vehicleData, + float& xGlob_mm, + float& yGlob_mm, + float& thetaGlob_mrad) const; + + /** + * @brief Publish the current EKF fusion pose via MQTT. + * + * @param[in] tsMs Timestamp in ms (local time base) associated with the EKF state. + */ + void publishFusionPose(uint32_t tsMs); + + /** + * @brief Copy construction of an instance (not allowed). * * @param[in] app Source instance. */ App(const App& app); /** - * Assignment of an instance. - * Not allowed. + * @brief Assignment of an instance (not allowed). * * @param[in] app Source instance. * diff --git a/lib/APPLineFollowerSensorFusion/src/SerialMuxChannels.h b/lib/APPLineFollowerSensorFusion/src/SerialMuxChannels.h index 25b66a46..83d449e9 100644 --- a/lib/APPLineFollowerSensorFusion/src/SerialMuxChannels.h +++ b/lib/APPLineFollowerSensorFusion/src/SerialMuxChannels.h @@ -144,23 +144,6 @@ namespace SMPChannelPayload } Status; /**< Status flag */ - /** - * Range in which a detected object may be. - * Equivalent to the brightness levels. - * Values estimated from user's guide. - */ - typedef enum : uint8_t - { - RANGE_NO_OBJECT = 0U, /**< No object detected */ - RANGE_25_30, /**< Object detected in range 25 to 30 cm */ - RANGE_20_25, /**< Object detected in range 20 to 25 cm */ - RANGE_15_20, /**< Object detected in range 15 to 20 cm */ - RANGE_10_15, /**< Object detected in range 10 to 15 cm */ - RANGE_5_10, /**< Object detected in range 5 to 10 cm */ - RANGE_0_5 /**< Object detected in range 0 to 5 cm */ - - } Range; /**< Proximity Sensor Ranges */ - } /* namespace SMPChannelPayload */ /** Struct of the "Command" channel payload. */ @@ -219,7 +202,8 @@ typedef struct _VehicleData int32_t left; /**< Left motor speed [mm/s]. */ int32_t right; /**< Right motor speed [mm/s]. */ int32_t center; /**< Center speed [mm/s]. */ - SMPChannelPayload::Range proximity; /**< Range at which object is found [range]. */ + int16_t accelerationX; /**< Raw acceleration in X [digit]. */ + int16_t turnRateZ; /**< Raw turn rate around Z [digit]. */ } __attribute__((packed)) VehicleData; /** Struct of the "Status" channel payload. */ diff --git a/lib/Service/src/EKF.cpp b/lib/Service/src/EKF.cpp new file mode 100644 index 00000000..4b8658b5 --- /dev/null +++ b/lib/Service/src/EKF.cpp @@ -0,0 +1,389 @@ +/* MIT License + * + * Copyright (c) 2023 - 2025 Andreas Merkle + * + * 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. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Implementation of the Extended Kalman Filter (5D state) + * @author Tobias Haeckel + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "EKF.h" +#include + +/* Some toolchains do not define M_PI. */ +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +/****************************************************************************** + * Local Namespace + *****************************************************************************/ + +namespace +{ + constexpr float PI_MRAD = 1000.0F * static_cast(M_PI); + constexpr float TWO_PI_MRAD = 2.0F * PI_MRAD; + + /* Default process noise standard deviations. */ + constexpr float SIGMA_PX = 1.0F; /* [mm] */ + constexpr float SIGMA_PY = 1.0F; /* [mm] */ + constexpr float SIGMA_THETA_P = 5.0F; /* [mrad] */ + constexpr float SIGMA_V_P = 5.0F; /* [mm/s] */ + constexpr float SIGMA_OMEGA_P = 2.0F; /* [mrad/s] */ + + /* Camera (SSR) measurement noise std dev. */ + constexpr float SIGMA_CAM_POS = 2.0F; /* [mm] */ + constexpr float SIGMA_CAM_THETA = 5.0F; /* [mrad] */ + constexpr float SIGMA_CAM_V = 20.0F; /* [mm/s] */ + + /* Odometry measurement noise std dev. */ + constexpr float SIGMA_ODO_POS = 100000.0F; /* [mm] */ + constexpr float SIGMA_ODO_V = 10.0F; /* [mm/s] */ + constexpr float SIGMA_ODO_THETA = 5.0F; /* [mrad] */ + + /* IMU yaw noise std dev. */ + constexpr float SIGMA_IMU_OMEGA = 20.0F; /* [mrad/s] */ + + /* Default initial state (will usually be overwritten by SSR init). */ + constexpr float EKF_START_X_MM = 705.0F; /* [mm] */ + constexpr float EKF_START_Y_MM = 939.0F; /* [mm] */ + constexpr float EKF_START_THETA_MRAD = 0.0F; /* [mrad] */ + constexpr float EKF_START_V_MMS = 0.0F; /* [mm/s] */ + constexpr float EKF_START_OMEGA_MRADPS = 0.0F; /* [mrad/s] */ +} + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +ExtendedKalmanFilter5D::ExtendedKalmanFilter5D() +{ + /* Initialize state and covariance. */ + m_state.setZero(); + m_covariance.setIdentity(); + + /* Process noise Q (diagonal). */ + m_Q.setZero(); + m_Q(0, 0) = SIGMA_PX * SIGMA_PX; + m_Q(1, 1) = SIGMA_PY * SIGMA_PY; + m_Q(2, 2) = SIGMA_THETA_P * SIGMA_THETA_P; + m_Q(3, 3) = SIGMA_V_P * SIGMA_V_P; + m_Q(4, 4) = SIGMA_OMEGA_P * SIGMA_OMEGA_P; + + /* Camera measurement noise R_cam. */ + m_R_cam.setZero(); + m_R_cam(0, 0) = SIGMA_CAM_POS * SIGMA_CAM_POS; + m_R_cam(1, 1) = SIGMA_CAM_POS * SIGMA_CAM_POS; + m_R_cam(2, 2) = SIGMA_CAM_THETA * SIGMA_CAM_THETA; + m_R_cam(3, 3) = SIGMA_CAM_V * SIGMA_CAM_V; + m_R_cam(4, 4) = SIGMA_CAM_V * SIGMA_CAM_V; + + /* Odometry measurement noise R_odo. */ + m_R_odo.setZero(); + m_R_odo(0, 0) = SIGMA_ODO_POS * SIGMA_ODO_POS; + m_R_odo(1, 1) = SIGMA_ODO_POS * SIGMA_ODO_POS; + m_R_odo(2, 2) = SIGMA_ODO_V * SIGMA_ODO_V; + m_R_odo(3, 3) = SIGMA_ODO_THETA * SIGMA_ODO_THETA; + + /* IMU yaw noise R_imu. */ + m_R_imu.setZero(); + m_R_imu(0, 0) = SIGMA_IMU_OMEGA * SIGMA_IMU_OMEGA; +} + +bool ExtendedKalmanFilter5D::init(const StateVector& x0, const StateMatrix& P0) +{ + m_state = x0; + m_covariance = P0; + return true; +} + +bool ExtendedKalmanFilter5D::init() +{ + m_state.setZero(); + + m_state(0) = EKF_START_X_MM; /* p_x [mm] */ + m_state(1) = EKF_START_Y_MM; /* p_y [mm] */ + m_state(2) = EKF_START_THETA_MRAD; /* theta [mrad] */ + m_state(3) = EKF_START_V_MMS; /* v [mm/s] */ + m_state(4) = EKF_START_OMEGA_MRADPS; /* omega [mrad/s] */ + + m_covariance.setIdentity(); + return true; +} + +void ExtendedKalmanFilter5D::predict(float a_x, float dt) +{ + /* Nonlinear prediction. */ + StateVector x_pred = processModel(m_state, a_x, dt); + + /* Wrap heading angle (index 2). */ + x_pred(2) = wrapAngleMrad(x_pred(2)); + + /* Linearization. */ + StateMatrix jacobianF = processJacobianF(m_state, a_x, dt); + + /* Covariance prediction. */ + StateMatrix P_pred = jacobianF * m_covariance * jacobianF.transpose() + m_Q; + + /* Commit. */ + m_state = x_pred; + m_covariance = P_pred; +} + +void ExtendedKalmanFilter5D::updateCamera(const CamMeasVector& z_cam) +{ + /* Predicted measurement. */ + CamMeasVector z_pred = cameraModel(m_state); + auto H = cameraJacobianH(m_state); + + /* Innovation. */ + CamMeasVector y = z_cam - z_pred; + + /* Wrap angle innovation (index 2 is theta). */ + y(2) = wrapAngleMrad(y(2)); + + /* EKF update. */ + const CamMeasMatrix S = H * m_covariance * H.transpose() + m_R_cam; + const Eigen::Matrix K = + m_covariance * H.transpose() * S.inverse(); + + m_state = m_state + K * y; + m_state(2) = wrapAngleMrad(m_state(2)); + m_covariance = (StateMatrix::Identity() - K * H) * m_covariance; +} + +void ExtendedKalmanFilter5D::updateOdometry(const OdoMeasVector& z_odo) +{ + /* Predicted measurement. */ + OdoMeasVector z_pred = odometryModel(m_state); + auto H = odometryJacobianH(m_state); + + /* Innovation. */ + OdoMeasVector y = z_odo - z_pred; + + /* Wrap angle innovation (index 3 is theta). */ + y(3) = wrapAngleMrad(y(3)); + + /* EKF update. */ + const OdoMeasMatrix S = H * m_covariance * H.transpose() + m_R_odo; + const Eigen::Matrix K = + m_covariance * H.transpose() * S.inverse(); + + m_state = m_state + K * y; + m_state(2) = wrapAngleMrad(m_state(2)); + m_covariance = (StateMatrix::Identity() - K * H) * m_covariance; +} + +void ExtendedKalmanFilter5D::updateImu(const ImuMeasVector& z_imu) +{ + /* Predicted measurement. */ + ImuMeasVector z_pred = imuModel(m_state); + auto H = imuJacobianH(m_state); + + /* Innovation (omega, no wrapping). */ + ImuMeasVector y = z_imu - z_pred; + + /* EKF update. */ + const ImuMeasMatrix S = H * m_covariance * H.transpose() + m_R_imu; + const Eigen::Matrix K = + m_covariance * H.transpose() * S.inverse(); + + m_state = m_state + K * y; + m_state(2) = wrapAngleMrad(m_state(2)); + m_covariance = (StateMatrix::Identity() - K * H) * m_covariance; +} + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +ExtendedKalmanFilter5D::StateVector +ExtendedKalmanFilter5D::processModel(const StateVector& x, float a_x, float dt) const +{ + const float px = x(0); + const float py = x(1); + const float thetaMrad = x(2); + const float v = x(3); + const float omegaMrad = x(4); + + const float thetaRad = thetaMrad / 1000.0F; + + StateVector x_next; + x_next(0) = px + v * std::cos(thetaRad) * dt; /* p_x */ + x_next(1) = py + v * std::sin(thetaRad) * dt; /* p_y */ + x_next(2) = thetaMrad + omegaMrad * dt; /* theta [mrad] */ + x_next(3) = v + a_x * dt; /* v */ + x_next(4) = omegaMrad; /* omega (no angular accel) */ + + return x_next; +} + +ExtendedKalmanFilter5D::StateMatrix +ExtendedKalmanFilter5D::processJacobianF(const StateVector& x, float /*a_x*/, float dt) const +{ + const float thetaMrad = x(2); + const float v = x(3); + const float thetaRad = thetaMrad / 1000.0F; + + StateMatrix jacobianF = StateMatrix::Identity(); + + /* d p_x / d theta */ + jacobianF(0, 2) = -v * std::sin(thetaRad) * dt / 1000.0F; + /* d p_x / d v */ + jacobianF(0, 3) = std::cos(thetaRad) * dt; + + /* d p_y / d theta */ + jacobianF(1, 2) = v * std::cos(thetaRad) * dt / 1000.0F; + /* d p_y / d v */ + jacobianF(1, 3) = std::sin(thetaRad) * dt; + + /* d theta / d omega */ + jacobianF(2, 4) = dt; + + return jacobianF; +} + +/* ---------------- Camera model ---------------- */ + +ExtendedKalmanFilter5D::CamMeasVector +ExtendedKalmanFilter5D::cameraModel(const StateVector& x) const +{ + const float px = x(0); + const float py = x(1); + const float thetaMrad = x(2); + const float v = x(3); + + const float thetaRad = thetaMrad / 1000.0F; + + CamMeasVector z; + z(0) = px; /* p_x */ + z(1) = py; /* p_y */ + z(2) = thetaMrad; /* theta [mrad] */ + z(3) = v * std::cos(thetaRad);/* v_x */ + z(4) = v * std::sin(thetaRad);/* v_y */ + return z; +} + +Eigen::Matrix +ExtendedKalmanFilter5D::cameraJacobianH(const StateVector& x) const +{ + const float thetaMrad = x(2); + const float v = x(3); + const float thetaRad = thetaMrad / 1000.0F; + + Eigen::Matrix H; + H.setZero(); + + /* p_x */ + H(0, 0) = 1.0F; + /* p_y */ + H(1, 1) = 1.0F; + /* theta */ + H(2, 2) = 1.0F; + + /* v_x = v cos(theta) */ + H(3, 2) = -v * std::sin(thetaRad) * (1.0F / 1000.0F); /* d v_x / d theta */ + H(3, 3) = std::cos(thetaRad); /* d v_x / d v */ + + /* v_y = v sin(theta) */ + H(4, 2) = v * std::cos(thetaRad) * (1.0F / 1000.0F); /* d v_y / d theta */ + H(4, 3) = std::sin(thetaRad); /* d v_y / d v */ + + return H; +} + +/* ---------------- Odometry model ---------------- */ + +ExtendedKalmanFilter5D::OdoMeasVector +ExtendedKalmanFilter5D::odometryModel(const StateVector& x) const +{ + OdoMeasVector z; + z(0) = x(0); /* p_x */ + z(1) = x(1); /* p_y */ + z(2) = x(3); /* v */ + z(3) = x(2); /* theta */ + return z; +} + +Eigen::Matrix +ExtendedKalmanFilter5D::odometryJacobianH(const StateVector& /*x*/) const +{ + Eigen::Matrix H; + H.setZero(); + + /* p_x */ + H(0, 0) = 1.0F; + /* p_y */ + H(1, 1) = 1.0F; + /* v */ + H(2, 3) = 1.0F; + /* theta */ + H(3, 2) = 1.0F; + + return H; +} + +/* ---------------- IMU yaw model ---------------- */ + +ExtendedKalmanFilter5D::ImuMeasVector +ExtendedKalmanFilter5D::imuModel(const StateVector& x) const +{ + ImuMeasVector z; + z(0) = x(4); /* omega [mrad/s] */ + return z; +} + +Eigen::Matrix +ExtendedKalmanFilter5D::imuJacobianH(const StateVector& /*x*/) const +{ + Eigen::Matrix H; + H.setZero(); + H(0, 4) = 1.0F; /* d omega / d state */ + return H; +} + +/* ---------------- Angle wrapping ---------------- */ + +float ExtendedKalmanFilter5D::wrapAngleMrad(float angleMrad) +{ + /* Wrap to [-pi, pi). */ + float x = std::fmod(angleMrad + PI_MRAD, TWO_PI_MRAD); + if (x < 0.0F) + { + x += TWO_PI_MRAD; + } + return x - PI_MRAD; +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/Service/src/EKF.h b/lib/Service/src/EKF.h new file mode 100644 index 00000000..1a9ef661 --- /dev/null +++ b/lib/Service/src/EKF.h @@ -0,0 +1,295 @@ +/* MIT License + * + * Copyright (c) 2023 - 2025 Andreas Merkle + * + * 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. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Extended Kalman Filter with 5D state + * @author Tobias Haeckel + * + * State vector: + * x = [p_x, p_y, theta, v, omega]^T + * p_x, p_y : position in mm + * theta : heading in mrad + * v : linear velocity in mm/s + * omega : yaw rate in mrad/s + * + * @addtogroup Application + * + * @{ + */ + +#ifndef EKF_H +#define EKF_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * @brief Extended Kalman Filter implementation for a 5D state. + */ +class ExtendedKalmanFilter5D +{ +public: + /** State dimension. */ + static constexpr uint8_t STATE_DIM = 5U; + /** Camera measurement dimension. */ + static constexpr uint8_t CAM_MEAS_DIM = 5U; + /** Odometry measurement dimension. */ + static constexpr uint8_t ODO_MEAS_DIM = 4U; + /** IMU measurement dimension. */ + static constexpr uint8_t IMU_MEAS_DIM = 1U; + + /** State vector type: [p_x, p_y, theta, v, omega]^T. */ + using StateVector = Eigen::Matrix; + /** State covariance matrix type. */ + using StateMatrix = Eigen::Matrix; + + /** Camera measurement vector type. */ + using CamMeasVector = Eigen::Matrix; + /** Camera measurement covariance matrix type. */ + using CamMeasMatrix = Eigen::Matrix; + + /** Odometry measurement vector type. */ + using OdoMeasVector = Eigen::Matrix; + /** Odometry measurement covariance matrix type. */ + using OdoMeasMatrix = Eigen::Matrix; + + /** IMU measurement vector type. */ + using ImuMeasVector = Eigen::Matrix; + /** IMU measurement covariance matrix type. */ + using ImuMeasMatrix = Eigen::Matrix; + + /** + * @brief Construct the EKF with default noise matrices. + */ + ExtendedKalmanFilter5D(); + + /** + * @brief Initialize filter with a given state and covariance. + * + * @param[in] x0 Initial state vector. + * @param[in] P0 Initial covariance matrix. + * + * @return true on success. + */ + bool init(const StateVector& x0, const StateMatrix& P0); + + /** + * @brief Initialize filter with default state and identity covariance. + * + * Default state is configured in EKF.cpp. + * + * @return true on success. + */ + bool init(); + + /** + * @brief Prediction step of the EKF. + * + * State model: + * p_x(k+1) = p_x + v cos(theta) dt + * p_y(k+1) = p_y + v sin(theta) dt + * theta(k+1) = theta + omega dt + * v(k+1) = v + a_x dt + * omega(k+1) = omega + * + * @param[in] a_x Longitudinal acceleration in mm/s^2. + * @param[in] dt Time step in seconds. + */ + void predict(float a_x, float dt); + + /** + * @brief Camera / Space Ship Radar measurement update. + * + * Measurement model: + * z_cam(0) = p_x + * z_cam(1) = p_y + * z_cam(2) = theta [mrad] + * z_cam(3) = v_x + * z_cam(4) = v_y + * + * @param[in] z_cam Camera measurement vector. + */ + void updateCamera(const CamMeasVector& z_cam); + + /** + * @brief Odometry measurement update. + * + * Measurement model: + * z_odo(0) = p_x + * z_odo(1) = p_y + * z_odo(2) = v + * z_odo(3) = theta + * + * @param[in] z_odo Odometry measurement vector. + */ + void updateOdometry(const OdoMeasVector& z_odo); + + /** + * @brief IMU yaw-rate measurement update. + * + * Measurement model: + * z_imu(0) = omega + * + * @param[in] z_imu IMU measurement vector. + */ + void updateImu(const ImuMeasVector& z_imu); + + /** + * @brief Get current state estimate. + * + * @return const reference to internal state vector. + */ + const StateVector& getState() const + { + return m_state; + } + +private: + /** State vector. */ + StateVector m_state; + + /** State covariance matrix. */ + StateMatrix m_covariance; + + /** Process noise covariance matrix. */ + StateMatrix m_Q; + + /** Camera measurement noise covariance matrix. */ + CamMeasMatrix m_R_cam; + + /** Odometry measurement noise covariance matrix. */ + OdoMeasMatrix m_R_odo; + + /** IMU measurement noise covariance matrix. */ + ImuMeasMatrix m_R_imu; + + /** + * @brief Nonlinear process model. + * + * @param[in] x Current state. + * @param[in] a_x Longitudinal acceleration in mm/s^2. + * @param[in] dt Time step in seconds. + * + * @return Predicted next state. + */ + StateVector processModel(const StateVector& x, float a_x, float dt) const; + + /** + * @brief Jacobian of the process model w.r.t. the state. + * + * @param[in] x Current state. + * @param[in] a_x Longitudinal acceleration in mm/s^2 (currently unused in Jacobian). + * @param[in] dt Time step in seconds. + * + * @return Process Jacobian matrix F. + */ + StateMatrix processJacobianF(const StateVector& x, float a_x, float dt) const; + + /** + * @brief Camera measurement model. + * + * @param[in] x Current state. + * + * @return Predicted camera measurement. + */ + CamMeasVector cameraModel(const StateVector& x) const; + + /** + * @brief Jacobian of the camera measurement model. + * + * @param[in] x Current state. + * + * @return Camera Jacobian matrix H_cam. + */ + Eigen::Matrix cameraJacobianH(const StateVector& x) const; + + /** + * @brief Odometry measurement model. + * + * @param[in] x Current state. + * + * @return Predicted odometry measurement. + */ + OdoMeasVector odometryModel(const StateVector& x) const; + + /** + * @brief Jacobian of the odometry measurement model. + * + * @param[in] x Current state (currently unused). + * + * @return Odometry Jacobian matrix H_odo. + */ + Eigen::Matrix odometryJacobianH(const StateVector& x) const; + + /** + * @brief IMU measurement model. + * + * @param[in] x Current state. + * + * @return Predicted IMU measurement. + */ + ImuMeasVector imuModel(const StateVector& x) const; + + /** + * @brief Jacobian of the IMU measurement model. + * + * @param[in] x Current state (currently unused). + * + * @return IMU Jacobian matrix H_imu. + */ + Eigen::Matrix imuJacobianH(const StateVector& x) const; + + /** + * @brief Wrap an angle in mrad to [-pi, pi). + * + * @param[in] angleMrad Angle in mrad. + * + * @return Wrapped angle in mrad. + */ + static float wrapAngleMrad(float angleMrad); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* EKF_H */ +/** @} */ diff --git a/platformio.ini b/platformio.ini index ae4c833e..8e3b7f1c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -425,11 +425,13 @@ build_flags = lib_deps = ${target:Sim.lib_deps} ${app:LineFollowerSensorFusion.lib_deps} + hideakitai/ArduinoEigen @ ^0.3.2 lib_ignore = ${target:Sim.lib_ignore} ${app:LineFollowerSensorFusion.lib_ignore} extra_scripts = ${target:Sim.extra_scripts} +lib_compat_mode = off ; ***************************************************************************** ; Line follower sensor fusion application on target @@ -443,6 +445,7 @@ lib_deps = ${target:esp32.lib_deps} ${app:LineFollowerSensorFusion.lib_deps} arduino-libraries/NTPClient@^3.2.1 + hideakitai/ArduinoEigen @ ^0.3.2 lib_ignore = ${target:esp32.lib_ignore} ${app:LineFollowerSensorFusion.lib_ignore} From ec655807e86892e968ec958f9d71220a8ec87792 Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Thu, 4 Dec 2025 16:35:04 +0100 Subject: [PATCH 02/14] added Startup Script --- .env | 13 +++ start_sensorFusion.bat | 177 ----------------------------------------- start_sensorFusion.ps1 | 80 +++++++++++++++++++ 3 files changed, 93 insertions(+), 177 deletions(-) create mode 100644 .env delete mode 100644 start_sensorFusion.bat create mode 100644 start_sensorFusion.ps1 diff --git a/.env b/.env new file mode 100644 index 00000000..bda6c1a2 --- /dev/null +++ b/.env @@ -0,0 +1,13 @@ +# remove the template from this file (change it to '.env') and adjust the paths + +# absolute path to the RadonUlzer exe +RadonUlzer_RemoteControlSim_PATH=C:\Users\thaeckel\Documents\Repos\RadonUlzer\.pio\build\RemoteControlSim\program.exe + +DroidControlShip_PATH=C:\Users\thaeckel\Documents\Repos\DroidControlShip\.pio\build\LineFollowerSensorFusionSim\program.exe + +# absolute path to the space ship radar script +SpaceShipRadar_PATH=C:\Users\thaeckel\Documents\Repos\SpaceShipRadar\src\space_ship_radar\space_ship_radar.py + +# absolute path to the img folder in this repo (found under src/) +# ! should end with backslash \ +CalibrationFolder_PATH=C:\Users\thaeckel\Documents\Repos\SpaceShipRadar\src\calibration\ diff --git a/start_sensorFusion.bat b/start_sensorFusion.bat deleted file mode 100644 index 50f5952a..00000000 --- a/start_sensorFusion.bat +++ /dev/null @@ -1,177 +0,0 @@ -@echo off -setlocal ENABLEEXTENSIONS - -rem ----------------------------------------------------------- -rem Usage: -rem start_sensorFusion.bat [zumoId] -rem zumoId: 1..5 (Default: 1) -rem -rem Starts: -rem - RadonUlzer RemoteControlSim (robot-name=Zumo) -rem - DCS LineFollowerSensorFusionSim (robot-name=ZumoComSystem) -rem -rem LineFollowerSensorFusionSim is started equivalent to the -rem webots_launcher.py (with cfgFilePath + serialRx/Tx). -rem ----------------------------------------------------------- - -set "SF_DEBUG=1" - -rem --- Clean up possible leftover Webots controllers / native controllers from previous runs --- -taskkill /IM program.exe /F >nul 2>&1 -taskkill /IM webots-controller.exe /F >nul 2>&1 - - - -rem === Default Zumo-ID = 1 === -set "ZUMO_ID=%1" -if "%ZUMO_ID%"=="" set "ZUMO_ID=1" - -rem === Validate range 1..5 === -if %ZUMO_ID% LSS 1 goto invalid_zumo -if %ZUMO_ID% GTR 5 goto invalid_zumo - -echo ========================================== -echo Starting Sensor Fusion for Zumo %ZUMO_ID% -echo Debug mode: %SF_DEBUG% -echo ========================================== -echo. - -rem === Check WEBOTS_HOME === -if "%WEBOTS_HOME%"=="" ( - echo [ERROR] WEBOTS_HOME is not set. - echo Please set WEBOTS_HOME to your Webots installation directory. - echo Example: set WEBOTS_HOME=C:\Program Files\Webots - goto error_exit -) - -rem === Paths / tools === -set "WEBOTS_CONTROLLER=%WEBOTS_HOME%\msys64\mingw64\bin\webots-controller.exe" -set "PROGRAM_NAME=program.exe" - -if not exist "%WEBOTS_CONTROLLER%" ( - echo [ERROR] webots-controller.exe not found: - echo %WEBOTS_CONTROLLER% - goto error_exit -) - -rem === Root paths === -set "DCS_ROOT=%~dp0" -set "RADON_ROOT=%~dp0..\RadonUlzer" - -rem === Config file for Zumo (DCS-side) === -set "CONFIG_FILE=config_zumo%ZUMO_ID%.json" -set "DCS_CONFIG_PATH=../../../data/config/%CONFIG_FILE%" - -rem === Build paths for executables === -set "RADON_PROGRAM_PATH=%RADON_ROOT%\.pio\build\RemoteControlSim" -set "DCS_PROGRAM_PATH=%DCS_ROOT%\.pio\build\LineFollowerSensorFusionSim" - -if "%SF_DEBUG%"=="1" ( - echo [DEBUG] WEBOTS_CONTROLLER = %WEBOTS_CONTROLLER% - echo [DEBUG] RADON_ROOT = %RADON_ROOT% - echo [DEBUG] RADON_PROGRAM_PATH = %RADON_PROGRAM_PATH% - echo [DEBUG] DCS_ROOT = %DCS_ROOT% - echo [DEBUG] DCS_PROGRAM_PATH = %DCS_PROGRAM_PATH% - echo [DEBUG] DCS_CONFIG_PATH = %DCS_CONFIG_PATH% - echo. -) - -rem ----------------------------------------------------------- -rem 1) RadonUlzer: build & start RemoteControlSim -rem ----------------------------------------------------------- -pushd "%RADON_ROOT%" -echo [RadonUlzer] Building RemoteControlSim ... -"%USERPROFILE%\.platformio\penv\Scripts\pio.exe" run --environment RemoteControlSim -if errorlevel 1 ( - echo [RadonUlzer] Build failed. Aborting. - popd - goto error_exit -) - -echo [RadonUlzer] Starting RemoteControlSim for Zumo %ZUMO_ID% ... -echo [DEBUG] Command: -echo "%WEBOTS_CONTROLLER%" --robot-name=Zumo --stdout-redirect "%RADON_PROGRAM_PATH%\%PROGRAM_NAME%" -c --supervisorRxCh 1 --supervisorTxCh 2 --serialRxCh 3 --serialTxCh 4 --settingsPath "%RADON_ROOT%\settings\settings.json" -v - -start "" /B ^ - "%WEBOTS_CONTROLLER%" ^ - --robot-name=Zumo ^ - --stdout-redirect "%RADON_PROGRAM_PATH%\%PROGRAM_NAME%" ^ - -c ^ - --supervisorRxCh 1 ^ - --supervisorTxCh 2 ^ - --serialRxCh 3 ^ - --serialTxCh 4 ^ - --settingsPath "%RADON_ROOT%\settings\settings.json" ^ - -v - -popd -echo. - -rem ----------------------------------------------------------- -rem 2) DCS: build & start LineFollowerSensorFusionSim -rem → exakt wie webots_launcher.py, nur mit parametrisierter config_zumoX -rem ----------------------------------------------------------- -pushd "%DCS_ROOT%" -echo [DCS] Building LineFollowerSensorFusionSim ... -"%USERPROFILE%\.platformio\penv\Scripts\pio.exe" run --environment LineFollowerSensorFusionSim -if errorlevel 1 ( - echo [DCS] Build failed. Aborting. - popd - goto error_exit -) - -echo [DCS] Starting LineFollowerSensorFusionSim for Zumo %ZUMO_ID% ... - -rem ======================= -rem Webots launcher pattern -rem ======================= -rem On Windows: -rem WEBOTS_CONTROLLER = ".../webots-controller.exe" -rem ROBOT_NAME = ZumoComSystem (from platformio.ini: webots_robot_name) -rem PROGRAM = .pio\build\LineFollowerSensorFusionSim\program.exe -rem PROGRAM_OPTIONS = --cfgFilePath "../../../data/config/config_zumoX.json" -rem --serialRxCh -rem --serialTxCh -rem -v -rem Everything AFTER program.exe is passed to program.exe (not to Webots). - -set "DCS_ROBOT_NAME=ZumoComSystem" -set "DCS_SERIAL_RX_CH=4" -set "DCS_SERIAL_TX_CH=3" - -if "%SF_DEBUG%"=="1" ( - echo [DEBUG] Command (DCS) - echo "%WEBOTS_CONTROLLER%" --robot-name=%DCS_ROBOT_NAME% --stdout-redirect "%DCS_PROGRAM_PATH%\%PROGRAM_NAME%" --cfgFilePath "%DCS_CONFIG_PATH%" --serialRxCh %DCS_SERIAL_RX_CH% --serialTxCh %DCS_SERIAL_TX_CH% -v - echo. -) - -"%WEBOTS_CONTROLLER%" --robot-name=%DCS_ROBOT_NAME% --stdout-redirect "%DCS_PROGRAM_PATH%\%PROGRAM_NAME%" --cfgFilePath "%DCS_CONFIG_PATH%" --serialRxCh %DCS_SERIAL_RX_CH% --serialTxCh %DCS_SERIAL_TX_CH% -v - -echo. -echo [DCS] LineFollowerSensorFusionSim terminated (see output above). -popd -echo. - -echo ========================================== -echo Script finished (see above output) -echo ========================================== -if "%SF_DEBUG%"=="1" ( - echo. - echo Press any key to close this window... - pause >nul -) - -endlocal -exit /b 0 - -:invalid_zumo -echo [ERROR] Invalid Zumo ID "%ZUMO_ID%". Use 1..5. -pause -endlocal -exit /b 1 - -:error_exit -echo. -echo Script aborted due to errors. -endlocal -exit /b 2 diff --git a/start_sensorFusion.ps1 b/start_sensorFusion.ps1 new file mode 100644 index 00000000..bd941659 --- /dev/null +++ b/start_sensorFusion.ps1 @@ -0,0 +1,80 @@ +$envFilePath = ".\.env" + +$content = Get-Content -Path $envFilePath + +if (-Not $content) { + Write-Output "Create a file with named '.env' (See README)" + Exit +} + +$RadonUlzerfilePathLine = $content | Where-Object { $_ -match '^RadonUlzer_RemoteControlSim_PATH=' } +if ($RadonUlzerfilePathLine) { + $RadonUlzerFilePath = $RadonUlzerfilePathLine -split '=' | Select-Object -Last 1 + Write-Output "Der Dateipfad ist: $RadonUlzerFilePath" +} else { + Write-Output "Key RadonUlzer_RemoteControlSim_PATH not found" + Exit +} + +$SpaceShipRadarfilePathLine = $content | Where-Object { $_ -match '^SpaceShipRadar_PATH=' } +if ($SpaceShipRadarfilePathLine) { + $SpaceShipRadarFilePath = $SpaceShipRadarfilePathLine -split '=' | Select-Object -Last 1 + Write-Output "Der Dateipfad ist: $SpaceShipRadarFilePath" +} else { + Write-Output "Key SpaceShipRadar_PATH not found" + Exit +} + +$DCSfilePathLine = $content | Where-Object { $_ -match '^DroidControlShip_PATH=' } +if ($DCSfilePathLine) { + $DCSFilePath = $DCSfilePathLine -split '=' | Select-Object -Last 1 + Write-Output "Der Dateipfad ist: $DCSFilePath" +} else { + Write-Output "Key DroidControlShip_PATH not found" + Exit +} + +# Pfad zum webots-controller +$webotsController = Join-Path $env:WEBOTS_HOME "msys64\mingw64\bin\webots-controller.exe" + +# ===== RadonUlzer (Zumo / RemoteControlSim) ================================== +# Entspricht deinem PIO-Build-Aufruf: +# --robot-name=Zumo --stdout-redirect -c --supervisorRxCh 1 ... +Start-Process -FilePath $webotsController ` + -ArgumentList @( + "--robot-name=Zumo", + "--stdout-redirect", $RadonUlzerFilePath, + "-c", + "--supervisorRxCh", "1", + "--supervisorTxCh", "2", + "--serialRxCh", "3", + "--serialTxCh", "4", + "--settingsPath", "C:\Users\thaeckel\Documents\Repos\RadonUlzer\./settings/settings.json", + "-v" + ) + +# ===== SpaceShipRadar (MyBot) ================================================ +Start-Process -FilePath $webotsController ` + -ArgumentList @( + "--robot-name=MyBot", + "--stdout-redirect", $SpaceShipRadarFilePath + ) + +# ===== DroidControlShip / LineFollowerSensorFusionSim ======================== +# Hier spiegeln wir den PIO-Aufruf: +# --robot-name=ZumoComSystem --stdout-redirect \ +# --cfgFilePath "../../../data/config/config.json" --serialRxCh 4 --serialTxCh 3 -v + +# Arbeitsverzeichnis so setzen, dass der relative cfg-Pfad passt +$DCSWorkDir = Split-Path $DCSFilePath + +Start-Process -FilePath $webotsController ` + -WorkingDirectory $DCSWorkDir ` + -ArgumentList @( + "--robot-name=ZumoComSystem", + "--stdout-redirect", $DCSFilePath, + "--cfgFilePath", "../../../data/config/config_zumo1.json", + "--serialRxCh", "4", + "--serialTxCh", "3", + "-v" + ) From 664087bd0d0906b8f1a3c99a2db8e18e9cf1bcb5 Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Fri, 5 Dec 2025 14:32:12 +0100 Subject: [PATCH 03/14] Working Fusion, but Vehicle Data is Dominating over SSR --- lib/APPLineFollowerSensorFusion/src/App.cpp | 61 +++-- .../src/EKF.cpp | 179 +++++++------- .../src/EKF.h | 222 +++++++++--------- .../src/SensorConstants.h | 77 ++++++ .../src/TimeSync.cpp | 19 ++ .../src/TimeSync.h | 11 + 6 files changed, 336 insertions(+), 233 deletions(-) rename lib/{Service => APPLineFollowerSensorFusion}/src/EKF.cpp (68%) rename lib/{Service => APPLineFollowerSensorFusion}/src/EKF.h (50%) create mode 100644 lib/APPLineFollowerSensorFusion/src/SensorConstants.h diff --git a/lib/APPLineFollowerSensorFusion/src/App.cpp b/lib/APPLineFollowerSensorFusion/src/App.cpp index fdbbcd56..4db74d18 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.cpp +++ b/lib/APPLineFollowerSensorFusion/src/App.cpp @@ -340,25 +340,26 @@ void App::ssrTopicCallback(const String& payload) JsonVariantConst xVel_mms = jsonPayload["speedX"]; /* int : in mm/s */ JsonVariantConst yVel_mms = jsonPayload["speedY"]; /* int : in mm/s */ JsonVariantConst angle_mrad = jsonPayload["angle"]; /* int : in mrad */ - JsonVariantConst id = jsonPayload["identifier"]; /* int : unique id of the target */ + JsonVariantConst timestamp_ms = jsonPayload["timestamp_ms"]; /* int : epoch in ms */ const int x_mm_i = xPos_mm.as(); const int y_mm_i = yPos_mm.as(); const int vx_mms_i = xVel_mms.as(); const int vy_mms_i = yVel_mms.as(); const int ang_mrad_i = angle_mrad.as(); + const uint64_t timestampEpochMs = timestamp_ms.as(); - LOG_INFO("SSR pose: xPos=%dmm yPos=%dmm angle=%dmrad vx=%dmm/s vy=%dmm/s", - x_mm_i, y_mm_i, ang_mrad_i, vx_mms_i, vy_mms_i); + LOG_INFO("SSR pose: xPos=%dmm yPos=%dmm angle=%dmrad vx=%dmm/s vy=%dmm/s ts_epoch_ms=%llu", + x_mm_i, y_mm_i, ang_mrad_i, vx_mms_i, vy_mms_i, timestampEpochMs); /* 1) Copy pose into struct. */ SpaceShipRadarPose ssrPose; - ssrPose.timestamp = static_cast(m_timeSync.localNowMs()); ssrPose.x = static_cast(x_mm_i); ssrPose.y = static_cast(y_mm_i); ssrPose.theta = static_cast(ang_mrad_i); ssrPose.v_x = static_cast(vx_mms_i); ssrPose.v_y = static_cast(vy_mms_i); + ssrPose.timestamp = static_cast(m_timeSync.epochToLocalMs(timestampEpochMs)); /* 2) Initialize odometry origin from first SSR pose. */ if (false == m_odoOriginInitialized) @@ -473,19 +474,20 @@ void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRada return; } - const uint32_t zumoTs32 = static_cast(vehicleData.timestamp); - const uint32_t ssrTs32 = static_cast(ssrPose.timestamp); + const uint32_t zumoTs32 = static_cast(vehicleData.timestamp); + const uint32_t zumoLocalMs32 = static_cast(m_timeSync.mapZumoToLocalMs(zumoTs32)); + const uint32_t ssrLocalMs32 = static_cast(ssrPose.timestamp); /* If EKF has no valid timestamp yet, initialize from the first available one. */ if (0U == m_lastEkfUpdateMs) { - if (0U != zumoTs32) + if (0U != zumoLocalMs32) { - m_lastEkfUpdateMs = zumoTs32; + m_lastEkfUpdateMs = zumoLocalMs32; } - else if (0U != ssrTs32) + else if (0U != ssrLocalMs32) { - m_lastEkfUpdateMs = ssrTs32; + m_lastEkfUpdateMs = ssrLocalMs32; } else { @@ -502,17 +504,17 @@ void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRada }; Source newestSource = Source::None; - uint32_t newestTs = m_lastEkfUpdateMs; + uint32_t newestLocalTs = m_lastEkfUpdateMs; - if (zumoTs32 > newestTs) + if (zumoLocalMs32 > newestLocalTs) { - newestTs = zumoTs32; + newestLocalTs = zumoLocalMs32; newestSource = Source::Vehicle; } - if (ssrTs32 > newestTs) + if (ssrLocalMs32 > newestLocalTs) { - newestTs = ssrTs32; + newestLocalTs = ssrLocalMs32; newestSource = Source::SSR; } @@ -523,32 +525,31 @@ void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRada } /* Time difference in seconds for prediction. */ - const uint32_t dtMs = newestTs - m_lastEkfUpdateMs; + const uint32_t dtMs = newestLocalTs - m_lastEkfUpdateMs; const float dt = static_cast(dtMs) / 1000.0F; - /* Longitudinal acceleration as input to the process model (mm/s^2). */ + /* Longitudinal acceleration input (raw accelerometer digits). */ const float a_x = static_cast(vehicleData.accelerationX); /* EKF prediction step. */ m_ekf.predict(a_x, dt); + LOG_INFO("EKF Source =%s dt=%.3fs", (Source::Vehicle == newestSource) ? "Vehicle" : "SSR", dt); + /* Measurement update depending on source. */ if (Source::Vehicle == newestSource) { /* IMU update: only yaw rate (turnRateZ). */ { - ImuMeasVector z_imu; - z_imu(0) = static_cast(vehicleData.turnRateZ); /* [mrad/s] */ - m_ekf.updateImu(z_imu); + const int16_t rawGyroZ = static_cast(vehicleData.turnRateZ); + m_ekf.updateImuFromDigits(rawGyroZ); } /* Odometry update in global frame. * * EKF odometryModel(x): - * z_odo(0) = p_x - * z_odo(1) = p_y - * z_odo(2) = v - * z_odo(3) = theta + * z_odo(0) = v + * z_odo(1) = theta */ { float xGlob_mm = 0.0F; @@ -558,10 +559,8 @@ void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRada transformOdometryToGlobal(vehicleData, xGlob_mm, yGlob_mm, thetaGlob_mrad); OdoMeasVector z_odo; - z_odo(0) = xGlob_mm; /* global p_x [mm] */ - z_odo(1) = yGlob_mm; /* global p_y [mm] */ - z_odo(2) = static_cast(vehicleData.center); /* speed [mm/s] (v_center) */ - z_odo(3) = thetaGlob_mrad; /* global theta [mrad] */ + z_odo(0) = static_cast(vehicleData.center); /* speed [mm/s] (v_center) */ + z_odo(1) = thetaGlob_mrad; /* global theta [mrad] */ m_ekf.updateOdometry(z_odo); } @@ -588,10 +587,10 @@ void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRada } /* Update timestamp of last EKF update. */ - m_lastEkfUpdateMs = newestTs; + m_lastEkfUpdateMs = newestLocalTs; /* Publish fused pose. */ - publishFusionPose(newestTs); + publishFusionPose(newestLocalTs); } void App::transformOdometryToGlobal(const VehicleData& vehicleData, @@ -601,7 +600,7 @@ void App::transformOdometryToGlobal(const VehicleData& vehicleData, { /* Y axis and heading sign differ between local odometry frame and SSR frame. */ constexpr float Y_SIGN = -1.0F; - constexpr float THETA_SIGN = -1.0F; + constexpr float THETA_SIGN = 1.0F; const float xLocal_mm = static_cast(vehicleData.xPos); const float yLocal_mm = static_cast(vehicleData.yPos); diff --git a/lib/Service/src/EKF.cpp b/lib/APPLineFollowerSensorFusion/src/EKF.cpp similarity index 68% rename from lib/Service/src/EKF.cpp rename to lib/APPLineFollowerSensorFusion/src/EKF.cpp index 4b8658b5..25a9794f 100644 --- a/lib/Service/src/EKF.cpp +++ b/lib/APPLineFollowerSensorFusion/src/EKF.cpp @@ -25,7 +25,7 @@ DESCRIPTION *******************************************************************************/ /** - * @brief Implementation of the Extended Kalman Filter (5D state) + * @brief Implementation of the Extended Kalman Filter * @author Tobias Haeckel */ @@ -34,47 +34,38 @@ *****************************************************************************/ #include "EKF.h" #include - -/* Some toolchains do not define M_PI. */ -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - -/****************************************************************************** - * Local Namespace - *****************************************************************************/ +#include "SensorConstants.h" namespace { - constexpr float PI_MRAD = 1000.0F * static_cast(M_PI); - constexpr float TWO_PI_MRAD = 2.0F * PI_MRAD; - - /* Default process noise standard deviations. */ - constexpr float SIGMA_PX = 1.0F; /* [mm] */ - constexpr float SIGMA_PY = 1.0F; /* [mm] */ - constexpr float SIGMA_THETA_P = 5.0F; /* [mrad] */ - constexpr float SIGMA_V_P = 5.0F; /* [mm/s] */ - constexpr float SIGMA_OMEGA_P = 2.0F; /* [mrad/s] */ - - /* Camera (SSR) measurement noise std dev. */ - constexpr float SIGMA_CAM_POS = 2.0F; /* [mm] */ - constexpr float SIGMA_CAM_THETA = 5.0F; /* [mrad] */ - constexpr float SIGMA_CAM_V = 20.0F; /* [mm/s] */ - - /* Odometry measurement noise std dev. */ - constexpr float SIGMA_ODO_POS = 100000.0F; /* [mm] */ - constexpr float SIGMA_ODO_V = 10.0F; /* [mm/s] */ - constexpr float SIGMA_ODO_THETA = 5.0F; /* [mrad] */ - - /* IMU yaw noise std dev. */ - constexpr float SIGMA_IMU_OMEGA = 20.0F; /* [mrad/s] */ - - /* Default initial state (will usually be overwritten by SSR init). */ - constexpr float EKF_START_X_MM = 705.0F; /* [mm] */ - constexpr float EKF_START_Y_MM = 939.0F; /* [mm] */ - constexpr float EKF_START_THETA_MRAD = 0.0F; /* [mrad] */ - constexpr float EKF_START_V_MMS = 0.0F; /* [mm/s] */ - constexpr float EKF_START_OMEGA_MRADPS = 0.0F; /* [mrad/s] */ + constexpr float PI_MRAD = 1000.0F * static_cast(M_PI); + constexpr float TWO_PI_MRAD = 2.0F * PI_MRAD; + + // Default process noise standard deviations. + constexpr float SIGMA_PX = 1.0F; // [mm] + constexpr float SIGMA_PY = 1.0F; // [mm] + constexpr float SIGMA_THETA_P = 5.0F; // [mrad] + constexpr float SIGMA_V_P = 5.0F; // [mm/s] + constexpr float SIGMA_OMEGA_P = 2.0F; // [mrad/s] + + // Camera noise. + constexpr float SIGMA_CAM_POS = 2.0F; // [mm] + constexpr float SIGMA_CAM_THETA = 5.0F; // [mrad] + constexpr float SIGMA_CAM_V = 20.0F; // [mm/s] + + // Odometry noise (only v and theta). + constexpr float SIGMA_ODO_V = 10.0F; // [mm/s] + constexpr float SIGMA_ODO_THETA = 5.0F; // [mrad] + + // IMU yaw noise. + constexpr float SIGMA_IMU_OMEGA = 1.0F; // [mrad/s] + + // Default initial state. + constexpr float EKF_START_X_MM = 705.0F; // [mm] + constexpr float EKF_START_Y_MM = 939.0F; // [mm] + constexpr float EKF_START_THETA_MRAD = 0.0F; // [mrad] + constexpr float EKF_START_V_MMS = 0.0F; // [mm/s] + constexpr float EKF_START_OMEGA_MRADPS = 0.0F; // [mrad/s] } /****************************************************************************** @@ -105,10 +96,8 @@ ExtendedKalmanFilter5D::ExtendedKalmanFilter5D() /* Odometry measurement noise R_odo. */ m_R_odo.setZero(); - m_R_odo(0, 0) = SIGMA_ODO_POS * SIGMA_ODO_POS; - m_R_odo(1, 1) = SIGMA_ODO_POS * SIGMA_ODO_POS; - m_R_odo(2, 2) = SIGMA_ODO_V * SIGMA_ODO_V; - m_R_odo(3, 3) = SIGMA_ODO_THETA * SIGMA_ODO_THETA; + m_R_odo(0, 0) = SIGMA_ODO_V * SIGMA_ODO_V; + m_R_odo(1, 1) = SIGMA_ODO_THETA * SIGMA_ODO_THETA; /* IMU yaw noise R_imu. */ m_R_imu.setZero(); @@ -125,27 +114,30 @@ bool ExtendedKalmanFilter5D::init(const StateVector& x0, const StateMatrix& P0) bool ExtendedKalmanFilter5D::init() { m_state.setZero(); - - m_state(0) = EKF_START_X_MM; /* p_x [mm] */ - m_state(1) = EKF_START_Y_MM; /* p_y [mm] */ - m_state(2) = EKF_START_THETA_MRAD; /* theta [mrad] */ - m_state(3) = EKF_START_V_MMS; /* v [mm/s] */ - m_state(4) = EKF_START_OMEGA_MRADPS; /* omega [mrad/s] */ + m_state(0) = EKF_START_X_MM; + m_state(1) = EKF_START_Y_MM; + m_state(2) = EKF_START_THETA_MRAD; + m_state(3) = EKF_START_V_MMS; + m_state(4) = EKF_START_OMEGA_MRADPS; m_covariance.setIdentity(); return true; } -void ExtendedKalmanFilter5D::predict(float a_x, float dt) +void ExtendedKalmanFilter5D::predict(float accX_raw, float dt) { + /* Convert raw accelerometer digits to physical acceleration [mm/s^2]. */ + const float a_x_mms = + accX_raw * SensorConstants::ACCELEROMETER_SENSITIVITY_FACTOR; + /* Nonlinear prediction. */ - StateVector x_pred = processModel(m_state, a_x, dt); + StateVector x_pred = processModel(m_state, a_x_mms, dt); /* Wrap heading angle (index 2). */ x_pred(2) = wrapAngleMrad(x_pred(2)); /* Linearization. */ - StateMatrix jacobianF = processJacobianF(m_state, a_x, dt); + StateMatrix jacobianF = processJacobianF(m_state, dt); /* Covariance prediction. */ StateMatrix P_pred = jacobianF * m_covariance * jacobianF.transpose() + m_Q; @@ -186,8 +178,8 @@ void ExtendedKalmanFilter5D::updateOdometry(const OdoMeasVector& z_odo) /* Innovation. */ OdoMeasVector y = z_odo - z_pred; - /* Wrap angle innovation (index 3 is theta). */ - y(3) = wrapAngleMrad(y(3)); + /* Wrap angle innovation (index 1 is theta). */ + y(1) = wrapAngleMrad(y(1)); /* EKF update. */ const OdoMeasMatrix S = H * m_covariance * H.transpose() + m_R_odo; @@ -218,18 +210,30 @@ void ExtendedKalmanFilter5D::updateImu(const ImuMeasVector& z_imu) m_covariance = (StateMatrix::Identity() - K * H) * m_covariance; } +void ExtendedKalmanFilter5D::updateImuFromDigits(int16_t rawGyroZ) +{ + ImuMeasVector z_imu; + /* Convert raw gyro digits to physical yaw rate [mrad/s]. */ + z_imu(0) = static_cast(rawGyroZ) * + SensorConstants::GYRO_SENSITIVITY_FACTOR; + + updateImu(z_imu); +} + /****************************************************************************** * Private Methods *****************************************************************************/ ExtendedKalmanFilter5D::StateVector -ExtendedKalmanFilter5D::processModel(const StateVector& x, float a_x, float dt) const +ExtendedKalmanFilter5D::processModel(const StateVector& x, + float a_x_mms, + float dt) const { const float px = x(0); const float py = x(1); const float thetaMrad = x(2); const float v = x(3); - const float omegaMrad = x(4); + const float omegaMrad = x(4); /* already in mrad/s */ const float thetaRad = thetaMrad / 1000.0F; @@ -237,35 +241,35 @@ ExtendedKalmanFilter5D::processModel(const StateVector& x, float a_x, float dt) x_next(0) = px + v * std::cos(thetaRad) * dt; /* p_x */ x_next(1) = py + v * std::sin(thetaRad) * dt; /* p_y */ x_next(2) = thetaMrad + omegaMrad * dt; /* theta [mrad] */ - x_next(3) = v + a_x * dt; /* v */ - x_next(4) = omegaMrad; /* omega (no angular accel) */ + x_next(3) = v + a_x_mms * dt; /* v */ + x_next(4) = omegaMrad; /* omega */ return x_next; } ExtendedKalmanFilter5D::StateMatrix -ExtendedKalmanFilter5D::processJacobianF(const StateVector& x, float /*a_x*/, float dt) const +ExtendedKalmanFilter5D::processJacobianF(const StateVector& x, float dt) const { const float thetaMrad = x(2); const float v = x(3); const float thetaRad = thetaMrad / 1000.0F; - StateMatrix jacobianF = StateMatrix::Identity(); + StateMatrix F = StateMatrix::Identity(); - /* d p_x / d theta */ - jacobianF(0, 2) = -v * std::sin(thetaRad) * dt / 1000.0F; - /* d p_x / d v */ - jacobianF(0, 3) = std::cos(thetaRad) * dt; + /* d p_x / d theta. */ + F(0, 2) = -v * std::sin(thetaRad) * dt / 1000.0F; + /* d p_x / d v. */ + F(0, 3) = std::cos(thetaRad) * dt; - /* d p_y / d theta */ - jacobianF(1, 2) = v * std::cos(thetaRad) * dt / 1000.0F; - /* d p_y / d v */ - jacobianF(1, 3) = std::sin(thetaRad) * dt; + /* d p_y / d theta. */ + F(1, 2) = v * std::cos(thetaRad) * dt / 1000.0F; + /* d p_y / d v. */ + F(1, 3) = std::sin(thetaRad) * dt; - /* d theta / d omega */ - jacobianF(2, 4) = dt; + /* d theta / d omega. */ + F(2, 4) = dt; - return jacobianF; + return F; } /* ---------------- Camera model ---------------- */ @@ -281,11 +285,11 @@ ExtendedKalmanFilter5D::cameraModel(const StateVector& x) const const float thetaRad = thetaMrad / 1000.0F; CamMeasVector z; - z(0) = px; /* p_x */ - z(1) = py; /* p_y */ - z(2) = thetaMrad; /* theta [mrad] */ - z(3) = v * std::cos(thetaRad);/* v_x */ - z(4) = v * std::sin(thetaRad);/* v_y */ + z(0) = px; /* p_x */ + z(1) = py; /* p_y */ + z(2) = thetaMrad; /* theta [mrad] */ + z(3) = v * std::cos(thetaRad); /* v_x */ + z(4) = v * std::sin(thetaRad); /* v_y */ return z; } @@ -323,10 +327,8 @@ ExtendedKalmanFilter5D::OdoMeasVector ExtendedKalmanFilter5D::odometryModel(const StateVector& x) const { OdoMeasVector z; - z(0) = x(0); /* p_x */ - z(1) = x(1); /* p_y */ - z(2) = x(3); /* v */ - z(3) = x(2); /* theta */ + z(0) = x(3); /* v */ + z(1) = x(2); /* theta */ return z; } @@ -336,14 +338,10 @@ ExtendedKalmanFilter5D::odometryJacobianH(const StateVector& /*x*/) const Eigen::Matrix H; H.setZero(); - /* p_x */ - H(0, 0) = 1.0F; - /* p_y */ - H(1, 1) = 1.0F; /* v */ - H(2, 3) = 1.0F; + H(0, 3) = 1.0F; /* theta */ - H(3, 2) = 1.0F; + H(1, 2) = 1.0F; return H; } @@ -377,13 +375,6 @@ float ExtendedKalmanFilter5D::wrapAngleMrad(float angleMrad) { x += TWO_PI_MRAD; } + return x - PI_MRAD; } - -/****************************************************************************** - * External Functions - *****************************************************************************/ - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/Service/src/EKF.h b/lib/APPLineFollowerSensorFusion/src/EKF.h similarity index 50% rename from lib/Service/src/EKF.h rename to lib/APPLineFollowerSensorFusion/src/EKF.h index 1a9ef661..cfdcf30c 100644 --- a/lib/Service/src/EKF.h +++ b/lib/APPLineFollowerSensorFusion/src/EKF.h @@ -29,34 +29,30 @@ * @author Tobias Haeckel * * State vector: - * x = [p_x, p_y, theta, v, omega]^T - * p_x, p_y : position in mm - * theta : heading in mrad - * v : linear velocity in mm/s - * omega : yaw rate in mrad/s + * x = [ p_x, p_y, theta, v, omega ]^T * - * @addtogroup Application + * Units: + * p_x, p_y : [mm] + * theta : [mrad] + * v : [mm/s] + * omega : [mrad/s] * - * @{ + * Measurement models: + * - Camera (SSR): absolute pose and Cartesian velocity in world frame + * - Odometry: longitudinal velocity and heading + * - IMU: yaw rate (simplified model, can be extended later) */ #ifndef EKF_H #define EKF_H -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - /****************************************************************************** * Includes *****************************************************************************/ + #include #include -/****************************************************************************** - * Macros - *****************************************************************************/ - /****************************************************************************** * Types and Classes *****************************************************************************/ @@ -68,208 +64,223 @@ class ExtendedKalmanFilter5D { public: /** State dimension. */ - static constexpr uint8_t STATE_DIM = 5U; - /** Camera measurement dimension. */ - static constexpr uint8_t CAM_MEAS_DIM = 5U; - /** Odometry measurement dimension. */ - static constexpr uint8_t ODO_MEAS_DIM = 4U; - /** IMU measurement dimension. */ - static constexpr uint8_t IMU_MEAS_DIM = 1U; - - /** State vector type: [p_x, p_y, theta, v, omega]^T. */ - using StateVector = Eigen::Matrix; - /** State covariance matrix type. */ - using StateMatrix = Eigen::Matrix; - - /** Camera measurement vector type. */ + static constexpr std::uint8_t STATE_DIM = 5U; + /** Camera measurement dimension: [x, y, theta, v_x, v_y]. */ + static constexpr std::uint8_t CAM_MEAS_DIM = 5U; + /** Odometry measurement dimension: [v, theta]. */ + static constexpr std::uint8_t ODO_MEAS_DIM = 2U; + /** IMU measurement dimension: [omega]. */ + static constexpr std::uint8_t IMU_MEAS_DIM = 1U; + + using StateVector = Eigen::Matrix; + using StateMatrix = Eigen::Matrix; + using CamMeasVector = Eigen::Matrix; - /** Camera measurement covariance matrix type. */ using CamMeasMatrix = Eigen::Matrix; - /** Odometry measurement vector type. */ using OdoMeasVector = Eigen::Matrix; - /** Odometry measurement covariance matrix type. */ using OdoMeasMatrix = Eigen::Matrix; - /** IMU measurement vector type. */ using ImuMeasVector = Eigen::Matrix; - /** IMU measurement covariance matrix type. */ using ImuMeasMatrix = Eigen::Matrix; +public: /** - * @brief Construct the EKF with default noise matrices. + * Constructs the EKF with default noise parameters and zero-initialized state. */ ExtendedKalmanFilter5D(); /** - * @brief Initialize filter with a given state and covariance. + * Initializes the EKF with a given state and covariance. * - * @param[in] x0 Initial state vector. + * @param[in] x0 Initial state vector (physical units). * @param[in] P0 Initial covariance matrix. * - * @return true on success. + * @return True if successful, otherwise false. */ bool init(const StateVector& x0, const StateMatrix& P0); /** - * @brief Initialize filter with default state and identity covariance. + * Initializes the EKF with a fixed default start pose and identity covariance. * - * Default state is configured in EKF.cpp. + * State is initialized to: + * p_x = EKF_START_X_MM + * p_y = EKF_START_Y_MM + * theta = EKF_START_THETA_MRAD + * v = 0 + * omega = 0 * - * @return true on success. + * @return True if successful, otherwise false. */ bool init(); /** - * @brief Prediction step of the EKF. + * EKF prediction step. + * + * Nonlinear state propagation using the motion model: + * p_x += v cos(theta) dt + * p_y += v sin(theta) dt + * theta += omega dt + * v += a_x dt + * omega = omega (no angular acceleration modeled) * - * State model: - * p_x(k+1) = p_x + v cos(theta) dt - * p_y(k+1) = p_y + v sin(theta) dt - * theta(k+1) = theta + omega dt - * v(k+1) = v + a_x dt - * omega(k+1) = omega + * @param[in] accX_raw Raw longitudinal acceleration from IMU [digits]. + * @param[in] dt Time step [s]. * - * @param[in] a_x Longitudinal acceleration in mm/s^2. - * @param[in] dt Time step in seconds. + * The raw acceleration is internally converted into [mm/s^2]. */ - void predict(float a_x, float dt); + void predict(float accX_raw, float dt); /** - * @brief Camera / Space Ship Radar measurement update. + * EKF update step for camera (SSR) measurements. * - * Measurement model: - * z_cam(0) = p_x - * z_cam(1) = p_y - * z_cam(2) = theta [mrad] - * z_cam(3) = v_x - * z_cam(4) = v_y + * Camera measurement: + * z_cam = [p_x, p_y, theta, v_x, v_y]^T + v_cam + * + * All quantities are expected in the same unit system as the state. * * @param[in] z_cam Camera measurement vector. */ void updateCamera(const CamMeasVector& z_cam); /** - * @brief Odometry measurement update. + * EKF update step for odometry measurements. + * + * Odometry measurement (drift-reduced): + * z_odo = [v_odo, theta_odo]^T * * Measurement model: - * z_odo(0) = p_x - * z_odo(1) = p_y - * z_odo(2) = v - * z_odo(3) = theta + * h_odo(x) = [ v, theta ]^T * * @param[in] z_odo Odometry measurement vector. */ void updateOdometry(const OdoMeasVector& z_odo); /** - * @brief IMU yaw-rate measurement update. + * EKF update step for IMU measurements (physical units). + * + * IMU measurement (simplified): + * z_imu = [omega]^T, omega in [mrad/s] * * Measurement model: - * z_imu(0) = omega + * h_imu(x) = [ omega ]^T * - * @param[in] z_imu IMU measurement vector. + * @param[in] z_imu IMU measurement vector (omega in mrad/s). */ void updateImu(const ImuMeasVector& z_imu); /** - * @brief Get current state estimate. + * EKF update step for IMU yaw rate using raw gyro digits. + * + * The raw gyro Z-axis value is internally converted to [mrad/s] + * using SensorConstants::GYRO_SENSITIVITY_FACTOR. + * + * @param[in] rawGyroZ Raw gyroscope value around Z [digits]. + */ + void updateImuFromDigits(int16_t rawGyroZ); + + /** + * Get current state estimate. * - * @return const reference to internal state vector. + * @return Const reference to state vector. */ const StateVector& getState() const { return m_state; } + /** + * Get current covariance estimate. + * + * @return Const reference to covariance matrix. + */ + const StateMatrix& getCovariance() const + { + return m_covariance; + } + private: - /** State vector. */ + /** Current state estimate. */ StateVector m_state; - - /** State covariance matrix. */ + /** Current covariance estimate. */ StateMatrix m_covariance; - /** Process noise covariance matrix. */ - StateMatrix m_Q; - - /** Camera measurement noise covariance matrix. */ + /** Process noise covariance Q. */ + StateMatrix m_Q; + /** Camera measurement noise covariance R_cam. */ CamMeasMatrix m_R_cam; - - /** Odometry measurement noise covariance matrix. */ + /** Odometry measurement noise covariance R_odo. */ OdoMeasMatrix m_R_odo; - - /** IMU measurement noise covariance matrix. */ + /** IMU measurement noise covariance R_imu. */ ImuMeasMatrix m_R_imu; +private: /** - * @brief Nonlinear process model. + * Nonlinear process model f(x, a_x, dt). * - * @param[in] x Current state. - * @param[in] a_x Longitudinal acceleration in mm/s^2. - * @param[in] dt Time step in seconds. + * @param[in] x Current state (physical units). + * @param[in] a_x_mms Longitudinal acceleration [mm/s^2]. + * @param[in] dt Time step [s]. * * @return Predicted next state. */ - StateVector processModel(const StateVector& x, float a_x, float dt) const; + StateVector processModel(const StateVector& x, float a_x_mms, float dt) const; /** - * @brief Jacobian of the process model w.r.t. the state. + * Jacobian of the process model F = df/dx. * - * @param[in] x Current state. - * @param[in] a_x Longitudinal acceleration in mm/s^2 (currently unused in Jacobian). - * @param[in] dt Time step in seconds. + * @param[in] x Current state. + * @param[in] dt Time step [s]. * - * @return Process Jacobian matrix F. + * @return Process Jacobian matrix. */ - StateMatrix processJacobianF(const StateVector& x, float a_x, float dt) const; + StateMatrix processJacobianF(const StateVector& x, float dt) const; /** - * @brief Camera measurement model. + * Camera measurement model h_cam(x). * * @param[in] x Current state. * - * @return Predicted camera measurement. + * @return Camera measurement prediction. */ CamMeasVector cameraModel(const StateVector& x) const; /** - * @brief Jacobian of the camera measurement model. + * Camera measurement Jacobian H_cam = dh_cam/dx. * * @param[in] x Current state. * - * @return Camera Jacobian matrix H_cam. + * @return Camera measurement Jacobian. */ Eigen::Matrix cameraJacobianH(const StateVector& x) const; /** - * @brief Odometry measurement model. + * Odometry measurement model h_odo(x) = [v, theta]^T. * * @param[in] x Current state. * - * @return Predicted odometry measurement. + * @return Odometry measurement prediction. */ OdoMeasVector odometryModel(const StateVector& x) const; /** - * @brief Jacobian of the odometry measurement model. + * Odometry measurement Jacobian H_odo = dh_odo/dx. * - * @param[in] x Current state (currently unused). - * - * @return Odometry Jacobian matrix H_odo. + * @return Odometry measurement Jacobian. */ Eigen::Matrix odometryJacobianH(const StateVector& x) const; /** - * @brief IMU measurement model. + * IMU measurement model h_imu(x) = [omega]^T. * * @param[in] x Current state. * - * @return Predicted IMU measurement. + * @return IMU measurement prediction. */ ImuMeasVector imuModel(const StateVector& x) const; /** - * @brief Jacobian of the IMU measurement model. + * IMU measurement Jacobian H_imu = dh_imu/dx. * * @param[in] x Current state (currently unused). * @@ -287,9 +298,4 @@ class ExtendedKalmanFilter5D static float wrapAngleMrad(float angleMrad); }; -/****************************************************************************** - * Functions - *****************************************************************************/ - #endif /* EKF_H */ -/** @} */ diff --git a/lib/APPLineFollowerSensorFusion/src/SensorConstants.h b/lib/APPLineFollowerSensorFusion/src/SensorConstants.h new file mode 100644 index 00000000..29cfc274 --- /dev/null +++ b/lib/APPLineFollowerSensorFusion/src/SensorConstants.h @@ -0,0 +1,77 @@ +/* MIT License + * + * Copyright (c) 2023 - 2025 Andreas Merkle + * + * 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. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Sensor specific constants + * @author Juliane Kerpe + * + * @addtogroup Application + * + * @{ + */ + +#ifndef SENSORCONSTANTS_H +#define SENSORCONSTANTS_H +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Abstracts the physical sensor constants. + */ +namespace SensorConstants +{ + /** Sensitivity Factor of the LSM303D and LSM6DS33 accelerometer in mm/s^2/digit at Range of +/- 2 g (g-force). + * Converts a raw accelerometer value in digits to mm/s^2. The value 0.061 is defined in the Data sheets of the + * LSM303D and the LSM6DS33 accelerometer. Then the value is converted from mg into mm/s^2. */ + static const float ACCELEROMETER_SENSITIVITY_FACTOR = 0.061F * 9.81F; + + /** Sensitivity Factor of the L3GD20H and LSM6DS33 gyro in mrad/s/digit at Range of +/- 500 dps (degrees per + * second). Converts a raw gyroscope value in digits to mrad/s. The sensitivity factor 20.825 mdps/digit is defined + * in the Data sheets of the L3GD20H and the LSM6DS33 gyro. Then the value is converted from mdps/digit into + * mrad/s/digit. + */ + static const float GYRO_SENSITIVITY_FACTOR = 20.825 * 2.0F * M_PI / 360.0F; +}; // namespace SensorConstants + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SENSORCONSTANTS_H */ +/** @} */ diff --git a/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp b/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp index dae77351..08b10298 100644 --- a/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp +++ b/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp @@ -273,6 +273,25 @@ void TimeSync::refreshRtcMapping() m_rtcSynced = true; } +uint64_t TimeSync::epochToLocalMs(uint64_t epochMs) const +{ + if (false == m_rtcSynced) + { + LOG_WARNING("TimeSync: epochToLocalMs() called without RTC sync, using localNowMs() as fallback."); + return localNowMs(); + } + + /* epoch = local + offset => local = epoch - offset */ + const int64_t localMs = static_cast(epochMs) - m_epochToLocalOffsetMs; + + if (localMs < 0) + { + return 0ULL; + } + + return static_cast(localMs); +} + /****************************************************************************** * Diagnostic Logging *****************************************************************************/ diff --git a/lib/APPLineFollowerSensorFusion/src/TimeSync.h b/lib/APPLineFollowerSensorFusion/src/TimeSync.h index 7d2917c5..173f05d7 100644 --- a/lib/APPLineFollowerSensorFusion/src/TimeSync.h +++ b/lib/APPLineFollowerSensorFusion/src/TimeSync.h @@ -116,6 +116,17 @@ class TimeSync */ uint64_t mapZumoToLocalMs(uint32_t zumoTsMs) const; + /** + * @brief Map epoch time in milliseconds to local milliseconds (millis()). + * + * Requires that the RTC/NTP mapping is synced (isRtcSynced() == true). + * If RTC is not synced, falls back to localNowMs(). + * + * @param epochMs Epoch time in ms (Unix). + * @return uint64_t Local time in ms. + */ + uint64_t epochToLocalMs(uint64_t epochMs) const; + /** * Get current local time [ms]. * @return Local time [ms]. From 4661eb021e06620e9a32099aa5de6dd452a10d3b Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Mon, 8 Dec 2025 14:25:00 +0100 Subject: [PATCH 04/14] Working TimeSync now --- lib/APPLineFollowerSensorFusion/src/App.cpp | 199 ++++++++++---- lib/APPLineFollowerSensorFusion/src/App.h | 18 ++ .../src/TimeSync.cpp | 259 ++++++++++-------- .../src/TimeSync.h | 202 ++++++++------ 4 files changed, 435 insertions(+), 243 deletions(-) diff --git a/lib/APPLineFollowerSensorFusion/src/App.cpp b/lib/APPLineFollowerSensorFusion/src/App.cpp index 4db74d18..ef967229 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.cpp +++ b/lib/APPLineFollowerSensorFusion/src/App.cpp @@ -46,6 +46,7 @@ #include "States/ReadyState.h" #include "States/DrivingState.h" + /****************************************************************************** * Compiler Switches *****************************************************************************/ @@ -94,6 +95,12 @@ const char* App::TOPIC_NAME_RAW_SENSORS = "zumo/sensors"; /** MQTT topic name for receiving Space Ship Radar pose. */ const char* App::TOPIC_NAME_RADAR_POSE = "ssr"; +/** MQTT topic name for host time sync request. */ +const char* App::TOPIC_NAME_HOST_TIMESYNC_REQ = "zumo/time_sync/request"; + +/** MQTT topic name for host time sync response. */ +const char* App::TOPIC_NAME_HOST_TIMESYNC_RSP = "zumo/time_sync/response"; + /** Buffer size for JSON serialization of birth / will message. */ static const uint32_t JSON_BIRTHMESSAGE_MAX_SIZE = 64U; @@ -221,6 +228,7 @@ void App::setup() /* Start network time (NTP) against host and Zumo serial ping-pong. */ m_timeSync.begin(); m_statusTimer.start(1000U); + m_hostTimeSyncTimer.start(1000U); isSuccessful = true; } } @@ -250,9 +258,15 @@ void App::loop() /* Process serial multiplexer. */ m_serMuxChannelProvider.process(); - /* Process time synchronization (NTP refresh + serial ping-pong). */ + /* Process time synchronization (serial ping-pong). */ m_timeSync.process(); + //if (m_hostTimeSyncTimer.isTimeout()) + //{ + // m_timeSync.sendHostTimeSyncRequest(m_mqttClient, TOPIC_NAME_HOST_TIMESYNC_REQ); + // m_hostTimeSyncTimer.restart(); + //} + /* Process state machine. */ m_stateMachine.process(); @@ -288,7 +302,7 @@ bool App::setupMqtt(const String& clientId, const String& brokerAddr, uint16_t b JsonDocument jsonBirthDoc; char birthMsgArray[JSON_BIRTHMESSAGE_MAX_SIZE]; String birthMessage; - const String ssrTopic = String(TOPIC_NAME_RADAR_POSE) + "/" + clientId; + const String ssrTopic = String(TOPIC_NAME_RADAR_POSE) + "/" + clientId; jsonBirthDoc["name"] = clientId.c_str(); (void)serializeJson(jsonBirthDoc, birthMsgArray); @@ -307,16 +321,22 @@ bool App::setupMqtt(const String& clientId, const String& brokerAddr, uint16_t b { LOG_FATAL("MQTT configuration could not be set."); } - /* Subscribe to Space Ship Radar topic. */ else if (false == m_mqttClient.subscribe(ssrTopic, false, [this](const String& payload) { ssrTopicCallback(payload); })) { LOG_FATAL("Could not subscribe to MQTT topic: %s.", TOPIC_NAME_RADAR_POSE); } + else if (false == + m_mqttClient.subscribe(TOPIC_NAME_HOST_TIMESYNC_RSP, true, + [this](const String& payload) { hostTimeSyncResponseCallback(payload); })) + { + LOG_FATAL("Could not subscribe to MQTT topic: %s.", TOPIC_NAME_HOST_TIMESYNC_RSP); + } else { isSuccessful = true; LOG_INFO("Subscribed to MQTT topic: %s.", ssrTopic.c_str()); + LOG_INFO("Subscribed to MQTT topic: %s.", TOPIC_NAME_HOST_TIMESYNC_RSP); } } @@ -324,6 +344,7 @@ bool App::setupMqtt(const String& clientId, const String& brokerAddr, uint16_t b return isSuccessful; } + void App::ssrTopicCallback(const String& payload) { JsonDocument jsonPayload; @@ -335,33 +356,37 @@ void App::ssrTopicCallback(const String& payload) } else { - JsonVariantConst xPos_mm = jsonPayload["positionX"]; /* int : in mm */ - JsonVariantConst yPos_mm = jsonPayload["positionY"]; /* int : in mm */ - JsonVariantConst xVel_mms = jsonPayload["speedX"]; /* int : in mm/s */ - JsonVariantConst yVel_mms = jsonPayload["speedY"]; /* int : in mm/s */ - JsonVariantConst angle_mrad = jsonPayload["angle"]; /* int : in mrad */ - JsonVariantConst timestamp_ms = jsonPayload["timestamp_ms"]; /* int : epoch in ms */ - - const int x_mm_i = xPos_mm.as(); - const int y_mm_i = yPos_mm.as(); - const int vx_mms_i = xVel_mms.as(); - const int vy_mms_i = yVel_mms.as(); - const int ang_mrad_i = angle_mrad.as(); - const uint64_t timestampEpochMs = timestamp_ms.as(); - - LOG_INFO("SSR pose: xPos=%dmm yPos=%dmm angle=%dmrad vx=%dmm/s vy=%dmm/s ts_epoch_ms=%llu", - x_mm_i, y_mm_i, ang_mrad_i, vx_mms_i, vy_mms_i, timestampEpochMs); - - /* 1) Copy pose into struct. */ + JsonVariantConst xPos_mm = jsonPayload["positionX"]; /* int : in mm */ + JsonVariantConst yPos_mm = jsonPayload["positionY"]; /* int : in mm */ + JsonVariantConst xVel_mms = jsonPayload["speedX"]; /* int : in mm/s */ + JsonVariantConst yVel_mms = jsonPayload["speedY"]; /* int : in mm/s */ + JsonVariantConst angle_mrad = jsonPayload["angle"]; /* int : in mrad */ + JsonVariantConst timestamp_ms = jsonPayload["timestamp_ms"];/* int : host epoch in ms */ + + const int x_mm_i = xPos_mm.as(); + const int y_mm_i = yPos_mm.as(); + const int vx_mms_i = xVel_mms.as(); + const int vy_mms_i = yVel_mms.as(); + const int ang_mrad_i = angle_mrad.as(); + const uint64_t hostEpochMs = timestamp_ms.as(); + + const bool hostSynced = m_timeSync.isHostSynced(); + const uint64_t ssrLocalTsMs = + hostSynced ? m_timeSync.hostToEspLocalMs(hostEpochMs) + : m_timeSync.localNowMs(); // fallback if no host sync yet + + LOG_INFO("SSR pose: ts_host_ms=%llu (hostSync=%s)", + hostEpochMs, + hostSynced ? "true" : "false"); + SpaceShipRadarPose ssrPose; ssrPose.x = static_cast(x_mm_i); ssrPose.y = static_cast(y_mm_i); ssrPose.theta = static_cast(ang_mrad_i); ssrPose.v_x = static_cast(vx_mms_i); ssrPose.v_y = static_cast(vy_mms_i); - ssrPose.timestamp = static_cast(m_timeSync.epochToLocalMs(timestampEpochMs)); + ssrPose.timestamp = static_cast(ssrLocalTsMs); - /* 2) Initialize odometry origin from first SSR pose. */ if (false == m_odoOriginInitialized) { m_odoOriginX_mm = ssrPose.x; @@ -371,29 +396,23 @@ void App::ssrTopicCallback(const String& payload) LOG_INFO("Odometry origin set from SSR: x=%dmm y=%dmm", x_mm_i, y_mm_i); } - /* 3) Initialize EKF from first SSR pose. */ if (false == m_ekfInitializedFromSSR) { StateVector x0; x0.setZero(); - /* Position and heading from SSR. */ - x0(0) = ssrPose.x; /* p_x [mm] */ - x0(1) = ssrPose.y; /* p_y [mm] */ - x0(2) = ssrPose.theta; /* theta [mrad] */ + x0(0) = ssrPose.x; + x0(1) = ssrPose.y; + x0(2) = ssrPose.theta; - /* Velocity magnitude from vx, vy. */ const float v_mms = std::sqrt(ssrPose.v_x * ssrPose.v_x + ssrPose.v_y * ssrPose.v_y); - x0(3) = v_mms; /* v [mm/s] */ - x0(4) = 0.0F; /* omega [mrad/s], unknown -> 0 */ + x0(3) = v_mms; + x0(4) = 0.0F; StateMatrix P0 = StateMatrix::Identity(); - /* Position uncertainty ~ 50 mm. */ P0(0,0) = 50.0F * 50.0F; P0(1,1) = 50.0F * 50.0F; - /* Heading uncertainty ~ 200 mrad. */ P0(2,2) = 200.0F * 200.0F; - /* Velocity and omega moderately uncertain. */ P0(3,3) = 200.0F * 200.0F; P0(4,4) = 200.0F * 200.0F; @@ -405,11 +424,9 @@ void App::ssrTopicCallback(const String& payload) x0(0), x0(1), x0(2), x0(3)); } - /* 4) Store last SSR pose. */ m_lastSsrPose = ssrPose; m_hasSsrPose = true; - /* 5) Run fusion if we already have at least one vehicle data sample. */ if (true == m_hasVehicleData) { filterLocationData(m_lastVehicleData, m_lastSsrPose); @@ -417,28 +434,24 @@ void App::ssrTopicCallback(const String& payload) } } + void App::publishVehicleAndSensorSnapshot(const VehicleData& data) { const uint32_t zumoTs32 = static_cast(data.timestamp); const uint64_t mappedLocalMs = m_timeSync.mapZumoToLocalMs(zumoTs32); - const uint64_t localNowMs = m_timeSync.localNowMs(); - const uint64_t epochNowMs = m_timeSync.nowEpochMs(); const int64_t offsetMs = m_timeSync.getZumoToEspOffsetMs(); const bool zumoSynced = m_timeSync.isZumoSynced(); - const bool rtcSynced = m_timeSync.isRtcSynced(); + const bool hostSynced = m_timeSync.isHostSynced(); - (void)localNowMs; - (void)offsetMs; - (void)zumoSynced; - (void)rtcSynced; + (void)offsetMs; // keep for future debug if not used directly - /* Publish snapshot of vehicle + line sensor data. */ const uint16_t* lineSensorValues = m_lineSensors.getSensorValues(); JsonDocument payloadJson; char payloadArray[JSON_SENSOR_SNAPSHOT_MAX_SIZE]; - payloadJson["ts_epoch"] = epochNowMs; - payloadJson["ts_local_ms"] = mappedLocalMs; + payloadJson["ts_local_ms"] = mappedLocalMs; + payloadJson["zumo_sync_ok"] = zumoSynced; + payloadJson["host_sync_ok"] = hostSynced; JsonObject vehicleObj = payloadJson["vehicle"].to(); vehicleObj["ts_zumo_ms"] = static_cast(data.timestamp); @@ -466,6 +479,7 @@ void App::publishVehicleAndSensorSnapshot(const VehicleData& data) } } + void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRadarPose& ssrPose) { /* Do not run fusion until EKF has been initialized from SSR. */ @@ -478,6 +492,8 @@ void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRada const uint32_t zumoLocalMs32 = static_cast(m_timeSync.mapZumoToLocalMs(zumoTs32)); const uint32_t ssrLocalMs32 = static_cast(ssrPose.timestamp); + LOG_INFO("Filtering location data: Zumo local ms=%u), SSR local ms=%u", + zumoLocalMs32, ssrLocalMs32); /* If EKF has no valid timestamp yet, initialize from the first available one. */ if (0U == m_lastEkfUpdateMs) { @@ -500,7 +516,8 @@ void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRada { None, Vehicle, - SSR + SSR, + VehicleAndSSR }; Source newestSource = Source::None; @@ -518,6 +535,13 @@ void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRada newestSource = Source::SSR; } + if ((ssrLocalMs32 > newestLocalTs) && (zumoLocalMs32 > newestLocalTs)) + { + newestLocalTs = (ssrLocalMs32 > zumoLocalMs32) ? ssrLocalMs32 : zumoLocalMs32; + newestSource = Source::VehicleAndSSR; + } + + /* No newer data available -> nothing to do. */ if (Source::None == newestSource) { @@ -534,11 +558,10 @@ void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRada /* EKF prediction step. */ m_ekf.predict(a_x, dt); - LOG_INFO("EKF Source =%s dt=%.3fs", (Source::Vehicle == newestSource) ? "Vehicle" : "SSR", dt); - /* Measurement update depending on source. */ if (Source::Vehicle == newestSource) { + LOG_INFO("EKF update from Vehicle only."); /* IMU update: only yaw rate (turnRateZ). */ { const int16_t rawGyroZ = static_cast(vehicleData.turnRateZ); @@ -567,6 +590,53 @@ void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRada } else if (Source::SSR == newestSource) { + LOG_INFO("EKF update from SSR only."); + /* Camera / SpaceShipRadar update. + * + * EKF cameraModel(x): + * z_cam(0) = p_x + * z_cam(1) = p_y + * z_cam(2) = theta [mrad] + * z_cam(3) = v_x + * z_cam(4) = v_y + */ + CamMeasVector z_cam; + z_cam(0) = ssrPose.x; /* p_x [mm] */ + z_cam(1) = ssrPose.y; /* p_y [mm] */ + z_cam(2) = ssrPose.theta; /* theta [mrad] */ + z_cam(3) = ssrPose.v_x; /* v_x [mm/s] */ + z_cam(4) = ssrPose.v_y; /* v_y [mm/s] */ + + m_ekf.updateCamera(z_cam); + } + else if (Source::VehicleAndSSR == newestSource) + { + LOG_INFO("EKF update from Vehicle and SSR."); + /* IMU update: only yaw rate (turnRateZ). */ + { + const int16_t rawGyroZ = static_cast(vehicleData.turnRateZ); + m_ekf.updateImuFromDigits(rawGyroZ); + } + + /* Odometry update in global frame. + * + * EKF odometryModel(x): + * z_odo(0) = v + * z_odo(1) = theta + */ + { + float xGlob_mm = 0.0F; + float yGlob_mm = 0.0F; + float thetaGlob_mrad = 0.0F; + + transformOdometryToGlobal(vehicleData, xGlob_mm, yGlob_mm, thetaGlob_mrad); + + OdoMeasVector z_odo; + z_odo(0) = static_cast(vehicleData.center); /* speed [mm/s] (v_center) */ + z_odo(1) = thetaGlob_mrad; /* global theta [mrad] */ + + m_ekf.updateOdometry(z_odo); + } /* Camera / SpaceShipRadar update. * * EKF cameraModel(x): @@ -638,6 +708,37 @@ void App::publishFusionPose(uint32_t tsMs) } } +void App::hostTimeSyncResponseCallback(const String& payload) +{ + uint64_t t4_ts = millis(); + JsonDocument doc; + DeserializationError err = deserializeJson(doc, payload); + + if (err != DeserializationError::Ok) + { + LOG_WARNING("HostTimeSync: JSON parse failed: %d", err.code()); + return; + } + + + if (!doc["seq"].is() || + !doc["t1_esp_ms"].is() || + !doc["t2_host_ms"].is() || + !doc["t3_host_ms"].is()) + { + LOG_WARNING("HostTimeSync: Missing required fields in response JSON."); + return; + } + + uint32_t seq = doc["seq"].as(); + uint64_t t1EspMs = doc["t1_esp_ms"].as(); + uint64_t t2HostMs = doc["t2_host_ms"].as(); + uint64_t t3HostMs = doc["t3_host_ms"].as(); + + m_timeSync.onHostTimeSyncResponse(seq, t1EspMs, t2HostMs, t3HostMs, t4_ts); +} + + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/APPLineFollowerSensorFusion/src/App.h b/lib/APPLineFollowerSensorFusion/src/App.h index 630b50be..04d6e96a 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.h +++ b/lib/APPLineFollowerSensorFusion/src/App.h @@ -123,6 +123,12 @@ class App /** MQTT topic name for receiving Space Ship Radar pose. */ static const char* TOPIC_NAME_RADAR_POSE; + /** MQTT topic name for host time sync request. */ + static const char* TOPIC_NAME_HOST_TIMESYNC_REQ; + + /** MQTT topic name for host time sync response. */ + static const char* TOPIC_NAME_HOST_TIMESYNC_RSP; + /** * @brief Flag for setting initial data through SMP. */ @@ -133,6 +139,11 @@ class App */ SimpleTimer m_statusTimer; + /** + * @brief Timer for host time synchronization requests. + */ + SimpleTimer m_hostTimeSyncTimer; + /** * @brief SerialMux channel provider handler. */ @@ -231,6 +242,13 @@ class App */ void ssrTopicCallback(const String& payload); + /** + * @brief Callback for receiving host time sync response over MQTT topic. + * + * @param[in] payload The topic payload as JSON string. + */ + void hostTimeSyncResponseCallback(const String& payload); + /** * @brief Publish a combined snapshot of vehicle and line sensor data via MQTT. * diff --git a/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp b/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp index 08b10298..571b255b 100644 --- a/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp +++ b/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp @@ -33,7 +33,10 @@ * Includes *****************************************************************************/ #include "TimeSync.h" +#include #include +#include + /****************************************************************************** * Local Variables / Constants @@ -41,7 +44,7 @@ namespace { /** Default ping period for time synchronization (ms). */ - constexpr uint32_t TSYNC_DEFAULT_PING_PERIOD_MS = 10000U; + constexpr uint32_t TSYNC_DEFAULT_PING_PERIOD_MS = 1000U; /** Initial value for the minimum RTT (max uint32_t). */ constexpr uint32_t TSYNC_MIN_RTT_INITIAL = 0xFFFFFFFFUL; @@ -52,13 +55,6 @@ namespace /** RTT margin: accept RTTs up to 20% above the best recorded RTT.*/ constexpr uint32_t TSYNC_RTT_MARGIN_NUM = 6U; constexpr uint32_t TSYNC_RTT_MARGIN_DEN = 5U; - - /** Default NTP client configuration. */ - constexpr const char* TSYNC_DEFAULT_NTP_SERVER = "pool.ntp.org"; - constexpr unsigned long TSYNC_NTP_UPDATE_INTERVAL_MS = 60000UL; - - /** Timezone config for NTP client (example: GMT+1). */ - constexpr long TSYNC_GMT_OFFSET_SEC = 3600L; } // namespace /****************************************************************************** @@ -67,28 +63,23 @@ namespace TimeSync::TimeSync(SerMuxChannelProvider& serMuxProvider) : m_serMuxProvider(serMuxProvider), - m_ntpUdp(), - m_ntpClient(m_ntpUdp, TSYNC_DEFAULT_NTP_SERVER, TSYNC_GMT_OFFSET_SEC, TSYNC_NTP_UPDATE_INTERVAL_MS), - m_rtcSynced(false), - m_epochToLocalOffsetMs(0), - m_rtcRefreshMs(TSYNC_NTP_UPDATE_INTERVAL_MS), - m_rtcTimer(), m_pingTimer(), m_pingPeriodMs(TSYNC_DEFAULT_PING_PERIOD_MS), m_seq(0U), - m_pendingSeq(0U), m_pendingT1_32(0U), m_minRttMs(TSYNC_MIN_RTT_INITIAL), m_zumoToEspOffsetMs(0), m_zumoGoodSamples(0U), - m_lastStatusLogMs(0), - m_lastRtcUpdateLocalMs(0), - m_lastRtcCorrectionMs(0), - m_maxRtcCorrectionMs(0), - m_lastAcceptedRttMs(0), + m_pendingSeq(0U), + m_lastStatusLogMs(0ULL), + m_lastAcceptedRttMs(0U), m_lastOffsetEstMs(0), m_minOffsetMs(0), - m_maxOffsetMs(0) + m_maxOffsetMs(0), + m_hostSeq(0U), + m_hostSyncValid(false), + m_hostOffsetMs(0), + m_lastHostRttMs(0U) { } @@ -97,16 +88,15 @@ void TimeSync::begin() m_pingTimer.start(m_pingPeriodMs); /* Register time sync response callback. */ - m_serMuxProvider.registerTimeSyncResponseCallback([this](const TimeSyncResponse& rsp) { onTimeSyncResponse(rsp); }); + m_serMuxProvider.registerTimeSyncResponseCallback( + [this](const TimeSyncResponse& rsp) { onTimeSyncResponse(rsp); }); - m_ntpClient.begin(); - - /* Establish initial epoch-to-local mapping if possible. */ - refreshRtcMapping(); + m_lastStatusLogMs = localNowMs(); } void TimeSync::process() { + /* Periodically send ping to Zumo for time sync. */ if ((true == m_serMuxProvider.isInSync()) && (true == m_pingTimer.isTimeout())) { const uint64_t now64 = localNowMs(); @@ -124,21 +114,39 @@ void TimeSync::process() } m_pingTimer.restart(); } - refreshRtcMapping(); + + /* Periodically log status. */ + const uint64_t nowMs = localNowMs(); + if ((nowMs - m_lastStatusLogMs) >= 10000ULL) + { + logZumoStatus(); + if (m_hostSyncValid) + { + LOG_INFO("Host sync: offset=%ld ms, last RTT=%lu ms", + static_cast(m_hostOffsetMs), + static_cast(m_lastHostRttMs)); + } + else + { + LOG_INFO("Host sync: not yet valid"); + } + m_lastStatusLogMs = nowMs; + } } uint64_t TimeSync::mapZumoToLocalMs(uint32_t zumoTsMs) const { /* Apply last known offset (no skew correction yet). */ - int64_t local = static_cast(zumoTsMs) - m_zumoToEspOffsetMs; + int64_t local32 = static_cast(zumoTsMs) - m_zumoToEspOffsetMs; /* Normalize to 64-bit local by referencing current time high word. */ - uint64_t now64 = localNowMs(); - uint32_t now32 = static_cast(now64); + const uint64_t now64 = localNowMs(); + const uint32_t now32 = static_cast(now64); /* Reconstruct 64-bit around now: choose the 32-bit epoch closest to now32. */ - int32_t diff = static_cast(local - static_cast(now32)); - uint64_t base64 = now64 - static_cast(now32); + const int32_t diff = static_cast(local32 - static_cast(now32)); + const uint64_t base64 = now64 - static_cast(now32); + return base64 + static_cast(static_cast(now32) + diff); } @@ -147,22 +155,101 @@ uint64_t TimeSync::localNowMs() const return static_cast(millis()); } -uint64_t TimeSync::nowEpochMs() const +uint64_t TimeSync::hostToEspLocalMs(uint64_t hostMs) const +{ + if (false == m_hostSyncValid) + { + /* Fallback: keine Host-Sync-Info, einfach aktuelle Local-Time. */ + return localNowMs(); + } + + /* host_time_ms ≈ esp_local_ms + offsetHostMinusEsp + * => esp_local_ms ≈ host_time_ms - offsetHostMinusEsp + */ + const int64_t localMs = static_cast(hostMs) - m_hostOffsetMs; + return (localMs >= 0) ? static_cast(localMs) : 0ULL; +} + +/****************************************************************************** + * Host <-> ESP MQTT TimeSync + *****************************************************************************/ + +void TimeSync::sendHostTimeSyncRequest(MqttClient& mqttClient, + const char* topic) { - if (false == m_rtcSynced) + if (nullptr == topic) { - return 0ULL; + return; } - const int64_t epochMs = static_cast(localNowMs()) + m_epochToLocalOffsetMs; - return (epochMs >= 0) ? static_cast(epochMs) : 0ULL; + const uint32_t seq = m_hostSeq++; + const uint64_t t1 = millis(); + + JsonDocument doc; + doc["seq"] = seq; + doc["t1_esp_ms"] = t1; + + String payload; + serializeJson(doc, payload); + + if (!mqttClient.publish(topic, true, payload)) + { + LOG_WARNING("HostTimeSync: Failed to publish sync request to topic %s.", topic); + } } + +void TimeSync::onHostTimeSyncResponse(uint32_t seq, + uint64_t t1EspMs, + uint64_t t2HostMs, + uint64_t t3HostMs, + uint64_t t4EspMs) +{ + // NTP-style formulas: + // offset = ((T2 - T1) + (T3 - T4)) / 2 + // rtt = (T4 - T1) - (T3 - T2) + // + // T1 = t1EspMs (ESP) + // T2 = t2HostMs (Host) + // T3 = t3HostMs (Host) + // T4 = t4EspMs (ESP) + LOG_INFO("Host sync: seq=%u", static_cast(seq)); + LOG_INFO("t1=%llu ms", t1EspMs); + LOG_INFO("t2=%llu ms", t2HostMs); + LOG_INFO("t3=%llu ms", t3HostMs); + LOG_INFO("t4=%llu ms", t4EspMs); + + + const int64_t d1 = static_cast(t2HostMs) - static_cast(t1EspMs); + const int64_t d2 = static_cast(t3HostMs) - static_cast(t4EspMs); + + const int64_t offset = (d1 + d2) / 2; + + const int64_t rtt = (static_cast(t4EspMs) - static_cast(t1EspMs)) + - (static_cast(t3HostMs) - static_cast(t2HostMs)); + + m_hostOffsetMs = offset; + m_lastHostRttMs = (rtt >= 0) ? static_cast(rtt) : 0U; + m_hostSyncValid = true; + + const double offset_s = static_cast(m_hostOffsetMs) / 1000.0; + + LOG_INFO("Host sync: seq=%u offset=%.3f s rtt=%lu ms", + static_cast(seq), + offset_s, + static_cast(m_lastHostRttMs)); +} + +/****************************************************************************** + * Zumo <-> ESP SerialMux TimeSync + *****************************************************************************/ + void TimeSync::onTimeSyncResponse(const TimeSyncResponse& rsp) { if (rsp.sequenceNumber != m_pendingSeq) { - LOG_WARNING("TimeSync: seq mismatch (expected=%u, got=%u) – using last T1", static_cast(m_pendingSeq), + LOG_WARNING("TimeSync: seq mismatch (expected=%u, got=%u) – using last T1", + static_cast(m_pendingSeq), rsp.sequenceNumber); } @@ -175,7 +262,8 @@ void TimeSync::onTimeSyncResponse(const TimeSyncResponse& rsp) if (delta_local < delta_zumo) { - LOG_WARNING("TimeSync: invalid deltas (delta_local=%u, delta_zumo=%u)", delta_local, delta_zumo); + LOG_WARNING("TimeSync: invalid deltas (delta_local=%u, delta_zumo=%u)", + delta_local, delta_zumo); return; } @@ -230,90 +318,21 @@ void TimeSync::onTimeSyncResponse(const TimeSyncResponse& rsp) } } -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void TimeSync::refreshRtcMapping() -{ - /* Try to update NTP time; if it fails, mark RTC as not synced. */ - if (false == m_ntpClient.update()) - { - if (true == m_rtcSynced) - { - LOG_WARNING("TimeSync: NTP update failed; RTC mapping unavailable"); - } - m_rtcSynced = false; - return; - } - - /* On success, compute epoch-to-local mapping in milliseconds. */ - const unsigned long epochSec = m_ntpClient.getEpochTime(); - const uint64_t epochMs = static_cast(epochSec) * 1000ULL; - const uint64_t localMs = localNowMs(); - - /* Log correction info for diagnostics. */ - if (true == m_rtcSynced) - { - const int64_t oldOffset = m_epochToLocalOffsetMs; - const uint64_t predictedEpoch = static_cast(static_cast(localMs) + oldOffset); - const int64_t correction = static_cast(epochMs) - static_cast(predictedEpoch); - const int64_t absCorrection = (correction >= 0) ? correction : -correction; - const int64_t absMaxCorrection = (m_maxRtcCorrectionMs >= 0) ? m_maxRtcCorrectionMs : -m_maxRtcCorrectionMs; - - m_lastRtcCorrectionMs = correction; - if (absCorrection > absMaxCorrection) - { - m_maxRtcCorrectionMs = correction; - } - } - - m_epochToLocalOffsetMs = static_cast(epochMs) - static_cast(localMs); - m_lastRtcUpdateLocalMs = localMs; - m_rtcSynced = true; -} - -uint64_t TimeSync::epochToLocalMs(uint64_t epochMs) const -{ - if (false == m_rtcSynced) - { - LOG_WARNING("TimeSync: epochToLocalMs() called without RTC sync, using localNowMs() as fallback."); - return localNowMs(); - } - - /* epoch = local + offset => local = epoch - offset */ - const int64_t localMs = static_cast(epochMs) - m_epochToLocalOffsetMs; - - if (localMs < 0) - { - return 0ULL; - } - - return static_cast(localMs); -} - /****************************************************************************** * Diagnostic Logging *****************************************************************************/ -void TimeSync::logRtcStatus() const -{ - LOG_INFO("RTC / NTP Status"); - LOG_INFO("RTC Synced: %s", m_rtcSynced ? "yes" : "no"); - LOG_INFO("Epoch to Local Offset: %lld ms", static_cast(m_epochToLocalOffsetMs)); - LOG_INFO("Local/Epoch time now: %llu / %llu ms", static_cast(localNowMs()), - static_cast(nowEpochMs())); -} - void TimeSync::logZumoStatus() const { const uint32_t bestRttMs = (TSYNC_MIN_RTT_INITIAL == m_minRttMs) ? 0U : m_minRttMs; const uint32_t estAccuMs = bestRttMs / 2U; - LOG_INFO("Zumo sync: lastSeq=%u goodSamples=%u", static_cast(m_pendingSeq), + LOG_INFO("Zumo sync: lastSeq=%u goodSamples=%u", + static_cast(m_pendingSeq), static_cast(m_zumoGoodSamples)); - LOG_INFO("Zumo sync: lastRTT=%lu ms minRTT=%lu ms", static_cast(m_lastAcceptedRttMs), + LOG_INFO("Zumo sync: lastRTT=%lu ms minRTT=%lu ms", + static_cast(m_lastAcceptedRttMs), static_cast(bestRttMs)); if (m_zumoGoodSamples > 0U) @@ -324,7 +343,8 @@ void TimeSync::logZumoStatus() const const long span32 = maxOffset32 - minOffset32; const long absSpan32 = (span32 >= 0) ? span32 : -span32; - LOG_INFO("Zumo sync: offsetZ2E=%ld ms span=[%ld .. %ld] ms", offset32, minOffset32, maxOffset32); + LOG_INFO("Zumo sync: offsetZ2E=%ld ms span=[%ld .. %ld] ms", + offset32, minOffset32, maxOffset32); LOG_INFO("Zumo sync: est. accuracy=+/- %lu ms (from minRTT/2, span=%ld ms)", static_cast(estAccuMs), absSpan32); } @@ -337,7 +357,16 @@ void TimeSync::logZumoStatus() const void TimeSync::logStatus() const { LOG_INFO("================= TimeSync Detailed Status ================="); - logRtcStatus(); logZumoStatus(); + if (m_hostSyncValid) + { + LOG_INFO("Host sync: offset=%ld ms, last RTT=%lu ms", + static_cast(m_hostOffsetMs), + static_cast(m_lastHostRttMs)); + } + else + { + LOG_INFO("Host sync: not yet valid"); + } LOG_INFO("============================================================"); -} +} \ No newline at end of file diff --git a/lib/APPLineFollowerSensorFusion/src/TimeSync.h b/lib/APPLineFollowerSensorFusion/src/TimeSync.h index 173f05d7..0ebee3c9 100644 --- a/lib/APPLineFollowerSensorFusion/src/TimeSync.h +++ b/lib/APPLineFollowerSensorFusion/src/TimeSync.h @@ -35,34 +35,26 @@ #ifndef TIMESYNC_H #define TIMESYNC_H -/****************************************************************************** - * Compile Switches - *****************************************************************************/ - /****************************************************************************** * Includes *****************************************************************************/ +#include #include -#include -#include #include "SerMuxChannelProvider.h" #include "SerialMuxChannels.h" - -/****************************************************************************** - * Macros - *****************************************************************************/ +#include /****************************************************************************** * Types and Classes *****************************************************************************/ /** - * @brief Time synchronization orchestrator (ESP32 side). + * @brief Time synchronization orchestrator (ESP32 side). * - * Provides: - * - Network time via SNTP (Host as NTP server) + * Responsibilities: * - Serial ping-pong time sync with Zumo32u4 over SerialMux - * - Linear time mapping helpers for future extensions + * - Mapping Zumo millis() timestamps to ESP32 local time (millis()) + * - Host (Laptop/PC) time sync over MQTT using NTP-style T1..T4 timestamps */ class TimeSync { @@ -74,101 +66,158 @@ class TimeSync */ TimeSync(SerMuxChannelProvider& serMuxProvider); - /** - * Log current time sync status (for debugging/testing). - */ - void logStatus() const; - /** * Initialize time synchronization. */ void begin(); /** - * Process periodic activities (send pings, housekeeping). + * Process periodic activities (send Zumo pings, housekeeping). */ void process(); /** - * Returns whether serial ping-pong has a valid offset estimate. - * @return true if synced; otherwise false. + * Log current time sync status (Zumo + host). */ - bool isZumoSynced() const - { - static const uint8_t REQUIRED_GOOD_SAMPLES = 3; - return m_zumoGoodSamples >= REQUIRED_GOOD_SAMPLES; - } + void logStatus() const; + + /********************************************************************** + * Zumo <-> ESP (SerialMux) + *********************************************************************/ /** - * Returns whether RTC (SNTP) is available. - * @return true if RTC is synced; otherwise false. + * Returns whether serial ping-pong has a valid offset estimate. + * + * @return true if Zumo<->ESP is considered synchronized; otherwise false. */ - bool isRtcSynced() const + bool isZumoSynced() const { - return m_rtcSynced; + static const uint8_t REQUIRED_GOOD_SAMPLES = 3U; + return m_zumoGoodSamples >= REQUIRED_GOOD_SAMPLES; } /** - * Map Zumo timestamp [ms] to ESP32 local time [ms] using latest offset. + * Map Zumo timestamp [ms] to ESP32 local time [ms] using the latest offset. * * @param[in] zumoTsMs Zumo millis() timestamp [ms]. - * @return ESP32 local time [ms]. + * + * @return ESP32 local time [ms] corresponding to the given Zumo timestamp. */ uint64_t mapZumoToLocalMs(uint32_t zumoTsMs) const; /** - * @brief Map epoch time in milliseconds to local milliseconds (millis()). + * Get current ESP32 local time [ms] (wrapper around millis()). * - * Requires that the RTC/NTP mapping is synced (isRtcSynced() == true). - * If RTC is not synced, falls back to localNowMs(). + * @return Local time [ms]. + */ + uint64_t localNowMs() const; + + /** + * Get estimated Zumo->ESP offset [ms]. Positive means Zumo is ahead of ESP. * - * @param epochMs Epoch time in ms (Unix). - * @return uint64_t Local time in ms. + * @return Time offset Zumo->ESP [ms]. */ - uint64_t epochToLocalMs(uint64_t epochMs) const; + int64_t getZumoToEspOffsetMs() const + { + return m_zumoToEspOffsetMs; + } /** - * Get current local time [ms]. - * @return Local time [ms]. + * Log detailed serial (Zumo) time sync status information. */ - uint64_t localNowMs() const; + void logZumoStatus() const; + + /********************************************************************** + * Host <-> ESP (MQTT, T1..T4) + * + * Protocol: + * - ESP sends request (T1_esp_ms, seq) + * - Host (Python) records T2_host_ms, T3_host_ms and sends a response + * - ESP records T4_esp_ms on reception and computes offset + RTT + *********************************************************************/ + /** - * Get current epoch time [ms] if RTC mapping is available, otherwise 0. - * @return Epoch time [ms] + * @brief Send a host time sync request message via MQTT. + * + * The function: + * - increments the host sequence counter + * - measures T1 (ESP localNowMs()) + * - builds a JSON payload { "seq", "t1_esp_ms" } + * - calls the provided publish callback + * + * @param[in] publishFn MQTT instance. + * @param[in] topic MQTT topic for the request message. */ - uint64_t nowEpochMs() const; + void sendHostTimeSyncRequest(MqttClient& mqttClient, + const char* topic); /** - * Get estimated Zumo->ESP offset [ms]. Positive means Zumo ahead of ESP. - * @return Time Offset Zumo->ESP [ms] + * @brief Handle a host time sync response received via MQTT. + * + * Expected payload (generated by the host): + * - seq : Sequence number (must match the sent request). + * - t1_esp_ms : T1 (as originally sent by the ESP). + * - t2_host_ms : T2 (host receive time of the request). + * - t3_host_ms : T3 (host send time of the response). + * + * The function: + * - measures T4 (ESP localNowMs()) + * - computes offset and RTT using NTP-style formulas + * - stores offset and RTT for later use + * + * @param[in] seq Sequence number. + * @param[in] t1EspMs T1 on ESP side [ms]. + * @param[in] t2HostMs T2 on host side [ms]. + * @param[in] t3HostMs T3 on host side [ms]. */ - int64_t getZumoToEspOffsetMs() const + void onHostTimeSyncResponse(uint32_t seq, + uint64_t t1EspMs, + uint64_t t2HostMs, + uint64_t t3HostMs, + uint64_t t4EspMs); + + /** + * @brief Returns whether a valid host synchronization offset is available. + * + * @return true if a valid host<->ESP offset is available; otherwise false. + */ + bool isHostSynced() const { - return m_zumoToEspOffsetMs; + return m_hostSyncValid; } /** - * Log detailed RTC/NTP status information. + * @brief Get the estimated Host->ESP offset [ms]. + * + * Interpretation: + * host_time_ms ≈ esp_local_ms + offsetHostMinusEsp + * + * @return Estimated Host->ESP offset [ms]. */ - void logRtcStatus() const; + int64_t getHostOffsetMs() const + { + return m_hostOffsetMs; + } /** - * Log detailed serial (Zumo) time sync status information. + * @brief Convert a host timestamp [ms] to ESP local time [ms]. + * + * Uses the estimated host offset: + * host_time_ms ≈ esp_local_ms + offset + * => esp_local_ms ≈ host_time_ms - offset + * + * If no valid host sync is available, localNowMs() is returned. + * + * @param[in] hostMs Host time [ms]. + * + * @return Approximate ESP local time [ms] corresponding to the host time. */ - void logZumoStatus() const; + uint64_t hostToEspLocalMs(uint64_t hostMs) const; private: SerMuxChannelProvider& m_serMuxProvider; /**< SerialMux provider. */ - /* --- RTC (NTP) --- */ - WiFiUDP m_ntpUdp; /**< UDP socket for NTP client. */ - NTPClient m_ntpClient; /**< NTP client instance. */ - bool m_rtcSynced; /**< RTC synchronized flag. */ - int64_t m_epochToLocalOffsetMs; /**< Epoch time - local time [ms]. */ - uint32_t m_rtcRefreshMs; /**< RTC mapping refresh period. */ - SimpleTimer m_rtcTimer; /**< Timer for refreshing RTC mapping. */ - /* --- Serial ping-pong with Zumo --- */ SimpleTimer m_pingTimer; /**< Ping timer. */ uint32_t m_pingPeriodMs; /**< Ping period [ms]. */ @@ -180,29 +229,24 @@ class TimeSync uint32_t m_pendingSeq; /**< Sequence number of pending request. */ uint64_t m_lastStatusLogMs; /**< Last status log time [ms]. */ - uint64_t m_lastRtcUpdateLocalMs; /**< Local time of last RTC update [ms]. */ - int64_t m_lastRtcCorrectionMs; /**< Last RTC correction applied [ms]. */ - int64_t m_maxRtcCorrectionMs; /**< Maximum RTC correction applied [ms]. */ - uint32_t m_lastAcceptedRttMs; /**< Last accepted RTT [ms]. */ - int64_t m_lastOffsetEstMs; /**< Last offset estimate [ms]. */ - int64_t m_minOffsetMs; /**< Minimum observed offset [ms]. */ - int64_t m_maxOffsetMs; /**< Maximum observed offset [ms]. */ + uint32_t m_lastAcceptedRttMs; /**< Last accepted RTT [ms]. */ + int64_t m_lastOffsetEstMs; /**< Last offset estimate [ms]. */ + int64_t m_minOffsetMs; /**< Minimum observed offset [ms]. */ + int64_t m_maxOffsetMs; /**< Maximum observed offset [ms]. */ - /** - * Handle an incoming time sync response from Zumo. - * @param[in] rsp Time sync response frame. - */ - void onTimeSyncResponse(const TimeSyncResponse& rsp); + /* --- Host time sync over MQTT --- */ + uint32_t m_hostSeq; /**< Host sync sequence counter. */ + bool m_hostSyncValid; /**< Host sync offset valid flag. */ + int64_t m_hostOffsetMs; /**< Estimated offset Host->ESP [ms]. */ + uint32_t m_lastHostRttMs; /**< Last RTT to host [ms]. */ /** - * Try to refresh epoch-to-local mapping using RTC (SNTP) if available. + * @brief Handle an incoming time sync response from the Zumo. + * + * @param[in] rsp Time sync response frame received via SerialMux. */ - void refreshRtcMapping(); + void onTimeSyncResponse(const TimeSyncResponse& rsp); }; -/****************************************************************************** - * Functions - *****************************************************************************/ - #endif /* TIMESYNC_H */ /** @} */ From cb30c0e17ae18163a3953dc5f7687630fc43f389 Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Mon, 8 Dec 2025 14:35:09 +0100 Subject: [PATCH 05/14] Added Doxygen Comments and changed From StateMatrix F to StateMatrix F_jacobian --- lib/APPLineFollowerSensorFusion/src/EKF.cpp | 14 +++++++------- lib/APPLineFollowerSensorFusion/src/EKF.h | 18 +++++++++++++++--- 2 files changed, 22 insertions(+), 10 deletions(-) diff --git a/lib/APPLineFollowerSensorFusion/src/EKF.cpp b/lib/APPLineFollowerSensorFusion/src/EKF.cpp index 25a9794f..0a22b2d0 100644 --- a/lib/APPLineFollowerSensorFusion/src/EKF.cpp +++ b/lib/APPLineFollowerSensorFusion/src/EKF.cpp @@ -254,22 +254,22 @@ ExtendedKalmanFilter5D::processJacobianF(const StateVector& x, float dt) const const float v = x(3); const float thetaRad = thetaMrad / 1000.0F; - StateMatrix F = StateMatrix::Identity(); + StateMatrix F_jacobian = StateMatrix::Identity(); /* d p_x / d theta. */ - F(0, 2) = -v * std::sin(thetaRad) * dt / 1000.0F; + F_jacobian(0, 2) = -v * std::sin(thetaRad) * dt / 1000.0F; /* d p_x / d v. */ - F(0, 3) = std::cos(thetaRad) * dt; + F_jacobian(0, 3) = std::cos(thetaRad) * dt; /* d p_y / d theta. */ - F(1, 2) = v * std::cos(thetaRad) * dt / 1000.0F; + F_jacobian(1, 2) = v * std::cos(thetaRad) * dt / 1000.0F; /* d p_y / d v. */ - F(1, 3) = std::sin(thetaRad) * dt; + F_jacobian(1, 3) = std::sin(thetaRad) * dt; /* d theta / d omega. */ - F(2, 4) = dt; + F_jacobian(2, 4) = dt; - return F; + return F_jacobian; } /* ---------------- Camera model ---------------- */ diff --git a/lib/APPLineFollowerSensorFusion/src/EKF.h b/lib/APPLineFollowerSensorFusion/src/EKF.h index cfdcf30c..f83b91bf 100644 --- a/lib/APPLineFollowerSensorFusion/src/EKF.h +++ b/lib/APPLineFollowerSensorFusion/src/EKF.h @@ -72,18 +72,28 @@ class ExtendedKalmanFilter5D /** IMU measurement dimension: [omega]. */ static constexpr std::uint8_t IMU_MEAS_DIM = 1U; + /** @brief State vector type x = [p_x, p_y, theta, v, omega]^T. */ using StateVector = Eigen::Matrix; + + /** @brief State covariance matrix type P (STATE_DIM x STATE_DIM). */ using StateMatrix = Eigen::Matrix; + /** @brief Camera measurement vector z_cam = [p_x, p_y, theta, v_x, v_y]^T. */ using CamMeasVector = Eigen::Matrix; + /** @brief Camera measurement covariance matrix R_cam. */ using CamMeasMatrix = Eigen::Matrix; + /** @brief Odometry measurement vector z_odo = [v, theta]^T. */ using OdoMeasVector = Eigen::Matrix; + /** @brief Odometry measurement covariance matrix R_odo. */ using OdoMeasMatrix = Eigen::Matrix; + /** @brief IMU measurement vector z_imu = [omega]^T. */ using ImuMeasVector = Eigen::Matrix; + /** @brief IMU measurement covariance matrix R_imu. */ using ImuMeasMatrix = Eigen::Matrix; + public: /** * Constructs the EKF with default noise parameters and zero-initialized state. @@ -229,7 +239,7 @@ class ExtendedKalmanFilter5D /** * Jacobian of the process model F = df/dx. * - * @param[in] x Current state. + * @param[in] x Current state vector. * @param[in] dt Time step [s]. * * @return Process Jacobian matrix. @@ -267,13 +277,15 @@ class ExtendedKalmanFilter5D * Odometry measurement Jacobian H_odo = dh_odo/dx. * * @return Odometry measurement Jacobian. + * + * @param[in] x Current state vector. */ Eigen::Matrix odometryJacobianH(const StateVector& x) const; /** * IMU measurement model h_imu(x) = [omega]^T. * - * @param[in] x Current state. + * @param[in] x Current state vector. * * @return IMU measurement prediction. */ @@ -282,7 +294,7 @@ class ExtendedKalmanFilter5D /** * IMU measurement Jacobian H_imu = dh_imu/dx. * - * @param[in] x Current state (currently unused). + * @param[in] x Current state vector. * * @return IMU Jacobian matrix H_imu. */ From c1348caf833e2eb8148e9a4f8d6aa9a18a65bff2 Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Mon, 8 Dec 2025 14:38:01 +0100 Subject: [PATCH 06/14] added Dooxygen comments --- lib/APPLineFollowerSensorFusion/src/TimeSync.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/lib/APPLineFollowerSensorFusion/src/TimeSync.h b/lib/APPLineFollowerSensorFusion/src/TimeSync.h index 0ebee3c9..04aea958 100644 --- a/lib/APPLineFollowerSensorFusion/src/TimeSync.h +++ b/lib/APPLineFollowerSensorFusion/src/TimeSync.h @@ -146,7 +146,7 @@ class TimeSync * - builds a JSON payload { "seq", "t1_esp_ms" } * - calls the provided publish callback * - * @param[in] publishFn MQTT instance. + * @param[in] mqttClient MQTT instance. * @param[in] topic MQTT topic for the request message. */ void sendHostTimeSyncRequest(MqttClient& mqttClient, @@ -170,6 +170,7 @@ class TimeSync * @param[in] t1EspMs T1 on ESP side [ms]. * @param[in] t2HostMs T2 on host side [ms]. * @param[in] t3HostMs T3 on host side [ms]. + * @param[in] t4EspMs T4 on ESP side [ms]. */ void onHostTimeSyncResponse(uint32_t seq, uint64_t t1EspMs, From 50811c6774439ee091197e1421b3e6ee28d2b323 Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Thu, 11 Dec 2025 16:54:56 +0100 Subject: [PATCH 07/14] Applied changes from Review --- .env | 13 - lib/APPLineFollowerSensorFusion/src/App.cpp | 389 +++++++++--------- lib/APPLineFollowerSensorFusion/src/App.h | 41 ++ lib/APPLineFollowerSensorFusion/src/EKF.cpp | 68 ++- lib/APPLineFollowerSensorFusion/src/EKF.h | 7 +- .../src/TimeSync.cpp | 17 +- 6 files changed, 283 insertions(+), 252 deletions(-) delete mode 100644 .env diff --git a/.env b/.env deleted file mode 100644 index bda6c1a2..00000000 --- a/.env +++ /dev/null @@ -1,13 +0,0 @@ -# remove the template from this file (change it to '.env') and adjust the paths - -# absolute path to the RadonUlzer exe -RadonUlzer_RemoteControlSim_PATH=C:\Users\thaeckel\Documents\Repos\RadonUlzer\.pio\build\RemoteControlSim\program.exe - -DroidControlShip_PATH=C:\Users\thaeckel\Documents\Repos\DroidControlShip\.pio\build\LineFollowerSensorFusionSim\program.exe - -# absolute path to the space ship radar script -SpaceShipRadar_PATH=C:\Users\thaeckel\Documents\Repos\SpaceShipRadar\src\space_ship_radar\space_ship_radar.py - -# absolute path to the img folder in this repo (found under src/) -# ! should end with backslash \ -CalibrationFolder_PATH=C:\Users\thaeckel\Documents\Repos\SpaceShipRadar\src\calibration\ diff --git a/lib/APPLineFollowerSensorFusion/src/App.cpp b/lib/APPLineFollowerSensorFusion/src/App.cpp index ef967229..938a81e9 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.cpp +++ b/lib/APPLineFollowerSensorFusion/src/App.cpp @@ -110,10 +110,16 @@ static const uint32_t JSON_SENSOR_SNAPSHOT_MAX_SIZE = 256U; /** Buffer size for JSON serialization of fusion pose. */ static const uint32_t JSON_FUSION_POSE_MAX_SIZE = 128U; +/** Status send interval in ms. */ +const uint16_t STATUS_SEND_INTERVAL_MS = 1000U; + +/** Host time sync interval in ms. */ +const uint16_t HOST_TIMESYNC_INTERVAL_MS = 10000U; + /* Convenience aliases for EKF types. */ -using ImuMeasVector = ExtendedKalmanFilter5D::ImuMeasVector; -using OdoMeasVector = ExtendedKalmanFilter5D::OdoMeasVector; -using CamMeasVector = ExtendedKalmanFilter5D::CamMeasVector; +using ImuMeasurementVector = ExtendedKalmanFilter5D::ImuMeasurementVector; +using OdoMeasurementVector = ExtendedKalmanFilter5D::OdoMeasurementVector; +using CamMeasurementVector = ExtendedKalmanFilter5D::CamMeasurementVector; using StateVector = ExtendedKalmanFilter5D::StateVector; using StateMatrix = ExtendedKalmanFilter5D::StateMatrix; @@ -211,24 +217,12 @@ void App::setup() { /* Log incoming vehicle data and corresponding time sync information. */ m_serMuxChannelProvider.registerVehicleDataCallback( - [this](const VehicleData& data) - { - publishVehicleAndSensorSnapshot(data); - - m_lastVehicleData = data; - m_hasVehicleData = true; - - /* Run sensor fusion whenever new vehicle data arrives. - * SSR data may or may not be available yet; fusion will - * only start after SSR-based EKF initialization. - */ - filterLocationData(data, m_lastSsrPose); - }); + [this](const VehicleData& data) { onVehicleData(data); }); /* Start network time (NTP) against host and Zumo serial ping-pong. */ m_timeSync.begin(); - m_statusTimer.start(1000U); - m_hostTimeSyncTimer.start(1000U); + m_statusTimer.start(STATUS_SEND_INTERVAL_MS); + m_hostTimeSyncTimer.start(HOST_TIMESYNC_INTERVAL_MS); isSuccessful = true; } } @@ -261,11 +255,11 @@ void App::loop() /* Process time synchronization (serial ping-pong). */ m_timeSync.process(); - //if (m_hostTimeSyncTimer.isTimeout()) - //{ - // m_timeSync.sendHostTimeSyncRequest(m_mqttClient, TOPIC_NAME_HOST_TIMESYNC_REQ); - // m_hostTimeSyncTimer.restart(); - //} + if (m_hostTimeSyncTimer.isTimeout()) + { + m_timeSync.sendHostTimeSyncRequest(m_mqttClient, TOPIC_NAME_HOST_TIMESYNC_REQ); + m_hostTimeSyncTimer.restart(); + } /* Process state machine. */ m_stateMachine.process(); @@ -443,7 +437,7 @@ void App::publishVehicleAndSensorSnapshot(const VehicleData& data) const bool zumoSynced = m_timeSync.isZumoSynced(); const bool hostSynced = m_timeSync.isHostSynced(); - (void)offsetMs; // keep for future debug if not used directly + (void)offsetMs; /* keep for future debug if not used directly */ const uint16_t* lineSensorValues = m_lineSensors.getSensorValues(); JsonDocument payloadJson; @@ -480,189 +474,91 @@ void App::publishVehicleAndSensorSnapshot(const VehicleData& data) } -void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRadarPose& ssrPose) +void App::filterLocationData(const VehicleData& vehicleData, + const SpaceShipRadarPose& ssrPose) { - /* Do not run fusion until EKF has been initialized from SSR. */ - if (false == m_ekfInitializedFromSSR) - { - return; - } - - const uint32_t zumoTs32 = static_cast(vehicleData.timestamp); - const uint32_t zumoLocalMs32 = static_cast(m_timeSync.mapZumoToLocalMs(zumoTs32)); - const uint32_t ssrLocalMs32 = static_cast(ssrPose.timestamp); - - LOG_INFO("Filtering location data: Zumo local ms=%u), SSR local ms=%u", - zumoLocalMs32, ssrLocalMs32); - /* If EKF has no valid timestamp yet, initialize from the first available one. */ - if (0U == m_lastEkfUpdateMs) - { - if (0U != zumoLocalMs32) - { - m_lastEkfUpdateMs = zumoLocalMs32; - } - else if (0U != ssrLocalMs32) - { - m_lastEkfUpdateMs = ssrLocalMs32; - } - else - { - return; - } - } - - /* Determine which sensor provides the newest information. */ - enum class Source - { - None, - Vehicle, - SSR, - VehicleAndSSR - }; + /* Local variables (all declared at top as requested). */ + uint32_t zumoTs32 = 0U; + uint32_t zumoLocalMs32 = 0U; + uint32_t ssrLocalMs32 = 0U; + uint32_t newestLocalTs = 0U; + uint32_t dtMs = 0U; + float dt = 0.0F; + float a_x = 0.0F; + Source newestSource = Source::None; + bool ekfReady = false; + bool hasTimestamp = false; + bool doProcessing = false; - Source newestSource = Source::None; - uint32_t newestLocalTs = m_lastEkfUpdateMs; + /* Do not run fusion until EKF has been initialized from SSR. */ + ekfReady = m_ekfInitializedFromSSR; - if (zumoLocalMs32 > newestLocalTs) + if (ekfReady) { - newestLocalTs = zumoLocalMs32; - newestSource = Source::Vehicle; + doProcessing = true; } - if (ssrLocalMs32 > newestLocalTs) + if (doProcessing) { - newestLocalTs = ssrLocalMs32; - newestSource = Source::SSR; - } + /* Timestamp conversion. */ + zumoTs32 = static_cast(vehicleData.timestamp); + zumoLocalMs32 = static_cast(m_timeSync.mapZumoToLocalMs(zumoTs32)); + ssrLocalMs32 = static_cast(ssrPose.timestamp); - if ((ssrLocalMs32 > newestLocalTs) && (zumoLocalMs32 > newestLocalTs)) - { - newestLocalTs = (ssrLocalMs32 > zumoLocalMs32) ? ssrLocalMs32 : zumoLocalMs32; - newestSource = Source::VehicleAndSSR; - } - + LOG_INFO("Filtering location data: Zumo=%u ms, SSR=%u ms", + zumoLocalMs32, ssrLocalMs32); - /* No newer data available -> nothing to do. */ - if (Source::None == newestSource) - { - return; - } + /* Initialize EKF time on first data. */ + hasTimestamp = initializeEkfTimestamp(zumoLocalMs32, ssrLocalMs32); - /* Time difference in seconds for prediction. */ - const uint32_t dtMs = newestLocalTs - m_lastEkfUpdateMs; - const float dt = static_cast(dtMs) / 1000.0F; - - /* Longitudinal acceleration input (raw accelerometer digits). */ - const float a_x = static_cast(vehicleData.accelerationX); - - /* EKF prediction step. */ - m_ekf.predict(a_x, dt); - - /* Measurement update depending on source. */ - if (Source::Vehicle == newestSource) - { - LOG_INFO("EKF update from Vehicle only."); - /* IMU update: only yaw rate (turnRateZ). */ + if (hasTimestamp) { - const int16_t rawGyroZ = static_cast(vehicleData.turnRateZ); - m_ekf.updateImuFromDigits(rawGyroZ); - } + newestLocalTs = m_lastEkfUpdateMs; - /* Odometry update in global frame. - * - * EKF odometryModel(x): - * z_odo(0) = v - * z_odo(1) = theta - */ - { - float xGlob_mm = 0.0F; - float yGlob_mm = 0.0F; - float thetaGlob_mrad = 0.0F; + /* Determine which sensor has the newest update. */ + newestSource = determineNewestSource(zumoLocalMs32, + ssrLocalMs32, + m_lastEkfUpdateMs, + newestLocalTs); - transformOdometryToGlobal(vehicleData, xGlob_mm, yGlob_mm, thetaGlob_mrad); + if (newestSource != Source::None) + { + /* Time delta for prediction step. */ + dtMs = newestLocalTs - m_lastEkfUpdateMs; + dt = static_cast(dtMs) / 1000.0F; - OdoMeasVector z_odo; - z_odo(0) = static_cast(vehicleData.center); /* speed [mm/s] (v_center) */ - z_odo(1) = thetaGlob_mrad; /* global theta [mrad] */ + /* Longitudinal acceleration input. */ + a_x = static_cast(vehicleData.accelerationX); - m_ekf.updateOdometry(z_odo); - } - } - else if (Source::SSR == newestSource) - { - LOG_INFO("EKF update from SSR only."); - /* Camera / SpaceShipRadar update. - * - * EKF cameraModel(x): - * z_cam(0) = p_x - * z_cam(1) = p_y - * z_cam(2) = theta [mrad] - * z_cam(3) = v_x - * z_cam(4) = v_y - */ - CamMeasVector z_cam; - z_cam(0) = ssrPose.x; /* p_x [mm] */ - z_cam(1) = ssrPose.y; /* p_y [mm] */ - z_cam(2) = ssrPose.theta; /* theta [mrad] */ - z_cam(3) = ssrPose.v_x; /* v_x [mm/s] */ - z_cam(4) = ssrPose.v_y; /* v_y [mm/s] */ - - m_ekf.updateCamera(z_cam); - } - else if (Source::VehicleAndSSR == newestSource) - { - LOG_INFO("EKF update from Vehicle and SSR."); - /* IMU update: only yaw rate (turnRateZ). */ - { - const int16_t rawGyroZ = static_cast(vehicleData.turnRateZ); - m_ekf.updateImuFromDigits(rawGyroZ); - } - - /* Odometry update in global frame. - * - * EKF odometryModel(x): - * z_odo(0) = v - * z_odo(1) = theta - */ - { - float xGlob_mm = 0.0F; - float yGlob_mm = 0.0F; - float thetaGlob_mrad = 0.0F; + /* EKF prediction. */ + m_ekf.predict(a_x, dt); - transformOdometryToGlobal(vehicleData, xGlob_mm, yGlob_mm, thetaGlob_mrad); + /* EKF correction step based on sensor source. */ + if (newestSource == Source::Vehicle) + { + updateFromVehicle(vehicleData); + } + else if (newestSource == Source::SSR) + { + updateFromSsr(ssrPose); + } + else if (newestSource == Source::VehicleAndSSR) + { + updateFromVehicle(vehicleData); + updateFromSsr(ssrPose); + } - OdoMeasVector z_odo; - z_odo(0) = static_cast(vehicleData.center); /* speed [mm/s] (v_center) */ - z_odo(1) = thetaGlob_mrad; /* global theta [mrad] */ + /* Update last EKF timestamp. */ + m_lastEkfUpdateMs = newestLocalTs; - m_ekf.updateOdometry(z_odo); + /* Publish fused pose. */ + publishFusionPose(newestLocalTs); + } } - /* Camera / SpaceShipRadar update. - * - * EKF cameraModel(x): - * z_cam(0) = p_x - * z_cam(1) = p_y - * z_cam(2) = theta [mrad] - * z_cam(3) = v_x - * z_cam(4) = v_y - */ - CamMeasVector z_cam; - z_cam(0) = ssrPose.x; /* p_x [mm] */ - z_cam(1) = ssrPose.y; /* p_y [mm] */ - z_cam(2) = ssrPose.theta; /* theta [mrad] */ - z_cam(3) = ssrPose.v_x; /* v_x [mm/s] */ - z_cam(4) = ssrPose.v_y; /* v_y [mm/s] */ - - m_ekf.updateCamera(z_cam); } - - /* Update timestamp of last EKF update. */ - m_lastEkfUpdateMs = newestLocalTs; - - /* Publish fused pose. */ - publishFusionPose(newestLocalTs); } + void App::transformOdometryToGlobal(const VehicleData& vehicleData, float& xGlob_mm, float& yGlob_mm, @@ -721,10 +617,10 @@ void App::hostTimeSyncResponseCallback(const String& payload) } - if (!doc["seq"].is() || - !doc["t1_esp_ms"].is() || - !doc["t2_host_ms"].is() || - !doc["t3_host_ms"].is()) + if ( (doc["seq"].is() == false) || + (doc["t1_esp_ms"].is() == false) || + (doc["t2_host_ms"].is() == false) || + (doc["t3_host_ms"].is() == false) ) { LOG_WARNING("HostTimeSync: Missing required fields in response JSON."); return; @@ -738,6 +634,121 @@ void App::hostTimeSyncResponseCallback(const String& payload) m_timeSync.onHostTimeSyncResponse(seq, t1EspMs, t2HostMs, t3HostMs, t4_ts); } +void App::onVehicleData(const VehicleData& data) +{ + publishVehicleAndSensorSnapshot(data); + + m_lastVehicleData = data; + m_hasVehicleData = true; + + /* Run sensor fusion whenever new vehicle data arrives. + * SSR data may or may not be available yet; fusion will + * only start after SSR-based EKF initialization. + */ + filterLocationData(data, m_lastSsrPose); +} + +bool App::initializeEkfTimestamp(uint32_t zumoLocalMs32, uint32_t ssrLocalMs32) +{ + bool initialized = true; + + /* If EKF has no reference timestamp yet, initialize from first valid source. */ + if (m_lastEkfUpdateMs == 0U) + { + if (zumoLocalMs32 != 0U) + { + m_lastEkfUpdateMs = zumoLocalMs32; + } + else if (ssrLocalMs32 != 0U) + { + m_lastEkfUpdateMs = ssrLocalMs32; + } + else + { + /* No valid timestamp available. */ + initialized = false; + } + } + + return initialized; +} + +Source App::determineNewestSource(uint32_t zumoLocalMs32, + uint32_t ssrLocalMs32, + uint32_t lastEkfUpdateMs, + uint32_t& newestLocalTs) const +{ + Source source = Source::None; + uint32_t candidateTs = lastEkfUpdateMs; + + const bool hasVehicle = (zumoLocalMs32 > lastEkfUpdateMs); + const bool hasSSR = (ssrLocalMs32 > lastEkfUpdateMs); + + if (hasVehicle && hasSSR) + { + source = Source::VehicleAndSSR; + candidateTs = (ssrLocalMs32 > zumoLocalMs32) ? ssrLocalMs32 : zumoLocalMs32; + } + else if (hasVehicle) + { + source = Source::Vehicle; + candidateTs = zumoLocalMs32; + } + else if (hasSSR) + { + source = Source::SSR; + candidateTs = ssrLocalMs32; + } + + newestLocalTs = candidateTs; + return source; +} + +void App::updateFromVehicle(const VehicleData& vehicleData) +{ + const int16_t rawGyroZ = static_cast(vehicleData.turnRateZ); + float xGlob_mm = 0.0F; + float yGlob_mm = 0.0F; + float thetaGlob_mrad = 0.0F; + OdoMeasVector z_odo; + + LOG_INFO("EKF update from Vehicle."); + + /* IMU update using yaw-rate only. */ + m_ekf.updateImuFromDigits(rawGyroZ); + + /* Convert odometry into global coordinates. */ + transformOdometryToGlobal(vehicleData, xGlob_mm, yGlob_mm, thetaGlob_mrad); + + /* Update odometry measurement: + * z_odo(0) = v + * z_odo(1) = theta + */ + z_odo(0) = static_cast(vehicleData.center); + z_odo(1) = thetaGlob_mrad; + + m_ekf.updateOdometry(z_odo); +} + +void App::updateFromSsr(const SpaceShipRadarPose& ssrPose) +{ + CamMeasVector z_cam; + + LOG_INFO("EKF update from SSR."); + + /* Camera measurement: + * z_cam(0..4) = [x, y, theta, v_x, v_y] + */ + z_cam(0) = ssrPose.x; + z_cam(1) = ssrPose.y; + z_cam(2) = ssrPose.theta; + z_cam(3) = ssrPose.v_x; + z_cam(4) = ssrPose.v_y; + + m_ekf.updateCamera(z_cam); +} + + /****************************************************************************** * External Functions diff --git a/lib/APPLineFollowerSensorFusion/src/App.h b/lib/APPLineFollowerSensorFusion/src/App.h index 04d6e96a..359f5160 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.h +++ b/lib/APPLineFollowerSensorFusion/src/App.h @@ -256,6 +256,13 @@ class App */ void publishVehicleAndSensorSnapshot(const VehicleData& data); + /** + * @brief Callback for receiving vehicle data via SerialMux. + * + * @param[in] data Vehicle data received via SerialMux. + */ + void onVehicleData(const VehicleData& data); + /** * @brief Run sensor fusion for vehicle data and Space Ship Radar pose. * @@ -295,6 +302,40 @@ class App */ void publishFusionPose(uint32_t tsMs); + /** + * @brief Update EKF from vehicle data (odometry + IMU). + * + * @param[in] vehicleData Vehicle data received via SerialMux. + */ + void updateFromVehicle(const VehicleData& vehicleData); + + /** + * @brief Update EKF from Space Ship Radar pose. + * + * @param[in] ssrPose Latest Space Ship Radar pose (global frame). + */ + void updateFromSsr(const SpaceShipRadarPose& ssrPose); + + /** + * @brief Determine which sensor source has the newest data for EKF update. + * + * @param[in] zumoLocalMs32 Latest vehicle data timestamp [ms] (local time base). + * @param[in] ssrLocalMs32 Latest SSR pose timestamp [ms] (local time base). + * @param[in] lastEkfUpdateMs Last EKF update timestamp [ms] (local time base). + * @param[out] newestLocalTs Newest local timestamp [ms] among the sources. + * + * @return Source enum indicating which sensor has the newest data. + */ + void determineNewestSource(uint32_t zumoLocalMs32, + uint32_t ssrLocalMs32, + uint32_t lastEkfUpdateMs, + uint32_t& newestLocalTs) const; + + + void initializeEkfTimestamp(uint32_t zumoLocalMs32, + uint32_t ssrLocalMs32); + + /** * @brief Copy construction of an instance (not allowed). * diff --git a/lib/APPLineFollowerSensorFusion/src/EKF.cpp b/lib/APPLineFollowerSensorFusion/src/EKF.cpp index 0a22b2d0..cbc16113 100644 --- a/lib/APPLineFollowerSensorFusion/src/EKF.cpp +++ b/lib/APPLineFollowerSensorFusion/src/EKF.cpp @@ -41,31 +41,30 @@ namespace constexpr float PI_MRAD = 1000.0F * static_cast(M_PI); constexpr float TWO_PI_MRAD = 2.0F * PI_MRAD; - // Default process noise standard deviations. - constexpr float SIGMA_PX = 1.0F; // [mm] - constexpr float SIGMA_PY = 1.0F; // [mm] - constexpr float SIGMA_THETA_P = 5.0F; // [mrad] - constexpr float SIGMA_V_P = 5.0F; // [mm/s] - constexpr float SIGMA_OMEGA_P = 2.0F; // [mrad/s] - - // Camera noise. - constexpr float SIGMA_CAM_POS = 2.0F; // [mm] - constexpr float SIGMA_CAM_THETA = 5.0F; // [mrad] - constexpr float SIGMA_CAM_V = 20.0F; // [mm/s] - - // Odometry noise (only v and theta). - constexpr float SIGMA_ODO_V = 10.0F; // [mm/s] - constexpr float SIGMA_ODO_THETA = 5.0F; // [mrad] - - // IMU yaw noise. - constexpr float SIGMA_IMU_OMEGA = 1.0F; // [mrad/s] - - // Default initial state. - constexpr float EKF_START_X_MM = 705.0F; // [mm] - constexpr float EKF_START_Y_MM = 939.0F; // [mm] - constexpr float EKF_START_THETA_MRAD = 0.0F; // [mrad] - constexpr float EKF_START_V_MMS = 0.0F; // [mm/s] - constexpr float EKF_START_OMEGA_MRADPS = 0.0F; // [mrad/s] + /** Default process noise standard deviations. */ + constexpr float SIGMA_PX = 1.0F; /* [mm] */ + constexpr float SIGMA_PY = 1.0F; /* [mm] */ + constexpr float SIGMA_THETA_P = 5.0F; /* [mrad] */ + constexpr float SIGMA_V_P = 5.0F; /* [mm/s] */ + constexpr float SIGMA_OMEGA_P = 2.0F; /* [mrad/s] */ + + /** Camera noise. */ + constexpr float SIGMA_CAM_POS = 2.0F; /* [mm] */ + constexpr float SIGMA_CAM_THETA = 5.0F; /* [mrad] */ + constexpr float SIGMA_CAM_V = 20.0F; /* [mm/s] */ + + /** Odometry noise (only v and theta). */ + constexpr float SIGMA_ODO_V = 10.0F; /* [mm/s] */ + constexpr float SIGMA_ODO_THETA = 5.0F; /* [mrad] */ + + /** IMU yaw noise. */ + constexpr float SIGMA_IMU_OMEGA = 1.0F; /* [mrad/s] */ + /** Default initial state. */ + constexpr float EKF_START_X_MM = 705.0F; /* [mm] */ + constexpr float EKF_START_Y_MM = 939.0F; /* [mm] */ + constexpr float EKF_START_THETA_MRAD = 0.0F; /* [mrad] */ + constexpr float EKF_START_V_MMS = 0.0F; /* [mm/s] */ + constexpr float EKF_START_OMEGA_MRADS = 0.0F; /* [mrad/s] */ } /****************************************************************************** @@ -104,11 +103,10 @@ ExtendedKalmanFilter5D::ExtendedKalmanFilter5D() m_R_imu(0, 0) = SIGMA_IMU_OMEGA * SIGMA_IMU_OMEGA; } -bool ExtendedKalmanFilter5D::init(const StateVector& x0, const StateMatrix& P0) +void ExtendedKalmanFilter5D::init(const StateVector& x0, const StateMatrix& P0) { m_state = x0; m_covariance = P0; - return true; } bool ExtendedKalmanFilter5D::init() @@ -118,10 +116,9 @@ bool ExtendedKalmanFilter5D::init() m_state(1) = EKF_START_Y_MM; m_state(2) = EKF_START_THETA_MRAD; m_state(3) = EKF_START_V_MMS; - m_state(4) = EKF_START_OMEGA_MRADPS; + m_state(4) = EKF_START_OMEGA_MRADS; m_covariance.setIdentity(); - return true; } void ExtendedKalmanFilter5D::predict(float accX_raw, float dt) @@ -151,7 +148,7 @@ void ExtendedKalmanFilter5D::updateCamera(const CamMeasVector& z_cam) { /* Predicted measurement. */ CamMeasVector z_pred = cameraModel(m_state); - auto H = cameraJacobianH(m_state); + Eigen::Matrix H = cameraJacobianH(m_state); /* Innovation. */ CamMeasVector y = z_cam - z_pred; @@ -173,7 +170,7 @@ void ExtendedKalmanFilter5D::updateOdometry(const OdoMeasVector& z_odo) { /* Predicted measurement. */ OdoMeasVector z_pred = odometryModel(m_state); - auto H = odometryJacobianH(m_state); + Eigen::Matrix H = odometryJacobianH(m_state); /* Innovation. */ OdoMeasVector y = z_odo - z_pred; @@ -195,8 +192,7 @@ void ExtendedKalmanFilter5D::updateImu(const ImuMeasVector& z_imu) { /* Predicted measurement. */ ImuMeasVector z_pred = imuModel(m_state); - auto H = imuJacobianH(m_state); - + Eigen::Matrix H = imuJacobianH(m_state); /* Innovation (omega, no wrapping). */ ImuMeasVector y = z_imu - z_pred; @@ -233,16 +229,16 @@ ExtendedKalmanFilter5D::processModel(const StateVector& x, const float py = x(1); const float thetaMrad = x(2); const float v = x(3); - const float omegaMrad = x(4); /* already in mrad/s */ + const float omegaMrads = x(4); const float thetaRad = thetaMrad / 1000.0F; StateVector x_next; x_next(0) = px + v * std::cos(thetaRad) * dt; /* p_x */ x_next(1) = py + v * std::sin(thetaRad) * dt; /* p_y */ - x_next(2) = thetaMrad + omegaMrad * dt; /* theta [mrad] */ + x_next(2) = thetaMrad + omegaMrads * dt; /* theta [mrad] */ x_next(3) = v + a_x_mms * dt; /* v */ - x_next(4) = omegaMrad; /* omega */ + x_next(4) = omegaMrads; /* omega */ return x_next; } diff --git a/lib/APPLineFollowerSensorFusion/src/EKF.h b/lib/APPLineFollowerSensorFusion/src/EKF.h index f83b91bf..6c702f49 100644 --- a/lib/APPLineFollowerSensorFusion/src/EKF.h +++ b/lib/APPLineFollowerSensorFusion/src/EKF.h @@ -106,9 +106,8 @@ class ExtendedKalmanFilter5D * @param[in] x0 Initial state vector (physical units). * @param[in] P0 Initial covariance matrix. * - * @return True if successful, otherwise false. */ - bool init(const StateVector& x0, const StateMatrix& P0); + void init(const StateVector& x0, const StateMatrix& P0); /** * Initializes the EKF with a fixed default start pose and identity covariance. @@ -119,10 +118,8 @@ class ExtendedKalmanFilter5D * theta = EKF_START_THETA_MRAD * v = 0 * omega = 0 - * - * @return True if successful, otherwise false. */ - bool init(); + void init(); /** * EKF prediction step. diff --git a/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp b/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp index 571b255b..e59b93c7 100644 --- a/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp +++ b/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp @@ -205,14 +205,13 @@ void TimeSync::onHostTimeSyncResponse(uint32_t seq, uint64_t t3HostMs, uint64_t t4EspMs) { - // NTP-style formulas: - // offset = ((T2 - T1) + (T3 - T4)) / 2 - // rtt = (T4 - T1) - (T3 - T2) - // - // T1 = t1EspMs (ESP) - // T2 = t2HostMs (Host) - // T3 = t3HostMs (Host) - // T4 = t4EspMs (ESP) + /* NTP-style formulas: */ + /* offset = ((T2 - T1) + (T3 - T4)) / 2 */ + /* rtt = (T4 - T1) - (T3 - T2) */ + /* T1 = t1EspMs (ESP) */ + /* T2 = t2HostMs (Host) */ + /* T3 = t3HostMs (Host) */ + /* T4 = t4EspMs (ESP) */ LOG_INFO("Host sync: seq=%u", static_cast(seq)); LOG_INFO("t1=%llu ms", t1EspMs); LOG_INFO("t2=%llu ms", t2HostMs); @@ -232,7 +231,7 @@ void TimeSync::onHostTimeSyncResponse(uint32_t seq, m_lastHostRttMs = (rtt >= 0) ? static_cast(rtt) : 0U; m_hostSyncValid = true; - const double offset_s = static_cast(m_hostOffsetMs) / 1000.0; + const float offset_s = static_cast(m_hostOffsetMs) / 1000.0F; LOG_INFO("Host sync: seq=%u offset=%.3f s rtt=%lu ms", static_cast(seq), From 70a4c213031aaf1b4e2c0832ebd707a755b044c8 Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Thu, 11 Dec 2025 17:35:31 +0100 Subject: [PATCH 08/14] Changed some Variable names, so it is consitent now --- lib/APPLineFollowerSensorFusion/src/App.cpp | 11 +++---- lib/APPLineFollowerSensorFusion/src/App.h | 15 +++++++-- lib/APPLineFollowerSensorFusion/src/EKF.cpp | 34 ++++++++++----------- lib/APPLineFollowerSensorFusion/src/EKF.h | 18 +++++------ 4 files changed, 44 insertions(+), 34 deletions(-) diff --git a/lib/APPLineFollowerSensorFusion/src/App.cpp b/lib/APPLineFollowerSensorFusion/src/App.cpp index 938a81e9..399b320f 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.cpp +++ b/lib/APPLineFollowerSensorFusion/src/App.cpp @@ -123,6 +123,8 @@ using CamMeasurementVector = ExtendedKalmanFilter5D::CamMeasurementVector; using StateVector = ExtendedKalmanFilter5D::StateVector; using StateMatrix = ExtendedKalmanFilter5D::StateMatrix; + + /****************************************************************************** * Public Methods *****************************************************************************/ @@ -209,10 +211,6 @@ void App::setup() { LOG_FATAL("MQTT connection could not be setup."); } - else if (false == m_ekf.init()) - { - LOG_FATAL("Extended Kalman Filter could not be initialized."); - } else { /* Log incoming vehicle data and corresponding time sync information. */ @@ -220,6 +218,7 @@ void App::setup() [this](const VehicleData& data) { onVehicleData(data); }); /* Start network time (NTP) against host and Zumo serial ping-pong. */ + m_ekf.init(); m_timeSync.begin(); m_statusTimer.start(STATUS_SEND_INTERVAL_MS); m_hostTimeSyncTimer.start(HOST_TIMESYNC_INTERVAL_MS); @@ -710,7 +709,7 @@ void App::updateFromVehicle(const VehicleData& vehicleData) float xGlob_mm = 0.0F; float yGlob_mm = 0.0F; float thetaGlob_mrad = 0.0F; - OdoMeasVector z_odo; + OdoMeasurementVector z_odo; LOG_INFO("EKF update from Vehicle."); @@ -732,7 +731,7 @@ void App::updateFromVehicle(const VehicleData& vehicleData) void App::updateFromSsr(const SpaceShipRadarPose& ssrPose) { - CamMeasVector z_cam; + CamMeasurementVector z_cam; LOG_INFO("EKF update from SSR."); diff --git a/lib/APPLineFollowerSensorFusion/src/App.h b/lib/APPLineFollowerSensorFusion/src/App.h index 359f5160..238df4f3 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.h +++ b/lib/APPLineFollowerSensorFusion/src/App.h @@ -76,6 +76,17 @@ struct SpaceShipRadarPose float v_y; /**< Velocity in Y direction in mm/s. */ }; +/** + * @brief Data source for sensor fusion. + */ +enum class Source +{ + None, + Vehicle, + SSR, + VehicleAndSSR +}; + /** * @brief Line follower Sensor Fusion application. */ @@ -326,13 +337,13 @@ class App * * @return Source enum indicating which sensor has the newest data. */ - void determineNewestSource(uint32_t zumoLocalMs32, + Source determineNewestSource(uint32_t zumoLocalMs32, uint32_t ssrLocalMs32, uint32_t lastEkfUpdateMs, uint32_t& newestLocalTs) const; - void initializeEkfTimestamp(uint32_t zumoLocalMs32, + bool initializeEkfTimestamp(uint32_t zumoLocalMs32, uint32_t ssrLocalMs32); diff --git a/lib/APPLineFollowerSensorFusion/src/EKF.cpp b/lib/APPLineFollowerSensorFusion/src/EKF.cpp index cbc16113..4c9edbf2 100644 --- a/lib/APPLineFollowerSensorFusion/src/EKF.cpp +++ b/lib/APPLineFollowerSensorFusion/src/EKF.cpp @@ -109,7 +109,7 @@ void ExtendedKalmanFilter5D::init(const StateVector& x0, const StateMatrix& P0) m_covariance = P0; } -bool ExtendedKalmanFilter5D::init() +void ExtendedKalmanFilter5D::init() { m_state.setZero(); m_state(0) = EKF_START_X_MM; @@ -144,14 +144,14 @@ void ExtendedKalmanFilter5D::predict(float accX_raw, float dt) m_covariance = P_pred; } -void ExtendedKalmanFilter5D::updateCamera(const CamMeasVector& z_cam) +void ExtendedKalmanFilter5D::updateCamera(const CamMeasurementVector& z_cam) { /* Predicted measurement. */ - CamMeasVector z_pred = cameraModel(m_state); + CamMeasurementVector z_pred = cameraModel(m_state); Eigen::Matrix H = cameraJacobianH(m_state); /* Innovation. */ - CamMeasVector y = z_cam - z_pred; + CamMeasurementVector y = z_cam - z_pred; /* Wrap angle innovation (index 2 is theta). */ y(2) = wrapAngleMrad(y(2)); @@ -166,14 +166,14 @@ void ExtendedKalmanFilter5D::updateCamera(const CamMeasVector& z_cam) m_covariance = (StateMatrix::Identity() - K * H) * m_covariance; } -void ExtendedKalmanFilter5D::updateOdometry(const OdoMeasVector& z_odo) +void ExtendedKalmanFilter5D::updateOdometry(const OdoMeasurementVector& z_odo) { /* Predicted measurement. */ - OdoMeasVector z_pred = odometryModel(m_state); + OdoMeasurementVector z_pred = odometryModel(m_state); Eigen::Matrix H = odometryJacobianH(m_state); /* Innovation. */ - OdoMeasVector y = z_odo - z_pred; + OdoMeasurementVector y = z_odo - z_pred; /* Wrap angle innovation (index 1 is theta). */ y(1) = wrapAngleMrad(y(1)); @@ -188,13 +188,13 @@ void ExtendedKalmanFilter5D::updateOdometry(const OdoMeasVector& z_odo) m_covariance = (StateMatrix::Identity() - K * H) * m_covariance; } -void ExtendedKalmanFilter5D::updateImu(const ImuMeasVector& z_imu) +void ExtendedKalmanFilter5D::updateImu(const ImuMeasurementVector& z_imu) { /* Predicted measurement. */ - ImuMeasVector z_pred = imuModel(m_state); + ImuMeasurementVector z_pred = imuModel(m_state); Eigen::Matrix H = imuJacobianH(m_state); /* Innovation (omega, no wrapping). */ - ImuMeasVector y = z_imu - z_pred; + ImuMeasurementVector y = z_imu - z_pred; /* EKF update. */ const ImuMeasMatrix S = H * m_covariance * H.transpose() + m_R_imu; @@ -208,7 +208,7 @@ void ExtendedKalmanFilter5D::updateImu(const ImuMeasVector& z_imu) void ExtendedKalmanFilter5D::updateImuFromDigits(int16_t rawGyroZ) { - ImuMeasVector z_imu; + ImuMeasurementVector z_imu; /* Convert raw gyro digits to physical yaw rate [mrad/s]. */ z_imu(0) = static_cast(rawGyroZ) * SensorConstants::GYRO_SENSITIVITY_FACTOR; @@ -270,7 +270,7 @@ ExtendedKalmanFilter5D::processJacobianF(const StateVector& x, float dt) const /* ---------------- Camera model ---------------- */ -ExtendedKalmanFilter5D::CamMeasVector +ExtendedKalmanFilter5D::CamMeasurementVector ExtendedKalmanFilter5D::cameraModel(const StateVector& x) const { const float px = x(0); @@ -280,7 +280,7 @@ ExtendedKalmanFilter5D::cameraModel(const StateVector& x) const const float thetaRad = thetaMrad / 1000.0F; - CamMeasVector z; + CamMeasurementVector z; z(0) = px; /* p_x */ z(1) = py; /* p_y */ z(2) = thetaMrad; /* theta [mrad] */ @@ -319,10 +319,10 @@ ExtendedKalmanFilter5D::cameraJacobianH(const StateVector& x) const /* ---------------- Odometry model ---------------- */ -ExtendedKalmanFilter5D::OdoMeasVector +ExtendedKalmanFilter5D::OdoMeasurementVector ExtendedKalmanFilter5D::odometryModel(const StateVector& x) const { - OdoMeasVector z; + OdoMeasurementVector z; z(0) = x(3); /* v */ z(1) = x(2); /* theta */ return z; @@ -344,10 +344,10 @@ ExtendedKalmanFilter5D::odometryJacobianH(const StateVector& /*x*/) const /* ---------------- IMU yaw model ---------------- */ -ExtendedKalmanFilter5D::ImuMeasVector +ExtendedKalmanFilter5D::ImuMeasurementVector ExtendedKalmanFilter5D::imuModel(const StateVector& x) const { - ImuMeasVector z; + ImuMeasurementVector z; z(0) = x(4); /* omega [mrad/s] */ return z; } diff --git a/lib/APPLineFollowerSensorFusion/src/EKF.h b/lib/APPLineFollowerSensorFusion/src/EKF.h index 6c702f49..7598604b 100644 --- a/lib/APPLineFollowerSensorFusion/src/EKF.h +++ b/lib/APPLineFollowerSensorFusion/src/EKF.h @@ -79,17 +79,17 @@ class ExtendedKalmanFilter5D using StateMatrix = Eigen::Matrix; /** @brief Camera measurement vector z_cam = [p_x, p_y, theta, v_x, v_y]^T. */ - using CamMeasVector = Eigen::Matrix; + using CamMeasurementVector = Eigen::Matrix; /** @brief Camera measurement covariance matrix R_cam. */ using CamMeasMatrix = Eigen::Matrix; /** @brief Odometry measurement vector z_odo = [v, theta]^T. */ - using OdoMeasVector = Eigen::Matrix; + using OdoMeasurementVector = Eigen::Matrix; /** @brief Odometry measurement covariance matrix R_odo. */ using OdoMeasMatrix = Eigen::Matrix; /** @brief IMU measurement vector z_imu = [omega]^T. */ - using ImuMeasVector = Eigen::Matrix; + using ImuMeasurementVector = Eigen::Matrix; /** @brief IMU measurement covariance matrix R_imu. */ using ImuMeasMatrix = Eigen::Matrix; @@ -148,7 +148,7 @@ class ExtendedKalmanFilter5D * * @param[in] z_cam Camera measurement vector. */ - void updateCamera(const CamMeasVector& z_cam); + void updateCamera(const CamMeasurementVector& z_cam); /** * EKF update step for odometry measurements. @@ -161,7 +161,7 @@ class ExtendedKalmanFilter5D * * @param[in] z_odo Odometry measurement vector. */ - void updateOdometry(const OdoMeasVector& z_odo); + void updateOdometry(const OdoMeasurementVector& z_odo); /** * EKF update step for IMU measurements (physical units). @@ -174,7 +174,7 @@ class ExtendedKalmanFilter5D * * @param[in] z_imu IMU measurement vector (omega in mrad/s). */ - void updateImu(const ImuMeasVector& z_imu); + void updateImu(const ImuMeasurementVector& z_imu); /** * EKF update step for IMU yaw rate using raw gyro digits. @@ -250,7 +250,7 @@ class ExtendedKalmanFilter5D * * @return Camera measurement prediction. */ - CamMeasVector cameraModel(const StateVector& x) const; + CamMeasurementVector cameraModel(const StateVector& x) const; /** * Camera measurement Jacobian H_cam = dh_cam/dx. @@ -268,7 +268,7 @@ class ExtendedKalmanFilter5D * * @return Odometry measurement prediction. */ - OdoMeasVector odometryModel(const StateVector& x) const; + OdoMeasurementVector odometryModel(const StateVector& x) const; /** * Odometry measurement Jacobian H_odo = dh_odo/dx. @@ -286,7 +286,7 @@ class ExtendedKalmanFilter5D * * @return IMU measurement prediction. */ - ImuMeasVector imuModel(const StateVector& x) const; + ImuMeasurementVector imuModel(const StateVector& x) const; /** * IMU measurement Jacobian H_imu = dh_imu/dx. From a70d80e2d6ecd749a5ce314c865056932341defd Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Thu, 11 Dec 2025 17:38:02 +0100 Subject: [PATCH 09/14] added doxygen comment --- lib/APPLineFollowerSensorFusion/src/App.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/lib/APPLineFollowerSensorFusion/src/App.h b/lib/APPLineFollowerSensorFusion/src/App.h index 238df4f3..258aec81 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.h +++ b/lib/APPLineFollowerSensorFusion/src/App.h @@ -81,10 +81,10 @@ struct SpaceShipRadarPose */ enum class Source { - None, - Vehicle, - SSR, - VehicleAndSSR + None, /**< No new data available (no newer timestamp than last EKF update). */ + Vehicle, /**< Newest data comes from vehicle (odometry / IMU). */ + SSR, /**< Newest data comes from Space Ship Radar (SSR). */ + VehicleAndSSR /**< Both vehicle and SSR provide newer data; use both updates. */ }; /** From cd20ffe2f0845a6909f590543342785983977583 Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Thu, 11 Dec 2025 17:41:42 +0100 Subject: [PATCH 10/14] More Doxygen --- lib/APPLineFollowerSensorFusion/src/App.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/lib/APPLineFollowerSensorFusion/src/App.h b/lib/APPLineFollowerSensorFusion/src/App.h index 258aec81..e3ecc4e6 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.h +++ b/lib/APPLineFollowerSensorFusion/src/App.h @@ -343,6 +343,12 @@ class App uint32_t& newestLocalTs) const; + /** + * @brief Initialize EKF timestamp on first data reception. + * + * @param[in] zumoLocalMs32 Latest vehicle data timestamp [ms] (local time base). + * @param[in] ssrLocalMs32 Latest SSR pose timestamp [ms] + */ bool initializeEkfTimestamp(uint32_t zumoLocalMs32, uint32_t ssrLocalMs32); From 955fcb6dff8d8600b2da425d492fca436551b55a Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Thu, 11 Dec 2025 17:43:55 +0100 Subject: [PATCH 11/14] And even more doxygen --- lib/APPLineFollowerSensorFusion/src/App.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/APPLineFollowerSensorFusion/src/App.h b/lib/APPLineFollowerSensorFusion/src/App.h index e3ecc4e6..2acc01dd 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.h +++ b/lib/APPLineFollowerSensorFusion/src/App.h @@ -348,6 +348,8 @@ class App * * @param[in] zumoLocalMs32 Latest vehicle data timestamp [ms] (local time base). * @param[in] ssrLocalMs32 Latest SSR pose timestamp [ms] + * + * @return true if EKF timestamp is initialized, otherwise false. */ bool initializeEkfTimestamp(uint32_t zumoLocalMs32, uint32_t ssrLocalMs32); From a7a868337098d0a1d573d36e810f7fc8e4a728cd Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Sat, 13 Dec 2025 13:56:15 +0100 Subject: [PATCH 12/14] Updated EKF Model --- lib/APPLineFollowerSensorFusion/src/EKF.cpp | 33 +++++++++++++++------ lib/APPLineFollowerSensorFusion/src/EKF.h | 6 ++-- 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/lib/APPLineFollowerSensorFusion/src/EKF.cpp b/lib/APPLineFollowerSensorFusion/src/EKF.cpp index 4c9edbf2..6a4d9335 100644 --- a/lib/APPLineFollowerSensorFusion/src/EKF.cpp +++ b/lib/APPLineFollowerSensorFusion/src/EKF.cpp @@ -53,12 +53,15 @@ namespace constexpr float SIGMA_CAM_THETA = 5.0F; /* [mrad] */ constexpr float SIGMA_CAM_V = 20.0F; /* [mm/s] */ - /** Odometry noise (only v and theta). */ + /** Odometry noise. */ + constexpr float SIGMA_ODO_POS_X = 20.0F; /* [mm] */ + constexpr float SIGMA_ODO_POS_Y = 20.0F; /* [mm] */ constexpr float SIGMA_ODO_V = 10.0F; /* [mm/s] */ constexpr float SIGMA_ODO_THETA = 5.0F; /* [mrad] */ /** IMU yaw noise. */ constexpr float SIGMA_IMU_OMEGA = 1.0F; /* [mrad/s] */ + /** Default initial state. */ constexpr float EKF_START_X_MM = 705.0F; /* [mm] */ constexpr float EKF_START_Y_MM = 939.0F; /* [mm] */ @@ -95,8 +98,10 @@ ExtendedKalmanFilter5D::ExtendedKalmanFilter5D() /* Odometry measurement noise R_odo. */ m_R_odo.setZero(); - m_R_odo(0, 0) = SIGMA_ODO_V * SIGMA_ODO_V; - m_R_odo(1, 1) = SIGMA_ODO_THETA * SIGMA_ODO_THETA; + m_R_odo(0, 0) = SIGMA_ODO_POS_X * SIGMA_ODO_POS_X; + m_R_odo(1, 1) = SIGMA_ODO_POS_Y * SIGMA_ODO_POS_Y; + m_R_odo(2, 2) = SIGMA_ODO_V * SIGMA_ODO_V; + m_R_odo(3, 3) = SIGMA_ODO_THETA * SIGMA_ODO_THETA; /* IMU yaw noise R_imu. */ m_R_imu.setZero(); @@ -175,8 +180,8 @@ void ExtendedKalmanFilter5D::updateOdometry(const OdoMeasurementVector& z_odo) /* Innovation. */ OdoMeasurementVector y = z_odo - z_pred; - /* Wrap angle innovation (index 1 is theta). */ - y(1) = wrapAngleMrad(y(1)); + /* Wrap angle innovation (index 4 is theta). */ + y(3) = wrapAngleMrad(y(3)); /* EKF update. */ const OdoMeasMatrix S = H * m_covariance * H.transpose() + m_R_odo; @@ -323,21 +328,31 @@ ExtendedKalmanFilter5D::OdoMeasurementVector ExtendedKalmanFilter5D::odometryModel(const StateVector& x) const { OdoMeasurementVector z; - z(0) = x(3); /* v */ - z(1) = x(2); /* theta */ + z.setZero(); + + z(0) = x(0); /* p_x */ + z(1) = x(1); /* p_y */ + z(2) = x(3); /* v */ + z(3) = x(2); /* theta [mrad] */ + return z; } + Eigen::Matrix ExtendedKalmanFilter5D::odometryJacobianH(const StateVector& /*x*/) const { Eigen::Matrix H; H.setZero(); + /* p_x */ + H(0, 0) = 1.0F; + /* p_y */ + H(1, 1) = 1.0F; /* v */ - H(0, 3) = 1.0F; + H(2, 3) = 1.0F; /* theta */ - H(1, 2) = 1.0F; + H(3, 2) = 1.0F; return H; } diff --git a/lib/APPLineFollowerSensorFusion/src/EKF.h b/lib/APPLineFollowerSensorFusion/src/EKF.h index 7598604b..2d573947 100644 --- a/lib/APPLineFollowerSensorFusion/src/EKF.h +++ b/lib/APPLineFollowerSensorFusion/src/EKF.h @@ -67,8 +67,8 @@ class ExtendedKalmanFilter5D static constexpr std::uint8_t STATE_DIM = 5U; /** Camera measurement dimension: [x, y, theta, v_x, v_y]. */ static constexpr std::uint8_t CAM_MEAS_DIM = 5U; - /** Odometry measurement dimension: [v, theta]. */ - static constexpr std::uint8_t ODO_MEAS_DIM = 2U; + /** Odometry measurement dimension: [x, y, v, theta]. */ + static constexpr std::uint8_t ODO_MEAS_DIM = 4U; /** IMU measurement dimension: [omega]. */ static constexpr std::uint8_t IMU_MEAS_DIM = 1U; @@ -83,7 +83,7 @@ class ExtendedKalmanFilter5D /** @brief Camera measurement covariance matrix R_cam. */ using CamMeasMatrix = Eigen::Matrix; - /** @brief Odometry measurement vector z_odo = [v, theta]^T. */ + /** @brief Odometry measurement vector z_odo = [p_x, p_y, v, theta]^T. */ using OdoMeasurementVector = Eigen::Matrix; /** @brief Odometry measurement covariance matrix R_odo. */ using OdoMeasMatrix = Eigen::Matrix; From e679a2c123dd08aebcf1d4486b5ef0855ca48dff Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Thu, 18 Dec 2025 12:50:30 +0100 Subject: [PATCH 13/14] Rebuild Startup Script --- start_sensorFusion.ps1 | 153 +++++++++++++++++++++++++++++------------ 1 file changed, 110 insertions(+), 43 deletions(-) diff --git a/start_sensorFusion.ps1 b/start_sensorFusion.ps1 index bd941659..137fd414 100644 --- a/start_sensorFusion.ps1 +++ b/start_sensorFusion.ps1 @@ -1,78 +1,145 @@ -$envFilePath = ".\.env" +#requires -Version 5.1 +Set-StrictMode -Version Latest +$ErrorActionPreference = "Stop" -$content = Get-Content -Path $envFilePath +# ============================================================================= +# Helpers +# ============================================================================= +function Find-Upwards { + param( + [Parameter(Mandatory)][string]$StartDir, + [Parameter(Mandatory)][string]$Target + ) -if (-Not $content) { - Write-Output "Create a file with named '.env' (See README)" - Exit -} + $dir = (Resolve-Path $StartDir).Path + while ($true) { + $candidate = Join-Path $dir $Target + if (Test-Path -LiteralPath $candidate) { return (Resolve-Path $candidate).Path } -$RadonUlzerfilePathLine = $content | Where-Object { $_ -match '^RadonUlzer_RemoteControlSim_PATH=' } -if ($RadonUlzerfilePathLine) { - $RadonUlzerFilePath = $RadonUlzerfilePathLine -split '=' | Select-Object -Last 1 - Write-Output "Der Dateipfad ist: $RadonUlzerFilePath" -} else { - Write-Output "Key RadonUlzer_RemoteControlSim_PATH not found" - Exit + $parent = Split-Path -Parent $dir + if ($parent -eq $dir) { break } + $dir = $parent + } + throw "Not found: '$Target' starting from '$StartDir'" } -$SpaceShipRadarfilePathLine = $content | Where-Object { $_ -match '^SpaceShipRadar_PATH=' } -if ($SpaceShipRadarfilePathLine) { - $SpaceShipRadarFilePath = $SpaceShipRadarfilePathLine -split '=' | Select-Object -Last 1 - Write-Output "Der Dateipfad ist: $SpaceShipRadarFilePath" -} else { - Write-Output "Key SpaceShipRadar_PATH not found" - Exit +function Get-GitRepoRoot { + param([Parameter(Mandatory)][string]$StartDir) + $gitPath = Find-Upwards -StartDir $StartDir -Target ".git" + return (Split-Path -Parent $gitPath) } -$DCSfilePathLine = $content | Where-Object { $_ -match '^DroidControlShip_PATH=' } -if ($DCSfilePathLine) { - $DCSFilePath = $DCSfilePathLine -split '=' | Select-Object -Last 1 - Write-Output "Der Dateipfad ist: $DCSFilePath" -} else { - Write-Output "Key DroidControlShip_PATH not found" - Exit +function Assert-Exists { + param( + [Parameter(Mandatory)][string]$Path, + [Parameter(Mandatory)][string]$Label + ) + if (-not (Test-Path -LiteralPath $Path)) { + throw "$Label not found: $Path" + } } -# Pfad zum webots-controller +# ============================================================================= +# A) Script -> RepoRoot -> Workspace +# ============================================================================= +$ScriptPath = $MyInvocation.MyCommand.Path +$ScriptDir = Split-Path -Parent $ScriptPath +$ScriptRepoRoot = Get-GitRepoRoot -StartDir $ScriptDir +$WorkspaceDir = Split-Path -Parent $ScriptRepoRoot + +Write-Host "Script: $ScriptPath" +Write-Host "ScriptDir: $ScriptDir" +Write-Host "RepoRoot: $ScriptRepoRoot" +Write-Host "Workspace: $WorkspaceDir" + +# ============================================================================= +# B) Repos in workspace (adjust names here if needed) +# ============================================================================= +$RadonUlzerRepo = Join-Path $WorkspaceDir "RadonUlzer" +$SSRRepo = Join-Path $WorkspaceDir "SpaceShipRadar" +$DCSRepo = Join-Path $WorkspaceDir "DroidControlShip" + +Assert-Exists $RadonUlzerRepo "RadonUlzer repo" +Assert-Exists $SSRRepo "SpaceShipRadar repo" +Assert-Exists $DCSRepo "DroidControlShip repo" + +Write-Host "`nRepos:" +Write-Host " RadonUlzer: $RadonUlzerRepo" +Write-Host " SpaceShipRadar: $SSRRepo" +Write-Host " DroidControlShip:$DCSRepo" + +# ============================================================================= +# C) Derive targets (relative to workspace) +# ============================================================================= +$RadonUlzer_RemoteControlSim = Join-Path $RadonUlzerRepo ".pio\build\RemoteControlSim\program.exe" +$DCS_LineFollowerSensorFusionSim = Join-Path $DCSRepo ".pio\build\LineFollowerSensorFusionSim\program.exe" + +$SSR_Script = Join-Path $SSRRepo "src\space_ship_radar\space_ship_radar.py" +$SSR_CalibrationFolder = Join-Path $SSRRepo "src\calibration\" + +$RadonUlzer_Settings = Join-Path $RadonUlzerRepo "settings\settings.json" + +# Path to webots-controller +if (-not $env:WEBOTS_HOME) { throw "WEBOTS_HOME is not set." } $webotsController = Join-Path $env:WEBOTS_HOME "msys64\mingw64\bin\webots-controller.exe" -# ===== RadonUlzer (Zumo / RemoteControlSim) ================================== -# Entspricht deinem PIO-Build-Aufruf: -# --robot-name=Zumo --stdout-redirect -c --supervisorRxCh 1 ... +# Validate +Assert-Exists $RadonUlzer_RemoteControlSim "RemoteControlSim program.exe" +Assert-Exists $DCS_LineFollowerSensorFusionSim "DCS program.exe" +Assert-Exists $SSR_Script "SpaceShipRadar script" +Assert-Exists $SSR_CalibrationFolder "Calibration folder" +Assert-Exists $RadonUlzer_Settings "RadonUlzer settings.json" +Assert-Exists $webotsController "webots-controller.exe" + +Write-Host "`nTargets:" +Write-Host " RadonUlzer RemoteControlSim: $RadonUlzer_RemoteControlSim" +Write-Host " DCS SensorFusionSim: $DCS_LineFollowerSensorFusionSim" +Write-Host " SSR script: $SSR_Script" +Write-Host " SSR calibration folder: $SSR_CalibrationFolder" +Write-Host " RadonUlzer settings.json: $RadonUlzer_Settings" +Write-Host " webots-controller.exe: $webotsController" + +# ============================================================================= +# IMPORTANT: +# This script assumes Webots is already running and the world contains +# controllers for the robots listed below. +# ============================================================================= + +# ============================================================================= +# Start RadonUlzer (Zumo / RemoteControlSim) +# ============================================================================= Start-Process -FilePath $webotsController ` -ArgumentList @( "--robot-name=Zumo", - "--stdout-redirect", $RadonUlzerFilePath, + "--stdout-redirect", $RadonUlzer_RemoteControlSim, "-c", "--supervisorRxCh", "1", "--supervisorTxCh", "2", "--serialRxCh", "3", "--serialTxCh", "4", - "--settingsPath", "C:\Users\thaeckel\Documents\Repos\RadonUlzer\./settings/settings.json", + "--settingsPath", $RadonUlzer_Settings, "-v" ) -# ===== SpaceShipRadar (MyBot) ================================================ +# ============================================================================= +# Start SpaceShipRadar (MyBot) +# ============================================================================= Start-Process -FilePath $webotsController ` -ArgumentList @( "--robot-name=MyBot", - "--stdout-redirect", $SpaceShipRadarFilePath + "--stdout-redirect", $SSR_Script ) -# ===== DroidControlShip / LineFollowerSensorFusionSim ======================== -# Hier spiegeln wir den PIO-Aufruf: -# --robot-name=ZumoComSystem --stdout-redirect \ -# --cfgFilePath "../../../data/config/config.json" --serialRxCh 4 --serialTxCh 3 -v - -# Arbeitsverzeichnis so setzen, dass der relative cfg-Pfad passt -$DCSWorkDir = Split-Path $DCSFilePath +# ============================================================================= +# Start DroidControlShip / LineFollowerSensorFusionSim +# ============================================================================= +$DCSWorkDir = Split-Path -Parent $DCS_LineFollowerSensorFusionSim Start-Process -FilePath $webotsController ` -WorkingDirectory $DCSWorkDir ` -ArgumentList @( "--robot-name=ZumoComSystem", - "--stdout-redirect", $DCSFilePath, + "--stdout-redirect", $DCS_LineFollowerSensorFusionSim, "--cfgFilePath", "../../../data/config/config_zumo1.json", "--serialRxCh", "4", "--serialTxCh", "3", From 28650bd55c57bda844378f5ff29f5116c24b3283 Mon Sep 17 00:00:00 2001 From: Tobias Haeckel Date: Fri, 19 Dec 2025 10:50:28 +0100 Subject: [PATCH 14/14] Changed File Names according to Robot Names, Startup Script and adjusted comment from // to /**/ --- .../{config_zumo5.json => config_zumo0.json} | 2 +- data/config/config_zumo1.json | 2 +- lib/APPLineFollowerSensorFusion/src/App.cpp | 126 +++++++----------- start_sensorFusion.ps1 | 52 ++++---- 4 files changed, 79 insertions(+), 103 deletions(-) rename data/config/{config_zumo5.json => config_zumo0.json} (95%) diff --git a/data/config/config_zumo5.json b/data/config/config_zumo0.json similarity index 95% rename from data/config/config_zumo5.json rename to data/config/config_zumo0.json index d410d67e..1f6e73b5 100644 --- a/data/config/config_zumo5.json +++ b/data/config/config_zumo0.json @@ -1,5 +1,5 @@ { - "robotName": "5", + "robotName": "0", "wifi": { "ssid": "", "pswd": "" diff --git a/data/config/config_zumo1.json b/data/config/config_zumo1.json index 1f6e73b5..161a5bea 100644 --- a/data/config/config_zumo1.json +++ b/data/config/config_zumo1.json @@ -1,5 +1,5 @@ { - "robotName": "0", + "robotName": "1", "wifi": { "ssid": "", "pswd": "" diff --git a/lib/APPLineFollowerSensorFusion/src/App.cpp b/lib/APPLineFollowerSensorFusion/src/App.cpp index 399b320f..4517c74b 100644 --- a/lib/APPLineFollowerSensorFusion/src/App.cpp +++ b/lib/APPLineFollowerSensorFusion/src/App.cpp @@ -46,7 +46,6 @@ #include "States/ReadyState.h" #include "States/DrivingState.h" - /****************************************************************************** * Compiler Switches *****************************************************************************/ @@ -120,10 +119,8 @@ const uint16_t HOST_TIMESYNC_INTERVAL_MS = 10000U; using ImuMeasurementVector = ExtendedKalmanFilter5D::ImuMeasurementVector; using OdoMeasurementVector = ExtendedKalmanFilter5D::OdoMeasurementVector; using CamMeasurementVector = ExtendedKalmanFilter5D::CamMeasurementVector; -using StateVector = ExtendedKalmanFilter5D::StateVector; -using StateMatrix = ExtendedKalmanFilter5D::StateMatrix; - - +using StateVector = ExtendedKalmanFilter5D::StateVector; +using StateMatrix = ExtendedKalmanFilter5D::StateMatrix; /****************************************************************************** * Public Methods @@ -214,8 +211,8 @@ void App::setup() else { /* Log incoming vehicle data and corresponding time sync information. */ - m_serMuxChannelProvider.registerVehicleDataCallback( - [this](const VehicleData& data) { onVehicleData(data); }); + m_serMuxChannelProvider.registerVehicleDataCallback([this](const VehicleData& data) + { onVehicleData(data); }); /* Start network time (NTP) against host and Zumo serial ping-pong. */ m_ekf.init(); @@ -295,7 +292,7 @@ bool App::setupMqtt(const String& clientId, const String& brokerAddr, uint16_t b JsonDocument jsonBirthDoc; char birthMsgArray[JSON_BIRTHMESSAGE_MAX_SIZE]; String birthMessage; - const String ssrTopic = String(TOPIC_NAME_RADAR_POSE) + "/" + clientId; + const String ssrTopic = String(TOPIC_NAME_RADAR_POSE) + "/" + clientId; jsonBirthDoc["name"] = clientId.c_str(); (void)serializeJson(jsonBirthDoc, birthMsgArray); @@ -319,9 +316,8 @@ bool App::setupMqtt(const String& clientId, const String& brokerAddr, uint16_t b { LOG_FATAL("Could not subscribe to MQTT topic: %s.", TOPIC_NAME_RADAR_POSE); } - else if (false == - m_mqttClient.subscribe(TOPIC_NAME_HOST_TIMESYNC_RSP, true, - [this](const String& payload) { hostTimeSyncResponseCallback(payload); })) + else if (false == m_mqttClient.subscribe(TOPIC_NAME_HOST_TIMESYNC_RSP, true, [this](const String& payload) + { hostTimeSyncResponseCallback(payload); })) { LOG_FATAL("Could not subscribe to MQTT topic: %s.", TOPIC_NAME_HOST_TIMESYNC_RSP); } @@ -337,7 +333,6 @@ bool App::setupMqtt(const String& clientId, const String& brokerAddr, uint16_t b return isSuccessful; } - void App::ssrTopicCallback(const String& payload) { JsonDocument jsonPayload; @@ -349,12 +344,12 @@ void App::ssrTopicCallback(const String& payload) } else { - JsonVariantConst xPos_mm = jsonPayload["positionX"]; /* int : in mm */ - JsonVariantConst yPos_mm = jsonPayload["positionY"]; /* int : in mm */ - JsonVariantConst xVel_mms = jsonPayload["speedX"]; /* int : in mm/s */ - JsonVariantConst yVel_mms = jsonPayload["speedY"]; /* int : in mm/s */ - JsonVariantConst angle_mrad = jsonPayload["angle"]; /* int : in mrad */ - JsonVariantConst timestamp_ms = jsonPayload["timestamp_ms"];/* int : host epoch in ms */ + JsonVariantConst xPos_mm = jsonPayload["positionX"]; /* int : in mm */ + JsonVariantConst yPos_mm = jsonPayload["positionY"]; /* int : in mm */ + JsonVariantConst xVel_mms = jsonPayload["speedX"]; /* int : in mm/s */ + JsonVariantConst yVel_mms = jsonPayload["speedY"]; /* int : in mm/s */ + JsonVariantConst angle_mrad = jsonPayload["angle"]; /* int : in mrad */ + JsonVariantConst timestamp_ms = jsonPayload["timestamp_ms"]; /* int : host epoch in ms */ const int x_mm_i = xPos_mm.as(); const int y_mm_i = yPos_mm.as(); @@ -363,14 +358,12 @@ void App::ssrTopicCallback(const String& payload) const int ang_mrad_i = angle_mrad.as(); const uint64_t hostEpochMs = timestamp_ms.as(); - const bool hostSynced = m_timeSync.isHostSynced(); + const bool hostSynced = m_timeSync.isHostSynced(); const uint64_t ssrLocalTsMs = hostSynced ? m_timeSync.hostToEspLocalMs(hostEpochMs) - : m_timeSync.localNowMs(); // fallback if no host sync yet + : m_timeSync.localNowMs(); /* Fallback: use local time if host is not synced */ - LOG_INFO("SSR pose: ts_host_ms=%llu (hostSync=%s)", - hostEpochMs, - hostSynced ? "true" : "false"); + LOG_INFO("SSR pose: ts_host_ms=%llu (hostSync=%s)", hostEpochMs, hostSynced ? "true" : "false"); SpaceShipRadarPose ssrPose; ssrPose.x = static_cast(x_mm_i); @@ -403,18 +396,18 @@ void App::ssrTopicCallback(const String& payload) x0(4) = 0.0F; StateMatrix P0 = StateMatrix::Identity(); - P0(0,0) = 50.0F * 50.0F; - P0(1,1) = 50.0F * 50.0F; - P0(2,2) = 200.0F * 200.0F; - P0(3,3) = 200.0F * 200.0F; - P0(4,4) = 200.0F * 200.0F; + P0(0, 0) = 50.0F * 50.0F; + P0(1, 1) = 50.0F * 50.0F; + P0(2, 2) = 200.0F * 200.0F; + P0(3, 3) = 200.0F * 200.0F; + P0(4, 4) = 200.0F * 200.0F; (void)m_ekf.init(x0, P0); m_lastEkfUpdateMs = ssrPose.timestamp; m_ekfInitializedFromSSR = true; - LOG_INFO("EKF initialized from SSR: x=%.1fmm y=%.1fmm theta=%.1fmrad v=%.1fmm/s", - x0(0), x0(1), x0(2), x0(3)); + LOG_INFO("EKF initialized from SSR: x=%.1fmm y=%.1fmm theta=%.1fmrad v=%.1fmm/s", x0(0), x0(1), x0(2), + x0(3)); } m_lastSsrPose = ssrPose; @@ -427,7 +420,6 @@ void App::ssrTopicCallback(const String& payload) } } - void App::publishVehicleAndSensorSnapshot(const VehicleData& data) { const uint32_t zumoTs32 = static_cast(data.timestamp); @@ -442,9 +434,9 @@ void App::publishVehicleAndSensorSnapshot(const VehicleData& data) JsonDocument payloadJson; char payloadArray[JSON_SENSOR_SNAPSHOT_MAX_SIZE]; - payloadJson["ts_local_ms"] = mappedLocalMs; - payloadJson["zumo_sync_ok"] = zumoSynced; - payloadJson["host_sync_ok"] = hostSynced; + payloadJson["ts_local_ms"] = mappedLocalMs; + payloadJson["zumo_sync_ok"] = zumoSynced; + payloadJson["host_sync_ok"] = hostSynced; JsonObject vehicleObj = payloadJson["vehicle"].to(); vehicleObj["ts_zumo_ms"] = static_cast(data.timestamp); @@ -472,22 +464,20 @@ void App::publishVehicleAndSensorSnapshot(const VehicleData& data) } } - -void App::filterLocationData(const VehicleData& vehicleData, - const SpaceShipRadarPose& ssrPose) +void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRadarPose& ssrPose) { /* Local variables (all declared at top as requested). */ - uint32_t zumoTs32 = 0U; - uint32_t zumoLocalMs32 = 0U; - uint32_t ssrLocalMs32 = 0U; - uint32_t newestLocalTs = 0U; - uint32_t dtMs = 0U; - float dt = 0.0F; - float a_x = 0.0F; - Source newestSource = Source::None; - bool ekfReady = false; - bool hasTimestamp = false; - bool doProcessing = false; + uint32_t zumoTs32 = 0U; + uint32_t zumoLocalMs32 = 0U; + uint32_t ssrLocalMs32 = 0U; + uint32_t newestLocalTs = 0U; + uint32_t dtMs = 0U; + float dt = 0.0F; + float a_x = 0.0F; + Source newestSource = Source::None; + bool ekfReady = false; + bool hasTimestamp = false; + bool doProcessing = false; /* Do not run fusion until EKF has been initialized from SSR. */ ekfReady = m_ekfInitializedFromSSR; @@ -504,8 +494,7 @@ void App::filterLocationData(const VehicleData& vehicleData, zumoLocalMs32 = static_cast(m_timeSync.mapZumoToLocalMs(zumoTs32)); ssrLocalMs32 = static_cast(ssrPose.timestamp); - LOG_INFO("Filtering location data: Zumo=%u ms, SSR=%u ms", - zumoLocalMs32, ssrLocalMs32); + LOG_INFO("Filtering location data: Zumo=%u ms, SSR=%u ms", zumoLocalMs32, ssrLocalMs32); /* Initialize EKF time on first data. */ hasTimestamp = initializeEkfTimestamp(zumoLocalMs32, ssrLocalMs32); @@ -515,10 +504,7 @@ void App::filterLocationData(const VehicleData& vehicleData, newestLocalTs = m_lastEkfUpdateMs; /* Determine which sensor has the newest update. */ - newestSource = determineNewestSource(zumoLocalMs32, - ssrLocalMs32, - m_lastEkfUpdateMs, - newestLocalTs); + newestSource = determineNewestSource(zumoLocalMs32, ssrLocalMs32, m_lastEkfUpdateMs, newestLocalTs); if (newestSource != Source::None) { @@ -557,11 +543,8 @@ void App::filterLocationData(const VehicleData& vehicleData, } } - -void App::transformOdometryToGlobal(const VehicleData& vehicleData, - float& xGlob_mm, - float& yGlob_mm, - float& thetaGlob_mrad) const +void App::transformOdometryToGlobal(const VehicleData& vehicleData, float& xGlob_mm, float& yGlob_mm, + float& thetaGlob_mrad) const { /* Y axis and heading sign differ between local odometry frame and SSR frame. */ constexpr float Y_SIGN = -1.0F; @@ -605,7 +588,7 @@ void App::publishFusionPose(uint32_t tsMs) void App::hostTimeSyncResponseCallback(const String& payload) { - uint64_t t4_ts = millis(); + uint64_t t4_ts = millis(); JsonDocument doc; DeserializationError err = deserializeJson(doc, payload); @@ -615,11 +598,8 @@ void App::hostTimeSyncResponseCallback(const String& payload) return; } - - if ( (doc["seq"].is() == false) || - (doc["t1_esp_ms"].is() == false) || - (doc["t2_host_ms"].is() == false) || - (doc["t3_host_ms"].is() == false) ) + if ((doc["seq"].is() == false) || (doc["t1_esp_ms"].is() == false) || + (doc["t2_host_ms"].is() == false) || (doc["t3_host_ms"].is() == false)) { LOG_WARNING("HostTimeSync: Missing required fields in response JSON."); return; @@ -672,12 +652,10 @@ bool App::initializeEkfTimestamp(uint32_t zumoLocalMs32, uint32_t ssrLocalMs32) return initialized; } -Source App::determineNewestSource(uint32_t zumoLocalMs32, - uint32_t ssrLocalMs32, - uint32_t lastEkfUpdateMs, +Source App::determineNewestSource(uint32_t zumoLocalMs32, uint32_t ssrLocalMs32, uint32_t lastEkfUpdateMs, uint32_t& newestLocalTs) const { - Source source = Source::None; + Source source = Source::None; uint32_t candidateTs = lastEkfUpdateMs; const bool hasVehicle = (zumoLocalMs32 > lastEkfUpdateMs); @@ -705,10 +683,10 @@ Source App::determineNewestSource(uint32_t zumoLocalMs32, void App::updateFromVehicle(const VehicleData& vehicleData) { - const int16_t rawGyroZ = static_cast(vehicleData.turnRateZ); - float xGlob_mm = 0.0F; - float yGlob_mm = 0.0F; - float thetaGlob_mrad = 0.0F; + const int16_t rawGyroZ = static_cast(vehicleData.turnRateZ); + float xGlob_mm = 0.0F; + float yGlob_mm = 0.0F; + float thetaGlob_mrad = 0.0F; OdoMeasurementVector z_odo; LOG_INFO("EKF update from Vehicle."); @@ -747,8 +725,6 @@ void App::updateFromSsr(const SpaceShipRadarPose& ssrPose) m_ekf.updateCamera(z_cam); } - - /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/start_sensorFusion.ps1 b/start_sensorFusion.ps1 index 137fd414..3d2e7cb7 100644 --- a/start_sensorFusion.ps1 +++ b/start_sensorFusion.ps1 @@ -42,10 +42,10 @@ function Assert-Exists { # ============================================================================= # A) Script -> RepoRoot -> Workspace # ============================================================================= -$ScriptPath = $MyInvocation.MyCommand.Path -$ScriptDir = Split-Path -Parent $ScriptPath +$ScriptPath = $MyInvocation.MyCommand.Path +$ScriptDir = Split-Path -Parent $ScriptPath $ScriptRepoRoot = Get-GitRepoRoot -StartDir $ScriptDir -$WorkspaceDir = Split-Path -Parent $ScriptRepoRoot +$WorkspaceDir = Split-Path -Parent $ScriptRepoRoot Write-Host "Script: $ScriptPath" Write-Host "ScriptDir: $ScriptDir" @@ -56,8 +56,8 @@ Write-Host "Workspace: $WorkspaceDir" # B) Repos in workspace (adjust names here if needed) # ============================================================================= $RadonUlzerRepo = Join-Path $WorkspaceDir "RadonUlzer" -$SSRRepo = Join-Path $WorkspaceDir "SpaceShipRadar" -$DCSRepo = Join-Path $WorkspaceDir "DroidControlShip" +$SSRRepo = Join-Path $WorkspaceDir "SpaceShipRadar" +$DCSRepo = Join-Path $WorkspaceDir "DroidControlShip" Assert-Exists $RadonUlzerRepo "RadonUlzer repo" Assert-Exists $SSRRepo "SpaceShipRadar repo" @@ -74,7 +74,7 @@ Write-Host " DroidControlShip:$DCSRepo" $RadonUlzer_RemoteControlSim = Join-Path $RadonUlzerRepo ".pio\build\RemoteControlSim\program.exe" $DCS_LineFollowerSensorFusionSim = Join-Path $DCSRepo ".pio\build\LineFollowerSensorFusionSim\program.exe" -$SSR_Script = Join-Path $SSRRepo "src\space_ship_radar\space_ship_radar.py" +$SSR_Script = Join-Path $SSRRepo "src\space_ship_radar\space_ship_radar.py" $SSR_CalibrationFolder = Join-Path $SSRRepo "src\calibration\" $RadonUlzer_Settings = Join-Path $RadonUlzerRepo "settings\settings.json" @@ -110,25 +110,25 @@ Write-Host " webots-controller.exe: $webotsController" # ============================================================================= Start-Process -FilePath $webotsController ` -ArgumentList @( - "--robot-name=Zumo", - "--stdout-redirect", $RadonUlzer_RemoteControlSim, - "-c", - "--supervisorRxCh", "1", - "--supervisorTxCh", "2", - "--serialRxCh", "3", - "--serialTxCh", "4", - "--settingsPath", $RadonUlzer_Settings, - "-v" - ) + "--robot-name=Zumo", + "--stdout-redirect", $RadonUlzer_RemoteControlSim, + "-c", + "--supervisorRxCh", "1", + "--supervisorTxCh", "2", + "--serialRxCh", "3", + "--serialTxCh", "4", + "--settingsPath", $RadonUlzer_Settings, + "-v" +) # ============================================================================= # Start SpaceShipRadar (MyBot) # ============================================================================= Start-Process -FilePath $webotsController ` -ArgumentList @( - "--robot-name=MyBot", - "--stdout-redirect", $SSR_Script - ) + "--robot-name=MyBot", + "--stdout-redirect", $SSR_Script +) # ============================================================================= # Start DroidControlShip / LineFollowerSensorFusionSim @@ -138,10 +138,10 @@ $DCSWorkDir = Split-Path -Parent $DCS_LineFollowerSensorFusionSim Start-Process -FilePath $webotsController ` -WorkingDirectory $DCSWorkDir ` -ArgumentList @( - "--robot-name=ZumoComSystem", - "--stdout-redirect", $DCS_LineFollowerSensorFusionSim, - "--cfgFilePath", "../../../data/config/config_zumo1.json", - "--serialRxCh", "4", - "--serialTxCh", "3", - "-v" - ) + "--robot-name=ZumoComSystem", + "--stdout-redirect", $DCS_LineFollowerSensorFusionSim, + "--cfgFilePath", "../../../data/config/config_zumo0.json", + "--serialRxCh", "4", + "--serialTxCh", "3", + "-v" +)