Merge pull request #22 from formulaslug/ETC

ETC Merge into main

Author: Mayamelon

ETC/main.cpp

@@ -35,8 +35,8 @@ void do_can_processing() {
         // Wait for any event flag to be set (defined in the can wrapper class)
         uint32_t triggered_flags =
             global_events.wait_any(can_handle->THROTTLE_FLAG | can_handle->STATE_FLAG |
-                                   can_handle->SYNC_FLAG | can_handle->RX_FLAG);
-
+                                   can_handle->SYNC_FLAG | can_handle->RX_FLAG | can_handle->LIMITS_FLAG);
+        can_handle->processCANRx();
         /* Check for every event, process and then clear the corresponding flag */
         if (triggered_flags & can_handle->THROTTLE_FLAG) {
             //            DPRINT("Flag Trigger: Throttle\n");
@@ -53,6 +53,10 @@ void do_can_processing() {
             can_handle->sendSync();
             global_events.clear(can_handle->SYNC_FLAG);
         }
+        if (triggered_flags & can_handle->LIMITS_FLAG) {
+            can_handle->sendCurrentLimits();
+            global_events.clear(can_handle->LIMITS_FLAG);
+        }
         if (triggered_flags & can_handle->RX_FLAG) {
             //            DPRINT("Flag: CAN RX\n");
             can_handle->processCANRx();

ETC/src/can_wrapper.cpp

@@ -9,6 +9,7 @@ CANWrapper::CANWrapper(ETCController& etcController, EventFlags& events)
       etc(etcController)
 {
     this->bus = new CAN(CANWrapper::CAN_RX_PIN, CANWrapper::CAN_TX_PIN, CANWrapper::CAN_FREQUENCY);
+    this->bus->filter(0x188, 0xFFFF, CANAny);
 
     // Start regular ISR routine for sending
     this->throttleTicker.attach(callback([this]() {
@@ -23,6 +24,10 @@ CANWrapper::CANWrapper(ETCController& etcController, EventFlags& events)
         this->globalEvents.set(CANWrapper::STATE_FLAG);
     }), 100ms);
 
+    this->limitsTicker.attach(callback([this]() {
+        this->globalEvents.set(CANWrapper::LIMITS_FLAG);
+    }), 100ms);
+
     // Set up CAN receive ISR
     this->bus->attach(callback([this]() {
         this->globalEvents.set(CANWrapper::RX_FLAG);
@@ -50,6 +55,7 @@ void CANWrapper::sendThrottle() {
     throttleMessage.data[7] = 0x00;
 
     this->bus->write(throttleMessage);
+    ThisThread::sleep_for(1ms);
 }
 
 
@@ -58,6 +64,7 @@ void CANWrapper::sendSync() {
     CANMessage syncMessage(0x80, data, 0);
 
     this->bus->write(syncMessage);
+    ThisThread::sleep_for(1ms);
 }
 
 
@@ -76,7 +83,7 @@ void CANWrapper::sendState() {
     stateMessage.data[4] = static_cast<uint8_t>(static_cast<int16_t>(state.brakes_read * 1000) & 0xFF);
     stateMessage.data[5] = static_cast<uint8_t>(static_cast<int16_t>(state.brakes_read * 1000) >> 8);
 
-    stateMessage.data[6] = static_cast<uint8_t>(state.pedal_travel * 255);
+    stateMessage.data[6] = static_cast<uint8_t>(state.pedal_travel * 100);
 
     stateMessage.data[7] =
         (state.cockpit) |
@@ -86,21 +93,9 @@ void CANWrapper::sendState() {
         (state.motor_enabled << 4) |
         (this->etc.hasImplausibility() << 5) |
         (state.ts_ready << 6);
-    
-
-    stateMessage.data[0] =
-        (0x01 & state.ts_ready) |
-        ((0x01 & state.motor_enabled) << 1) |
-        ((0x01 & state.cockpit << 4));
-    stateMessage.data[1] = static_cast<int8_t>(state.brakes_read * 100);
-    stateMessage.data[2] = static_cast<int8_t>(state.he1_travel * 100);
-    stateMessage.data[3] = static_cast<int8_t>(state.he2_travel * 100);
-    stateMessage.data[4] = static_cast<int8_t>(state.pedal_travel * 100);
-    stateMessage.data[5] = 0x00;
-    stateMessage.data[6] = 0x00;
-    stateMessage.data[7] = 0x00;
 
     this->bus->write(stateMessage);
+    ThisThread::sleep_for(1ms);
 }
 
 
@@ -113,21 +108,31 @@ void CANWrapper::sendCurrentLimits() {
     currentMessage.data[1] = 0x00;
 
     // Constant discharge current = 400A (split into little endian order)
-    currentMessage.data[2] = 0b10010000;
-    currentMessage.data[3] = 0b00000001;
+    currentMessage.data[2] = static_cast<uint8_t>(400);
+    currentMessage.data[3] = static_cast<uint8_t>(400 >> 8);
 
     currentMessage.data[4] = 0x00;
     currentMessage.data[5] = 0x00;
     currentMessage.data[6] = 0x00;
     currentMessage.data[7] = 0x00;
 
-    this->bus->write(currentMessage);    
+    this->bus->write(currentMessage);
+    ThisThread::sleep_for(1ms);
 }
 
 
 void CANWrapper::processCANRx() {
+    // printf("rxerr: %d\n", this->bus->rderror());
+    // printf("txerr: %d\n", this->bus->tderror());
+    // printf("rx\n");
     CANMessage rx;
-    if (this->bus->read(rx)) {
-        /** TODO: process the received message... */
+    while (this->bus->read(rx)) {
+        switch (rx.id) {
+            case 0x188: // ACC_TPDO_STATUS
+                ETCState state = this->etc.getState();
+                state.ts_ready = rx.data[0] & 0x08;
+                this->etc.updateStateFromCAN(state);
+                break;
+        }
     }
 }

ETC/src/can_wrapper.h

@@ -27,6 +27,8 @@ class CANWrapper {
     Ticker throttleTicker;
     /** Ticker to send regular sync messages. */
     Ticker syncTicker;
+    /** Ticker to send regular current limits messages. */
+    Ticker limitsTicker;
     /** Ticker to send regular ETC state summary messages. */
     Ticker stateTicker;
 
@@ -40,12 +42,18 @@ class CANWrapper {
 
     /** The flag bit indicating a throttle message needs to be sent. */
     static constexpr int32_t THROTTLE_FLAG = (1UL << 0);
+
     /** The flag bit indicating a sync message needs to be sent. */
     static constexpr int32_t SYNC_FLAG = (1UL << 1);
+
     /** The flag bit indicating a state message needs to be sent. */
     static constexpr int32_t STATE_FLAG = (1UL << 2);
+
+    /** The flag bit indicating a current limits flag needs to be sent. */
+    static constexpr int32_t LIMITS_FLAG = (1UL << 3);
+
     /** The flag bit indicating received messages need to be processed. */
-    static constexpr int32_t RX_FLAG = (1UL << 3);
+    static constexpr int32_t RX_FLAG = (1UL << 4);
 
 
     /**

ETC/src/etc_controller.cpp

@@ -222,7 +222,7 @@ bool ETCController::isCockpitSwitchSet() const {
 
 
 void ETCController::switchReverseMotor() {
-    this->state.motor_forward = false;
+    // this->state.motor_forward = false;
 }
 
 

ETC/src/etc_controller.h

@@ -88,23 +88,24 @@ class ETCController {
     static constexpr float MAX_VOLTAGE = 3.3f;
 
     /** The percentage tolerance for the brake pedal to be considered pressed. */
-    static constexpr float BRAKE_TOLERANCE = 0.382f;
+    // static constexpr float BRAKE_TOLERANCE = 0.382f;
+    static constexpr float BRAKE_TOLERANCE = 0.0f;
 
     /** The voltage divider slope for the hall-effect 1 sensor. */
-    static constexpr float HE1_SCALE = 330.0f / 480.0f;
+    static constexpr float HE1_SCALE = (330.0f / 480.0f);
     /** The voltage for HE1 corresponding to 0% pedal travel. */
-    static constexpr float HE1_LOW_VOLTAGE = 2.4000000f;
+    static constexpr float HE1_LOW_VOLTAGE = 1.824f;
     /** The voltage for HE1 corresponding to 100% pedal travel. */
-    static constexpr float HE1_HIGH_VOLTAGE = 2.8000000f;
+    static constexpr float HE1_HIGH_VOLTAGE = 2.806f;
     /** The difference between the maximum and minimum voltages (100% and 0% travel) for HE1. */
     static constexpr float HE1_RANGE =
         ETCController::HE1_HIGH_VOLTAGE - ETCController::HE1_LOW_VOLTAGE;
     /** The voltage divider slope for the hall-effect 2 sensor. */
-    static constexpr float HE2_SCALE = 1.0f / 2.0f;
+    static constexpr float HE2_SCALE = (1.0f / 2.0f);
     /** The voltage for HE2 corresponding to 0% pedal travel. */
-    static constexpr float HE2_LOW_VOLTAGE = 1.900000f;
+    static constexpr float HE2_LOW_VOLTAGE = 1.449f;
     /** The voltage for HE2 corresponding to 100% pedal travel. */
-    static constexpr float HE2_HIGH_VOLTAGE = 2.2290114f;
+    static constexpr float HE2_HIGH_VOLTAGE = 2.206f;
     /** The difference between the maximum and minimum voltages (100% and 0% travel) for HE2. */
     static constexpr float HE2_RANGE =
         ETCController::HE2_HIGH_VOLTAGE - ETCController::HE2_LOW_VOLTAGE;

README.md

@@ -6,3 +6,5 @@ Projects in this repo require different build toolchains. To make sure that you
 __FOR ALL TEAM MEMBERS:__ Create a fork on your personal github and start pull requests when you are ready to submit your work.
 
 
+## Requirements
+Python 3.10 or below.