diff --git a/micro-rdk-nmea-macros/src/attributes.rs b/micro-rdk-nmea-macros/src/attributes.rs index 6c5138ede..de2f7346a 100644 --- a/micro-rdk-nmea-macros/src/attributes.rs +++ b/micro-rdk-nmea-macros/src/attributes.rs @@ -14,6 +14,7 @@ use crate::utils::{error_tokens, UnitConversion}; /// `scale_token`: scale factor to be applied to the raw value of the field /// `unit`: unit that the field's value should be converted to before serialization /// `is_lookup`: whether the field is a lookup field +/// `is_fieldset`: whether the field represents a repeating set of fields /// `length_field`: if this field is a list of fieldsets, this is the field whose value represents /// the expected length of this list /// @@ -43,6 +44,7 @@ pub(crate) struct MacroAttributes { pub(crate) scale_token: Option, pub(crate) unit: Option, pub(crate) is_lookup: bool, + pub(crate) is_fieldset: bool, pub(crate) length_field: Option, } @@ -89,6 +91,7 @@ impl MacroAttributes { label: None, length_field: None, unit: None, + is_fieldset: false, }; for attr in field.attrs.iter() { @@ -246,6 +249,7 @@ impl MacroAttributes { } }) } + "fieldset" => macro_attrs.is_fieldset = true, _ => {} }; } diff --git a/micro-rdk-nmea-macros/src/composition.rs b/micro-rdk-nmea-macros/src/composition.rs index acc45fc5f..9378c183f 100644 --- a/micro-rdk-nmea-macros/src/composition.rs +++ b/micro-rdk-nmea-macros/src/composition.rs @@ -1,11 +1,17 @@ use proc_macro::TokenStream; use proc_macro2::{Span, TokenStream as TokenStream2}; -use quote::{format_ident, quote}; -use syn::{DeriveInput, Field, Ident, Type}; +use quote::{format_ident, quote, ToTokens}; +use syn::{DeriveInput, Field, GenericArgument, Ident, PathArguments, Type}; use crate::attributes::MacroAttributes; use crate::utils::{error_tokens, get_micro_nmea_crate_ident, is_supported_integer_type}; +#[derive(Debug, Clone, Copy)] +pub(crate) enum CodeGenPurpose { + Message, + Fieldset, +} + /// Represents a subset of auto-generated code statements for the implementation of a particular /// NMEA message. Each field in a message struct contributes its own set of statements to the macro /// sorted into buckets by category. Those statements are then merged into the set of statements @@ -37,7 +43,7 @@ impl PgnComposition { .append(&mut other.proto_conversion_logic); } - pub(crate) fn from_field(field: &Field) -> Result { + pub(crate) fn from_field(field: &Field, purpose: CodeGenPurpose) -> Result { let mut statements = Self::new(); if let Some(name) = &field.ident { if name == "source_id" { @@ -51,16 +57,23 @@ impl PgnComposition { let macro_attrs = MacroAttributes::from_field(field)?; if macro_attrs.offset != 0 { - let offset = macro_attrs.offset / 8; + let offset = macro_attrs.offset; statements .parsing_logic .push(quote! { let _ = cursor.read(#offset)?; }); } let new_statements = if is_supported_integer_type(&field.ty) { - handle_number_field(name, field, ¯o_attrs)? + handle_number_field(name, field, ¯o_attrs, purpose)? } else if macro_attrs.is_lookup { - handle_lookup_field(name, &field.ty, ¯o_attrs)? + handle_lookup_field(name, &field.ty, ¯o_attrs, purpose)? + } else if field.attrs.iter().any(|attr| { + attr.path() + .segments + .iter() + .any(|seg| seg.ident.to_string().as_str() == "fieldset") + }) { + handle_fieldset(name, field, ¯o_attrs, purpose)? } else { let err_msg = format!( "field type for {:?} unsupported for PGN message, macro attributes: {:?}", @@ -79,7 +92,10 @@ impl PgnComposition { } } - pub(crate) fn from_input(input: &DeriveInput) -> Result { + pub(crate) fn from_input( + input: &DeriveInput, + purpose: CodeGenPurpose, + ) -> Result { let src_fields = if let syn::Data::Struct(syn::DataStruct { ref fields, .. }) = input.data { fields } else { @@ -99,7 +115,7 @@ impl PgnComposition { }; let mut statements = Self::new(); for field in named_fields.iter() { - match PgnComposition::from_field(field) { + match PgnComposition::from_field(field, purpose) { Ok(new_statements) => { statements.merge(new_statements); } @@ -123,9 +139,8 @@ impl PgnComposition { let mrdk_crate = crate::utils::get_micro_rdk_crate_ident(); quote! { impl #impl_generics #name #src_generics #src_where_clause { - pub fn from_bytes(data: Vec, source_id: u8) -> Result { - use #crate_ident::parse_helpers::parsers::{DataCursor, FieldReader}; - let mut cursor = DataCursor::new(data); + pub fn from_cursor(mut cursor: #crate_ident::parse_helpers::parsers::DataCursor, source_id: u8) -> Result { + use #crate_ident::parse_helpers::parsers::FieldReader; #(#parsing_logic)* Ok(Self { #(#struct_initialization)* @@ -141,12 +156,55 @@ impl PgnComposition { } } } + + pub(crate) fn into_fieldset_token_stream(self, input: &DeriveInput) -> TokenStream2 { + let name = &input.ident; + let parsing_logic = self.parsing_logic; + let attribute_getters = self.attribute_getters; + let struct_initialization = self.struct_initialization; + let proto_conversion_logic = self.proto_conversion_logic; + let (impl_generics, src_generics, src_where_clause) = input.generics.split_for_impl(); + let crate_ident = crate::utils::get_micro_nmea_crate_ident(); + let mrdk_crate = crate::utils::get_micro_rdk_crate_ident(); + let error_ident = quote! {#crate_ident::parse_helpers::errors::NmeaParseError}; + let field_set_ident = quote! {#crate_ident::parse_helpers::parsers::FieldSet}; + + quote! { + impl #impl_generics #name #src_generics #src_where_clause { + #(#attribute_getters)* + } + + impl #impl_generics #field_set_ident for #name #src_generics #src_where_clause { + fn from_data(cursor: &mut DataCursor) -> Result { + use #crate_ident::parse_helpers::parsers::FieldReader; + #(#parsing_logic)* + Ok(Self { + #(#struct_initialization)* + }) + } + + fn to_readings(&self) -> Result<#mrdk_crate::common::sensor::GenericReadingsResult, #error_ident> { + let mut readings = std::collections::HashMap::new(); + #(#proto_conversion_logic)* + Ok(readings) + } + } + } + } +} + +fn get_read_statement(name: &Ident, purpose: CodeGenPurpose) -> TokenStream2 { + match purpose { + CodeGenPurpose::Message => quote! {let #name = reader.read_from_cursor(&mut cursor)?;}, + CodeGenPurpose::Fieldset => quote! {let #name = reader.read_from_cursor(cursor)?;}, + } } fn handle_number_field( name: &Ident, field: &Field, macro_attrs: &MacroAttributes, + purpose: CodeGenPurpose, ) -> Result { let bits_size: usize = macro_attrs.bits; let scale_token = macro_attrs.scale_token.as_ref(); @@ -238,9 +296,10 @@ fn handle_number_field( }); let nmea_crate = get_micro_nmea_crate_ident(); + let read_statement = get_read_statement(name, purpose); new_statements.parsing_logic.push(quote! { let reader = #nmea_crate::parse_helpers::parsers::NumberField::<#num_ty>::new(#bits_size)?; - let #name = reader.read_from_cursor(&mut cursor)?; + #read_statement }); new_statements.struct_initialization.push(quote! {#name,}); @@ -251,6 +310,7 @@ fn handle_lookup_field( name: &Ident, field_type: &Type, macro_attrs: &MacroAttributes, + purpose: CodeGenPurpose, ) -> Result { let mut new_statements = PgnComposition::new(); let bits_size = macro_attrs.bits; @@ -261,9 +321,10 @@ fn handle_lookup_field( }); let nmea_crate = get_micro_nmea_crate_ident(); + let read_statement = get_read_statement(name, purpose); let setters = quote! { let reader = #nmea_crate::parse_helpers::parsers::LookupField::<#enum_type>::new(#bits_size)?; - let #name = reader.read_from_cursor(&mut cursor)?; + #read_statement }; new_statements.parsing_logic.push(setters); @@ -282,3 +343,80 @@ fn handle_lookup_field( } Ok(new_statements) } + +fn handle_fieldset( + name: &Ident, + field: &Field, + macro_attrs: &MacroAttributes, + purpose: CodeGenPurpose, +) -> Result { + let mut new_statements = PgnComposition::new(); + if field.attrs.iter().any(|attr| { + attr.path() + .segments + .iter() + .any(|seg| seg.ident.to_string().as_str() == "fieldset") + }) { + let f_type = match &field.ty { + Type::Path(type_path) => { + let vec_seg = &type_path.path.segments[0]; + if &vec_seg.ident.to_string() != "Vec" { + Err(error_tokens("fieldset must be Vec")) + } else { + if let PathArguments::AngleBracketed(args) = &vec_seg.arguments { + let type_arg = &args.args[0]; + if let GenericArgument::Type(f_type) = type_arg { + Ok(f_type.to_token_stream()) + } else { + Err(error_tokens("fieldset must be Vec with type")) + } + } else { + Err(error_tokens("fieldset must be Vec with angle brackets")) + } + } + } + _ => Err(error_tokens("improper field type")), + }?; + + let length_field_token = macro_attrs.length_field.as_ref().ok_or(error_tokens( + "length_field field must be specified for fieldset", + ))?; + + let nmea_crate = get_micro_nmea_crate_ident(); + let read_statement = get_read_statement(name, purpose); + new_statements.parsing_logic.push(quote! { + let reader = #nmea_crate::parse_helpers::parsers::FieldSetList::<#f_type>::new(#length_field_token as usize); + #read_statement + }); + + new_statements.attribute_getters.push(quote! { + pub fn #name(&self) -> Vec<#f_type> { self.#name.clone() } + }); + new_statements.struct_initialization.push(quote! {#name,}); + let proto_import_prefix = crate::utils::get_proto_import_prefix(); + let prop_name = name.to_string(); + let label = macro_attrs.label.clone().unwrap_or(quote! {#prop_name}); + let crate_ident = crate::utils::get_micro_nmea_crate_ident(); + let error_ident = quote! {#crate_ident::parse_helpers::errors::NmeaParseError}; + new_statements.proto_conversion_logic.push(quote! { + let values: Result, #error_ident> = self.#name().iter().map(|inst| { + inst.to_readings().map(|fields| { + #proto_import_prefix::Value { + kind: Some(#proto_import_prefix::value::Kind::StructValue(#proto_import_prefix::Struct { + fields: fields + })) + } + }) + }).collect(); + let value = #proto_import_prefix::Value { + kind: Some(#proto_import_prefix::value::Kind::ListValue(#proto_import_prefix::ListValue { + values: values? + })) + }; + readings.insert(#label.to_string(), value); + }); + Ok(new_statements) + } else { + Err(error_tokens("msg")) + } +} diff --git a/micro-rdk-nmea-macros/src/lib.rs b/micro-rdk-nmea-macros/src/lib.rs index 3d2eda15b..4bc05d2f3 100644 --- a/micro-rdk-nmea-macros/src/lib.rs +++ b/micro-rdk-nmea-macros/src/lib.rs @@ -2,7 +2,7 @@ pub(crate) mod attributes; pub(crate) mod composition; pub(crate) mod utils; -use crate::composition::PgnComposition; +use crate::composition::{CodeGenPurpose, PgnComposition}; use proc_macro::TokenStream; /// PgnMessageDerive is a macro that implements parsing logic for a struct in the form of a method @@ -17,8 +17,23 @@ use proc_macro::TokenStream; pub fn pgn_message_derive(item: TokenStream) -> TokenStream { let input = syn::parse_macro_input!(item as syn::DeriveInput); - match PgnComposition::from_input(&input) { + match PgnComposition::from_input(&input, CodeGenPurpose::Message) { Ok(statements) => statements.into_token_stream(&input).into(), Err(tokens) => tokens, } } + +/// FieldsetDerive is a macro that defines a struct implementing parsing logic for +/// data found in a repeated field in a NMEA message via the `FieldSet` trait +#[proc_macro_derive( + FieldsetDerive, + attributes(label, scale, lookup, bits, offset, fieldset, length_field, unit) +)] +pub fn fieldset_derive(item: TokenStream) -> TokenStream { + let input = syn::parse_macro_input!(item as syn::DeriveInput); + + match PgnComposition::from_input(&input, CodeGenPurpose::Fieldset) { + Ok(statements) => statements.into_fieldset_token_stream(&input).into(), + Err(tokens) => tokens, + } +} diff --git a/micro-rdk-nmea/src/lib.rs b/micro-rdk-nmea/src/lib.rs index e70951f91..ab9bc0b91 100644 --- a/micro-rdk-nmea/src/lib.rs +++ b/micro-rdk-nmea/src/lib.rs @@ -6,8 +6,14 @@ mod tests { use base64::{engine::general_purpose, Engine}; use crate::{ - messages::pgns::{TemperatureExtendedRange, WaterDepth}, - parse_helpers::{enums::TemperatureSource, errors::NumberFieldError}, + messages::pgns::{GnssPositionData, GnssSatsInView, TemperatureExtendedRange, WaterDepth}, + parse_helpers::{ + enums::{ + Gns, GnsIntegrity, GnsMethod, RangeResidualMode, SatelliteStatus, TemperatureSource, + }, + errors::NumberFieldError, + parsers::DataCursor, + }, }; // The strings in the below test represent base64-encoded data examples taken from raw results @@ -20,7 +26,8 @@ mod tests { let mut data = Vec::::new(); let res = general_purpose::STANDARD.decode_vec(water_depth_str, &mut data); assert!(res.is_ok()); - let message = WaterDepth::from_bytes(data[33..].to_vec(), 13); + let cursor = DataCursor::new(data[33..].to_vec()); + let message = WaterDepth::from_cursor(cursor, 13); assert!(message.is_ok()); let message = message.unwrap(); assert_eq!(message.source_id(), 13); @@ -42,7 +49,8 @@ mod tests { let mut data = Vec::::new(); let res = general_purpose::STANDARD.decode_vec(water_depth_str, &mut data); assert!(res.is_ok()); - let message = WaterDepth::from_bytes(data[33..].to_vec(), 13); + let cursor = DataCursor::new(data[33..].to_vec()); + let message = WaterDepth::from_cursor(cursor, 13); assert!(message.is_ok()); let message = message.unwrap(); assert_eq!(message.source_id(), 13); @@ -65,7 +73,8 @@ mod tests { let res = general_purpose::STANDARD.decode_vec(temp_str, &mut data); assert!(res.is_ok()); - let message = TemperatureExtendedRange::from_bytes(data[33..].to_vec(), 23); + let cursor = DataCursor::new(data[33..].to_vec()); + let message = TemperatureExtendedRange::from_cursor(cursor, 23); assert!(message.is_ok()); let message = message.unwrap(); assert_eq!(message.source_id(), 23); @@ -83,4 +92,121 @@ mod tests { TemperatureSource::SeaTemperature )); } + + #[test] + fn gnss_parse() { + let gnss_str = "BfgBAHg+gD9ugwBnAAAAAIS5CQAAAAAAKwD/AAMAAwA6IU4Ar0sAANWDfvaZpwWAW1SoTaa69XxJjf7/////JPwePACEAOby//8A"; + let mut data = Vec::::new(); + let res = general_purpose::STANDARD.decode_vec(gnss_str, &mut data); + assert!(res.is_ok()); + let cursor = DataCursor::new(data[33..].to_vec()); + let message = GnssPositionData::from_cursor(cursor, 3); + assert!(message.is_ok()); + let message = message.unwrap(); + + let altitude = message.altitude(); + assert!(altitude.is_ok()); + let altitude = altitude.unwrap(); + assert_eq!(altitude, -24.295043999999997); + + let gnss_type = message.gnss_type(); + assert!(matches!(gnss_type, Gns::GpsSbasWaasGlonass)); + + // could not find an example containing any reference stations + let ref_stations = message.reference_station_structs(); + assert_eq!(ref_stations.len(), 0); + + let latitude = message.latitude(); + assert!(latitude.is_ok()); + let latitude = latitude.unwrap(); + assert_eq!(latitude, 40.746357526389275); + + let longitude = message.longitude(); + assert!(longitude.is_ok()); + let longitude = longitude.unwrap(); + assert_eq!(longitude, -74.0096336282232); + + let method = message.method(); + assert!(matches!(method, GnsMethod::DgnssFix)); + + let integrity = message.integrity(); + assert!(matches!(integrity, GnsIntegrity::NoIntegrityChecking)); + } + + #[test] + fn gnss_sats_in_view_parse() { + let msg_str = "BPoBAHg+gD/wh5FnAAAAAJwaDAAAAAAAwwD/AAUAAgCi/RAGlimH6ocR////f/UJdAV+Q20N////f/ELFiJwuR0R////f/UOxROhZ2gQ////f/URlhpnMNIQ////f/UT0CRzFOsQ////f/UUORlaiOsR////f/UYfwe2thwO////f/VE6ArQJG0M////f/VJ3Bc3gvcN////f/VKOSjlr0IO////f/VLCxHZ2qYO////f/VVuREIxcsN////f/VFCxFESGEQ////f/UGlimH6tIP////f/ULFiJwucwQ////f/U="; + let mut data = Vec::::new(); + let res = general_purpose::STANDARD.decode_vec(msg_str, &mut data); + assert!(res.is_ok()); + + let cursor = DataCursor::new(data[33..].to_vec()); + let message = GnssSatsInView::from_cursor(cursor, 3); + assert!(message.is_ok()); + let message = message.unwrap(); + println!("message: {:?}", message); + + let range_residual_mode = message.range_residual_mode(); + println!("range_residual_mode: {:?}", range_residual_mode); + assert!(matches!( + range_residual_mode, + RangeResidualMode::PostCalculation + )); + + let sats_in_view = message.sats_in_view(); + println!("sats in view: {:?}", sats_in_view); + assert!(sats_in_view.is_ok()); + let sats_in_view = sats_in_view.unwrap(); + assert_eq!(sats_in_view, 16); + + let sats = message.satellites(); + println!("sats: {:?}", sats); + assert_eq!(sats.len(), sats_in_view as usize); + + let azimuth_1 = sats[1].azimuth(); + assert!(azimuth_1.is_ok()); + let azimuth_1 = azimuth_1.unwrap(); + assert_eq!(azimuth_1, 98.99564784270363); + + let elevation_1 = sats[1].elevation(); + assert!(elevation_1.is_ok()); + let elevation_1 = elevation_1.unwrap(); + assert_eq!(elevation_1, 7.9984908200262925); + + let prn_1 = sats[1].prn(); + assert!(prn_1.is_ok()); + let prn_1 = prn_1.unwrap(); + assert_eq!(prn_1, 9); + + let snr_1 = sats[1].snr(); + assert!(snr_1.is_ok()); + let snr_1 = snr_1.unwrap(); + assert_eq!(snr_1, 34.37); + + let status_1 = sats[1].status(); + assert!(matches!(status_1, SatelliteStatus::Tracked)); + + let azimuth_2 = sats[2].azimuth(); + assert!(azimuth_2.is_ok()); + let azimuth_2 = azimuth_2.unwrap(); + assert_eq!(azimuth_2, 271.9945245045044); + + let elevation_2 = sats[2].elevation(); + assert!(elevation_2.is_ok()); + let elevation_2 = elevation_2.unwrap(); + assert_eq!(elevation_2, 49.99629720311564); + + let prn_2 = sats[2].prn(); + assert!(prn_2.is_ok()); + let prn_2 = prn_2.unwrap(); + assert_eq!(prn_2, 11); + + let snr_2 = sats[2].snr(); + assert!(snr_2.is_ok()); + let snr_2 = snr_2.unwrap(); + assert_eq!(snr_2, 43.81); + + let status_2 = sats[2].status(); + assert!(matches!(status_2, SatelliteStatus::UsedDiff)); + } } diff --git a/micro-rdk-nmea/src/messages/pgns.rs b/micro-rdk-nmea/src/messages/pgns.rs index 62fbc203d..b7067bfd4 100644 --- a/micro-rdk-nmea/src/messages/pgns.rs +++ b/micro-rdk-nmea/src/messages/pgns.rs @@ -1,6 +1,9 @@ -use micro_rdk_nmea_macros::PgnMessageDerive; +use micro_rdk_nmea_macros::{FieldsetDerive, PgnMessageDerive}; -use crate::parse_helpers::enums::TemperatureSource; +use crate::parse_helpers::{ + enums::{Gns, GnsIntegrity, GnsMethod, RangeResidualMode, SatelliteStatus, TemperatureSource}, + parsers::{DataCursor, FieldSet}, +}; #[derive(PgnMessageDerive, Debug)] pub struct WaterDepth { @@ -26,3 +29,80 @@ pub struct TemperatureExtendedRange { #[scale = 0.1] set_temperature: u16, } + +#[derive(FieldsetDerive, Clone, Debug)] +pub struct ReferenceStation { + #[bits = 12] + reference_station_id: u16, + #[scale = 0.01] + age_of_dgnss_corrections: u16, +} + +#[derive(PgnMessageDerive, Clone, Debug)] +pub struct GnssPositionData { + source_id: u8, + date: u16, + #[scale = 0.0001] + time: u32, + #[scale = 1e-16] + latitude: i64, + #[scale = 1e-16] + longitude: i64, + #[scale = 1e-06] + altitude: i64, + #[lookup] + #[bits = 4] + gnss_type: Gns, + #[lookup] + #[bits = 4] + method: GnsMethod, + #[lookup] + #[bits = 2] + integrity: GnsIntegrity, + #[offset = 6] + number_of_svs: u8, + #[scale = 0.01] + hdop: i16, + #[scale = 0.01] + pdop: i16, + #[scale = 0.01] + geoidal_separation: i32, + reference_stations: u8, + #[fieldset] + #[length_field = "reference_stations"] + reference_station_structs: Vec, +} + +#[derive(FieldsetDerive, Clone, Debug)] +pub struct Satellite { + prn: u8, + #[scale = 0.0001] + #[unit = "deg"] + elevation: i16, + #[scale = 0.0001] + #[unit = "deg"] + azimuth: u16, + #[scale = 0.01] + snr: u16, + range_residuals: i32, + #[lookup] + #[bits = 4] + status: SatelliteStatus, + // normally we would handle "reserved" fields by using the offset attribute + // on the next field, but in the edge case of a reserved field being the last + // field of a fieldset we need to include it + #[bits = 4] + reserved: u8, +} + +#[derive(PgnMessageDerive, Clone, Debug)] +pub struct GnssSatsInView { + #[lookup] + #[bits = 2] + range_residual_mode: RangeResidualMode, + #[offset = 6] + sats_in_view: u8, + #[fieldset] + #[length_field = "sats_in_view"] + satellites: Vec, +} diff --git a/micro-rdk-nmea/src/parse_helpers/enums.rs b/micro-rdk-nmea/src/parse_helpers/enums.rs index f48bfcbbc..f965962be 100644 --- a/micro-rdk-nmea/src/parse_helpers/enums.rs +++ b/micro-rdk-nmea/src/parse_helpers/enums.rs @@ -330,3 +330,29 @@ define_nmea_enum!( (2, Caution, "Caution"), UnknownLookupField ); + +define_nmea_enum!( + RangeResidualMode, + ( + 0, + PreCalculation, + "Range residuals were used to calculate data" + ), + ( + 1, + PostCalculation, + "Range residuals were calculated after the position" + ), + UnknownLookupField +); + +define_nmea_enum!( + SatelliteStatus, + (0, NotTracked, "Not tracked"), + (1, Tracked, "Tracked"), + (2, Used, "Used"), + (3, NotTrackedDiff, "Not tracked+Diff"), + (4, TrackedDiff, "Tracked+Diff"), + (5, UsedDiff, "Used+Diff"), + UnknownLookupField +); diff --git a/micro-rdk-nmea/src/parse_helpers/parsers.rs b/micro-rdk-nmea/src/parse_helpers/parsers.rs index 4e1ce2f63..7aa128cd0 100644 --- a/micro-rdk-nmea/src/parse_helpers/parsers.rs +++ b/micro-rdk-nmea/src/parse_helpers/parsers.rs @@ -11,40 +11,42 @@ use super::{ /// Cursor that consumes can consume bytes from a data vector by bit size. pub struct DataCursor { data: Vec, + // the amount of bits by which the previously read field overflowed into + // the first byte + bit_offset: usize, } impl DataCursor { pub fn new(data: Vec) -> Self { - DataCursor { data } + DataCursor { + data, + bit_offset: 0, + } } pub fn read(&mut self, bit_size: usize) -> Result, NmeaParseError> { - if bit_size / 8 > self.data.len() { + let bits_to_read = bit_size + self.bit_offset; + if bits_to_read > self.data.len() * 8 { return Err(NmeaParseError::NotEnoughData); } let mut res = Vec::new(); - res.extend(self.data.drain(..(bit_size / 8))); - - // If the next bit_size takes us into the middle of a byte, then - // we want the next byte of the result to be shifted so as to ignore the remaining bits. - // The first byte of the data should have the bits that were included in the result - // set to zero. - // - // This is due to the way number fields in NMEA messages appear to be formatted - // from observation of successfully parsed data, which is that overflowing bits - // are read in MsB order, but the resulting bytes are read in Little-Endian. - // - // For example, let's say the data is [179, 152], and we want to read a u16 from a bit size of 12 - // - // [179, 152] is 39091 in u16, reading the first 12 bits and ignoring the last 4 - // should yield [10110011 10011000] => [10110011 00001001] => 2483 (in Little-Endian) - if bit_size % 8 != 0 { - res.push(self.data[0] >> (8 - (bit_size % 8))); - let mask: u8 = 255 >> (bit_size % 8); - if let Some(first_byte) = self.data.get_mut(0) { - *first_byte |= mask; + res.extend(self.data.drain(..(bits_to_read / 8))); + + let next_offset = bits_to_read % 8; + // if our bit_size overflows to the middle of a byte by x bits, we want to select the last + // x bits of the next byte without consuming it + if next_offset != 0 { + res.push(self.data[0] & (255 >> (8 - next_offset))); + } + + // if the previous field overflowed into the first byte by x bits, we want + // to shift that byte by x + if self.bit_offset != 0 { + if let Some(first_byte) = res.get_mut(0) { + *first_byte >>= self.bit_offset; } } + self.bit_offset = next_offset; Ok(res) } } @@ -306,8 +308,8 @@ mod tests { assert!(cursor.read(16).is_ok()); let res = reader.read_from_cursor(&mut cursor); assert!(res.is_ok()); - // 125 = 01111101, first four bits as byte => 00000111 = 7 - assert_eq!(res.unwrap(), 7); + // 125 = 01111101, last four bits as byte => 00001101 = 13 + assert_eq!(res.unwrap(), 13); let reader = NumberField::::new(12); assert!(reader.is_ok()); @@ -316,12 +318,12 @@ mod tests { let data_vec: Vec = vec![100, 6, 125, 179, 152, 113]; let mut cursor = DataCursor::new(data_vec); - // [179, 152] is 39091 in u16, reading the first 12 bits and ignoring the last 4 - // should yield [10110011 10011000] => [10110011 00001001] => 2483 ( in Little-Endian) + // [179, 152] is 39091 in u16, reading the first 12 bits (8 + last 4 of the second byte) + // should yield [10110011 10011000] => [10110011 00001000] => 2227 (in Little-Endian) assert!(cursor.read(24).is_ok()); let res = reader.read_from_cursor(&mut cursor); assert!(res.is_ok()); - assert_eq!(res.unwrap(), 2483); + assert_eq!(res.unwrap(), 2227); let reader = NumberField::::new(3); assert!(reader.is_ok()); @@ -330,11 +332,10 @@ mod tests { let data_vec: Vec = vec![154, 6, 125, 179, 152, 113]; let mut cursor = DataCursor::new(data_vec); - // 154 is 10011010, reading the first 3 bits should yield 100 = 4 + // 154 is 10011010, reading the last 3 bits should yield 010 = 2 let res = reader.read_from_cursor(&mut cursor); - println!("res: {:?}", res); assert!(res.is_ok()); - assert_eq!(res.unwrap(), 4); + assert_eq!(res.unwrap(), 2); } #[test] @@ -343,7 +344,7 @@ mod tests { assert!(reader.is_ok()); let reader = reader.unwrap(); - let data_vec: Vec = vec![100, 6, 111, 138, 152, 113]; + let data_vec: Vec = vec![100, 6, 111, 72, 152, 113]; let mut cursor = DataCursor::new(data_vec); assert!(cursor.read(24).is_ok());