Commit 98aa5f07 authored by Roque's avatar Roque

erp5_officejs_drone_simulator: fix drone position update, speed and coordinates conversion

See merge request nexedi/erp5!1806
parents ebdc34d4 76226c46
Pipeline #29298 failed with stage
in 0 seconds
...@@ -61,16 +61,14 @@ var DroneLogAPI = /** @class */ (function () { ...@@ -61,16 +61,14 @@ var DroneLogAPI = /** @class */ (function () {
timestamp = parseInt(splitted_log_entry[0], 10); timestamp = parseInt(splitted_log_entry[0], 10);
lat = parseFloat(splitted_log_entry[1]); lat = parseFloat(splitted_log_entry[1]);
lon = parseFloat(splitted_log_entry[2]); 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]); height = parseFloat(splitted_log_entry[4]);
if (height < min_height) { if (height < min_height) {
height = min_height; height = min_height;
} }
converted_log_point_list.push([position[0], position = this._mapManager.convertToLocalCoordinates(lat, lon, height);
position[1], converted_log_point_list.push([position.x,
height, timestamp / time_offset]); position.y,
position.z, timestamp / time_offset]);
} }
this._flight_parameters.converted_log_point_list = converted_log_point_list; this._flight_parameters.converted_log_point_list = converted_log_point_list;
}; };
......
...@@ -232,7 +232,7 @@ ...@@ -232,7 +232,7 @@
</item> </item>
<item> <item>
<key> <string>actor</string> </key> <key> <string>actor</string> </key>
<value> <string>zope</string> </value> <value> <unicode>zope</unicode> </value>
</item> </item>
<item> <item>
<key> <string>comment</string> </key> <key> <string>comment</string> </key>
...@@ -246,7 +246,7 @@ ...@@ -246,7 +246,7 @@
</item> </item>
<item> <item>
<key> <string>serial</string> </key> <key> <string>serial</string> </key>
<value> <string>1009.38419.20212.62293</string> </value> <value> <string>1010.13157.33937.23381</string> </value>
</item> </item>
<item> <item>
<key> <string>state</string> </key> <key> <string>state</string> </key>
...@@ -266,7 +266,7 @@ ...@@ -266,7 +266,7 @@
</tuple> </tuple>
<state> <state>
<tuple> <tuple>
<float>1688635412.87</float> <float>1691051883.97</float>
<string>UTC</string> <string>UTC</string>
</tuple> </tuple>
</state> </state>
......
...@@ -92,7 +92,7 @@ var FixedWingDroneAPI = /** @class */ (function () { ...@@ -92,7 +92,7 @@ var FixedWingDroneAPI = /** @class */ (function () {
** Function called on every drone update, right before onUpdate AI script ** Function called on every drone update, right before onUpdate AI script
*/ */
FixedWingDroneAPI.prototype.internal_update = function (context, delta_time) { FixedWingDroneAPI.prototype.internal_update = function (context, delta_time) {
var diff, newrot, orientationValue, rotStep, updateSpeed; var diff, newrot, orientationValue, rotStep;
//TODO rotation //TODO rotation
if (context._rotationTarget) { if (context._rotationTarget) {
...@@ -116,18 +116,8 @@ var FixedWingDroneAPI = /** @class */ (function () { ...@@ -116,18 +116,8 @@ var FixedWingDroneAPI = /** @class */ (function () {
} }
this._updateSpeed(context, delta_time); 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 //TODO rotation
orientationValue = context._maxOrientation * orientationValue = context._maxOrientation *
(context._speed / context._maxSpeed); (context._speed / context._maxSpeed);
...@@ -180,67 +170,112 @@ var FixedWingDroneAPI = /** @class */ (function () { ...@@ -180,67 +170,112 @@ var FixedWingDroneAPI = /** @class */ (function () {
} }
}; };
FixedWingDroneAPI.prototype._updateDirection = function (drone, delta_time) { FixedWingDroneAPI.prototype._updatePosition = function (drone, delta_time) {
var horizontalCoeff, newX, newY, newZ, tangentYaw; 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 if (this._loiter_mode
&& Math.sqrt( && Math.sqrt(
Math.pow(drone._targetCoordinates.x - drone.position.x, 2) Math.pow(drone._targetCoordinates.x - drone.position.x, 2)
+ Math.pow(drone._targetCoordinates.y - drone.position.y, 2) + Math.pow(drone._targetCoordinates.y - drone.position.y, 2)
) <= this._loiter_radius) { ) <= this._loiter_radius) {
tangentYaw = this._computeBearing( newYaw = bearing - 90;
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);
} else { } 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( verticalSpeed = this._getVerticalSpeed(drone);
( groundSpeed = Math.sqrt(
Math.pow(drone.getAirSpeed(), 2) - Math.pow(newY, 2) Math.pow(drone.getAirSpeed(), 2) - Math.pow(verticalSpeed, 2)
) / (
Math.pow(newX, 2) + Math.pow(newZ, 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) { distance = (groundSpeed * delta_time / 1000) / R;
// swap y and z axis so z axis represents altitude distanceCos = Math.cos(distance);
var bearing = this._computeBearing( distanceSin = Math.sin(distance);
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;
if (yawUpdate >= Math.abs(yawDiff)) { newLatRad = Math.asin(
yawUpdate = yawDiff; currentSinLat * distanceCos
} else if (yawDiff < 0) { + currentCosLat * distanceSin * Math.cos(newYawRad)
yawUpdate *= -1; );
} newLonRad = currentLonRad + Math.atan2(
yaw += yawUpdate; Math.sin(newYawRad) * distanceSin * currentCosLat,
distanceCos - currentSinLat * Math.sin(newLatRad)
);
// trigonometric circle is east oriented, yaw angle is clockwise newCoordinates = this._mapManager.convertToLocalCoordinates(
yaw = this._toRad(-yaw + 90); this._toDeg(newLatRad),
return [Math.cos(yaw), Math.sin(yaw)]; 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 // swap y and z axis so z axis represents altitude
var altitudeDiff = drone._targetCoordinates.z - drone.position.z, var altitudeDiff = drone._targetCoordinates.z - drone.position.z,
verticalSpeed; verticalSpeed;
...@@ -356,24 +391,17 @@ var FixedWingDroneAPI = /** @class */ (function () { ...@@ -356,24 +391,17 @@ var FixedWingDroneAPI = /** @class */ (function () {
if (isNaN(lat) || isNaN(lon) || isNaN(z)) { if (isNaN(lat) || isNaN(lon) || isNaN(z)) {
throw new Error('Target coordinates must be numbers'); throw new Error('Target coordinates must be numbers');
} }
var x = this._mapManager.longitudToX(lon, this._map_dict.map_size), var processed_coordinates =
y = this._mapManager.latitudeToY(lat, this._map_dict.map_size), this._mapManager.convertToLocalCoordinates(lat, lon, z);
position = this._mapManager.normalize(x, y, this._map_dict), if (processed_coordinates.z > this._map_dict.start_AMSL) {
processed_coordinates; processed_coordinates.z -= this._map_dict.start_AMSL;
if (z > this._map_dict.start_AMSL) {
z -= this._map_dict.start_AMSL;
} }
processed_coordinates = {
x: position[0],
y: position[1],
z: z
};
//this._last_altitude_point_reached = -1; //this._last_altitude_point_reached = -1;
//this.takeoff_path = []; //this.takeoff_path = [];
return processed_coordinates; return processed_coordinates;
}; };
FixedWingDroneAPI.prototype.getCurrentPosition = function (x, y, z) { 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 () { FixedWingDroneAPI.prototype.getDroneAI = function () {
return null; return null;
...@@ -419,11 +447,19 @@ var FixedWingDroneAPI = /** @class */ (function () { ...@@ -419,11 +447,19 @@ var FixedWingDroneAPI = /** @class */ (function () {
}; };
FixedWingDroneAPI.prototype.getYaw = function (drone) { FixedWingDroneAPI.prototype.getYaw = function (drone) {
var direction = drone.worldDirection; var direction = drone.worldDirection;
return this._computeBearing(0, 0, direction.x, direction.z); return this._toDeg(Math.atan2(direction.x, direction.z));
}; };
FixedWingDroneAPI.prototype._computeBearing = function (x1, z1, x2, z2) { FixedWingDroneAPI.prototype._computeBearing =
return this._toDeg(Math.atan2(x2 - x1, z2 - z1)); 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) { FixedWingDroneAPI.prototype._computeYawDiff = function (yaw1, yaw2) {
var diff = yaw2 - yaw1; var diff = yaw2 - yaw1;
diff += (diff > 180) ? -360 : (diff < -180) ? 360 : 0; diff += (diff > 180) ? -360 : (diff < -180) ? 360 : 0;
......
...@@ -240,7 +240,7 @@ ...@@ -240,7 +240,7 @@
</item> </item>
<item> <item>
<key> <string>serial</string> </key> <key> <string>serial</string> </key>
<value> <string>1009.59163.16294.47701</string> </value> <value> <string>1010.13155.61424.54442</string> </value>
</item> </item>
<item> <item>
<key> <string>state</string> </key> <key> <string>state</string> </key>
...@@ -260,7 +260,7 @@ ...@@ -260,7 +260,7 @@
</tuple> </tuple>
<state> <state>
<tuple> <tuple>
<float>1690395852.2</float> <float>1691051762.91</float>
<string>UTC</string> <string>UTC</string>
</tuple> </tuple>
</state> </state>
......
...@@ -367,41 +367,11 @@ var DroneManager = /** @class */ (function () { ...@@ -367,41 +367,11 @@ var DroneManager = /** @class */ (function () {
var MapManager = /** @class */ (function () { var MapManager = /** @class */ (function () {
"use strict"; "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 //** CONSTRUCTOR
function MapManager(scene) { function MapManager(scene) {
var _this = this, max_sky, skybox, skyboxMat, largeGroundMat, var _this = this, max_sky, skybox, skyboxMat, largeGroundMat,
largeGroundBottom, width, depth, terrain, max; largeGroundBottom, width, depth, terrain, max;
_this.map_info = calculateMapInfo(_this, GAMEPARAMETERS.map, this.setMapInfo(GAMEPARAMETERS.map, GAMEPARAMETERS.initialPosition);
GAMEPARAMETERS.initialPosition);
max = _this.map_info.width; max = _this.map_info.width;
if (_this.map_info.depth > max) { if (_this.map_info.depth > max) {
max = _this.map_info.depth; max = _this.map_info.depth;
...@@ -446,14 +416,37 @@ var MapManager = /** @class */ (function () { ...@@ -446,14 +416,37 @@ var MapManager = /** @class */ (function () {
terrain.scaling = new BABYLON.Vector3(depth / 50000, depth / 50000, terrain.scaling = new BABYLON.Vector3(depth / 50000, depth / 50000,
width / 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 () { MapManager.prototype.getMapInfo = function () {
return this.map_info; return this.map_info;
}; };
MapManager.prototype.longitudToX = function (lon, map_size) { MapManager.prototype.longitudToX = function (lon) {
return (map_size / 360.0) * (180 + lon); return (this.map_info.map_size / 360.0) * (180 + lon);
}; };
MapManager.prototype.latitudeToY = function (lat, map_size) { MapManager.prototype.latitudeToY = function (lat) {
return (map_size / 180.0) * (90 - lat); return (this.map_info.map_size / 180.0) * (90 - lat);
}; };
MapManager.prototype.latLonDistance = function (c1, c2) { MapManager.prototype.latLonDistance = function (c1, c2) {
var R = 6371e3, var R = 6371e3,
...@@ -467,23 +460,30 @@ var MapManager = /** @class */ (function () { ...@@ -467,23 +460,30 @@ var MapManager = /** @class */ (function () {
c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a)); c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
return R * c; return R * c;
}; };
MapManager.prototype.normalize = function (x, y, map_dict) { MapManager.prototype.convertToLocalCoordinates =
var n_x = (x - map_dict.min_x) / (map_dict.max_x - map_dict.min_x), function (latitude, longitude, altitude) {
n_y = (y - map_dict.min_y) / (map_dict.max_y - map_dict.min_y); var map_info = this.map_info,
return [n_x * 1000 - map_dict.width / 2, x = this.longitudToX(longitude),
n_y * 1000 - map_dict.depth / 2]; y = this.latitudeToY(latitude);
}; return {
MapManager.prototype.convertToGeoCoordinates = function (x, y, z, map_dict) { x: ((x - map_info.min_x) / (map_info.max_x - map_info.min_x))
var lon = x + map_dict.width / 2, * 1000 - map_info.width / 2,
lat = y + map_dict.depth / 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 / 1000;
lon = lon * (map_dict.max_x - map_dict.min_x) + lon = lon * (this.map_info.max_x - this.map_info.min_x) +
map_dict.min_x; this.map_info.min_x;
lon = lon / (map_dict.width / 360.0) - 180; lon = lon / (this.map_info.map_size / 360.0) - 180;
lat = lat / 1000; lat = lat / 1000;
lat = lat * (map_dict.max_y - map_dict.min_y) + lat = lat * (this.map_info.max_y - this.map_info.min_y) +
map_dict.min_y; this.map_info.min_y;
lat = 90 - lat / (map_dict.depth / 180.0); lat = 90 - lat / (this.map_info.map_size / 180.0);
return { return {
x: lat, x: lat,
y: lon, y: lon,
...@@ -503,7 +503,7 @@ var GameManager = /** @class */ (function () { ...@@ -503,7 +503,7 @@ var GameManager = /** @class */ (function () {
"use strict"; "use strict";
// *** CONSTRUCTOR *** // *** CONSTRUCTOR ***
function GameManager(canvas, game_parameters_json) { function GameManager(canvas, game_parameters_json) {
var drone, header_list; var drone, header_list, i;
this._canvas = canvas; this._canvas = canvas;
this._canvas_width = canvas.width; this._canvas_width = canvas.width;
this._canvas_height = canvas.height; this._canvas_height = canvas.height;
...@@ -555,7 +555,7 @@ var GameManager = /** @class */ (function () { ...@@ -555,7 +555,7 @@ var GameManager = /** @class */ (function () {
console.log = function () { console.log = function () {
baseLogFunction.apply(console, arguments); baseLogFunction.apply(console, arguments);
var args = Array.prototype.slice.call(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"; console_log += args[i] + "\n";
} }
}; };
...@@ -706,8 +706,7 @@ var GameManager = /** @class */ (function () { ...@@ -706,8 +706,7 @@ var GameManager = /** @class */ (function () {
geo_coordinates = map_manager.convertToGeoCoordinates( geo_coordinates = map_manager.convertToGeoCoordinates(
drone_position.x, drone_position.x,
drone_position.y, drone_position.y,
drone_position.z, drone_position.z
map_info
); );
game_manager._flight_log[index].push([ game_manager._flight_log[index].push([
game_manager._game_duration, geo_coordinates.x, game_manager._game_duration, geo_coordinates.x,
......
...@@ -240,7 +240,7 @@ ...@@ -240,7 +240,7 @@
</item> </item>
<item> <item>
<key> <string>serial</string> </key> <key> <string>serial</string> </key>
<value> <string>1010.3592.54606.53538</string> </value> <value> <string>1010.13183.29224.8874</string> </value>
</item> </item>
<item> <item>
<key> <string>state</string> </key> <key> <string>state</string> </key>
...@@ -260,7 +260,7 @@ ...@@ -260,7 +260,7 @@
</tuple> </tuple>
<state> <state>
<tuple> <tuple>
<float>1690478004.4</float> <float>1691053915.45</float>
<string>UTC</string> <string>UTC</string>
</tuple> </tuple>
</state> </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