diff --git a/src/mission_executor/src/main.rs b/src/mission_executor/src/main.rs index d4ca9a0..b776cb9 100644 --- a/src/mission_executor/src/main.rs +++ b/src/mission_executor/src/main.rs @@ -54,6 +54,8 @@ struct MissionExecutor { } const CLOSE_ENOUGH: f64 = 1.0; +const THRUSTER_USAGE: f64 = 7000.0; //in milliamps +const BATTERY_CAPACITY: f64 = 10000.0; //in mAh impl MissionExecutor { pub fn new() -> Self { @@ -318,9 +320,10 @@ async fn main() { // r2r::log_info!("thurstor_values", "{thurstor_values:?}"); - let minn = Vector8::repeat(-THRUSTOR_SATURATE); - let maxx = Vector8::repeat(THRUSTOR_SATURATE); - thurstor_values = thurstor_values.simd_clamp(minn, maxx) / THRUSTOR_SATURATE; + for val in &mut thurstor_values { + *val = val.clamp(-5.0, 5.0); + *val /= 5.0; + } let mut thrusters_msg = Float64MultiArray::default(); thrusters_msg.data.extend(thurstor_values.iter()); @@ -330,7 +333,7 @@ async fn main() { let mut sum_curr = 0.0; for val in &thurstor_values { - sum_curr += val.abs(); + sum_curr += val.powf(2.0) * THRUSTER_USAGE; } let mut avg_curr = td.avg_current.lock().await; *avg_curr = (*avg_curr * (count - 1.0) + sum_curr) / count; @@ -346,6 +349,11 @@ async fn main() { "Current sum of thrusters: {:.2}", sum_curr ); + r2r::log_info!( + "thruster_report", + "Estimated battery life remaining: {:.2}", + BATTERY_CAPACITY / *avg_curr + ); last_log = now; } drop(avg_curr);