diff --git a/src/bbl_format.rs b/src/bbl_format.rs deleted file mode 100644 index 1c75778..0000000 --- a/src/bbl_format.rs +++ /dev/null @@ -1,655 +0,0 @@ -use anyhow::Result; -use std::collections::HashMap; - -// BBL Encoding constants - directly from JavaScript reference -pub const ENCODING_SIGNED_VB: u8 = 0; -pub const ENCODING_UNSIGNED_VB: u8 = 1; -pub const ENCODING_NEG_14BIT: u8 = 3; -#[allow(dead_code)] -pub const ENCODING_TAG8_8SVB: u8 = 6; -#[allow(dead_code)] -pub const ENCODING_TAG2_3S32: u8 = 7; -#[allow(dead_code)] -pub const ENCODING_TAG8_4S16: u8 = 8; -pub const ENCODING_NULL: u8 = 9; -#[allow(dead_code)] -pub const ENCODING_TAG2_3SVARIABLE: u8 = 10; - -// Predictor constants - directly from JavaScript reference -#[allow(dead_code)] -pub const PREDICT_0: u8 = 0; -#[allow(dead_code)] -pub const PREDICT_PREVIOUS: u8 = 1; -#[allow(dead_code)] -pub const PREDICT_STRAIGHT_LINE: u8 = 2; -#[allow(dead_code)] -pub const PREDICT_AVERAGE_2: u8 = 3; -#[allow(dead_code)] -pub const PREDICT_MINTHROTTLE: u8 = 4; -#[allow(dead_code)] -pub const PREDICT_MOTOR_0: u8 = 5; -#[allow(dead_code)] -pub const PREDICT_INC: u8 = 6; -#[allow(dead_code)] -pub const PREDICT_HOME_COORD: u8 = 7; -#[allow(dead_code)] -pub const PREDICT_1500: u8 = 8; -#[allow(dead_code)] -pub const PREDICT_VBATREF: u8 = 9; -#[allow(dead_code)] -pub const PREDICT_LAST_MAIN_FRAME_TIME: u8 = 10; -#[allow(dead_code)] -pub const PREDICT_MINMOTOR: u8 = 11; - -pub struct BBLDataStream<'a> { - data: &'a [u8], - pub pos: usize, - end: usize, - pub eof: bool, -} - -impl<'a> BBLDataStream<'a> { - pub fn new(data: &'a [u8]) -> Self { - Self { - data, - pos: 0, - end: data.len(), - eof: false, - } - } - - #[allow(dead_code)] - pub fn set_position(&mut self, pos: usize) { - self.pos = pos; - self.eof = pos >= self.end; - } - - pub fn read_byte(&mut self) -> Result { - if self.pos < self.end { - let byte = self.data[self.pos]; - self.pos += 1; - Ok(byte) - } else { - self.eof = true; - Err(anyhow::anyhow!("EOF")) - } - } - - #[allow(dead_code)] - pub fn read_char(&mut self) -> Result { - Ok(self.read_byte()? as char) - } - - // Read unsigned variable byte - exact replica of JavaScript implementation - pub fn read_unsigned_vb(&mut self) -> Result { - let mut result = 0u32; - let mut shift = 0; - - // 5 bytes is enough to encode 32-bit unsigned quantities - for _ in 0..5 { - let b = match self.read_byte() { - Ok(byte) => byte, - Err(_) => return Ok(0), - }; - - result |= ((b & !0x80) as u32) << shift; - - // Final byte? - if b < 128 { - return Ok(result); - } - - shift += 7; - } - - // This VB-encoded int is too long! - Ok(0) - } - - // Read signed variable byte - exact replica of JavaScript implementation - pub fn read_signed_vb(&mut self) -> Result { - let unsigned = self.read_unsigned_vb()?; - - // Apply ZigZag decoding to recover the signed value - Ok(((unsigned >> 1) as i32) ^ -((unsigned & 1) as i32)) - } - - // Read Tag8_4S16 encoding - exact replica of JavaScript implementation - pub fn read_tag8_4s16_v2(&mut self, values: &mut [i32]) -> Result<()> { - let selector = self.read_byte()?; - let mut nibble_index = 0; - let mut buffer = 0u8; - - #[allow(clippy::needless_range_loop)] - for i in 0..4 { - let field_type = (selector >> (i * 2)) & 0x03; - - match field_type { - 0 => values[i] = 0, // FIELD_ZERO - 1 => { - // FIELD_4BIT - if nibble_index == 0 { - buffer = self.read_byte()?; - values[i] = sign_extend_4bit(buffer >> 4); - nibble_index = 1; - } else { - values[i] = sign_extend_4bit(buffer & 0x0f); - nibble_index = 0; - } - } - 2 => { - // FIELD_8BIT - if nibble_index == 0 { - values[i] = sign_extend_8bit(self.read_byte()?); - } else { - let mut char1 = (buffer & 0x0f) << 4; - buffer = self.read_byte()?; - char1 |= buffer >> 4; - values[i] = sign_extend_8bit(char1); - } - } - 3 => { - // FIELD_16BIT - if nibble_index == 0 { - let char1 = self.read_byte()?; - let char2 = self.read_byte()?; - values[i] = sign_extend_16bit(((char1 as u16) << 8) | (char2 as u16)); - } else { - let char1 = self.read_byte()?; - let char2 = self.read_byte()?; - values[i] = sign_extend_16bit( - (((buffer & 0x0f) as u16) << 12) - | ((char1 as u16) << 4) - | ((char2 as u16) >> 4), - ); - buffer = char2; - } - } - _ => unreachable!(), - } - } - - Ok(()) - } - - // Read Tag2_3S32 encoding - exact replica of JavaScript implementation - pub fn read_tag2_3s32(&mut self, values: &mut [i32]) -> Result<()> { - let lead_byte = self.read_byte()?; - - match lead_byte >> 6 { - 0 => { - // 2-bit fields - values[0] = sign_extend_2bit((lead_byte >> 4) & 0x03); - values[1] = sign_extend_2bit((lead_byte >> 2) & 0x03); - values[2] = sign_extend_2bit(lead_byte & 0x03); - } - 1 => { - // 4-bit fields - values[0] = sign_extend_4bit(lead_byte & 0x0f); - let second_byte = self.read_byte()?; - values[1] = sign_extend_4bit(second_byte >> 4); - values[2] = sign_extend_4bit(second_byte & 0x0f); - } - 2 => { - // 6-bit fields - values[0] = sign_extend_6bit(lead_byte & 0x3f); - let byte2 = self.read_byte()?; - values[1] = sign_extend_6bit(byte2 & 0x3f); - let byte3 = self.read_byte()?; - values[2] = sign_extend_6bit(byte3 & 0x3f); - } - 3 => { - // 8, 16 or 24 bit fields - let mut selector = lead_byte; - #[allow(clippy::needless_range_loop)] - for i in 0..3 { - match selector & 0x03 { - 0 => { - // 8-bit - let byte1 = self.read_byte()?; - values[i] = sign_extend_8bit(byte1); - } - 1 => { - // 16-bit - let byte1 = self.read_byte()?; - let byte2 = self.read_byte()?; - values[i] = sign_extend_16bit((byte1 as u16) | ((byte2 as u16) << 8)); - } - 2 => { - // 24-bit - let byte1 = self.read_byte()?; - let byte2 = self.read_byte()?; - let byte3 = self.read_byte()?; - values[i] = sign_extend_24bit( - (byte1 as u32) | ((byte2 as u32) << 8) | ((byte3 as u32) << 16), - ); - } - 3 => { - // 32-bit - let byte1 = self.read_byte()?; - let byte2 = self.read_byte()?; - let byte3 = self.read_byte()?; - let byte4 = self.read_byte()?; - values[i] = (byte1 as i32) - | ((byte2 as i32) << 8) - | ((byte3 as i32) << 16) - | ((byte4 as i32) << 24); - } - _ => unreachable!(), - } - selector >>= 2; - } - } - _ => unreachable!(), - } - - Ok(()) - } - - // Read Tag8_8SVB encoding - exact replica of JavaScript implementation - pub fn read_tag8_8svb(&mut self, values: &mut [i32], value_count: usize) -> Result<()> { - if value_count == 1 { - values[0] = self.read_signed_vb()?; - } else { - let mut header = self.read_byte()?; - #[allow(clippy::needless_range_loop)] - for i in 0..8.min(value_count) { - values[i] = if header & 0x01 != 0 { - self.read_signed_vb()? - } else { - 0 - }; - header >>= 1; - } - } - Ok(()) - } -} - -// Sign extension functions - exact replicas of JavaScript implementations -pub fn sign_extend_2bit(value: u8) -> i32 { - let val = value as i32; - if (val & 0x02) != 0 { - val | !0x03 - } else { - val & 0x03 - } -} - -pub fn sign_extend_4bit(value: u8) -> i32 { - let val = value as i32; - if (val & 0x08) != 0 { - val | !0x0f - } else { - val & 0x0f - } -} - -pub fn sign_extend_6bit(value: u8) -> i32 { - let val = value as i32; - if (val & 0x20) != 0 { - val | !0x3f - } else { - val & 0x3f - } -} - -pub fn sign_extend_8bit(value: u8) -> i32 { - value as i8 as i32 -} - -pub fn sign_extend_16bit(value: u16) -> i32 { - value as i16 as i32 -} - -pub fn sign_extend_24bit(value: u32) -> i32 { - if (value & 0x800000) != 0 { - (value | 0xff000000) as i32 - } else { - (value & 0x7fffff) as i32 - } -} - -pub fn sign_extend_14bit(value: u16) -> i32 { - if (value & 0x2000) != 0 { - -((value & 0x1fff) as i32) - } else { - (value & 0x1fff) as i32 - } -} - -#[allow(clippy::too_many_arguments)] -#[allow(dead_code)] -pub fn apply_predictor( - field_index: usize, - predictor: u8, - raw_value: i32, - current_frame: &[i32], - previous_frame: Option<&[i32]>, - previous2_frame: Option<&[i32]>, - skipped_frames: u32, - sysconfig: &HashMap, - field_names: &[String], - debug: bool, -) -> i32 { - match predictor { - PREDICT_0 => raw_value, - - PREDICT_PREVIOUS => { - if let Some(prev) = previous_frame { - if field_index < prev.len() { - let result = prev[field_index] + raw_value; - - // CRITICAL FIX: Prevent corruption propagation for vbatLatest - if field_names - .get(field_index) - .map(|name| name == "vbatLatest") - .unwrap_or(false) - { - // Check if previous value is corrupted (way too high for voltage) - if prev[field_index] > 1000 { - if debug { - eprintln!("DEBUG: Fixed corrupted vbatLatest previous value {} replaced with reasonable estimate", prev[field_index]); - } - // Use a reasonable voltage estimate based on vbatref - let vbatref = sysconfig.get("vbatref").copied().unwrap_or(4095); - return vbatref + raw_value; // Use vbatref as baseline + current delta - } - } - - result - } else { - raw_value - } - } else { - raw_value - } - } - - PREDICT_STRAIGHT_LINE => { - if let (Some(prev), Some(prev2)) = (previous_frame, previous2_frame) { - if field_index < prev.len() && field_index < prev2.len() { - raw_value + 2 * prev[field_index] - prev2[field_index] - } else { - raw_value - } - } else { - raw_value - } - } - - PREDICT_AVERAGE_2 => { - if let (Some(prev), Some(prev2)) = (previous_frame, previous2_frame) { - if field_index < prev.len() && field_index < prev2.len() { - raw_value + ((prev[field_index] + prev2[field_index]) / 2) - } else { - raw_value - } - } else { - raw_value - } - } - - PREDICT_MINTHROTTLE => { - let minthrottle = sysconfig.get("minthrottle").copied().unwrap_or(1150); - raw_value + minthrottle - } - - PREDICT_MOTOR_0 => { - // Find motor[0] field index - if let Some(motor0_idx) = field_names.iter().position(|name| name == "motor[0]") { - if motor0_idx < current_frame.len() { - current_frame[motor0_idx] + raw_value - } else { - raw_value - } - } else { - raw_value - } - } - - PREDICT_INC => { - let mut result = skipped_frames as i32 + 1; - if let Some(prev) = previous_frame { - if field_index < prev.len() { - result += prev[field_index]; - } - } - result - } - - PREDICT_1500 => raw_value + 1500, - - PREDICT_VBATREF => { - let vbatref = sysconfig.get("vbatref").copied().unwrap_or(4095); - - // CRITICAL FIX: Check for corrupted raw values in vbatLatest - // Normal vbatLatest raw values should be small deltas (-50 to +50) or small absolute values (<1000) - // Large values (>4000) indicate stream parsing corruption or wrong predictor application - if field_names - .get(field_index) - .map(|name| name == "vbatLatest") - .unwrap_or(false) - && !(-1000..=4000).contains(&raw_value) - { - // This is clearly a corrupted value - likely caused by stream parsing error - // Instead of propagating corruption, use a safe default value - if debug { - eprintln!( - "DEBUG: Fixed corrupted vbatLatest raw_value {} replaced with 0", - raw_value - ); - } - return vbatref; // Return just vbatref (safe default) - } - - raw_value + vbatref - } - - PREDICT_MINMOTOR => { - // Get the min motor value from motorOutput "min,max" format - let minmotor = if let Some(motor_output) = sysconfig.get("motorOutput") { - // Parse "48,2047" format to get first value (48) - let motor_output_str = motor_output.to_string(); - if let Some(comma_pos) = motor_output_str.find(',') { - motor_output_str[..comma_pos].parse().unwrap_or(48) - } else { - motor_output_str.parse().unwrap_or(48) - } - } else { - 48 // Default min motor output value - }; - raw_value + minmotor - } - - _ => raw_value, - } -} - -pub fn decode_frame_field( - stream: &mut BBLDataStream, - encoding: u8, - _data_version: u8, -) -> Result { - match encoding { - ENCODING_SIGNED_VB => stream.read_signed_vb(), - - ENCODING_UNSIGNED_VB => Ok(stream.read_unsigned_vb()? as i32), - - ENCODING_NEG_14BIT => { - let value = stream.read_unsigned_vb()? as u16; - Ok(-sign_extend_14bit(value)) - } - - ENCODING_NULL => Ok(0), - - _ => Err(anyhow::anyhow!("Unsupported encoding: {}", encoding)), - } -} - -#[allow(clippy::too_many_arguments)] -#[allow(dead_code)] -pub fn parse_frame_data( - stream: &mut BBLDataStream, - frame_def: &crate::FrameDefinition, - current_frame: &mut [i32], - previous_frame: Option<&[i32]>, - previous2_frame: Option<&[i32]>, - skipped_frames: u32, - raw: bool, - data_version: u8, - sysconfig: &HashMap, - debug: bool, -) -> Result<()> { - let mut i = 0; - let mut values = [0i32; 8]; - - while i < frame_def.fields.len() { - let field = &frame_def.fields[i]; - - if field.predictor == PREDICT_INC { - current_frame[i] = apply_predictor( - i, - field.predictor, - 0, - current_frame, - previous_frame, - previous2_frame, - skipped_frames, - sysconfig, - &frame_def.field_names, - debug, - ); - i += 1; - continue; - } - - match field.encoding { - ENCODING_TAG8_4S16 => { - if data_version < 2 { - // v1 implementation would be different but we'll use v2 - } - stream.read_tag8_4s16_v2(&mut values)?; - - // Apply predictors for the 4 fields - for j in 0..4 { - if i + j >= frame_def.fields.len() { - break; - } - let predictor = if raw { - PREDICT_0 - } else { - frame_def.fields[i + j].predictor - }; - - current_frame[i + j] = apply_predictor( - i + j, - predictor, - values[j], - current_frame, - previous_frame, - previous2_frame, - skipped_frames, - sysconfig, - &frame_def.field_names, - debug, - ); - } - i += 4; - continue; - } - - ENCODING_TAG2_3S32 => { - stream.read_tag2_3s32(&mut values)?; - - // Apply predictors for the 3 fields - for j in 0..3 { - if i + j >= frame_def.fields.len() { - break; - } - let predictor = if raw { - PREDICT_0 - } else { - frame_def.fields[i + j].predictor - }; - current_frame[i + j] = apply_predictor( - i + j, - predictor, - values[j], - current_frame, - previous_frame, - previous2_frame, - skipped_frames, - sysconfig, - &frame_def.field_names, - debug, - ); - } - i += 3; - continue; - } - - ENCODING_TAG8_8SVB => { - // Count how many fields use this encoding - let mut group_count = 1; - for j in i + 1..i + 8.min(frame_def.fields.len() - i) { - if frame_def.fields[j].encoding != ENCODING_TAG8_8SVB { - break; - } - group_count += 1; - } - - stream.read_tag8_8svb(&mut values, group_count)?; - - // Apply predictors for the group - for j in 0..group_count { - if i + j >= frame_def.fields.len() { - break; - } - let predictor = if raw { - PREDICT_0 - } else { - frame_def.fields[i + j].predictor - }; - - current_frame[i + j] = apply_predictor( - i + j, - predictor, - values[j], - current_frame, - previous_frame, - previous2_frame, - skipped_frames, - sysconfig, - &frame_def.field_names, - debug, - ); - } - i += group_count; - continue; - } - - _ => { - let raw_value = decode_frame_field(stream, field.encoding, data_version)?; - let predictor = if raw { PREDICT_0 } else { field.predictor }; - - current_frame[i] = apply_predictor( - i, - predictor, - raw_value, - current_frame, - previous_frame, - previous2_frame, - skipped_frames, - sysconfig, - &frame_def.field_names, - debug, - ); - } - } - - i += 1; - } - - Ok(()) -} diff --git a/src/lib.rs b/src/lib.rs index 501ccd5..1ba6df7 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -16,7 +16,6 @@ //! ``` // Module declarations -mod bbl_format; pub mod conversion; pub mod error; pub mod export; @@ -25,8 +24,6 @@ pub mod types; // Re-export everything from modules for convenience #[allow(ambiguous_glob_reexports)] -pub use bbl_format::*; -#[allow(ambiguous_glob_reexports)] pub use conversion::*; #[allow(ambiguous_glob_reexports)] pub use error::*; diff --git a/src/main.rs b/src/main.rs index dc43911..698f2bf 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,5 +1,3 @@ -mod bbl_format; - use anyhow::{Context, Result}; use clap::{Arg, Command}; use glob::glob; @@ -15,6 +13,16 @@ use bbl_parser::conversion::{ format_failsafe_phase, format_flight_mode_flags, format_state_flags, }; +// Import parser types from crate library +use bbl_parser::parser::helpers::sign_extend_14bit; +use bbl_parser::parser::{ + parse_frame_data, BBLDataStream, ENCODING_NEG_14BIT, ENCODING_NULL, ENCODING_SIGNED_VB, + ENCODING_TAG2_3S32, ENCODING_UNSIGNED_VB, +}; + +// Import types from crate library +use bbl_parser::types::FrameDefinition; + /// Maximum recursion depth to prevent stack overflow const MAX_RECURSION_DEPTH: usize = 100; @@ -224,72 +232,7 @@ fn find_bbl_files_in_dir_with_depth( Ok(bbl_files) } -#[derive(Debug, Clone)] -struct FieldDefinition { - name: String, - signed: bool, - predictor: u8, - encoding: u8, -} - -#[derive(Debug, Clone)] -struct FrameDefinition { - fields: Vec, - field_names: Vec, - count: usize, -} - -impl FrameDefinition { - fn new() -> Self { - Self { - fields: Vec::new(), - field_names: Vec::new(), - count: 0, - } - } - - fn from_field_names(names: Vec) -> Self { - let fields = names - .iter() - .map(|name| FieldDefinition { - name: name.clone(), - signed: false, - predictor: 0, - encoding: 0, - }) - .collect(); - let count = names.len(); - Self { - fields, - field_names: names, - count, - } - } - - fn update_signed(&mut self, signed_data: &[bool]) { - for (i, field) in self.fields.iter_mut().enumerate() { - if i < signed_data.len() { - field.signed = signed_data[i]; - } - } - } - - fn update_predictors(&mut self, predictors: &[u8]) { - for (i, field) in self.fields.iter_mut().enumerate() { - if i < predictors.len() { - field.predictor = predictors[i]; - } - } - } - - fn update_encoding(&mut self, encodings: &[u8]) { - for (i, field) in self.fields.iter_mut().enumerate() { - if i < encodings.len() { - field.encoding = encodings[i]; - } - } - } -} +// FieldDefinition and FrameDefinition now imported from bbl_parser::types #[derive(Debug)] struct BBLHeader { @@ -1763,7 +1706,7 @@ fn parse_frames( // GPS frame history for differential encoding let mut gps_frame_history: Vec = Vec::new(); - let mut stream = bbl_format::BBLDataStream::new(binary_data); + let mut stream = BBLDataStream::new(binary_data); // Main frame parsing loop - process frames as a stream, don't store all while !stream.eof { @@ -1804,7 +1747,7 @@ fn parse_frames( // I-frames reset the prediction history frame_history.current_frame.fill(0); - if bbl_format::parse_frame_data( + if parse_frame_data( &mut stream, &header.i_frame_def, &mut frame_history.current_frame, @@ -1886,7 +1829,7 @@ fn parse_frames( if header.p_frame_def.count > 0 && frame_history.valid { let mut p_frame_values = vec![0i32; header.p_frame_def.count]; - if bbl_format::parse_frame_data( + if parse_frame_data( &mut stream, &header.p_frame_def, &mut p_frame_values, @@ -2080,7 +2023,7 @@ fn parse_frames( let mut g_frame_values = vec![0i32; header.g_frame_def.count]; - if bbl_format::parse_frame_data( + if parse_frame_data( &mut stream, &header.g_frame_def, &mut g_frame_values, @@ -2354,7 +2297,7 @@ fn parse_frames( #[allow(dead_code)] fn parse_i_frame( - stream: &mut bbl_format::BBLDataStream, + stream: &mut BBLDataStream, frame_def: &FrameDefinition, debug: bool, ) -> Result> { @@ -2363,12 +2306,10 @@ fn parse_i_frame( // Parse each field according to the frame definition for field in &frame_def.fields { let value = match field.encoding { - bbl_format::ENCODING_SIGNED_VB => stream.read_signed_vb()?, - bbl_format::ENCODING_UNSIGNED_VB => stream.read_unsigned_vb()? as i32, - bbl_format::ENCODING_NEG_14BIT => { - -(bbl_format::sign_extend_14bit(stream.read_unsigned_vb()? as u16)) - } - bbl_format::ENCODING_NULL => 0, + ENCODING_SIGNED_VB => stream.read_signed_vb()?, + ENCODING_UNSIGNED_VB => stream.read_unsigned_vb()? as i32, + ENCODING_NEG_14BIT => -(sign_extend_14bit(stream.read_unsigned_vb()? as u16)), + ENCODING_NULL => 0, _ => { if debug { println!( @@ -2387,7 +2328,7 @@ fn parse_i_frame( } fn parse_s_frame( - stream: &mut bbl_format::BBLDataStream, + stream: &mut BBLDataStream, frame_def: &FrameDefinition, debug: bool, ) -> Result> { @@ -2398,22 +2339,22 @@ fn parse_s_frame( let field = &frame_def.fields[field_index]; match field.encoding { - bbl_format::ENCODING_SIGNED_VB => { + ENCODING_SIGNED_VB => { let value = stream.read_signed_vb()?; data.insert(field.name.clone(), value); field_index += 1; } - bbl_format::ENCODING_UNSIGNED_VB => { + ENCODING_UNSIGNED_VB => { let value = stream.read_unsigned_vb()? as i32; data.insert(field.name.clone(), value); field_index += 1; } - bbl_format::ENCODING_NEG_14BIT => { - let value = -(bbl_format::sign_extend_14bit(stream.read_unsigned_vb()? as u16)); + ENCODING_NEG_14BIT => { + let value = -(sign_extend_14bit(stream.read_unsigned_vb()? as u16)); data.insert(field.name.clone(), value); field_index += 1; } - bbl_format::ENCODING_TAG2_3S32 => { + ENCODING_TAG2_3S32 => { // This encoding handles 3 fields at once let mut values = [0i32; 8]; stream.read_tag2_3s32(&mut values)?; @@ -2427,7 +2368,7 @@ fn parse_s_frame( } field_index += 3; } - bbl_format::ENCODING_NULL => { + ENCODING_NULL => { data.insert(field.name.clone(), 0); field_index += 1; } @@ -2450,7 +2391,7 @@ fn parse_s_frame( } fn parse_h_frame( - stream: &mut bbl_format::BBLDataStream, + stream: &mut BBLDataStream, frame_def: &FrameDefinition, debug: bool, ) -> Result> { @@ -2467,12 +2408,10 @@ fn parse_h_frame( } let value = match field.encoding { - bbl_format::ENCODING_SIGNED_VB => stream.read_signed_vb()?, - bbl_format::ENCODING_UNSIGNED_VB => stream.read_unsigned_vb()? as i32, - bbl_format::ENCODING_NEG_14BIT => { - -(bbl_format::sign_extend_14bit(stream.read_unsigned_vb()? as u16)) - } - bbl_format::ENCODING_NULL => 0, + ENCODING_SIGNED_VB => stream.read_signed_vb()?, + ENCODING_UNSIGNED_VB => stream.read_unsigned_vb()? as i32, + ENCODING_NEG_14BIT => -(sign_extend_14bit(stream.read_unsigned_vb()? as u16)), + ENCODING_NULL => 0, _ => { if debug { println!( @@ -2494,7 +2433,7 @@ fn parse_h_frame( // like P frames in the main parsing loop for correct GPS coordinate calculation // Parse E frames (Event frames) - based on C reference implementation -fn parse_e_frame(stream: &mut bbl_format::BBLDataStream, debug: bool) -> Result { +fn parse_e_frame(stream: &mut BBLDataStream, debug: bool) -> Result { if debug { println!("Parsing E frame (Event frame)"); } @@ -2660,7 +2599,7 @@ fn parse_e_frame(stream: &mut bbl_format::BBLDataStream, debug: bool) -> Result< }) } -fn skip_frame(stream: &mut bbl_format::BBLDataStream, frame_type: char, debug: bool) -> Result<()> { +fn skip_frame(stream: &mut BBLDataStream, frame_type: char, debug: bool) -> Result<()> { if debug { println!("Skipping {frame_type} frame"); } diff --git a/src/parser/decoder.rs b/src/parser/decoder.rs index 7e06a78..bd563c2 100644 --- a/src/parser/decoder.rs +++ b/src/parser/decoder.rs @@ -25,6 +25,10 @@ pub const PREDICT_VBATREF: u8 = 9; pub const PREDICT_LAST_MAIN_FRAME_TIME: u8 = 10; pub const PREDICT_MINMOTOR: u8 = 11; +// Domain-specific constants for corruption detection +// Maximum reasonable raw vbatLatest value before considering it corrupted +const MAX_REASONABLE_VBAT_RAW: i32 = 1000; + /// Decode a field value using the specified encoding pub fn decode_field_value( stream: &mut BBLDataStream, @@ -52,6 +56,9 @@ pub fn decode_field_value( Ok(()) } +/// Apply predictor to decode frame field value +/// Enhanced version with debug support, field names lookup, and corruption prevention +#[allow(clippy::too_many_arguments)] pub fn apply_predictor( predictor: u8, value: i32, @@ -61,72 +68,173 @@ pub fn apply_predictor( previous2_frame: &[i32], sysconfig: &std::collections::HashMap, ) -> Result { + // Call the enhanced version with default parameters + Ok(apply_predictor_with_debug( + field_index, + predictor, + value, + current_frame, + Some(previous_frame), + Some(previous2_frame), + 0, + sysconfig, + &[], + false, + )) +} + +/// Enhanced apply_predictor with debug support, field names lookup, and corruption prevention +/// This matches the CLI implementation's full feature set +#[allow(clippy::too_many_arguments)] +pub fn apply_predictor_with_debug( + field_index: usize, + predictor: u8, + raw_value: i32, + current_frame: &[i32], + previous_frame: Option<&[i32]>, + previous2_frame: Option<&[i32]>, + skipped_frames: u32, + sysconfig: &std::collections::HashMap, + field_names: &[String], + debug: bool, +) -> i32 { match predictor { - PREDICT_0 => Ok(value), + PREDICT_0 => raw_value, + PREDICT_PREVIOUS => { - if field_index < previous_frame.len() { - Ok(value + previous_frame[field_index]) + if let Some(prev) = previous_frame { + if field_index < prev.len() { + let result = prev[field_index] + raw_value; + + // CRITICAL FIX: Prevent corruption propagation for vbatLatest + if field_names + .get(field_index) + .map(|name| name == "vbatLatest") + .unwrap_or(false) + { + // Check if previous value is corrupted (way too high for voltage) + if prev[field_index] > MAX_REASONABLE_VBAT_RAW { + if debug { + eprintln!("DEBUG: Fixed corrupted vbatLatest previous value {} replaced with reasonable estimate", prev[field_index]); + } + // Use a reasonable voltage estimate based on vbatref + let vbatref = sysconfig.get("vbatref").copied().unwrap_or(4095); + return vbatref + raw_value; + } + } + + result + } else { + raw_value + } } else { - Ok(value) + raw_value } } + PREDICT_STRAIGHT_LINE => { - if field_index < previous_frame.len() && field_index < previous2_frame.len() { - let prediction = 2 * previous_frame[field_index] - previous2_frame[field_index]; - Ok(value + prediction) - } else if field_index < previous_frame.len() { - Ok(value + previous_frame[field_index]) + if let (Some(prev), Some(prev2)) = (previous_frame, previous2_frame) { + if field_index < prev.len() && field_index < prev2.len() { + raw_value + 2 * prev[field_index] - prev2[field_index] + } else { + raw_value + } } else { - Ok(value) + raw_value } } + PREDICT_AVERAGE_2 => { - if field_index < previous_frame.len() && field_index < previous2_frame.len() { - let average = (previous_frame[field_index] + previous2_frame[field_index]) / 2; - Ok(value + average) - } else if field_index < previous_frame.len() { - Ok(value + previous_frame[field_index]) + if let (Some(prev), Some(prev2)) = (previous_frame, previous2_frame) { + if field_index < prev.len() && field_index < prev2.len() { + raw_value + ((prev[field_index] + prev2[field_index]) / 2) + } else { + raw_value + } } else { - Ok(value) + raw_value } } + PREDICT_MINTHROTTLE => { - let minthrottle = sysconfig.get("minthrottle").copied().unwrap_or(1000); - Ok(value + minthrottle) + let minthrottle = sysconfig.get("minthrottle").copied().unwrap_or(1150); + raw_value + minthrottle } + PREDICT_MOTOR_0 => { - // motor[1], motor[2], motor[3] are predicted based on motor[0] - // Find motor[0] field index (typically field 39 in I-frame) - // For now, use current_frame[39] as motor[0] position based on header analysis - let motor0_index = 39; // Based on field analysis: motor[0] is at position 39 + // Find motor[0] field index dynamically if field_names available + if !field_names.is_empty() { + if let Some(motor0_idx) = field_names.iter().position(|name| name == "motor[0]") { + if motor0_idx < current_frame.len() { + return current_frame[motor0_idx] + raw_value; + } + } + } + // Fallback: use hardcoded position (typically field 39 in I-frame) + let motor0_index = 39; if motor0_index < current_frame.len() { - Ok(value + current_frame[motor0_index]) + if debug { + eprintln!( + "DEBUG: PREDICT_MOTOR_0 using hardcoded fallback index {}", + motor0_index + ); + } + current_frame[motor0_index] + raw_value } else { - Ok(value) + raw_value } } + PREDICT_INC => { - if field_index < previous_frame.len() { - Ok(previous_frame[field_index] + value) - } else { - Ok(value) + let mut result = skipped_frames as i32 + 1; + if let Some(prev) = previous_frame { + if field_index < prev.len() { + result += prev[field_index]; + } } + result } + PREDICT_HOME_COORD => { // GPS home coordinate prediction - for now just return value - Ok(value) + raw_value } - PREDICT_1500 => Ok(value + 1500), + + PREDICT_1500 => raw_value + 1500, + PREDICT_VBATREF => { let vbatref = sysconfig.get("vbatref").copied().unwrap_or(4095); - Ok(value + vbatref) + + // CRITICAL FIX: Check for corrupted raw values in vbatLatest + if !field_names.is_empty() + && field_names + .get(field_index) + .map(|name| name == "vbatLatest") + .unwrap_or(false) + && !(-1000..=4000).contains(&raw_value) + { + if debug { + eprintln!( + "DEBUG: Fixed corrupted vbatLatest raw_value {} replaced with 0", + raw_value + ); + } + return vbatref; + } + + raw_value + vbatref } + PREDICT_MINMOTOR => { - // predictor 11 - // motor[0] prediction: value + motorOutput[0] (minimum motor output) - let motor_output_min = sysconfig.get("motorOutput[0]").copied().unwrap_or(48); - Ok(value + motor_output_min) // Force signed 32-bit like Betaflight + // Get the min motor value from motorOutput[0] or motorOutput + let minmotor = sysconfig + .get("motorOutput[0]") + .or_else(|| sysconfig.get("motorOutput")) + .copied() + .unwrap_or(48); + raw_value + minmotor } - _ => Err(anyhow::anyhow!("Invalid predictor type: {}", predictor)), + + _ => raw_value, } } diff --git a/src/parser/frame.rs b/src/parser/frame.rs index b29e6ab..2b75b72 100644 --- a/src/parser/frame.rs +++ b/src/parser/frame.rs @@ -1,4 +1,7 @@ -use crate::parser::{decoder::*, event::parse_e_frame, gps::*, stream::BBLDataStream}; +use crate::parser::{ + decoder::apply_predictor_with_debug, decoder::*, event::parse_e_frame, gps::*, + stream::BBLDataStream, +}; use crate::types::{ DecodedFrame, EventFrame, FrameDefinition, FrameHistory, FrameStats, GpsCoordinate, GpsHomeCoordinate, @@ -116,6 +119,7 @@ pub fn parse_frames( false, // Not raw header.data_version, &header.sysconfig, + debug, ) .is_ok() { @@ -169,6 +173,7 @@ pub fn parse_frames( false, // Not raw header.data_version, &header.sysconfig, + debug, ) .is_ok() { @@ -438,10 +443,11 @@ pub fn parse_frame_data( current_frame: &mut [i32], previous_frame: Option<&[i32]>, previous2_frame: Option<&[i32]>, - _skipped_frames: u32, + skipped_frames: u32, raw: bool, _data_version: u8, sysconfig: &HashMap, + debug: bool, ) -> Result<()> { let mut i = 0; let mut values = [0i32; 8]; @@ -450,15 +456,18 @@ pub fn parse_frame_data( let field = &frame_def.fields[i]; if field.predictor == PREDICT_INC { - current_frame[i] = apply_predictor( + current_frame[i] = apply_predictor_with_debug( + i, field.predictor, 0, - i, current_frame, - previous_frame.unwrap_or(&[]), - previous2_frame.unwrap_or(&[]), + previous_frame, + previous2_frame, + skipped_frames, sysconfig, - )?; + &frame_def.field_names, + debug, + ); i += 1; continue; } @@ -477,15 +486,18 @@ pub fn parse_frame_data( } else { frame_def.fields[i + j].predictor }; - current_frame[i + j] = apply_predictor( + current_frame[i + j] = apply_predictor_with_debug( + i + j, predictor, values[j], - i + j, current_frame, - previous_frame.unwrap_or(&[]), - previous2_frame.unwrap_or(&[]), + previous_frame, + previous2_frame, + skipped_frames, sysconfig, - )?; + &frame_def.field_names, + debug, + ); } i += 4; continue; @@ -504,25 +516,37 @@ pub fn parse_frame_data( } else { frame_def.fields[i + j].predictor }; - current_frame[i + j] = apply_predictor( + current_frame[i + j] = apply_predictor_with_debug( + i + j, predictor, values[j], - i + j, current_frame, - previous_frame.unwrap_or(&[]), - previous2_frame.unwrap_or(&[]), + previous_frame, + previous2_frame, + skipped_frames, sysconfig, - )?; + &frame_def.field_names, + debug, + ); } i += 3; continue; } ENCODING_TAG8_8SVB => { - stream.read_tag8_8svb(&mut values)?; + // Count how many consecutive fields use this encoding + let mut group_count = 1; + for j in i + 1..i + 8.min(frame_def.fields.len() - i) { + if frame_def.fields[j].encoding != ENCODING_TAG8_8SVB { + break; + } + group_count += 1; + } + + stream.read_tag8_8svb_counted(&mut values, group_count)?; - // Apply predictors for the 8 fields - for j in 0..8 { + // Apply predictors for the group + for j in 0..group_count { if i + j >= frame_def.fields.len() { break; } @@ -531,17 +555,20 @@ pub fn parse_frame_data( } else { frame_def.fields[i + j].predictor }; - current_frame[i + j] = apply_predictor( + current_frame[i + j] = apply_predictor_with_debug( + i + j, predictor, values[j], - i + j, current_frame, - previous_frame.unwrap_or(&[]), - previous2_frame.unwrap_or(&[]), + previous_frame, + previous2_frame, + skipped_frames, sysconfig, - )?; + &frame_def.field_names, + debug, + ); } - i += 8; + i += group_count; continue; } @@ -549,15 +576,18 @@ pub fn parse_frame_data( decode_field_value(stream, field.encoding, &mut values, 0)?; let raw_value = values[0]; let predictor = if raw { PREDICT_0 } else { field.predictor }; - current_frame[i] = apply_predictor( + current_frame[i] = apply_predictor_with_debug( + i, predictor, raw_value, - i, current_frame, - previous_frame.unwrap_or(&[]), - previous2_frame.unwrap_or(&[]), + previous_frame, + previous2_frame, + skipped_frames, sysconfig, - )?; + &frame_def.field_names, + debug, + ); } } diff --git a/src/parser/gps.rs b/src/parser/gps.rs index 977b920..3e4af95 100644 --- a/src/parser/gps.rs +++ b/src/parser/gps.rs @@ -129,6 +129,7 @@ pub fn parse_g_frame( false, // Not raw data_version, sysconfig, + false, // debug - GPS parsing doesn't need verbose output )?; // Update GPS frame history with new values diff --git a/src/parser/helpers.rs b/src/parser/helpers.rs new file mode 100644 index 0000000..950e350 --- /dev/null +++ b/src/parser/helpers.rs @@ -0,0 +1,130 @@ +//! Helper functions for BBL parsing +//! +//! This module provides sign extension functions used for decoding various +//! fixed-width signed values from the blackbox binary format. + +/// Sign-extend a 2-bit value to i32 +pub fn sign_extend_2bit(value: u8) -> i32 { + let val = value as i32; + if (val & 0x02) != 0 { + val | !0x03 + } else { + val & 0x03 + } +} + +/// Sign-extend a 4-bit value to i32 +pub fn sign_extend_4bit(value: u8) -> i32 { + let val = value as i32; + if (val & 0x08) != 0 { + val | !0x0f + } else { + val & 0x0f + } +} + +/// Sign-extend a 6-bit value to i32 +pub fn sign_extend_6bit(value: u8) -> i32 { + let val = value as i32; + if (val & 0x20) != 0 { + val | !0x3f + } else { + val & 0x3f + } +} + +/// Sign-extend an 8-bit value to i32 +pub fn sign_extend_8bit(value: u8) -> i32 { + value as i8 as i32 +} + +/// Sign-extend a 16-bit value to i32 +pub fn sign_extend_16bit(value: u16) -> i32 { + value as i16 as i32 +} + +/// Sign-extend a 24-bit value to i32 +pub fn sign_extend_24bit(value: u32) -> i32 { + if (value & 0x800000) != 0 { + (value | 0xff000000) as i32 + } else { + (value & 0x7fffff) as i32 + } +} + +/// Sign-extend a 14-bit value to i32 (sign-magnitude format) +/// Bit 13 indicates sign, bits 0-12 are the magnitude. +/// Returns negative value if sign bit is set. +pub fn sign_extend_14bit(value: u16) -> i32 { + if (value & 0x2000) != 0 { + -((value & 0x1fff) as i32) + } else { + (value & 0x1fff) as i32 + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_sign_extend_2bit() { + assert_eq!(sign_extend_2bit(0), 0); + assert_eq!(sign_extend_2bit(1), 1); + assert_eq!(sign_extend_2bit(2), -2); + assert_eq!(sign_extend_2bit(3), -1); + } + + #[test] + fn test_sign_extend_4bit() { + assert_eq!(sign_extend_4bit(0), 0); + assert_eq!(sign_extend_4bit(7), 7); + assert_eq!(sign_extend_4bit(8), -8); + assert_eq!(sign_extend_4bit(15), -1); + } + + #[test] + fn test_sign_extend_6bit() { + assert_eq!(sign_extend_6bit(0), 0); + assert_eq!(sign_extend_6bit(31), 31); + assert_eq!(sign_extend_6bit(32), -32); + assert_eq!(sign_extend_6bit(63), -1); + } + + #[test] + fn test_sign_extend_8bit() { + assert_eq!(sign_extend_8bit(0), 0); + assert_eq!(sign_extend_8bit(127), 127); + assert_eq!(sign_extend_8bit(128), -128); + assert_eq!(sign_extend_8bit(255), -1); + } + + #[test] + fn test_sign_extend_16bit() { + assert_eq!(sign_extend_16bit(0), 0); + assert_eq!(sign_extend_16bit(32767), 32767); + assert_eq!(sign_extend_16bit(32768), -32768); + assert_eq!(sign_extend_16bit(65535), -1); + } + + #[test] + fn test_sign_extend_24bit() { + assert_eq!(sign_extend_24bit(0), 0); + assert_eq!(sign_extend_24bit(0x7FFFFF), 0x7FFFFF); + assert_eq!(sign_extend_24bit(0x800000), -8388608); + assert_eq!(sign_extend_24bit(0xFFFFFF), -1); + } + + #[test] + fn test_sign_extend_14bit() { + // Positive values (bit 13 clear) + assert_eq!(sign_extend_14bit(0), 0); + assert_eq!(sign_extend_14bit(1), 1); + assert_eq!(sign_extend_14bit(0x1FFF), 0x1FFF); // 8191 + + // Negative values (bit 13 set) + assert_eq!(sign_extend_14bit(0x2000), 0); // -0 + assert_eq!(sign_extend_14bit(0x2001), -1); + assert_eq!(sign_extend_14bit(0x3FFF), -8191); + } +} diff --git a/src/parser/mod.rs b/src/parser/mod.rs index fb5025d..1a94493 100644 --- a/src/parser/mod.rs +++ b/src/parser/mod.rs @@ -3,6 +3,7 @@ pub mod event; pub mod frame; pub mod gps; pub mod header; +pub mod helpers; pub mod main; pub mod stream; @@ -11,5 +12,6 @@ pub use event::*; pub use frame::*; pub use gps::*; pub use header::*; +pub use helpers::*; pub use main::*; pub use stream::*; diff --git a/src/parser/stream.rs b/src/parser/stream.rs index b6a8672..bf8687f 100644 --- a/src/parser/stream.rs +++ b/src/parser/stream.rs @@ -1,3 +1,7 @@ +use crate::parser::helpers::{ + sign_extend_14bit, sign_extend_16bit, sign_extend_24bit, sign_extend_2bit, sign_extend_4bit, + sign_extend_6bit, sign_extend_8bit, +}; use anyhow::Result; /// BBL data stream for reading binary data @@ -205,8 +209,11 @@ impl<'a> BBLDataStream<'a> { } /// Read Tag8_8SVB encoding - exact replica of JavaScript implementation + /// When value_count is 1, reads single signed VB without header byte. + /// Otherwise reads header byte followed by up to 8 values based on header bits. #[allow(clippy::needless_range_loop)] pub fn read_tag8_8svb(&mut self, values: &mut [i32]) -> Result<()> { + // Fixed 8-value version for internal use let selector = self.read_byte()?; for i in 0..8 { @@ -220,88 +227,59 @@ impl<'a> BBLDataStream<'a> { Ok(()) } + /// Read Tag8_8SVB encoding with variable count + /// When value_count is 1, reads single signed VB without header byte. + /// Otherwise reads header byte followed by up to value_count values based on header bits. + #[allow(clippy::needless_range_loop)] + pub fn read_tag8_8svb_counted(&mut self, values: &mut [i32], value_count: usize) -> Result<()> { + if value_count == 1 { + values[0] = self.read_signed_vb()?; + } else { + let mut header = self.read_byte()?; + for i in 0..8.min(value_count) { + values[i] = if header & 0x01 != 0 { + self.read_signed_vb()? + } else { + 0 + }; + header >>= 1; + } + } + Ok(()) + } + /// Read negative 14-bit encoding (sign-magnitude format) /// Reads an unsigned variable byte and interprets it as a 14-bit sign-magnitude value. /// Bit 13 is the sign bit, bits 0-12 are the magnitude. /// Returns the negated value to match blackbox_decode behavior. pub fn read_neg_14bit(&mut self) -> Result { let unsigned = self.read_unsigned_vb()? as u16; - Ok(-sign_extend_14bit_sign_magnitude(unsigned)) - } -} - -/// Sign-magnitude 14-bit encoding (matches bbl_format::sign_extend_14bit and blackbox_decode) -/// Bit 13 indicates sign, bits 0-12 are the magnitude -fn sign_extend_14bit_sign_magnitude(value: u16) -> i32 { - if (value & 0x2000) != 0 { - -((value & 0x1fff) as i32) - } else { - (value & 0x1fff) as i32 - } -} - -// Sign extension helper functions - exact replicas of JavaScript implementation -fn sign_extend_2bit(value: u8) -> i32 { - if (value & 0x02) != 0 { - (value as i32) | !0x03 - } else { - value as i32 - } -} - -fn sign_extend_4bit(value: u8) -> i32 { - if (value & 0x08) != 0 { - (value as i32) | !0x0f - } else { - value as i32 - } -} - -fn sign_extend_6bit(value: u8) -> i32 { - if (value & 0x20) != 0 { - (value as i32) | !0x3f - } else { - value as i32 - } -} - -fn sign_extend_8bit(value: u8) -> i32 { - value as i8 as i32 -} - -fn sign_extend_16bit(value: u16) -> i32 { - value as i16 as i32 -} - -fn sign_extend_24bit(value: u32) -> i32 { - if (value & 0x800000) != 0 { - (value as i32) | !0xffffff - } else { - value as i32 + Ok(-sign_extend_14bit(unsigned)) } } #[cfg(test)] mod tests { use super::*; + use crate::parser::helpers::sign_extend_14bit; #[test] fn test_sign_extend_14bit_sign_magnitude_positive() { // Positive values have bit 13 = 0 (sign bit clear) - assert_eq!(sign_extend_14bit_sign_magnitude(0x0000), 0); // 0 - assert_eq!(sign_extend_14bit_sign_magnitude(0x0001), 1); // 1 - assert_eq!(sign_extend_14bit_sign_magnitude(0x1FFF), 0x1FFF); // 8191 (max positive magnitude) + assert_eq!(sign_extend_14bit(0x0000), 0); // 0 + assert_eq!(sign_extend_14bit(0x0001), 1); // 1 + assert_eq!(sign_extend_14bit(0x1FFF), 0x1FFF); // 8191 (max positive magnitude) } #[test] fn test_sign_extend_14bit_sign_magnitude_negative() { // Negative values have bit 13 = 1 (sign bit set), magnitude in bits 0-12 // 0x2000 = bit 13 set, magnitude 0 -> returns -0 = 0 (actually negative zero) - assert_eq!(sign_extend_14bit_sign_magnitude(0x2000), 0); // -0 - // 0x2001 = bit 13 set, magnitude 1 -> returns -1 - assert_eq!(sign_extend_14bit_sign_magnitude(0x2001), -1); + assert_eq!(sign_extend_14bit(0x2000), 0); // -0 + // 0x2001 = bit 13 set, magnitude 1 -> returns -1 + assert_eq!(sign_extend_14bit(0x2001), -1); // 0x3FFF = bit 13 set, magnitude 0x1FFF (8191) -> returns -8191 - assert_eq!(sign_extend_14bit_sign_magnitude(0x3FFF), -8191); + assert_eq!(sign_extend_14bit(0x3FFF), -8191); } #[test]