added land and home storage

src/uas/payload.rs

@@ -33,6 +33,7 @@ pub struct UasSystemPayload {
     pub pitch_deg: f32,
     pub yaw_deg: f32,
     pub flying: bool,
+    pub landed: bool,
     pub gimbal_roll_deg: f32,
     pub gimbal_pitch_deg: f32,
     pub gimbal_yaw_deg: f32,
@@ -43,6 +44,7 @@ pub struct UasSystemPayload {
     pub image_lon_deg: f64,
     pub image_alt_msl_m: f32,
     pub has_turret_camera: bool,
+    pub battery_remaining_pct: i8,
 }
 
 impl FromArma for UasTelemetryPayload {
@@ -115,6 +117,7 @@ impl FromArma for UasSystemPayload {
             pitch_deg,
             yaw_deg,
             flying,
+            landed,
             gimbal_roll_deg,
             gimbal_pitch_deg,
             gimbal_yaw_deg,
@@ -125,6 +128,7 @@ impl FromArma for UasSystemPayload {
             image_lon_deg,
             image_alt_msl_m,
             has_turret_camera,
+            battery_remaining_pct,
         ) = <(
             String,
             String,
@@ -140,6 +144,7 @@ impl FromArma for UasSystemPayload {
             f32,
             f32,
             i32,
+            i32,
             f32,
             f32,
             f32,
@@ -150,6 +155,7 @@ impl FromArma for UasSystemPayload {
             f64,
             f32,
             i32,
+            i32,
         )>::from_arma(data)?;
 
         Ok(Self {
@@ -167,6 +173,7 @@ impl FromArma for UasSystemPayload {
             pitch_deg,
             yaw_deg,
             flying: flying != 0,
+            landed: landed != 0,
             gimbal_roll_deg,
             gimbal_pitch_deg,
             gimbal_yaw_deg,
@@ -177,6 +184,7 @@ impl FromArma for UasSystemPayload {
             image_lon_deg,
             image_alt_msl_m,
             has_turret_camera: has_turret_camera != 0,
+            battery_remaining_pct: battery_remaining_pct.clamp(0, 100) as i8,
         })
     }
 }

src/uas/send.rs

@@ -100,6 +100,19 @@ pub fn send_uas_system(ctx: Context, payload: UasSystemPayload) -> &'static str
     let active_camera_component = latest_system(system_id)
         .map(|system| system.active_camera_component)
         .unwrap_or(CAMERA_COMPONENT_ID);
+    let (home_lat_deg, home_lon_deg, home_alt_msl_m) = latest_system(system_id)
+        .map(|system| {
+            (
+                system.home_lat_deg,
+                system.home_lon_deg,
+                system.home_alt_msl_m,
+            )
+        })
+        .unwrap_or((
+            payload.lat_deg,
+            payload.lon_deg,
+            payload.alt_msl_m - payload.rel_alt_m,
+        ));
 
     info!(
         "MAVLink system send requested to {} entity_uuid={} mavlink_identity={} sysid={} callsign={} lat={} lon={} alt_msl={} rel_alt={} heading={} gimbal_pitch={} gimbal_yaw={} video_uri={}",
@@ -186,14 +199,14 @@ pub fn send_uas_system(ctx: Context, payload: UasSystemPayload) -> &'static str
         global_position_int_packet(&autopilot_payload),
         attitude_packet(&autopilot_payload),
         vfr_hud_packet(&autopilot_payload),
-        system_status_packet(system_id),
+        system_status_packet(system_id, payload.battery_remaining_pct),
-        extended_sys_state_packet(system_id, payload.flying),
+        extended_sys_state_packet(system_id, payload.landed),
         autopilot_version_packet(system_id, &mavlink_identity),
         home_position_packet(
             system_id,
-            payload.lat_deg,
+            home_lat_deg,
-            payload.lon_deg,
+            home_lon_deg,
-            payload.alt_msl_m - payload.rel_alt_m,
+            home_alt_msl_m,
             payload.heading_deg,
         ),
         component_heartbeat_packet(system_id, CAMERA_COMPONENT_ID, MAV_TYPE_CAMERA),

src/uas/state.rs

@@ -26,6 +26,9 @@ pub(crate) struct LatestUasSystem {
     pub image_alt_msl_m: f32,
     pub has_turret_camera: bool,
     pub active_camera_component: u8,
+    pub home_lat_deg: f64,
+    pub home_lon_deg: f64,
+    pub home_alt_msl_m: f32,
 }
 
 lazy_static! {
@@ -38,6 +41,20 @@ pub(crate) fn record_system(system_id: u8, mavlink_identity: &str, payload: &Uas
             .get(&system_id)
             .map(|system| system.active_camera_component)
             .unwrap_or(super::constants::CAMERA_COMPONENT_ID);
+        let home = systems
+            .get(&system_id)
+            .map(|system| {
+                (
+                    system.home_lat_deg,
+                    system.home_lon_deg,
+                    system.home_alt_msl_m,
+                )
+            })
+            .unwrap_or((
+                payload.lat_deg,
+                payload.lon_deg,
+                payload.alt_msl_m - payload.rel_alt_m,
+            ));
 
         systems.insert(
             system_id,
@@ -61,11 +78,24 @@ pub(crate) fn record_system(system_id: u8, mavlink_identity: &str, payload: &Uas
                 image_alt_msl_m: payload.image_alt_msl_m,
                 has_turret_camera: payload.has_turret_camera,
                 active_camera_component,
+                home_lat_deg: home.0,
+                home_lon_deg: home.1,
+                home_alt_msl_m: home.2,
             },
         );
     }
 }
 
+pub(crate) fn set_home(system_id: u8, lat_deg: f64, lon_deg: f64, alt_msl_m: f32) {
+    if let Ok(mut systems) = LATEST_UAS_SYSTEMS.lock() {
+        if let Some(system) = systems.get_mut(&system_id) {
+            system.home_lat_deg = lat_deg;
+            system.home_lon_deg = lon_deg;
+            system.home_alt_msl_m = alt_msl_m;
+        }
+    }
+}
+
 pub(crate) fn latest_system(system_id: u8) -> Option<LatestUasSystem> {
     LATEST_UAS_SYSTEMS
         .lock()