Skip to content

erp5
erp5
Commit 98aa5f07 authored by Roque's avatar Roque
Browse files
Options

erp5_officejs_drone_simulator: fix drone position update, speed and coordinates conversion 

See merge request !1806
parents ebdc34d4 76226c46
Pipeline #29298 failed with stage
    in 0 seconds
    Hide whitespace changes
    Inline Side-by-side
    Showing with 175 additions and 142 deletions
    bt5/erp5_officejs_drone_simulator/PathTemplateItem/web_page_module/drone_simulator_dronelogfollower_js.js
    View file @ 98aa5f07
    ......@@ -61,16 +61,14 @@ var DroneLogAPI = /** @class */ (function () {
    timestamp = parseInt(splitted_log_entry[0], 10);
    lat = parseFloat(splitted_log_entry[1]);
    lon = parseFloat(splitted_log_entry[2]);
    x = this._mapManager.longitudToX(lon, map_dict.map_size);
    y = this._mapManager.latitudeToY(lat, map_dict.map_size);
    position = this._mapManager.normalize(x, y, map_dict);
    height = parseFloat(splitted_log_entry[4]);
    if (height < min_height) {
    height = min_height;
    }
    converted_log_point_list.push([position[0],
    position[1],
    height, timestamp / time_offset]);
    position = this._mapManager.convertToLocalCoordinates(lat, lon, height);
    converted_log_point_list.push([position.x,
    position.y,
    position.z, timestamp / time_offset]);
    }
    this._flight_parameters.converted_log_point_list = converted_log_point_list;
    };
    ......
    bt5/erp5_officejs_drone_simulator/PathTemplateItem/web_page_module/drone_simulator_dronelogfollower_js.xml
    View file @ 98aa5f07
    ......@@ -232,7 +232,7 @@
    </item>
    <item>
    <key> <string>actor</string> </key>
    <value> <string>zope</string> </value>
    <value> <unicode>zope</unicode> </value>
    </item>
    <item>
    <key> <string>comment</string> </key>
    ......@@ -246,7 +246,7 @@
    </item>
    <item>
    <key> <string>serial</string> </key>
    <value> <string>1009.38419.20212.62293</string> </value>
    <value> <string>1010.13157.33937.23381</string> </value>
    </item>
    <item>
    <key> <string>state</string> </key>
    ......@@ -266,7 +266,7 @@
    </tuple>
    <state>
    <tuple>
    <float>1688635412.87</float>
    <float>1691051883.97</float>
    <string>UTC</string>
    </tuple>
    </state>
    ......
    bt5/erp5_officejs_drone_simulator/PathTemplateItem/web_page_module/drone_simulator_fixedwingdrone_js.js
    View file @ 98aa5f07
    ......@@ -92,7 +92,7 @@ var FixedWingDroneAPI = /** @class */ (function () {
    ** Function called on every drone update, right before onUpdate AI script
    */
    FixedWingDroneAPI.prototype.internal_update = function (context, delta_time) {
    var diff, newrot, orientationValue, rotStep, updateSpeed;
    var diff, newrot, orientationValue, rotStep;
    //TODO rotation
    if (context._rotationTarget) {
    ......@@ -116,18 +116,8 @@ var FixedWingDroneAPI = /** @class */ (function () {
    }
    this._updateSpeed(context, delta_time);
    this._updateDirection(context, delta_time);
    this._updatePosition(context, delta_time);
    updateSpeed = context._speed * delta_time / 1000;
    if (context._direction.x !== 0 ||
    context._direction.y !== 0 ||
    context._direction.z !== 0) {
    context._controlMesh.position.addInPlace(new BABYLON.Vector3(
    context._direction.x * updateSpeed,
    context._direction.y * updateSpeed,
    context._direction.z * updateSpeed
    ));
    }
    //TODO rotation
    orientationValue = context._maxOrientation *
    (context._speed / context._maxSpeed);
    ......@@ -180,67 +170,112 @@ var FixedWingDroneAPI = /** @class */ (function () {
    }
    };
    FixedWingDroneAPI.prototype._updateDirection = function (drone, delta_time) {
    var horizontalCoeff, newX, newY, newZ, tangentYaw;
    FixedWingDroneAPI.prototype._updatePosition = function (drone, delta_time) {
    var R = 6371e3,
    currentGeoCoordinates = this._mapManager.convertToGeoCoordinates(
    drone.position.x,
    drone.position.y,
    drone.position.z
    ),
    targetCoordinates = this._mapManager.convertToGeoCoordinates(
    drone._targetCoordinates.x,
    drone._targetCoordinates.y,
    drone._targetCoordinates.z
    ),
    bearing = this._computeBearing(
    currentGeoCoordinates.x,
    currentGeoCoordinates.y,
    targetCoordinates.x,
    targetCoordinates.y
    ),
    currentCosLat,
    currentLatRad,
    distance,
    distanceCos,
    distanceSin,
    currentSinLat,
    currentLonRad,
    groundSpeed,
    newCoordinates,
    newLatRad,
    newLonRad,
    newYaw,
    newYawRad,
    verticalSpeed,
    yawToDirection;
    if (this._loiter_mode
    && Math.sqrt(
    Math.pow(drone._targetCoordinates.x - drone.position.x, 2)
    + Math.pow(drone._targetCoordinates.y - drone.position.y, 2)
    ) <= this._loiter_radius) {
    tangentYaw = this._computeBearing(
    drone.position.x,
    drone.position.y,
    drone._targetCoordinates.x,
    drone._targetCoordinates.y
    ) - 90;
    // trigonometric circle is east oriented, yaw angle is clockwise
    tangentYaw = this._toRad(-tangentYaw + 90);
    newX = Math.cos(tangentYaw);
    newZ = Math.sin(tangentYaw);
    newYaw = bearing - 90;
    } else {
    [newX, newZ] = this._getNewYaw(drone, delta_time);
    newYaw = this._getNewYaw(drone, bearing, delta_time);
    }
    newY = this._getNewAltitude(drone);
    newYawRad = this._toRad(newYaw);
    currentLatRad = this._toRad(currentGeoCoordinates.x);
    currentCosLat = Math.cos(currentLatRad);
    currentSinLat = Math.sin(currentLatRad);
    currentLonRad = this._toRad(currentGeoCoordinates.y);
    horizontalCoeff = Math.sqrt(
    (
    Math.pow(drone.getAirSpeed(), 2) - Math.pow(newY, 2)
    ) / (
    Math.pow(newX, 2) + Math.pow(newZ, 2)
    )
    verticalSpeed = this._getVerticalSpeed(drone);
    groundSpeed = Math.sqrt(
    Math.pow(drone.getAirSpeed(), 2) - Math.pow(verticalSpeed, 2)
    );
    newX *= horizontalCoeff;
    newZ *= horizontalCoeff;
    // swap y and z axis so z axis represents altitude
    drone.setDirection(newX, newZ, newY);
    };
    FixedWingDroneAPI.prototype._getNewYaw = function (drone, delta_time) {
    // swap y and z axis so z axis represents altitude
    var bearing = this._computeBearing(
    drone.position.x,
    drone.position.y,
    drone._targetCoordinates.x,
    drone._targetCoordinates.y
    ),
    yaw = drone.getYaw(),
    yawDiff = this._computeYawDiff(yaw, bearing),
    yawUpdate = this.getYawVelocity(drone) * delta_time / 1000;
    distance = (groundSpeed * delta_time / 1000) / R;
    distanceCos = Math.cos(distance);
    distanceSin = Math.sin(distance);
    if (yawUpdate >= Math.abs(yawDiff)) {
    yawUpdate = yawDiff;
    } else if (yawDiff < 0) {
    yawUpdate *= -1;
    }
    yaw += yawUpdate;
    newLatRad = Math.asin(
    currentSinLat * distanceCos
    + currentCosLat * distanceSin * Math.cos(newYawRad)
    );
    newLonRad = currentLonRad + Math.atan2(
    Math.sin(newYawRad) * distanceSin * currentCosLat,
    distanceCos - currentSinLat * Math.sin(newLatRad)
    );
    // trigonometric circle is east oriented, yaw angle is clockwise
    yaw = this._toRad(-yaw + 90);
    return [Math.cos(yaw), Math.sin(yaw)];
    newCoordinates = this._mapManager.convertToLocalCoordinates(
    this._toDeg(newLatRad),
    this._toDeg(newLonRad),
    drone.position.z
    );
    // swap y and z axis so z axis represents altitude
    drone._controlMesh.position.addInPlace(new BABYLON.Vector3(
    Math.abs(newCoordinates.x - drone.position.x)
    * (newCoordinates.x < drone.position.x ? -1 : 1),
    verticalSpeed * delta_time / 1000,
    Math.abs(newCoordinates.y - drone.position.y)
    * (newCoordinates.y < drone.position.y ? -1 : 1)
    ));
    yawToDirection = this._toRad(-newYaw + 90);
    drone.setDirection(
    groundSpeed * Math.cos(yawToDirection),
    groundSpeed * Math.sin(yawToDirection),
    verticalSpeed
    );
    };
    FixedWingDroneAPI.prototype._getNewAltitude = function (drone) {
    FixedWingDroneAPI.prototype._getNewYaw =
    function (drone, bearing, delta_time) {
    // swap y and z axis so z axis represents altitude
    var yaw = drone.getYaw(),
    yawDiff = this._computeYawDiff(yaw, bearing),
    yawUpdate = this.getYawVelocity(drone) * delta_time / 1000;
    if (yawUpdate >= Math.abs(yawDiff)) {
    yawUpdate = yawDiff;
    } else if (yawDiff < 0) {
    yawUpdate *= -1;
    }
    return yaw + yawUpdate;
    };
    FixedWingDroneAPI.prototype._getVerticalSpeed = function (drone) {
    // swap y and z axis so z axis represents altitude
    var altitudeDiff = drone._targetCoordinates.z - drone.position.z,
    verticalSpeed;
    ......@@ -356,24 +391,17 @@ var FixedWingDroneAPI = /** @class */ (function () {
    if (isNaN(lat) || isNaN(lon) || isNaN(z)) {
    throw new Error('Target coordinates must be numbers');
    }
    var x = this._mapManager.longitudToX(lon, this._map_dict.map_size),
    y = this._mapManager.latitudeToY(lat, this._map_dict.map_size),
    position = this._mapManager.normalize(x, y, this._map_dict),
    processed_coordinates;
    if (z > this._map_dict.start_AMSL) {
    z -= this._map_dict.start_AMSL;
    var processed_coordinates =
    this._mapManager.convertToLocalCoordinates(lat, lon, z);
    if (processed_coordinates.z > this._map_dict.start_AMSL) {
    processed_coordinates.z -= this._map_dict.start_AMSL;
    }
    processed_coordinates = {
    x: position[0],
    y: position[1],
    z: z
    };
    //this._last_altitude_point_reached = -1;
    //this.takeoff_path = [];
    return processed_coordinates;
    };
    FixedWingDroneAPI.prototype.getCurrentPosition = function (x, y, z) {
    return this._mapManager.convertToGeoCoordinates(x, y, z, this._map_dict);
    return this._mapManager.convertToGeoCoordinates(x, y, z);
    };
    FixedWingDroneAPI.prototype.getDroneAI = function () {
    return null;
    ......@@ -419,11 +447,19 @@ var FixedWingDroneAPI = /** @class */ (function () {
    };
    FixedWingDroneAPI.prototype.getYaw = function (drone) {
    var direction = drone.worldDirection;
    return this._computeBearing(0, 0, direction.x, direction.z);
    };
    FixedWingDroneAPI.prototype._computeBearing = function (x1, z1, x2, z2) {
    return this._toDeg(Math.atan2(x2 - x1, z2 - z1));
    };
    return this._toDeg(Math.atan2(direction.x, direction.z));
    };
    FixedWingDroneAPI.prototype._computeBearing =
    function (lat1, lon1, lat2, lon2) {
    var dLon = this._toRad(lon2 - lon1),
    lat1Rad = this._toRad(lat1),
    lat2Rad = this._toRad(lat2),
    x = Math.cos(lat2Rad) * Math.sin(dLon),
    y = Math.cos(lat1Rad) * Math.sin(lat2Rad)
    - Math.sin(lat1Rad) * Math.cos(lat2Rad) * Math.cos(dLon);
    return this._toDeg(Math.atan2(x, y));
    };
    FixedWingDroneAPI.prototype._computeYawDiff = function (yaw1, yaw2) {
    var diff = yaw2 - yaw1;
    diff += (diff > 180) ? -360 : (diff < -180) ? 360 : 0;
    ......
    bt5/erp5_officejs_drone_simulator/PathTemplateItem/web_page_module/drone_simulator_fixedwingdrone_js.xml
    View file @ 98aa5f07
    ......@@ -240,7 +240,7 @@
    </item>
    <item>
    <key> <string>serial</string> </key>
    <value> <string>1009.59163.16294.47701</string> </value>
    <value> <string>1010.13155.61424.54442</string> </value>
    </item>
    <item>
    <key> <string>state</string> </key>
    ......@@ -260,7 +260,7 @@
    </tuple>
    <state>
    <tuple>
    <float>1690395852.2</float>
    <float>1691051762.91</float>
    <string>UTC</string>
    </tuple>
    </state>
    ......
    bt5/erp5_officejs_drone_simulator/PathTemplateItem/web_page_module/drone_simulator_logic_js.js
    View file @ 98aa5f07
    ......@@ -367,41 +367,11 @@ var DroneManager = /** @class */ (function () {
    var MapManager = /** @class */ (function () {
    "use strict";
    function calculateMapInfo(map, map_dict, initial_position) {
    var max_width = map.latLonDistance([map_dict.min_lat, map_dict.min_lon],
    [map_dict.min_lat, map_dict.max_lon]),
    max_height = map.latLonDistance([map_dict.min_lat, map_dict.min_lon],
    [map_dict.max_lat, map_dict.min_lon]),
    map_size = Math.ceil(Math.max(max_width, max_height)) * 0.6,
    map_info = {
    "depth": map_size,
    "height": map_dict.height,
    "width": map_size,
    "map_size": map_size,
    "min_x": map.longitudToX(map_dict.min_lon, map_size),
    "min_y": map.latitudeToY(map_dict.min_lat, map_size),
    "max_x": map.longitudToX(map_dict.max_lon, map_size),
    "max_y": map.latitudeToY(map_dict.max_lat, map_size),
    "start_AMSL": map_dict.start_AMSL
    },
    position = map.normalize(
    map.longitudToX(initial_position.longitude, map_size),
    map.latitudeToY(initial_position.latitude, map_size),
    map_info
    );
    map_info.initial_position = {
    "x": position[0],
    "y": position[1],
    "z": initial_position.z
    };
    return map_info;
    }
    //** CONSTRUCTOR
    function MapManager(scene) {
    var _this = this, max_sky, skybox, skyboxMat, largeGroundMat,
    largeGroundBottom, width, depth, terrain, max;
    _this.map_info = calculateMapInfo(_this, GAMEPARAMETERS.map,
    GAMEPARAMETERS.initialPosition);
    this.setMapInfo(GAMEPARAMETERS.map, GAMEPARAMETERS.initialPosition);
    max = _this.map_info.width;
    if (_this.map_info.depth > max) {
    max = _this.map_info.depth;
    ......@@ -446,14 +416,37 @@ var MapManager = /** @class */ (function () {
    terrain.scaling = new BABYLON.Vector3(depth / 50000, depth / 50000,
    width / 50000);
    }
    MapManager.prototype.setMapInfo = function (map_dict, initial_position) {
    var max_width = this.latLonDistance([map_dict.min_lat, map_dict.min_lon],
    [map_dict.min_lat, map_dict.max_lon]),
    max_height = this.latLonDistance([map_dict.min_lat, map_dict.min_lon],
    [map_dict.max_lat, map_dict.min_lon]),
    map_size = Math.ceil(Math.max(max_width, max_height)) * 0.6;
    this.map_info = {
    "depth": map_size,
    "height": map_dict.height,
    "width": map_size,
    "map_size": map_size,
    "start_AMSL": map_dict.start_AMSL
    };
    this.map_info.min_x = this.longitudToX(map_dict.min_lon);
    this.map_info.min_y = this.latitudeToY(map_dict.min_lat);
    this.map_info.max_x = this.longitudToX(map_dict.max_lon);
    this.map_info.max_y = this.latitudeToY(map_dict.max_lat);
    this.map_info.initial_position = this.convertToLocalCoordinates(
    initial_position.latitude,
    initial_position.longitude,
    initial_position.z
    );
    };
    MapManager.prototype.getMapInfo = function () {
    return this.map_info;
    };
    MapManager.prototype.longitudToX = function (lon, map_size) {
    return (map_size / 360.0) * (180 + lon);
    MapManager.prototype.longitudToX = function (lon) {
    return (this.map_info.map_size / 360.0) * (180 + lon);
    };
    MapManager.prototype.latitudeToY = function (lat, map_size) {
    return (map_size / 180.0) * (90 - lat);
    MapManager.prototype.latitudeToY = function (lat) {
    return (this.map_info.map_size / 180.0) * (90 - lat);
    };
    MapManager.prototype.latLonDistance = function (c1, c2) {
    var R = 6371e3,
    ......@@ -467,23 +460,30 @@ var MapManager = /** @class */ (function () {
    c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
    return R * c;
    };
    MapManager.prototype.normalize = function (x, y, map_dict) {
    var n_x = (x - map_dict.min_x) / (map_dict.max_x - map_dict.min_x),
    n_y = (y - map_dict.min_y) / (map_dict.max_y - map_dict.min_y);
    return [n_x * 1000 - map_dict.width / 2,
    n_y * 1000 - map_dict.depth / 2];
    };
    MapManager.prototype.convertToGeoCoordinates = function (x, y, z, map_dict) {
    var lon = x + map_dict.width / 2,
    lat = y + map_dict.depth / 2;
    MapManager.prototype.convertToLocalCoordinates =
    function (latitude, longitude, altitude) {
    var map_info = this.map_info,
    x = this.longitudToX(longitude),
    y = this.latitudeToY(latitude);
    return {
    x: ((x - map_info.min_x) / (map_info.max_x - map_info.min_x))
    * 1000 - map_info.width / 2,
    y: ((y - map_info.min_y) / (map_info.max_y - map_info.min_y))
    * 1000 - map_info.depth / 2,
    z: altitude
    };
    };
    MapManager.prototype.convertToGeoCoordinates = function (x, y, z) {
    var lon = x + this.map_info.width / 2,
    lat = y + this.map_info.depth / 2;
    lon = lon / 1000;
    lon = lon * (map_dict.max_x - map_dict.min_x) +
    map_dict.min_x;
    lon = lon / (map_dict.width / 360.0) - 180;
    lon = lon * (this.map_info.max_x - this.map_info.min_x) +
    this.map_info.min_x;
    lon = lon / (this.map_info.map_size / 360.0) - 180;
    lat = lat / 1000;
    lat = lat * (map_dict.max_y - map_dict.min_y) +
    map_dict.min_y;
    lat = 90 - lat / (map_dict.depth / 180.0);
    lat = lat * (this.map_info.max_y - this.map_info.min_y) +
    this.map_info.min_y;
    lat = 90 - lat / (this.map_info.map_size / 180.0);
    return {
    x: lat,
    y: lon,
    ......@@ -503,7 +503,7 @@ var GameManager = /** @class */ (function () {
    "use strict";
    // *** CONSTRUCTOR ***
    function GameManager(canvas, game_parameters_json) {
    var drone, header_list;
    var drone, header_list, i;
    this._canvas = canvas;
    this._canvas_width = canvas.width;
    this._canvas_height = canvas.height;
    ......@@ -555,7 +555,7 @@ var GameManager = /** @class */ (function () {
    console.log = function () {
    baseLogFunction.apply(console, arguments);
    var args = Array.prototype.slice.call(arguments);
    for (var i = 0;i < args.length;i++) {
    for (i = 0; i < args.length; i += 1) {
    console_log += args[i] + "\n";
    }
    };
    ......@@ -706,8 +706,7 @@ var GameManager = /** @class */ (function () {
    geo_coordinates = map_manager.convertToGeoCoordinates(
    drone_position.x,
    drone_position.y,
    drone_position.z,
    map_info
    drone_position.z
    );
    game_manager._flight_log[index].push([
    game_manager._game_duration, geo_coordinates.x,
    ......
    bt5/erp5_officejs_drone_simulator/PathTemplateItem/web_page_module/drone_simulator_logic_js.xml
    View file @ 98aa5f07
    ......@@ -240,7 +240,7 @@
    </item>
    <item>
    <key> <string>serial</string> </key>
    <value> <string>1010.3592.54606.53538</string> </value>
    <value> <string>1010.13183.29224.8874</string> </value>
    </item>
    <item>
    <key> <string>state</string> </key>
    ......@@ -260,7 +260,7 @@
    </tuple>
    <state>
    <tuple>
    <float>1690478004.4</float>
    <float>1691053915.45</float>
    <string>UTC</string>
    </tuple>
    </state>
    ......
    Markdown is supported
    0% or
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or to comment
    GitLab Nexedi Edition | About GitLab | About Nexedi | 沪ICP备2021021310号-2 | 沪ICP备2021021310号-7