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/lib/APPLineFollowerSensorFusion/src/App.cpp b/lib/APPLineFollowerSensorFusion/src/App.cpp index dfc31097..4517c74b 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,43 @@ 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 */ +/** 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; -/** 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; + +/** 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 ImuMeasurementVector = ExtendedKalmanFilter5D::ImuMeasurementVector; +using OdoMeasurementVector = ExtendedKalmanFilter5D::OdoMeasurementVector; +using CamMeasurementVector = ExtendedKalmanFilter5D::CamMeasurementVector; +using StateVector = ExtendedKalmanFilter5D::StateVector; +using StateMatrix = ExtendedKalmanFilter5D::StateMatrix; + /****************************************************************************** * Public Methods *****************************************************************************/ @@ -111,7 +134,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 +185,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(); @@ -179,11 +212,13 @@ void App::setup() { /* Log incoming vehicle data and corresponding time sync information. */ m_serMuxChannelProvider.registerVehicleDataCallback([this](const VehicleData& data) - { publishVehicleAndSensorSnapshot(data); }); + { onVehicleData(data); }); - /* Start network time (NTP) against Host and Zumo serial ping-pong. */ + /* Start network time (NTP) against host and Zumo serial ping-pong. */ + m_ekf.init(); m_timeSync.begin(); - m_statusTimer.start(1000U); + m_statusTimer.start(STATUS_SEND_INTERVAL_MS); + m_hostTimeSyncTimer.start(HOST_TIMESYNC_INTERVAL_MS); isSuccessful = true; } } @@ -207,16 +242,22 @@ void App::loop() /* Process battery, device and network. */ Board::getInstance().process(); - /* Process MQTT Communication */ + /* Process MQTT communication. */ m_mqttClient.process(); /* Process serial multiplexer. */ m_serMuxChannelProvider.process(); - /* Process time synchronization (NTP refresh + serial ping-pong). */ + /* Process time synchronization (serial ping-pong). */ m_timeSync.process(); - /* Process statemachine. */ + if (m_hostTimeSyncTimer.isTimeout()) + { + m_timeSync.sendHostTimeSyncRequest(m_mqttClient, TOPIC_NAME_HOST_TIMESYNC_REQ); + m_hostTimeSyncTimer.restart(); + } + + /* Process state machine. */ m_stateMachine.process(); /* Send heartbeat status to Radon Ulzer controller periodically. */ @@ -244,6 +285,7 @@ void App::loop() /****************************************************************************** * Private Methods *****************************************************************************/ + bool App::setupMqtt(const String& clientId, const String& brokerAddr, uint16_t brokerPort) { bool isSuccessful = false; @@ -269,16 +311,21 @@ 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 subcribe to MQTT topic: %s.", TOPIC_NAME_RADAR_POSE); + 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); } } @@ -293,19 +340,83 @@ 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 { - 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 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); + 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: use local time if host is not synced */ + + 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(ssrLocalTsMs); + + 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); + } + + if (false == m_ekfInitializedFromSSR) + { + StateVector x0; + x0.setZero(); + + x0(0) = ssrPose.x; + x0(1) = ssrPose.y; + x0(2) = ssrPose.theta; + + const float v_mms = std::sqrt(ssrPose.v_x * ssrPose.v_x + ssrPose.v_y * ssrPose.v_y); + x0(3) = v_mms; + 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; + + (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)); + } + + m_lastSsrPose = ssrPose; + m_hasSsrPose = true; + + if (true == m_hasVehicleData) + { + filterLocationData(m_lastVehicleData, m_lastSsrPose); + } } } @@ -313,19 +424,19 @@ 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)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); @@ -335,7 +446,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 +464,267 @@ void App::publishVehicleAndSensorSnapshot(const VehicleData& data) } } +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; + + /* Do not run fusion until EKF has been initialized from SSR. */ + ekfReady = m_ekfInitializedFromSSR; + + if (ekfReady) + { + doProcessing = true; + } + + if (doProcessing) + { + /* Timestamp conversion. */ + zumoTs32 = static_cast(vehicleData.timestamp); + 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); + + /* Initialize EKF time on first data. */ + hasTimestamp = initializeEkfTimestamp(zumoLocalMs32, ssrLocalMs32); + + if (hasTimestamp) + { + newestLocalTs = m_lastEkfUpdateMs; + + /* Determine which sensor has the newest update. */ + newestSource = determineNewestSource(zumoLocalMs32, ssrLocalMs32, m_lastEkfUpdateMs, newestLocalTs); + + if (newestSource != Source::None) + { + /* Time delta for prediction step. */ + dtMs = newestLocalTs - m_lastEkfUpdateMs; + dt = static_cast(dtMs) / 1000.0F; + + /* Longitudinal acceleration input. */ + a_x = static_cast(vehicleData.accelerationX); + + /* EKF prediction. */ + m_ekf.predict(a_x, dt); + + /* 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); + } + + /* Update last EKF timestamp. */ + m_lastEkfUpdateMs = newestLocalTs; + + /* Publish fused pose. */ + publishFusionPose(newestLocalTs); + } + } + } +} + +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."); + } +} + +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() == 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; + } + + 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); +} + +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; + OdoMeasurementVector 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) +{ + CamMeasurementVector 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 7a90941d..2acc01dd 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,55 @@ * 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 Data source for sensor fusion. + */ +enum class Source +{ + 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. */ +}; + +/** + * @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 +125,245 @@ 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; + /** 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; + /** - * 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 Timer for host time synchronization requests. + */ + SimpleTimer m_hostTimeSyncTimer; + + /** + * @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 Callback for receiving host time sync response over MQTT topic. * - * @param[in] data Vehicle data received via SerialMux. + * @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. + * + * @param[in] data Vehicle data received via SerialMux. */ void publishVehicleAndSensorSnapshot(const VehicleData& data); /** - * Copy construction of an instance. - * Not allowed. + * @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. + * + * 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 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. + */ + Source determineNewestSource(uint32_t zumoLocalMs32, + uint32_t ssrLocalMs32, + uint32_t lastEkfUpdateMs, + 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] + * + * @return true if EKF timestamp is initialized, otherwise false. + */ + bool initializeEkfTimestamp(uint32_t zumoLocalMs32, + uint32_t ssrLocalMs32); + + + /** + * @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/EKF.cpp b/lib/APPLineFollowerSensorFusion/src/EKF.cpp new file mode 100644 index 00000000..6a4d9335 --- /dev/null +++ b/lib/APPLineFollowerSensorFusion/src/EKF.cpp @@ -0,0 +1,391 @@ +/* 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 + * @author Tobias Haeckel + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "EKF.h" +#include +#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 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. */ + 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] */ + 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] */ +} + +/****************************************************************************** + * 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_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(); + m_R_imu(0, 0) = SIGMA_IMU_OMEGA * SIGMA_IMU_OMEGA; +} + +void ExtendedKalmanFilter5D::init(const StateVector& x0, const StateMatrix& P0) +{ + m_state = x0; + m_covariance = P0; +} + +void ExtendedKalmanFilter5D::init() +{ + m_state.setZero(); + 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_MRADS; + + m_covariance.setIdentity(); +} + +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_mms, dt); + + /* Wrap heading angle (index 2). */ + x_pred(2) = wrapAngleMrad(x_pred(2)); + + /* Linearization. */ + StateMatrix jacobianF = processJacobianF(m_state, 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 CamMeasurementVector& z_cam) +{ + /* Predicted measurement. */ + CamMeasurementVector z_pred = cameraModel(m_state); + Eigen::Matrix H = cameraJacobianH(m_state); + + /* Innovation. */ + CamMeasurementVector 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 OdoMeasurementVector& z_odo) +{ + /* Predicted measurement. */ + OdoMeasurementVector z_pred = odometryModel(m_state); + Eigen::Matrix H = odometryJacobianH(m_state); + + /* Innovation. */ + OdoMeasurementVector y = z_odo - z_pred; + + /* 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; + 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 ImuMeasurementVector& z_imu) +{ + /* Predicted measurement. */ + ImuMeasurementVector z_pred = imuModel(m_state); + Eigen::Matrix H = imuJacobianH(m_state); + /* Innovation (omega, no wrapping). */ + ImuMeasurementVector 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; +} + +void ExtendedKalmanFilter5D::updateImuFromDigits(int16_t rawGyroZ) +{ + ImuMeasurementVector 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_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 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 + omegaMrads * dt; /* theta [mrad] */ + x_next(3) = v + a_x_mms * dt; /* v */ + x_next(4) = omegaMrads; /* omega */ + + return x_next; +} + +ExtendedKalmanFilter5D::StateMatrix +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 F_jacobian = StateMatrix::Identity(); + + /* d p_x / d theta. */ + F_jacobian(0, 2) = -v * std::sin(thetaRad) * dt / 1000.0F; + /* d p_x / d v. */ + F_jacobian(0, 3) = std::cos(thetaRad) * dt; + + /* d p_y / d theta. */ + F_jacobian(1, 2) = v * std::cos(thetaRad) * dt / 1000.0F; + /* d p_y / d v. */ + F_jacobian(1, 3) = std::sin(thetaRad) * dt; + + /* d theta / d omega. */ + F_jacobian(2, 4) = dt; + + return F_jacobian; +} + +/* ---------------- Camera model ---------------- */ + +ExtendedKalmanFilter5D::CamMeasurementVector +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; + + CamMeasurementVector 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::OdoMeasurementVector +ExtendedKalmanFilter5D::odometryModel(const StateVector& x) const +{ + OdoMeasurementVector z; + 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(2, 3) = 1.0F; + /* theta */ + H(3, 2) = 1.0F; + + return H; +} + +/* ---------------- IMU yaw model ---------------- */ + +ExtendedKalmanFilter5D::ImuMeasurementVector +ExtendedKalmanFilter5D::imuModel(const StateVector& x) const +{ + ImuMeasurementVector 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; +} diff --git a/lib/APPLineFollowerSensorFusion/src/EKF.h b/lib/APPLineFollowerSensorFusion/src/EKF.h new file mode 100644 index 00000000..2d573947 --- /dev/null +++ b/lib/APPLineFollowerSensorFusion/src/EKF.h @@ -0,0 +1,310 @@ +/* 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 + * + * 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 + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * @brief Extended Kalman Filter implementation for a 5D state. + */ +class ExtendedKalmanFilter5D +{ +public: + /** State dimension. */ + 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: [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; + + /** @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 CamMeasurementVector = Eigen::Matrix; + /** @brief Camera measurement covariance matrix R_cam. */ + using CamMeasMatrix = Eigen::Matrix; + + /** @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; + + /** @brief IMU measurement vector z_imu = [omega]^T. */ + using ImuMeasurementVector = 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. + */ + ExtendedKalmanFilter5D(); + + /** + * Initializes the EKF with a given state and covariance. + * + * @param[in] x0 Initial state vector (physical units). + * @param[in] P0 Initial covariance matrix. + * + */ + void init(const StateVector& x0, const StateMatrix& P0); + + /** + * Initializes the EKF with a fixed default start pose and identity covariance. + * + * 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 + */ + void init(); + + /** + * 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) + * + * @param[in] accX_raw Raw longitudinal acceleration from IMU [digits]. + * @param[in] dt Time step [s]. + * + * The raw acceleration is internally converted into [mm/s^2]. + */ + void predict(float accX_raw, float dt); + + /** + * EKF update step for camera (SSR) measurements. + * + * 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 CamMeasurementVector& z_cam); + + /** + * EKF update step for odometry measurements. + * + * Odometry measurement (drift-reduced): + * z_odo = [v_odo, theta_odo]^T + * + * Measurement model: + * h_odo(x) = [ v, theta ]^T + * + * @param[in] z_odo Odometry measurement vector. + */ + void updateOdometry(const OdoMeasurementVector& z_odo); + + /** + * EKF update step for IMU measurements (physical units). + * + * IMU measurement (simplified): + * z_imu = [omega]^T, omega in [mrad/s] + * + * Measurement model: + * h_imu(x) = [ omega ]^T + * + * @param[in] z_imu IMU measurement vector (omega in mrad/s). + */ + void updateImu(const ImuMeasurementVector& z_imu); + + /** + * 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 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: + /** Current state estimate. */ + StateVector m_state; + /** Current covariance estimate. */ + StateMatrix m_covariance; + + /** Process noise covariance Q. */ + StateMatrix m_Q; + /** Camera measurement noise covariance R_cam. */ + CamMeasMatrix m_R_cam; + /** Odometry measurement noise covariance R_odo. */ + OdoMeasMatrix m_R_odo; + /** IMU measurement noise covariance R_imu. */ + ImuMeasMatrix m_R_imu; + +private: + /** + * Nonlinear process model f(x, a_x, dt). + * + * @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_mms, float dt) const; + + /** + * Jacobian of the process model F = df/dx. + * + * @param[in] x Current state vector. + * @param[in] dt Time step [s]. + * + * @return Process Jacobian matrix. + */ + StateMatrix processJacobianF(const StateVector& x, float dt) const; + + /** + * Camera measurement model h_cam(x). + * + * @param[in] x Current state. + * + * @return Camera measurement prediction. + */ + CamMeasurementVector cameraModel(const StateVector& x) const; + + /** + * Camera measurement Jacobian H_cam = dh_cam/dx. + * + * @param[in] x Current state. + * + * @return Camera measurement Jacobian. + */ + Eigen::Matrix cameraJacobianH(const StateVector& x) const; + + /** + * Odometry measurement model h_odo(x) = [v, theta]^T. + * + * @param[in] x Current state. + * + * @return Odometry measurement prediction. + */ + OdoMeasurementVector odometryModel(const StateVector& x) const; + + /** + * 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 vector. + * + * @return IMU measurement prediction. + */ + ImuMeasurementVector imuModel(const StateVector& x) const; + + /** + * IMU measurement Jacobian H_imu = dh_imu/dx. + * + * @param[in] x Current state vector. + * + * @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); +}; + +#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/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/APPLineFollowerSensorFusion/src/TimeSync.cpp b/lib/APPLineFollowerSensorFusion/src/TimeSync.cpp index dae77351..e59b93c7 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_ntpClient.begin(); + m_serMuxProvider.registerTimeSyncResponseCallback( + [this](const TimeSyncResponse& rsp) { onTimeSyncResponse(rsp); }); - /* 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,100 @@ 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 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); } +} - const int64_t epochMs = static_cast(localNowMs()) + m_epochToLocalOffsetMs; - return (epochMs >= 0) ? static_cast(epochMs) : 0ULL; + +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 float offset_s = static_cast(m_hostOffsetMs) / 1000.0F; + + 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 +261,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,71 +317,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; -} - /****************************************************************************** * 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) @@ -305,7 +342,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); } @@ -318,7 +356,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 7d2917c5..04aea958 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,90 +66,159 @@ 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; /** - * Get current local time [ms]. + * Get current ESP32 local time [ms] (wrapper around millis()). + * * @return Local time [ms]. */ uint64_t localNowMs() const; /** - * Get current epoch time [ms] if RTC mapping is available, otherwise 0. - * @return Epoch time [ms] + * Get estimated Zumo->ESP offset [ms]. Positive means Zumo is ahead of ESP. + * + * @return Time offset Zumo->ESP [ms]. */ - uint64_t nowEpochMs() const; + int64_t getZumoToEspOffsetMs() const + { + return m_zumoToEspOffsetMs; + } /** - * Get estimated Zumo->ESP offset [ms]. Positive means Zumo ahead of ESP. - * @return Time Offset Zumo->ESP [ms] + * Log detailed serial (Zumo) time sync status information. */ - int64_t getZumoToEspOffsetMs() 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 + *********************************************************************/ + + + /** + * @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] mqttClient MQTT instance. + * @param[in] topic MQTT topic for the request message. + */ + void sendHostTimeSyncRequest(MqttClient& mqttClient, + const char* topic); + + /** + * @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]. + * @param[in] t4EspMs T4 on ESP side [ms]. + */ + 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]. */ @@ -169,29 +230,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 */ /** @} */ 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} 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..3d2e7cb7 --- /dev/null +++ b/start_sensorFusion.ps1 @@ -0,0 +1,147 @@ +#requires -Version 5.1 +Set-StrictMode -Version Latest +$ErrorActionPreference = "Stop" + +# ============================================================================= +# Helpers +# ============================================================================= +function Find-Upwards { + param( + [Parameter(Mandatory)][string]$StartDir, + [Parameter(Mandatory)][string]$Target + ) + + $dir = (Resolve-Path $StartDir).Path + while ($true) { + $candidate = Join-Path $dir $Target + if (Test-Path -LiteralPath $candidate) { return (Resolve-Path $candidate).Path } + + $parent = Split-Path -Parent $dir + if ($parent -eq $dir) { break } + $dir = $parent + } + throw "Not found: '$Target' starting from '$StartDir'" +} + +function Get-GitRepoRoot { + param([Parameter(Mandatory)][string]$StartDir) + $gitPath = Find-Upwards -StartDir $StartDir -Target ".git" + return (Split-Path -Parent $gitPath) +} + +function Assert-Exists { + param( + [Parameter(Mandatory)][string]$Path, + [Parameter(Mandatory)][string]$Label + ) + if (-not (Test-Path -LiteralPath $Path)) { + throw "$Label not found: $Path" + } +} + +# ============================================================================= +# 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" + +# 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", $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 +) + +# ============================================================================= +# Start DroidControlShip / LineFollowerSensorFusionSim +# ============================================================================= +$DCSWorkDir = Split-Path -Parent $DCS_LineFollowerSensorFusionSim + +Start-Process -FilePath $webotsController ` + -WorkingDirectory $DCSWorkDir ` + -ArgumentList @( + "--robot-name=ZumoComSystem", + "--stdout-redirect", $DCS_LineFollowerSensorFusionSim, + "--cfgFilePath", "../../../data/config/config_zumo0.json", + "--serialRxCh", "4", + "--serialTxCh", "3", + "-v" +)