Compare commits
No commits in common. "821072eae571f1e6f3d62074f15922e4ab93e1e1" and "e5bebc86cc7457984e39c4c0cf1bc6066e3c2d5a" have entirely different histories.
821072eae5
...
e5bebc86cc
23 changed files with 37 additions and 436 deletions
|
|
@ -25,8 +25,6 @@ var _rotor_azimuth: float = 0
|
|||
@onready var bone_cyclic: int = skeleton.find_bone("Cyclic")
|
||||
@onready var bone_rotor: int = skeleton.find_bone("Rotor")
|
||||
|
||||
@onready var instruments: Node = $AttitudeRoot/Instruments
|
||||
|
||||
func _process(delta: float) -> void:
|
||||
var target: Transform3D = connector.get_aircraft()
|
||||
position = target.origin
|
||||
|
|
@ -46,10 +44,6 @@ func _process(delta: float) -> void:
|
|||
var cyc_att := Quaternion.from_euler(Vector3(cyc_angles_rad.y, cyc_angles_rad.x, 0))
|
||||
skeleton.set_bone_pose_rotation(bone_cyclic, cyc_att * cyc_rest)
|
||||
|
||||
# Update the instruments
|
||||
instruments.call("set_controls", connector.get_controls())
|
||||
instruments.call("set_trim", connector.get_trim())
|
||||
|
||||
# Spin the rotor only when receiving flight data
|
||||
if connector.get_model_connected():
|
||||
_rotor_azimuth += rotor_rpm * 2 * PI / 60 * delta
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
[ext_resource type="Script" uid="uid://cx30pr7kn4c74" path="res://aircraft/aircraft.gd" id="1_l4uib"]
|
||||
[ext_resource type="PackedScene" uid="uid://cux4tju0ovvly" path="res://assets/mi2/Mi-2.glb" id="1_mrxe8"]
|
||||
[ext_resource type="PackedScene" uid="uid://fmygcraoturj" path="res://reference_axes.tscn" id="3_2bi7g"]
|
||||
[ext_resource type="PackedScene" uid="uid://cis4s43ubuynp" path="res://instruments/instruments.tscn" id="3_5w717"]
|
||||
[ext_resource type="PackedScene" uid="uid://cis4s43ubuynp" path="res://instruments.tscn" id="3_5w717"]
|
||||
[ext_resource type="Script" uid="uid://bt32fse84itrp" path="res://aircraft/fallback_input.gd" id="3_gf6ud"]
|
||||
|
||||
[sub_resource type="StandardMaterial3D" id="StandardMaterial3D_mrxe8"]
|
||||
|
|
|
|||
|
|
@ -1,12 +1,11 @@
|
|||
extends Node
|
||||
|
||||
const PFD_PATH: String = "pfd#adi,vsi,alt,ias,rht,rpm,hsi"
|
||||
const CONTROLS_PATH: String = "controls#collective,cyclic,rudder"
|
||||
|
||||
@export var lidia_hostname: String = "localhost"
|
||||
@export var lidia_port: int = 5555
|
||||
|
||||
@onready var controls: Node = $SubViewport2/Controls
|
||||
|
||||
func _ready():
|
||||
if !$CEF.initialize({"incognito":true, "locale":"en-US"}):
|
||||
push_error($CEF.get_error())
|
||||
|
|
@ -25,8 +24,10 @@ func _ready():
|
|||
browser_pfd.name = "pfd"
|
||||
browser_pfd.enable_ad_block(false) # Required for lidia static assets
|
||||
|
||||
# Propagate the calls to child
|
||||
func set_controls(current: Vector4):
|
||||
controls.call("set_controls", current)
|
||||
func set_trim(trim: Vector4):
|
||||
controls.call("set_trim", trim)
|
||||
var browser_ctrl = $CEF.create_browser(
|
||||
"http://{}:{}/{}".format([lidia_hostname, lidia_port, CONTROLS_PATH], "{}"),
|
||||
$SubViewport2/TextureRect,
|
||||
{ "frame_rate": 90, "javascript": true },
|
||||
)
|
||||
browser_ctrl.name = "controls"
|
||||
browser_ctrl.enable_ad_block(false) # Required for lidia static assets
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
[gd_scene load_steps=8 format=3 uid="uid://cis4s43ubuynp"]
|
||||
[gd_scene load_steps=7 format=3 uid="uid://cis4s43ubuynp"]
|
||||
|
||||
[ext_resource type="Script" uid="uid://01bmfj4wthwg" path="res://instruments.gd" id="1_wlkep"]
|
||||
[ext_resource type="PackedScene" uid="uid://bgkpwebqksth5" path="res://instruments/controls.tscn" id="2_372d7"]
|
||||
[ext_resource type="Script" uid="uid://01bmfj4wthwg" path="res://instruments.gd" id="1_h5at3"]
|
||||
|
||||
[sub_resource type="QuadMesh" id="QuadMesh_nowl7"]
|
||||
|
||||
|
|
@ -20,7 +19,7 @@ resource_local_to_scene = true
|
|||
albedo_texture = SubResource("ViewportTexture_h5at3")
|
||||
|
||||
[node name="Instruments" type="Node3D"]
|
||||
script = ExtResource("1_wlkep")
|
||||
script = ExtResource("1_h5at3")
|
||||
lidia_hostname = "192.168.1.2"
|
||||
|
||||
[node name="SubViewport" type="SubViewport" parent="."]
|
||||
|
|
@ -37,7 +36,10 @@ surface_material_override/0 = SubResource("StandardMaterial3D_h5at3")
|
|||
|
||||
[node name="SubViewport2" type="SubViewport" parent="."]
|
||||
|
||||
[node name="Controls" parent="SubViewport2" instance=ExtResource("2_372d7")]
|
||||
[node name="TextureRect" type="TextureRect" parent="SubViewport2"]
|
||||
offset_right = 512.0
|
||||
offset_bottom = 512.0
|
||||
expand_mode = 5
|
||||
|
||||
[node name="Quad2" type="MeshInstance3D" parent="."]
|
||||
transform = Transform3D(0.6, 0, 0, 0, 0.6, 0, 0, 0, 0.6, 0.425, 0, 0)
|
||||
BIN
project/instruments/controls-background.png
(Stored with Git LFS)
BIN
project/instruments/controls-background.png
(Stored with Git LFS)
Binary file not shown.
|
|
@ -1,34 +0,0 @@
|
|||
[remap]
|
||||
|
||||
importer="texture"
|
||||
type="CompressedTexture2D"
|
||||
uid="uid://b0oeokas7mq4q"
|
||||
path="res://.godot/imported/controls-background.png-626db610a70b7b467da1123d55529d5c.ctex"
|
||||
metadata={
|
||||
"vram_texture": false
|
||||
}
|
||||
|
||||
[deps]
|
||||
|
||||
source_file="res://instruments/controls-background.png"
|
||||
dest_files=["res://.godot/imported/controls-background.png-626db610a70b7b467da1123d55529d5c.ctex"]
|
||||
|
||||
[params]
|
||||
|
||||
compress/mode=0
|
||||
compress/high_quality=false
|
||||
compress/lossy_quality=0.7
|
||||
compress/hdr_compression=1
|
||||
compress/normal_map=0
|
||||
compress/channel_pack=0
|
||||
mipmaps/generate=false
|
||||
mipmaps/limit=-1
|
||||
roughness/mode=0
|
||||
roughness/src_normal=""
|
||||
process/fix_alpha_border=true
|
||||
process/premult_alpha=false
|
||||
process/normal_map_invert_y=false
|
||||
process/hdr_as_srgb=false
|
||||
process/hdr_clamp_exposure=false
|
||||
process/size_limit=0
|
||||
detect_3d/compress_to=1
|
||||
BIN
project/instruments/controls-ref.png
(Stored with Git LFS)
BIN
project/instruments/controls-ref.png
(Stored with Git LFS)
Binary file not shown.
|
|
@ -1,34 +0,0 @@
|
|||
[remap]
|
||||
|
||||
importer="texture"
|
||||
type="CompressedTexture2D"
|
||||
uid="uid://c5bcvbphgg1nk"
|
||||
path="res://.godot/imported/controls-ref.png-ed1b9d803223424fa895ad19151b7e9f.ctex"
|
||||
metadata={
|
||||
"vram_texture": false
|
||||
}
|
||||
|
||||
[deps]
|
||||
|
||||
source_file="res://instruments/controls-ref.png"
|
||||
dest_files=["res://.godot/imported/controls-ref.png-ed1b9d803223424fa895ad19151b7e9f.ctex"]
|
||||
|
||||
[params]
|
||||
|
||||
compress/mode=0
|
||||
compress/high_quality=false
|
||||
compress/lossy_quality=0.7
|
||||
compress/hdr_compression=1
|
||||
compress/normal_map=0
|
||||
compress/channel_pack=0
|
||||
mipmaps/generate=false
|
||||
mipmaps/limit=-1
|
||||
roughness/mode=0
|
||||
roughness/src_normal=""
|
||||
process/fix_alpha_border=true
|
||||
process/premult_alpha=false
|
||||
process/normal_map_invert_y=false
|
||||
process/hdr_as_srgb=false
|
||||
process/hdr_clamp_exposure=false
|
||||
process/size_limit=0
|
||||
detect_3d/compress_to=1
|
||||
|
|
@ -1,33 +0,0 @@
|
|||
extends Node2D
|
||||
## Recreate the controls page from lidia Python package
|
||||
|
||||
@onready var trim_longitudinal: Node2D = $CyclicOrigin/TrimLongitudinal
|
||||
@onready var trim_lateral: Node2D = $CyclicOrigin/TrimLateral
|
||||
@onready var current_cyclic: Node2D = $CyclicOrigin/CurrentCyclic
|
||||
@onready var trim_collective: Node2D = $CollectiveOrigin/TrimCollective
|
||||
@onready var current_collective: Node2D = $CollectiveOrigin/CurrentCollective
|
||||
@onready var trim_pedals: Node2D = $PedalsOrigin/TrimPedals
|
||||
@onready var current_pedals: Node2D = $PedalsOrigin/CurrentPedals
|
||||
|
||||
const AXIS_SIZE: float = 326
|
||||
|
||||
func set_controls(current: Vector4):
|
||||
set_cyclic(Vector2(current.x, current.y))
|
||||
set_collective(current.w)
|
||||
set_pedals(current.z)
|
||||
|
||||
func set_cyclic(current: Vector2):
|
||||
current_cyclic.position.x = AXIS_SIZE / 2.0 * current.x
|
||||
current_cyclic.position.y = AXIS_SIZE / 2.0 * current.y
|
||||
|
||||
func set_collective(current: float):
|
||||
current_collective.position.y = -AXIS_SIZE * current
|
||||
|
||||
func set_pedals(current: float):
|
||||
current_pedals.position.x = AXIS_SIZE / 2.0 * current
|
||||
|
||||
func set_trim(trim: Vector4):
|
||||
trim_lateral.position.x = AXIS_SIZE / 2.0 * trim.x
|
||||
trim_longitudinal.position.y = AXIS_SIZE / 2.0 * trim.y
|
||||
trim_pedals.position.x = AXIS_SIZE / 2.0 * trim.z
|
||||
trim_collective.position.y = -AXIS_SIZE * trim.w
|
||||
|
|
@ -1 +0,0 @@
|
|||
uid://ds6f8cur05ka0
|
||||
|
|
@ -1,54 +0,0 @@
|
|||
[gd_scene load_steps=7 format=3 uid="uid://bgkpwebqksth5"]
|
||||
|
||||
[ext_resource type="Script" uid="uid://ds6f8cur05ka0" path="res://instruments/controls.gd" id="1_g7kvs"]
|
||||
[ext_resource type="Texture2D" uid="uid://b0oeokas7mq4q" path="res://instruments/controls-background.png" id="2_wbvwt"]
|
||||
[ext_resource type="Texture2D" uid="uid://c561m3pvc0kop" path="res://instruments/trim-long.png" id="3_37ekc"]
|
||||
[ext_resource type="Texture2D" uid="uid://wdqnlguxmi2a" path="res://instruments/current-dot.png" id="4_4updv"]
|
||||
[ext_resource type="Texture2D" uid="uid://dytpn8rcdimr" path="res://instruments/trim-short.png" id="5_ni3tk"]
|
||||
[ext_resource type="Texture2D" uid="uid://dkda6ju5daocf" path="res://instruments/current-arrow.png" id="6_mfntf"]
|
||||
|
||||
[node name="Controls" type="Node2D"]
|
||||
script = ExtResource("1_g7kvs")
|
||||
|
||||
[node name="Background" type="Sprite2D" parent="."]
|
||||
position = Vector2(256, 256)
|
||||
texture = ExtResource("2_wbvwt")
|
||||
|
||||
[node name="CyclicOrigin" type="Node2D" parent="."]
|
||||
position = Vector2(309, 189)
|
||||
|
||||
[node name="TrimLongitudinal" type="Sprite2D" parent="CyclicOrigin"]
|
||||
position = Vector2(10, 0)
|
||||
texture = ExtResource("3_37ekc")
|
||||
|
||||
[node name="TrimLateral" type="Sprite2D" parent="CyclicOrigin"]
|
||||
position = Vector2(0, 10)
|
||||
rotation = 1.5708
|
||||
texture = ExtResource("3_37ekc")
|
||||
|
||||
[node name="CurrentCyclic" type="Sprite2D" parent="CyclicOrigin"]
|
||||
texture = ExtResource("4_4updv")
|
||||
|
||||
[node name="CollectiveOrigin" type="Node2D" parent="."]
|
||||
position = Vector2(72.5, 352)
|
||||
|
||||
[node name="TrimCollective" type="Sprite2D" parent="CollectiveOrigin"]
|
||||
position = Vector2(-10.5, 0)
|
||||
texture = ExtResource("5_ni3tk")
|
||||
|
||||
[node name="CurrentCollective" type="Sprite2D" parent="CollectiveOrigin"]
|
||||
position = Vector2(20.5, 0)
|
||||
texture = ExtResource("6_mfntf")
|
||||
|
||||
[node name="PedalsOrigin" type="Node2D" parent="."]
|
||||
position = Vector2(309, 425.5)
|
||||
|
||||
[node name="TrimPedals" type="Sprite2D" parent="PedalsOrigin"]
|
||||
position = Vector2(0, 10.5)
|
||||
rotation = -1.5708
|
||||
texture = ExtResource("5_ni3tk")
|
||||
|
||||
[node name="CurrentPedals" type="Sprite2D" parent="PedalsOrigin"]
|
||||
position = Vector2(0, -21.5)
|
||||
rotation = -1.5708
|
||||
texture = ExtResource("6_mfntf")
|
||||
BIN
project/instruments/current-arrow.png
(Stored with Git LFS)
BIN
project/instruments/current-arrow.png
(Stored with Git LFS)
Binary file not shown.
|
|
@ -1,34 +0,0 @@
|
|||
[remap]
|
||||
|
||||
importer="texture"
|
||||
type="CompressedTexture2D"
|
||||
uid="uid://dkda6ju5daocf"
|
||||
path="res://.godot/imported/current-arrow.png-8e092288abc9ec8d31cbc8f708a9c800.ctex"
|
||||
metadata={
|
||||
"vram_texture": false
|
||||
}
|
||||
|
||||
[deps]
|
||||
|
||||
source_file="res://instruments/current-arrow.png"
|
||||
dest_files=["res://.godot/imported/current-arrow.png-8e092288abc9ec8d31cbc8f708a9c800.ctex"]
|
||||
|
||||
[params]
|
||||
|
||||
compress/mode=0
|
||||
compress/high_quality=false
|
||||
compress/lossy_quality=0.7
|
||||
compress/hdr_compression=1
|
||||
compress/normal_map=0
|
||||
compress/channel_pack=0
|
||||
mipmaps/generate=false
|
||||
mipmaps/limit=-1
|
||||
roughness/mode=0
|
||||
roughness/src_normal=""
|
||||
process/fix_alpha_border=true
|
||||
process/premult_alpha=false
|
||||
process/normal_map_invert_y=false
|
||||
process/hdr_as_srgb=false
|
||||
process/hdr_clamp_exposure=false
|
||||
process/size_limit=0
|
||||
detect_3d/compress_to=1
|
||||
BIN
project/instruments/current-dot.png
(Stored with Git LFS)
BIN
project/instruments/current-dot.png
(Stored with Git LFS)
Binary file not shown.
|
|
@ -1,34 +0,0 @@
|
|||
[remap]
|
||||
|
||||
importer="texture"
|
||||
type="CompressedTexture2D"
|
||||
uid="uid://wdqnlguxmi2a"
|
||||
path="res://.godot/imported/current-dot.png-789da8759af9526ce5f8705121a1077d.ctex"
|
||||
metadata={
|
||||
"vram_texture": false
|
||||
}
|
||||
|
||||
[deps]
|
||||
|
||||
source_file="res://instruments/current-dot.png"
|
||||
dest_files=["res://.godot/imported/current-dot.png-789da8759af9526ce5f8705121a1077d.ctex"]
|
||||
|
||||
[params]
|
||||
|
||||
compress/mode=0
|
||||
compress/high_quality=false
|
||||
compress/lossy_quality=0.7
|
||||
compress/hdr_compression=1
|
||||
compress/normal_map=0
|
||||
compress/channel_pack=0
|
||||
mipmaps/generate=false
|
||||
mipmaps/limit=-1
|
||||
roughness/mode=0
|
||||
roughness/src_normal=""
|
||||
process/fix_alpha_border=true
|
||||
process/premult_alpha=false
|
||||
process/normal_map_invert_y=false
|
||||
process/hdr_as_srgb=false
|
||||
process/hdr_clamp_exposure=false
|
||||
process/size_limit=0
|
||||
detect_3d/compress_to=1
|
||||
BIN
project/instruments/pfd-ref.png
(Stored with Git LFS)
BIN
project/instruments/pfd-ref.png
(Stored with Git LFS)
Binary file not shown.
|
|
@ -1,34 +0,0 @@
|
|||
[remap]
|
||||
|
||||
importer="texture"
|
||||
type="CompressedTexture2D"
|
||||
uid="uid://toojp8r3424r"
|
||||
path="res://.godot/imported/pfd-ref.png-18e0fc5274406321f68279cb4defcf25.ctex"
|
||||
metadata={
|
||||
"vram_texture": false
|
||||
}
|
||||
|
||||
[deps]
|
||||
|
||||
source_file="res://instruments/pfd-ref.png"
|
||||
dest_files=["res://.godot/imported/pfd-ref.png-18e0fc5274406321f68279cb4defcf25.ctex"]
|
||||
|
||||
[params]
|
||||
|
||||
compress/mode=0
|
||||
compress/high_quality=false
|
||||
compress/lossy_quality=0.7
|
||||
compress/hdr_compression=1
|
||||
compress/normal_map=0
|
||||
compress/channel_pack=0
|
||||
mipmaps/generate=false
|
||||
mipmaps/limit=-1
|
||||
roughness/mode=0
|
||||
roughness/src_normal=""
|
||||
process/fix_alpha_border=true
|
||||
process/premult_alpha=false
|
||||
process/normal_map_invert_y=false
|
||||
process/hdr_as_srgb=false
|
||||
process/hdr_clamp_exposure=false
|
||||
process/size_limit=0
|
||||
detect_3d/compress_to=1
|
||||
BIN
project/instruments/trim-long.png
(Stored with Git LFS)
BIN
project/instruments/trim-long.png
(Stored with Git LFS)
Binary file not shown.
|
|
@ -1,34 +0,0 @@
|
|||
[remap]
|
||||
|
||||
importer="texture"
|
||||
type="CompressedTexture2D"
|
||||
uid="uid://c561m3pvc0kop"
|
||||
path="res://.godot/imported/trim-long.png-0b9b67642209f6a2ae4b2165f3b33d5d.ctex"
|
||||
metadata={
|
||||
"vram_texture": false
|
||||
}
|
||||
|
||||
[deps]
|
||||
|
||||
source_file="res://instruments/trim-long.png"
|
||||
dest_files=["res://.godot/imported/trim-long.png-0b9b67642209f6a2ae4b2165f3b33d5d.ctex"]
|
||||
|
||||
[params]
|
||||
|
||||
compress/mode=0
|
||||
compress/high_quality=false
|
||||
compress/lossy_quality=0.7
|
||||
compress/hdr_compression=1
|
||||
compress/normal_map=0
|
||||
compress/channel_pack=0
|
||||
mipmaps/generate=false
|
||||
mipmaps/limit=-1
|
||||
roughness/mode=0
|
||||
roughness/src_normal=""
|
||||
process/fix_alpha_border=true
|
||||
process/premult_alpha=false
|
||||
process/normal_map_invert_y=false
|
||||
process/hdr_as_srgb=false
|
||||
process/hdr_clamp_exposure=false
|
||||
process/size_limit=0
|
||||
detect_3d/compress_to=1
|
||||
BIN
project/instruments/trim-short.png
(Stored with Git LFS)
BIN
project/instruments/trim-short.png
(Stored with Git LFS)
Binary file not shown.
|
|
@ -1,34 +0,0 @@
|
|||
[remap]
|
||||
|
||||
importer="texture"
|
||||
type="CompressedTexture2D"
|
||||
uid="uid://dytpn8rcdimr"
|
||||
path="res://.godot/imported/trim-short.png-707c75166ad2d77e18c02dc4882968b1.ctex"
|
||||
metadata={
|
||||
"vram_texture": false
|
||||
}
|
||||
|
||||
[deps]
|
||||
|
||||
source_file="res://instruments/trim-short.png"
|
||||
dest_files=["res://.godot/imported/trim-short.png-707c75166ad2d77e18c02dc4882968b1.ctex"]
|
||||
|
||||
[params]
|
||||
|
||||
compress/mode=0
|
||||
compress/high_quality=false
|
||||
compress/lossy_quality=0.7
|
||||
compress/hdr_compression=1
|
||||
compress/normal_map=0
|
||||
compress/channel_pack=0
|
||||
mipmaps/generate=false
|
||||
mipmaps/limit=-1
|
||||
roughness/mode=0
|
||||
roughness/src_normal=""
|
||||
process/fix_alpha_border=true
|
||||
process/premult_alpha=false
|
||||
process/normal_map_invert_y=false
|
||||
process/hdr_as_srgb=false
|
||||
process/hdr_clamp_exposure=false
|
||||
process/size_limit=0
|
||||
detect_3d/compress_to=1
|
||||
|
|
@ -18,7 +18,6 @@
|
|||
#include "mavlink/all/mavlink.h" // IWYU pragma: keep; always include the mavlink.h file for selected dialect
|
||||
#include "mavlink/common/mavlink_msg_command_long.h"
|
||||
#include "mavlink/common/mavlink_msg_manual_control.h"
|
||||
#include "mavlink/common/mavlink_msg_manual_setpoint.h"
|
||||
#include "mavlink/common/mavlink_msg_param_request_list.h"
|
||||
#include "mavlink/common/mavlink_msg_param_set.h"
|
||||
#include "mavlink/common/mavlink_msg_param_value.h"
|
||||
|
|
@ -27,7 +26,6 @@
|
|||
#include "mavlink/mavlink_types.h"
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <string.h>
|
||||
|
||||
using namespace godot;
|
||||
|
||||
|
|
@ -38,8 +36,6 @@ void MarshConnector::_bind_methods() {
|
|||
ClassDB::bind_method(D_METHOD("get_collective"),
|
||||
&MarshConnector::get_collective);
|
||||
ClassDB::bind_method(D_METHOD("get_pedals"), &MarshConnector::get_pedals);
|
||||
ClassDB::bind_method(D_METHOD("get_controls"), &MarshConnector::get_controls);
|
||||
ClassDB::bind_method(D_METHOD("get_trim"), &MarshConnector::get_trim);
|
||||
ClassDB::bind_method(D_METHOD("get_model_connected"),
|
||||
&MarshConnector::get_model_connected);
|
||||
ClassDB::bind_method(D_METHOD("get_manager_connected"),
|
||||
|
|
@ -185,9 +181,6 @@ void MarshConnector::receive_data(const PackedByteArray &data) {
|
|||
case MAVLINK_MSG_ID_MANUAL_CONTROL:
|
||||
handle_manual_control(message);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_MANUAL_SETPOINT:
|
||||
handle_manual_setpoint(message);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
handle_heartbeat(message);
|
||||
break;
|
||||
|
|
@ -250,10 +243,6 @@ float MarshConnector::get_collective() { return last_controls.w; }
|
|||
|
||||
float MarshConnector::get_pedals() { return last_controls.z; }
|
||||
|
||||
Vector4 MarshConnector::get_controls() { return last_controls; }
|
||||
|
||||
Vector4 MarshConnector::get_trim() { return last_trim; }
|
||||
|
||||
bool MarshConnector::get_manager_connected() { return manager_connected; }
|
||||
|
||||
bool MarshConnector::get_model_connected() { return model_connected; }
|
||||
|
|
@ -414,11 +403,10 @@ Error MarshConnector::send_param(Parameter index) {
|
|||
}
|
||||
|
||||
mavlink_param_value_t param_value;
|
||||
memset(param_value.param_id, 0, sizeof(param_value.param_id));
|
||||
CharString nameBuffer = parameter_names[index].ascii();
|
||||
const char *name = nameBuffer.get_data();
|
||||
|
||||
// strncpy will write nulls after data until reaching target count
|
||||
strncpy(param_value.param_id, name, sizeof(param_value.param_id));
|
||||
strcpy_s(param_value.param_id, name);
|
||||
param_value.param_value = parameters[index];
|
||||
param_value.param_type = MAV_PARAM_TYPE_REAL32;
|
||||
param_value.param_count = PARAM_COUNT;
|
||||
|
|
@ -447,22 +435,6 @@ void MarshConnector::handle_manual_control(mavlink_message_t message) {
|
|||
last_controls.w = manual_control.z / 1000.0f;
|
||||
}
|
||||
|
||||
void MarshConnector::handle_manual_setpoint(mavlink_message_t message) {
|
||||
if (message.msgid != MAVLINK_MSG_ID_MANUAL_SETPOINT)
|
||||
return;
|
||||
|
||||
mavlink_manual_setpoint_t manual_setpoint;
|
||||
mavlink_msg_manual_setpoint_decode(&message, &manual_setpoint);
|
||||
if (manual_setpoint.mode_switch == MARSH_MANUAL_SETPOINT_MODE_TRIM) {
|
||||
last_trim = Vector4{
|
||||
manual_setpoint.roll,
|
||||
manual_setpoint.pitch,
|
||||
manual_setpoint.yaw,
|
||||
manual_setpoint.thrust,
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
void MarshConnector::handle_heartbeat(mavlink_message_t message) {
|
||||
if (message.msgid != MAVLINK_MSG_ID_HEARTBEAT)
|
||||
return;
|
||||
|
|
@ -473,16 +445,27 @@ void MarshConnector::handle_heartbeat(mavlink_message_t message) {
|
|||
if (!manager_connected) {
|
||||
print_line("Connected to MARSH Manager");
|
||||
|
||||
Error result = subscribe_message(message.sysid, message.compid,
|
||||
MAVLINK_MSG_ID_MANUAL_CONTROL);
|
||||
if (result != OK) {
|
||||
print_line("Subscribe control send result ", result);
|
||||
}
|
||||
// subscribe to messages not sent to visualisation node by default
|
||||
mavlink_command_long_t command;
|
||||
command.target_system = 1;
|
||||
command.target_component =
|
||||
MAV_COMP_ID_USER1; // HACK: Should get it from received HEARTBEAT
|
||||
command.command = MAV_CMD_SET_MESSAGE_INTERVAL;
|
||||
command.confirmation = 0;
|
||||
command.param1 = static_cast<float>(MAVLINK_MSG_ID_MANUAL_CONTROL);
|
||||
command.param2 = 0; // Default rate
|
||||
command.param3 = 0; // Not used
|
||||
command.param4 = 0;
|
||||
command.param5 = 0;
|
||||
command.param6 = 0;
|
||||
command.param7 = 1; // Address of requestor
|
||||
|
||||
result = subscribe_message(message.sysid, message.compid,
|
||||
MAVLINK_MSG_ID_MANUAL_SETPOINT);
|
||||
mavlink_message_t message_sent;
|
||||
mavlink_msg_command_long_encode_chan(
|
||||
system_id, component_id, MAVLINK_COMM_0, &message_sent, &command);
|
||||
Error result = send_message(message_sent);
|
||||
if (result != OK) {
|
||||
print_line("Subscribe setpoint send result ", result);
|
||||
print_line("Subscribe send result ", result);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -491,29 +474,6 @@ void MarshConnector::handle_heartbeat(mavlink_message_t message) {
|
|||
}
|
||||
}
|
||||
|
||||
Error MarshConnector::subscribe_message(uint8_t manager_system,
|
||||
uint8_t manager_component,
|
||||
uint32_t msgid) {
|
||||
// subscribe to messages not sent to visualisation node by default
|
||||
mavlink_command_long_t command;
|
||||
command.target_system = manager_system;
|
||||
command.target_component = manager_component;
|
||||
command.command = MAV_CMD_SET_MESSAGE_INTERVAL;
|
||||
command.confirmation = 0;
|
||||
command.param1 = static_cast<float>(msgid);
|
||||
command.param2 = 0; // Default rate
|
||||
command.param3 = 0; // Not used
|
||||
command.param4 = 0;
|
||||
command.param5 = 0;
|
||||
command.param6 = 0;
|
||||
command.param7 = 1; // Address of requester
|
||||
|
||||
mavlink_message_t message_sent;
|
||||
mavlink_msg_command_long_encode_chan(system_id, component_id, MAVLINK_COMM_0,
|
||||
&message_sent, &command);
|
||||
return send_message(message_sent);
|
||||
}
|
||||
|
||||
void MarshConnector::manager_timeout() {
|
||||
print_line("Lost connection to MARSH Manager");
|
||||
manager_connected = false;
|
||||
|
|
|
|||
|
|
@ -45,10 +45,6 @@ public:
|
|||
float get_collective();
|
||||
// Get normalized (-1 to 1) pedals position, positive turn right
|
||||
float get_pedals();
|
||||
// Get normalized position for all controls
|
||||
Vector4 get_controls();
|
||||
// Get normalized trim position
|
||||
Vector4 get_trim();
|
||||
|
||||
// Is there a connection to MARSH Manager
|
||||
bool get_manager_connected();
|
||||
|
|
@ -80,7 +76,6 @@ private:
|
|||
void handle_attitude(mavlink_message_t message);
|
||||
void handle_param(mavlink_message_t message);
|
||||
void handle_manual_control(mavlink_message_t message);
|
||||
void handle_manual_setpoint(mavlink_message_t message);
|
||||
void handle_heartbeat(mavlink_message_t message);
|
||||
|
||||
void receive_model_data();
|
||||
|
|
@ -108,18 +103,14 @@ private:
|
|||
"LOCAL_FRAME_LAT", "LOCAL_FRAME_LON",
|
||||
};
|
||||
Error send_param(Parameter index);
|
||||
Error subscribe_message(uint8_t manager_system, uint8_t manager_component,
|
||||
uint32_t msgid);
|
||||
|
||||
uint8_t system_id;
|
||||
uint8_t component_id;
|
||||
|
||||
// TODO: Interpolate with some delay
|
||||
// Or not, in practice the simple solution works nicely
|
||||
Vector3 last_location;
|
||||
Quaternion last_rotation;
|
||||
Vector4 last_controls;
|
||||
Vector4 last_trim;
|
||||
|
||||
double time_passed;
|
||||
Timer *heartbeat_timer;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue