From 05246b32066b603f3c42117b1315e2252c0f8591 Mon Sep 17 00:00:00 2001
From: Yorick de Wid <yorickdewid@users.noreply.github.com>
Date: Thu, 15 Feb 2024 19:42:38 +0100
Subject: [PATCH] Always send Volvo engine message

---
 m-ecu/src/main.rs | 71 ++++++++++++++++++++---------------------------
 1 file changed, 30 insertions(+), 41 deletions(-)

diff --git a/m-ecu/src/main.rs b/m-ecu/src/main.rs
index 2d79720..f8c4f4f 100644
--- a/m-ecu/src/main.rs
+++ b/m-ecu/src/main.rs
@@ -421,47 +421,39 @@ mod app {
                     }
                 }
                 PGN::TorqueSpeedControl1 => {
-                    let control_mode = frame.pdu()[0];
-                    let rpm = spn::rpm::dec(&frame.pdu()[1..3]);
-
-                    #[allow(dead_code)]
-                    enum EngineMode {
-                        /// Engine shutdown.
-                        Shutdown = 0x07,
-                        /// Engine starter locked.
-                        Locked = 0x47,
-                        /// Engine running at requested speed.
-                        Nominal = 0x43,
-                        /// Engine starter engaged.
-                        Starting = 0xC3,
-                    }
+                    if frame.pdu()[0] != 0xff {
+                        let control_mode = frame.pdu()[0];
+                        let rpm = spn::rpm::dec(&frame.pdu()[1..3])
+                            .unwrap_or(700)
+                            .clamp(700, 2_300);
 
-                    if frame.pdu()[0] != 0xff && control_mode == 0x1 {
-                        if let Some(rpm) = rpm {
-                            let frame = FrameBuilder::new(
-                                IdBuilder::from_pgn(PGN::ProprietaryB(65_282))
-                                    .priority(3)
-                                    .sa(0x11)
-                                    .build(),
-                            )
-                            .copy_from_slice(&[
-                                0x00,
-                                EngineMode::Nominal as u8,
-                                0x1f,
-                                0x00,
-                                0x00,
-                                0x00,
-                                0x20,
-                                (rpm as f32 / 10.0) as u8,
-                            ])
-                            .build();
+                        #[allow(dead_code)]
+                        #[derive(PartialEq, Eq)]
+                        enum EngineMode {
+                            /// Engine shutdown.
+                            Shutdown = 0x07,
+                            /// Engine starter locked.
+                            Locked = 0x47,
+                            /// Engine running at requested speed.
+                            Nominal = 0x43,
+                            /// Engine starter engaged.
+                            Starting = 0xC3,
+                        }
+
+                        let mode = match 0b11 & control_mode {
+                            0b01 => EngineMode::Nominal,
+                            0b11 => EngineMode::Starting,
+                            _ => EngineMode::Locked,
+                        };
 
+                        if mode == EngineMode::Starting {
+                            ctx.local.in1.set_high();
+                            ctx.local.in2.set_low();
+                        } else {
                             ctx.local.in1.set_low();
                             ctx.local.in2.set_low();
-
-                            ctx.shared.canbus1.lock(|canbus1| canbus1.send(frame));
                         }
-                    } else if frame.pdu()[0] != 0xff && control_mode == 0x3 {
+
                         let frame = FrameBuilder::new(
                             IdBuilder::from_pgn(PGN::ProprietaryB(65_282))
                                 .priority(3)
@@ -470,19 +462,16 @@ mod app {
                         )
                         .copy_from_slice(&[
                             0x00,
-                            EngineMode::Starting as u8,
+                            mode as u8,
                             0x1f,
                             0x00,
                             0x00,
                             0x00,
                             0x20,
-                            0x50,
+                            (rpm as f32 / 10.0) as u8,
                         ])
                         .build();
 
-                        ctx.local.in1.set_high();
-                        ctx.local.in2.set_low();
-
                         ctx.shared.canbus1.lock(|canbus1| canbus1.send(frame));
                     }
                 }