diff --git a/src/bin/sailtrack-kalman.rs b/src/bin/sailtrack-kalman.rs index fc3ffe1..d805c66 100644 --- a/src/bin/sailtrack-kalman.rs +++ b/src/bin/sailtrack-kalman.rs @@ -76,10 +76,10 @@ struct Boat { #[derive(Debug, Clone, Copy)] struct Measure { - position: Point3, - velocity_xy: Vector2, + position: Point3, + velocity_xy: Vector2, velocity_z: f32, - pos_variance: Matrix3, + pos_variance: Matrix3, vel_variance: Matrix2, vel_z_variance: f32, new_measure: bool, @@ -92,17 +92,8 @@ struct Input { new_input: bool, } -#[derive(Debug, Clone, Copy)] -struct Debug { - mqtt_recieve_process_counter:u64, - mqtt_send_process_counter:u64, - filter_process_counter:u64, -} - - // Function to continuously try locking the mutex until successful fn acquire_lock(mutex: &Arc>) -> std::sync::MutexGuard { - let mut count = 0; loop { if let Ok(guard) = mutex.try_lock() { return guard; @@ -110,33 +101,42 @@ fn acquire_lock(mutex: &Arc>) -> std::sync::MutexGuard { let mut rng = rand::thread_rng(); let sleep_time: u64 = rng.gen_range(5..10); thread::sleep(Duration::from_millis(sleep_time)); - count += 1; - - if count > 500{ - println!("Acquire lock failed on mutex containig value {:?}", std::any::type_name::()); - } - } } // Function to compute the measure for the Kalman filter from the raw GPS data -fn get_measure_forom_gps( - measure: &Gps, reference: &Gps, -) -> Measure { +fn get_measure_forom_gps(measure: &Gps, reference: &Gps) -> Measure { let position = Point3::new( - (measure.lat*f32::powf(10.0, -7.0) - reference.lat*f32::powf(10.0, -7.0)) * EARTH_CIRCUMFERENCE_METERS / 360.0, - (measure.lon*f32::powf(10.0, -7.0) - reference.lon*f32::powf(10.0, -7.0)) * EARTH_CIRCUMFERENCE_METERS * LAT_FACTOR / 360.0, - measure.h_msl*f32::powf(10.0, -3.0) - reference.h_msl*f32::powf(10.0, -3.0), + (measure.lat * f32::powf(10.0, -7.0) - reference.lat * f32::powf(10.0, -7.0)) + * EARTH_CIRCUMFERENCE_METERS + / 360.0, + (measure.lon * f32::powf(10.0, -7.0) - reference.lon * f32::powf(10.0, -7.0)) + * EARTH_CIRCUMFERENCE_METERS + * LAT_FACTOR + / 360.0, + measure.h_msl * f32::powf(10.0, -3.0) - reference.h_msl * f32::powf(10.0, -3.0), ); - let velocity_xy = Vector2::new(measure.vel_n*f32::powf(10.0, -3.0), measure.vel_e*f32::powf(10.0, -3.0)); - let velocity_z = -measure.vel_d*f32::powf(10.0, -3.0); + let velocity_xy = Vector2::new( + measure.vel_n * f32::powf(10.0, -3.0), + measure.vel_e * f32::powf(10.0, -3.0), + ); + let velocity_z = -measure.vel_d * f32::powf(10.0, -3.0); + let accuracy_penality_factor = 100.0; let mut pos_variance = Matrix3::zeros(); - pos_variance[(0, 0)] = 0.25 * (measure.h_acc*f32::powf(10.0, -3.0)).powf(2.0); - pos_variance[(1, 1)] = 0.25 * (measure.h_acc*f32::powf(10.0, -3.0)).powf(2.0); - pos_variance[(2, 2)] = 0.25 * (measure.v_acc*f32::powf(10.0, -3.0)).powf(2.0); - let vel_variance = Matrix2::identity() * 0.25 * (measure.s_acc*f32::powf(10.0, -3.0)).powf(2.0); - let vel_z_variance = 0.25 * (measure.s_acc*f32::powf(10.0, -3.0)).powf(2.0); - let measure = Measure { + pos_variance[(0, 0)] = 0.25 * (measure.h_acc * f32::powf(10.0, -3.0)).powf(2.0); + pos_variance[(1, 1)] = 0.25 * (measure.h_acc * f32::powf(10.0, -3.0)).powf(2.0); + pos_variance[(2, 2)] = 0.25 * (measure.v_acc * f32::powf(10.0, -3.0)).powf(2.0); + let mut vel_variance = + Matrix2::identity() * 0.25 * (measure.s_acc * f32::powf(10.0, -3.0)).powf(2.0); + let mut vel_z_variance = 0.25 * (measure.s_acc * f32::powf(10.0, -3.0)).powf(2.0); + if measure.fix_type != 3 { + pos_variance[(0, 0)] *= accuracy_penality_factor; + pos_variance[(1, 1)] *= accuracy_penality_factor; + pos_variance[(2, 2)] *= accuracy_penality_factor; + vel_variance *= accuracy_penality_factor; + vel_z_variance *= accuracy_penality_factor; + } + Measure { position, velocity_xy, velocity_z, @@ -144,18 +144,22 @@ fn get_measure_forom_gps( vel_variance, vel_z_variance, new_measure: true, - }; - measure + } } - // Function that keeps on controll if the GPS fix is obtained -fn wait_for_fix_tipe(gps_ref_arc: &Arc>) { +fn wait_for_fix_tipe(gps_ref_arc: &Arc>) -> bool { + let gps_ref_lock = acquire_lock(gps_ref_arc); + if gps_ref_lock.fix_type == 3 { + drop(gps_ref_lock); + return true; + } + drop(gps_ref_lock); loop { let gps_ref_lock = acquire_lock(gps_ref_arc); if gps_ref_lock.fix_type == 3 { drop(gps_ref_lock); - break; + return true; } drop(gps_ref_lock); thread::sleep(Duration::from_millis(1000)); @@ -177,21 +181,16 @@ fn on_message_imu(message: Imu, input: &Arc>) { } fn on_message_gps(message: Gps, gps_ref_arc: &Arc>, measure_arc: &Arc>) { - let mut gps_ref_lock = acquire_lock(gps_ref_arc); let mut measure_lock = acquire_lock(measure_arc); - println!("Message recieved: {:?}", message); if gps_ref_lock.fix_type != 3 { *gps_ref_lock = message; - println!("GPS fix obtained: {:?}", gps_ref_lock); } let measure = get_measure_forom_gps(&message, &gps_ref_lock); *measure_lock = measure; - println!("Measure obtained: {:?}", measure); drop(measure_lock); drop(gps_ref_lock); - println!("Mutex Dropped"); } // Kalman predict function on new input @@ -203,15 +202,19 @@ fn filter_predict(kalman: &mut ESKF, input: &mut Input, filter_ts: Duration) { // Kalman update function on new measure fn filter_update(kalman: &mut ESKF, measure: &mut Measure) { kalman - .observe_position_velocity2d(measure.position, measure.pos_variance, measure.velocity_xy, measure.vel_variance) - .unwrap(); + .observe_position_velocity2d( + measure.position, + measure.pos_variance, + measure.velocity_xy, + measure.vel_variance, + ) + .unwrap(); kalman - .observe_height(measure.velocity_z, measure.vel_z_variance) - .unwrap(); + .observe_height(measure.velocity_z, measure.vel_z_variance) + .unwrap(); measure.new_measure = false; } - fn angle_wrap_180(angle: f32) -> f32 { (angle + 180.0) % 360.0 - 180.0 } @@ -274,16 +277,6 @@ fn main() { let input_mutex = Arc::new(Mutex::new(input)); let filter_mutex = Arc::new(Mutex::new(filter)); - // DEBUG - let debug = Debug { - mqtt_recieve_process_counter:0, - mqtt_send_process_counter:0, - filter_process_counter:0, - }; - - let debug_mutex = Arc::new(Mutex::new(debug)); - - // TODO: Add username and password authentication let mqqt_opts = MqttOptions::new("sailtrack-aggregator", "localhost", 1883); @@ -295,7 +288,6 @@ fn main() { let gps_ref_clone = Arc::clone(&gps_ref_mutex); let measure_clone = Arc::clone(&measure_mutex); let input_clone = Arc::clone(&input_mutex); - let debug_clone = Arc::clone(&debug_mutex); thread::spawn(move || { for notification in connection.iter().flatten() { if let Event::Incoming(Incoming::Publish(packet)) = notification { @@ -320,10 +312,6 @@ fn main() { _ => (), } } - // Degug - let mut debug_lock = acquire_lock(&debug_clone); - debug_lock.mqtt_recieve_process_counter += 1; - drop(debug_lock); } }); @@ -332,7 +320,6 @@ fn main() { let measure_clone = Arc::clone(&measure_mutex); let input_clone = Arc::clone(&input_mutex); let filter_clone = Arc::clone(&filter_mutex); - let debug_clone = Arc::clone(&debug_mutex); thread::spawn(move || loop { // Check if the GPS fix has been obtained wait_for_fix_tipe(&gps_ref_clone); @@ -341,44 +328,34 @@ fn main() { let mut input_lock = acquire_lock(&input_clone); let mut filter_lock = acquire_lock(&filter_clone); - // Reliable GPS and IMU data: predict and update - if measure_lock.new_measure && input_lock.new_input { - println!("Predict and update"); - filter_predict(&mut filter_lock, &mut input_lock, filter_ts); - filter_update(&mut filter_lock, &mut measure_lock); - drop(input_lock); - drop(measure_lock); - drop(filter_lock); - } - // Only reliable GPS data: only update - else if !input_lock.new_input && measure_lock.new_measure { - println!("Update"); - filter_update(&mut filter_lock, &mut measure_lock); - drop(measure_lock); - drop(filter_lock); - } - // Only reliable or old IMU data: only predict - else { - println!("Predict"); - filter_predict(&mut filter_lock, &mut input_lock, filter_ts); - drop(input_lock); - drop(filter_lock); + match (measure_lock.new_measure, input_lock.new_input) { + (true, true) => { + filter_predict(&mut filter_lock, &mut input_lock, filter_ts); + filter_update(&mut filter_lock, &mut measure_lock); + drop(input_lock); + drop(measure_lock); + drop(filter_lock); + } + (true, false) => { + filter_update(&mut filter_lock, &mut measure_lock); + drop(measure_lock); + drop(filter_lock); + } + _ => { + filter_predict(&mut filter_lock, &mut input_lock, filter_ts); + drop(input_lock); + drop(filter_lock); + } } let elapsed = thread_start.elapsed(); if elapsed.as_millis() < filter_ts.as_millis() { thread::sleep(filter_ts - elapsed); } - // Degug - let mut debug_lock = acquire_lock(&debug_clone); - debug_lock.filter_process_counter += 1; - drop(debug_lock); }); - //MQTT publish thread let gps_ref_clone = Arc::clone(&gps_ref_mutex); let filter_clone = Arc::clone(&filter_mutex); - let debug_clone = Arc::clone(&debug_mutex); thread::spawn(move || loop { // Check if the GPS fix has been obtained wait_for_fix_tipe(&gps_ref_clone); @@ -410,10 +387,11 @@ fn main() { } } let gps_ref_lock = acquire_lock(&gps_ref_clone); - let lat = - position.x * 360.0 / EARTH_CIRCUMFERENCE_METERS / LAT_FACTOR + gps_ref_lock.lat*f32::powf(10.0, -7.0); - let lon: f32 = position.y * 360.0 / EARTH_CIRCUMFERENCE_METERS + gps_ref_lock.lon*f32::powf(10.0, -7.0); - let altitude = position.z + gps_ref_lock.h_msl*f32::powf(10.0, -3.0); + let lat = position.x * 360.0 / EARTH_CIRCUMFERENCE_METERS / LAT_FACTOR + + gps_ref_lock.lat * f32::powf(10.0, -7.0); + let lon: f32 = position.y * 360.0 / EARTH_CIRCUMFERENCE_METERS + + gps_ref_lock.lon * f32::powf(10.0, -7.0); + let altitude = position.z + gps_ref_lock.h_msl * f32::powf(10.0, -3.0); drop(gps_ref_lock); let message = Boat { @@ -437,27 +415,5 @@ fn main() { ) .unwrap(); thread::sleep(Duration::from_millis(1000 / MQTT_PUBLISH_FREQ_HZ)); - - // Degug - let mut debug_lock = acquire_lock(&debug_clone); - debug_lock.mqtt_send_process_counter += 1; - drop(debug_lock); }); - - let debug_clone = Arc::clone(&debug_mutex); - let gps_ref_clone = Arc::clone(&gps_ref_mutex); - loop { - let debug_lock = acquire_lock(&debug_clone); - println!( - "MQTT send process counter: {}, MQTT receive process counter: {}, Filter process counter: {}", - debug_lock.mqtt_send_process_counter, debug_lock.mqtt_recieve_process_counter, debug_lock.filter_process_counter - ); - drop(debug_lock); - thread::sleep(Duration::from_secs(1)); - let gps_ref_lock = acquire_lock(&gps_ref_clone); - drop(gps_ref_lock); - println!("GPS ref access granted:"); - }; - } -