diff --git a/src/uas/payload.rs b/src/uas/payload.rs index f5cf971..41a4c3b 100644 --- a/src/uas/payload.rs +++ b/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, }) } } diff --git a/src/uas/send.rs b/src/uas/send.rs index ff82f39..5544fc5 100644 --- a/src/uas/send.rs +++ b/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), - extended_sys_state_packet(system_id, payload.flying), + system_status_packet(system_id, payload.battery_remaining_pct), + extended_sys_state_packet(system_id, payload.landed), autopilot_version_packet(system_id, &mavlink_identity), home_position_packet( system_id, - payload.lat_deg, - payload.lon_deg, - payload.alt_msl_m - payload.rel_alt_m, + home_lat_deg, + home_lon_deg, + home_alt_msl_m, payload.heading_deg, ), component_heartbeat_packet(system_id, CAMERA_COMPONENT_ID, MAV_TYPE_CAMERA), diff --git a/src/uas/state.rs b/src/uas/state.rs index 7dca78f..57cc041 100644 --- a/src/uas/state.rs +++ b/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 { LATEST_UAS_SYSTEMS .lock()