1. // Created by Clemens Elflein on 3/07/22.
2. // Copyright (c) 2022 Clemens Elflein. All rights reserved.
3. //
4. // This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.
5. //
6. // Feel free to use the design in your private/educational projects, but don't try to sell the design or products based on it without getting my consent first.
7. //
8. // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
9. // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
10. // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
11. // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
12. // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
13. // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
14. // SOFTWARE.
15. //
16. //
17. #include <NeoPixelConnect.h>
18. #include <Arduino.h>
19. #include <FastCRC.h>
20. #include <PacketSerial.h>
21. #include "datatypes.h"
22. #include "pins.h"
23. #include "ui_board.h"
24. #include "imu.h"
25. #ifdef ENABLE_SOUND_MODULE
26. #include <DFPlayerMini_Fast.h>
27. #include <soundsystem.h>
28. #endif
29.  
30. #define IMU_CYCLETIME 20          // cycletime for refresh IMU data
31. #define STATUS_CYCLETIME 100      // cycletime for refresh analog and digital Statusvalues
32. #define UI_SET_LED_CYCLETIME 1000 // cycletime for refresh UI status LEDs
33.  
34. #define LIFT_EMERGENCY_MILLIS 500  // Time for wheels to be lifted in order to count as emergency. This is to filter uneven ground.
35. #define BUTTON_EMERGENCY_MILLIS 20 // Time for button emergency to activate. This is to debounce the button if triggered on bumpy surfaces
36.  
37. // Define to stream debugging messages via USB
38. // #define USB_DEBUG
39.  
40. // Only define DEBUG_SERIAL if USB_DEBUG is actually enabled.
41. // This enforces compile errors if it's used incorrectly.
42. #ifdef USB_DEBUG
43. #define DEBUG_SERIAL Serial
44. #endif
45. #define PACKET_SERIAL Serial1
46.  
47. SerialPIO uiSerial(PIN_UI_TX, PIN_UI_RX, 250);
48.  
49. #define UI1_SERIAL uiSerial
50.  
51. #define ANZ_SOUND_SD_FILES 3
52.  
53. // Millis after charging is retried
54. #define CHARGING_RETRY_MILLIS 10000
55.  
56. /**
57.  * @brief Some hardware parameters
58.  */
59. #define VIN_R1 10000.0f
60. #define VIN_R2 1000.0f
61. #define R_SHUNT 0.003f
62. #define CURRENT_SENSE_GAIN 100.0f
63.  
64. #define BATT_ABS_MAX 28.7f
65. #define BATT_ABS_Min 21.7f
66.  
67. #define BATT_FULL BATT_ABS_MAX - 0.3f
68. #define BATT_EMPTY BATT_ABS_Min + 0.3f
69.  
70. // Emergency will be engaged, if no heartbeat was received in this time frame.
71. #define HEARTBEAT_MILLIS 500
72.  
73. NeoPixelConnect p(PIN_NEOPIXEL, 1, pio1, 0); // use state machine 1, sm 0 is used by hardwareserial class
74. uint8_t led_blink_counter = 0;
75.  
76. PacketSerial packetSerial; // COBS communication PICO <> Raspi
77. PacketSerial UISerial;     // COBS communication PICO UI-Board
78. FastCRC16 CRC16;
79.  
80. #ifdef ENABLE_SOUND_MODULE
81. MP3Sound my_sound; // Soundsystem
82. #endif
83.  
84. unsigned long last_imu_millis = 0;
85. unsigned long last_status_update_millis = 0;
86. unsigned long last_heartbeat_millis = 0;
87. unsigned long last_UILED_millis = 0;
88.  
89. unsigned long lift_emergency_started = 0;
90. unsigned long button_emergency_started = 0;
91.  
92. // Predefined message buffers, so that we don't need to allocate new ones later.
93. struct ll_imu imu_message = {0};
94. struct ll_status status_message = {0};
95. // current high level state
96. struct ll_high_level_state last_high_level_state = {0};
97.  
98. // Struct for the current LEDs. This gets sent to the UI periodically
99. struct msg_set_leds leds_message = {0};
100.  
101. // A mutex which is used by core1 each time status_message is modified.
102. // We can lock it during message transmission to prevent core1 to modify data in this time.
103. auto_init_mutex(mtx_status_message);
104.  
105. bool emergency_latch = true;
106. bool sound_available = false;
107. bool charging_allowed = false;
108. bool ROS_running = false;
109. unsigned long charging_disabled_time = 0;
110.  
111. float imu_temp[9];
112.  
113. void sendMessage(void *message, size_t size);
114. void sendUIMessage(void *message, size_t size);
115. void onPacketReceived(const uint8_t *buffer, size_t size);
116. void onUIPacketReceived(const uint8_t *buffer, size_t size);
117. void manageUILEDS();
118.  
119. void setRaspiPower(bool power)
120. {
121.   // Update status bits in the status message
122.   status_message.status_bitmask = (status_message.status_bitmask & 0b11111101) | ((power & 0b1) << 1);
123.   digitalWrite(PIN_RASPI_POWER, power);
124. }
125.  
126. void updateEmergency()
127. {
128.  
129.   if (millis() - last_heartbeat_millis > HEARTBEAT_MILLIS)
130.   {
131.     emergency_latch = true;
132.     ROS_running = false;
133.   }
134.   uint8_t last_emergency = status_message.emergency_bitmask & 1;
135.  
136.   // Mask the emergency bits. 2x Lift sensor, 2x Emergency Button
137.   bool emergency1 = !gpio_get(PIN_EMERGENCY_1);
138.   bool emergency2 = !gpio_get(PIN_EMERGENCY_2);
139.   bool emergency3 = !gpio_get(PIN_EMERGENCY_3);
140.   bool emergency4 = !gpio_get(PIN_EMERGENCY_4);
141.  
142.   uint8_t emergency_state = 0;
143.  
144.   bool is_lifted = emergency1 || emergency2;
145.   bool stop_pressed = emergency3 || emergency4;
146.  
147.   if (is_lifted)
148.   {
149.     // We just lifted, store the timestamp
150.     if (lift_emergency_started == 0)
151.     {
152.       lift_emergency_started = millis();
153.     }
154.   }
155.   else
156.   {
157.     // Not lifted, reset the time
158.     lift_emergency_started = 0;
159.   }
160.  
161.   if (stop_pressed)
162.   {
163.     // We just pressed, store the timestamp
164.     if (button_emergency_started == 0)
165.     {
166.       button_emergency_started = millis();
167.     }
168.   }
169.   else
170.   {
171.     // Not pressed, reset the time
172.     button_emergency_started = 0;
173.   }
174.  
175.   if (lift_emergency_started > 0 && (millis() - lift_emergency_started) >= LIFT_EMERGENCY_MILLIS)
176.   {
177.     // Emergency bit 2 (lift wheel 1)set?
178.     if (emergency1)
179.       emergency_state |= 0b01000;
180.     // Emergency bit 1 (lift wheel 2)set?
181.     if (emergency2)
182.       emergency_state |= 0b10000;
183.   }
184.  
185.   if (button_emergency_started > 0 && (millis() - button_emergency_started) >= BUTTON_EMERGENCY_MILLIS)
186.   {
187.     // Emergency bit 2 (stop button) set?
188.     if (emergency3)
189.       emergency_state |= 0b00010;
190.     // Emergency bit 1 (stop button)set?
191.     if (emergency4)
192.       emergency_state |= 0b00100;
193.   }
194.  
195.   if (emergency_state || emergency_latch)
196.   {
197.     emergency_latch |= 1;
198.     emergency_state |= 1;
199.   }
200.  
201.   status_message.emergency_bitmask = emergency_state;
202.  
203.   // If it's a new emergency, instantly send the message. This is to not spam the channel during emergencies.
204.   if (last_emergency != (emergency_state & 1))
205.   {
206.     sendMessage(&status_message, sizeof(struct ll_status));
207.  
208.     // Update LEDs instantly
209.     manageUILEDS();
210.   }
211. }
212.  
213. // deals with the pyhsical information an control the UI-LEDs und buzzer in depency of voltage und current values
214. void manageUILEDS()
215. {
216.   // Show Info Docking LED
217.   if ((status_message.charging_current > 0.80f) && (status_message.v_charge > 20.0f))
218.     setLed(leds_message, LED_CHARGING, LED_blink_fast);
219.   else if ((status_message.charging_current <= 0.80f) && (status_message.charging_current >= 0.15f) && (status_message.v_charge > 20.0f))
220.     setLed(leds_message, LED_CHARGING, LED_blink_slow);
221.   else if ((status_message.charging_current < 0.15f) && (status_message.v_charge > 20.0f))
222.     setLed(leds_message, LED_CHARGING, LED_on);
223.   else
224.     setLed(leds_message, LED_CHARGING, LED_off);
225.  
226.   // Show Info Battery state
227.   if (status_message.v_battery >= (BATT_EMPTY + 2.0f))
228.     setLed(leds_message, LED_BATTERY_LOW, LED_off);
229.   else
230.     setLed(leds_message, LED_BATTERY_LOW, LED_on);
231.  
232.   if (status_message.v_charge < 10.0f) // activate only when undocked
233.   {
234.     // use the first LED row as bargraph
235.     setBars7(leds_message, status_message.batt_percentage / 100.0);
236.     if (last_high_level_state.gps_quality == 0)
237.     {
238.       // if quality is 0, flash all LEDs to notify the user to calibrate.
239.       setBars4(leds_message, -1.0);
240.     }
241.     else
242.     {
243.       setBars4(leds_message, last_high_level_state.gps_quality / 100.0);
244.     }
245.   }
246.   else
247.   {
248.     setBars7(leds_message, 0);
249.     setBars4(leds_message, 0);
250.   }
251.  
252.   if(last_high_level_state.gps_quality < 25) {
253.     setLed(leds_message, LED_POOR_GPS, LED_on);
254.   } else if(last_high_level_state.gps_quality < 50) {
255.     setLed(leds_message, LED_POOR_GPS, LED_blink_fast);
256.   } else if(last_high_level_state.gps_quality < 75) {
257.     setLed(leds_message, LED_POOR_GPS, LED_blink_slow);
258.   } else {
259.     setLed(leds_message, LED_POOR_GPS, LED_off);
260.   }
261.  
262.   // Let S1 show if ros is connected and which state it's in
263.   if (!ROS_running)
264.   {
265.     setLed(leds_message, LED_S1, LED_off);
266.   }
267.   else
268.   {
269.     switch (last_high_level_state.current_mode & 0b111111)
270.     {
271.     case HighLevelMode::MODE_IDLE:
272.       setLed(leds_message, LED_S1, LED_on);
273.       break;
274.     case HighLevelMode::MODE_AUTONOMOUS:
275.       setLed(leds_message, LED_S1, LED_blink_slow);
276.       break;
277.     default:
278.       setLed(leds_message, LED_S1, LED_blink_fast);
279.       break;
280.     }
281.     switch ((last_high_level_state.current_mode >> 6) & 0b11)
282.     {
283.     case 1:
284.       setLed(leds_message, LED_S2, LED_blink_slow);
285.       break;
286.     case 2:
287.       setLed(leds_message, LED_S2, LED_blink_fast);
288.       break;
289.     case 3:
290.       setLed(leds_message, LED_S2, LED_on);
291.       break;
292.     default:
293.       setLed(leds_message, LED_S2, LED_off);
294.       break;
295.     }
296.   }
297.  
298.   // Show Info mower lifted or stop button pressed
299.   if (status_message.emergency_bitmask & 0b00110)
300.   {
301.     setLed(leds_message, LED_MOWER_LIFTED, LED_blink_fast);
302.   }
303.   else if (status_message.emergency_bitmask & 0b11000)
304.   {
305.     setLed(leds_message, LED_MOWER_LIFTED, LED_blink_slow);
306.   }
307.   else if (status_message.emergency_bitmask & 0b0000001)
308.   {
309.     setLed(leds_message, LED_MOWER_LIFTED, LED_on);
310.   }
311.   else
312.   {
313.     setLed(leds_message, LED_MOWER_LIFTED, LED_off);
314.   }
315.  
316.   sendUIMessage(&leds_message, sizeof(leds_message));
317. }
318.  
319. void setup1()
320. {
321.   // Core
322.   digitalWrite(LED_BUILTIN, HIGH);
323. }
324.  
325. void loop1()
326. {
327.   // Loop through the mux and query actions. Store the result in the multicore fifo
328.   for (uint8_t mux_address = 0; mux_address < 7; mux_address++)
329.   {
330.     gpio_put_masked(0b111 << 13, mux_address << 13);
331.     delay(1);
332.     bool state = gpio_get(PIN_MUX_IN);
333.  
334.     switch (mux_address)
335.     {
336.     case 5:
337.       mutex_enter_blocking(&mtx_status_message);
338.  
339.       if (state)
340.       {
341.         status_message.status_bitmask |= 0b00010000;
342.       }
343.       else
344.       {
345.         status_message.status_bitmask &= 0b11101111;
346.       }
347.       mutex_exit(&mtx_status_message);
348.  
349.       break;
350.     case 6:
351.       mutex_enter_blocking(&mtx_status_message);
352.       if (state)
353.       {
354.         status_message.status_bitmask |= 0b00100000;
355.       }
356.       else
357.       {
358.         status_message.status_bitmask &= 0b11011111;
359.       }
360.       mutex_exit(&mtx_status_message);
361.       break;
362.     default:
363.       break;
364.     }
365.   }
366.  
367.   delay(100);
368. }
369.  
370. void setup()
371. {
372.   //  We do hardware init in this core, so that we don't get invalid states.
373.   //  Therefore, we pause the other core until setup() was a success
374.   rp2040.idleOtherCore();
375.  
376.   emergency_latch = true;
377.   ROS_running = false;
378.  
379.   lift_emergency_started = 0;
380.   button_emergency_started = 0;
381.   // Initialize messages
382.   imu_message = {0};
383.   status_message = {0};
384.   imu_message.type = PACKET_ID_LL_IMU;
385.   status_message.type = PACKET_ID_LL_STATUS;
386.  
387.   // Setup pins
388.   pinMode(LED_BUILTIN, OUTPUT);
389.   pinMode(PIN_ENABLE_CHARGE, OUTPUT);
390.   digitalWrite(PIN_ENABLE_CHARGE, LOW);
391.  
392.   gpio_init(PIN_RASPI_POWER);
393.   gpio_put(PIN_RASPI_POWER, true);
394.   gpio_set_dir(PIN_RASPI_POWER, true);
395.   gpio_put(PIN_RASPI_POWER, true);
396.  
397.   // Enable raspi power
398.   p.neoPixelSetValue(0, 32, 0, 0, true);
399.   delay(1000);
400.   setRaspiPower(true);
401.   p.neoPixelSetValue(0, 255, 0, 0, true);
402.  
403.   pinMode(PIN_MUX_OUT, OUTPUT);
404.   pinMode(PIN_MUX_ADDRESS_0, OUTPUT);
405.   pinMode(PIN_MUX_ADDRESS_1, OUTPUT);
406.   pinMode(PIN_MUX_ADDRESS_2, OUTPUT);
407.  
408.   pinMode(PIN_EMERGENCY_1, INPUT);
409.   pinMode(PIN_EMERGENCY_2, INPUT);
410.   pinMode(PIN_EMERGENCY_3, INPUT);
411.   pinMode(PIN_EMERGENCY_4, INPUT);
412.  
413.   analogReadResolution(12);
414.  
415. #ifdef USB_DEBUG
416.   DEBUG_SERIAL.begin(115200);
417. #endif
418.  
419.   // init serial com to RasPi
420.   PACKET_SERIAL.begin(115200);
421.   packetSerial.setStream(&PACKET_SERIAL);
422.   packetSerial.setPacketHandler(&onPacketReceived);
423.  
424.   UI1_SERIAL.begin(115200);
425.   UISerial.setStream(&UI1_SERIAL);
426.   UISerial.setPacketHandler(&onUIPacketReceived);
427.  
428.   /*
429.    * IMU INITIALIZATION
430.    */
431.  
432.   if (!init_imu())
433.   {
434. #ifdef USB_DEBUG
435.     DEBUG_SERIAL.println("IMU initialization unsuccessful");
436.     DEBUG_SERIAL.println("Check IMU wiring or try cycling power");
437. #endif
438.     status_message.status_bitmask = 0;
439.     while (1)
440.     {
441.       p.neoPixelSetValue(0, 255, 0, 0, true);
442.       delay(500);
443.       p.neoPixelSetValue(0, 0, 0, 0, true);
444.       delay(500);
445.     }
446.  
447.     {
448. #ifdef USB_DEBUG
449.       DEBUG_SERIAL.println("Error: Imu init failed");
450. #endif
451.       // We don't need to lock the mutex here, since core 1 is sleeping anyways
452.       sendMessage(&status_message, sizeof(struct ll_status));
453.       delay(1000);
454.     }
455.   }
456.  
457. #ifdef USB_DEBUG
458.   DEBUG_SERIAL.println("Imu initialized");
459. #endif
460.  
461.   /*
462.    * /IMU INITIALIZATION
463.    */
464.  
465.   status_message.status_bitmask |= 1;
466.  
467. #ifdef ENABLE_SOUND_MODULE
468.   p.neoPixelSetValue(0, 0, 255, 255, true);
469.  
470.   sound_available = my_sound.begin();
471.   if (sound_available)
472.   {
473.     p.neoPixelSetValue(0, 0, 0, 255, true);
474.     my_sound.setvolume(100);
475.     my_sound.playSoundAdHoc(1);
476.     p.neoPixelSetValue(0, 255, 255, 0, true);
477.   }
478.   else
479.   {
480.     for (uint8_t b = 0; b < 3; b++)
481.     {
482.       p.neoPixelSetValue(0, 0, 0, 0, true);
483.       delay(200);
484.       p.neoPixelSetValue(0, 0, 0, 255, true);
485.       delay(200);
486.     }
487.   }
488.  
489.  
490. #else
491.   sound_available = false;
492. #endif
493.  
494.   rp2040.resumeOtherCore();
495.  
496.   // UIboard clear all LEDs
497.   leds_message.type = Set_LEDs;
498.   leds_message.leds = 0;
499.   sendUIMessage(&leds_message, sizeof(leds_message));
500. }
501.  
502. void onUIPacketReceived(const uint8_t *buffer, size_t size)
503. {
504.  
505.   u_int16_t *crc_pointer = (uint16_t *)(buffer + (size - 2));
506.   u_int16_t readcrc = *crc_pointer;
507.  
508.   // check structure size
509.   if (size < 4)
510.     return;
511.  
512.   // check the CRC
513.   uint16_t crc = CRC16.ccitt(buffer, size - 2);
514.  
515.   if (buffer[size - 1] != ((crc >> 8) & 0xFF) ||
516.       buffer[size - 2] != (crc & 0xFF))
517.     return;
518.  
519.   if(buffer[0] == Get_Button && size == sizeof(struct msg_event_button)) {
520.     struct msg_event_button* msg = (struct msg_event_button*)buffer;
521.     struct ll_ui_event ui_event;
522.     ui_event.type = PACKET_ID_LL_UI_EVENT;
523.     ui_event.button_id = msg->button_id;
524.     ui_event.press_duration = msg->press_duration;
525.     sendMessage(&ui_event, sizeof(ui_event));
526.   }
527. }
528.  
529. void onPacketReceived(const uint8_t *buffer, size_t size)
530. {
531.   // sanity check for CRC to work (1 type, 1 data, 2 CRC)
532.   if (size < 4)
533.     return;
534.  
535.   // check the CRC
536.   uint16_t crc = CRC16.ccitt(buffer, size - 2);
537.  
538.   if (buffer[size - 1] != ((crc >> 8) & 0xFF) ||
539.       buffer[size - 2] != (crc & 0xFF))
540.     return;
541.  
542.   if (buffer[0] == PACKET_ID_LL_HEARTBEAT && size == sizeof(struct ll_heartbeat))
543.   {
544.  
545.     // CRC and packet is OK, reset watchdog
546.     last_heartbeat_millis = millis();
547.     ROS_running = true;
548.     struct ll_heartbeat *heartbeat = (struct ll_heartbeat *)buffer;
549.     if (heartbeat->emergency_release_requested)
550.     {
551.       emergency_latch = false;
552.     }
553.     // Check in this order, so we can set it again in the same packet if required.
554.     if (heartbeat->emergency_requested)
555.     {
556.       emergency_latch = true;
557.     }
558.   }
559.   else if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_STATE && size == sizeof(struct ll_high_level_state))
560.   {
561.     // copy the state
562.     last_high_level_state = *((struct ll_high_level_state *)buffer);
563.   }
564. }
565.  
566. // returns true, if it's a good idea to charge the battery (current, voltages, ...)
567. bool checkShouldCharge()
568. {
569.   return status_message.v_charge < 30.0 && status_message.charging_current < 1.5 && status_message.v_battery < 29.0;
570. }
571.  
572. void updateChargingEnabled()
573. {
574.   if (charging_allowed)
575.   {
576.     if (!checkShouldCharge())
577.     {
578.       digitalWrite(PIN_ENABLE_CHARGE, LOW);
579.       charging_allowed = false;
580.       charging_disabled_time = millis();
581.     }
582.   }
583.   else
584.   {
585.     // enable charging after CHARGING_RETRY_MILLIS
586.     if (millis() - charging_disabled_time > CHARGING_RETRY_MILLIS)
587.     {
588.       if (!checkShouldCharge())
589.       {
590.         digitalWrite(PIN_ENABLE_CHARGE, LOW);
591.         charging_allowed = false;
592.         charging_disabled_time = millis();
593.       }
594.       else
595.       {
596.         digitalWrite(PIN_ENABLE_CHARGE, HIGH);
597.         charging_allowed = true;
598.       }
599.     }
600.   }
601. }
602.  
603. void updateNeopixel()
604. {
605.   led_blink_counter++;
606.   // flash red on emergencies
607.   if (emergency_latch && led_blink_counter & 0b10)
608.   {
609.     p.neoPixelSetValue(0, 128, 0, 0, true);
610.   }
611.   else
612.   {
613.     if (ROS_running)
614.     {
615.       // Green, if ROS is running
616.       p.neoPixelSetValue(0, 0, 255, 0, true);
617.     }
618.     else
619.     {
620.       // Yellow, if it's not running
621.       p.neoPixelSetValue(0, 255, 50, 0, true);
622.     }
623.   }
624. }
625.  
626. void loop()
627. {
628.   packetSerial.update();
629.   UISerial.update();
630.   imu_loop();
631.   updateChargingEnabled();
632.   updateEmergency();
633.  
634.   unsigned long now = millis();
635.   if (now - last_imu_millis > IMU_CYCLETIME)
636.   {
637.     // we have to copy to the temp data structure due to alignment issues
638.     imu_read(imu_temp, imu_temp + 3, imu_temp + 6);
639.     imu_message.acceleration_mss[0] = imu_temp[0];
640.     imu_message.acceleration_mss[1] = imu_temp[1];
641.     imu_message.acceleration_mss[2] = imu_temp[2];
642.     imu_message.gyro_rads[0] = imu_temp[3];
643.     imu_message.gyro_rads[1] = imu_temp[4];
644.     imu_message.gyro_rads[2] = imu_temp[5];
645.     imu_message.mag_uT[0] = imu_temp[6];
646.     imu_message.mag_uT[1] = imu_temp[7];
647.     imu_message.mag_uT[2] = imu_temp[8];
648.  
649.     imu_message.dt_millis = now - last_imu_millis;
650.     sendMessage(&imu_message, sizeof(struct ll_imu));
651.  
652.     last_imu_millis = now;
653.   }
654.  
655.   if (now - last_status_update_millis > STATUS_CYCLETIME)
656.   {
657.     updateNeopixel();
658.  
659.     status_message.v_battery = (float)analogRead(PIN_ANALOG_BATTERY_VOLTAGE) * (3.3f / 4096.0f) * ((VIN_R1 + VIN_R2) / VIN_R2);
660.     status_message.v_charge = (float)analogRead(PIN_ANALOG_CHARGE_VOLTAGE) * (3.3f / 4096.0f) * ((VIN_R1 + VIN_R2) / VIN_R2);
661.     status_message.charging_current = (float)analogRead(PIN_ANALOG_CHARGE_CURRENT) * (3.3f / 4096.0f) / (CURRENT_SENSE_GAIN * R_SHUNT);
662.     status_message.status_bitmask = (status_message.status_bitmask & 0b11111011) | ((charging_allowed & 0b1) << 2);
663.     status_message.status_bitmask = (status_message.status_bitmask & 0b11011111) | ((sound_available & 0b1) << 5);
664.  
665.     // calculate percent value accu filling
666.     float delta = BATT_FULL - BATT_EMPTY;
667.     float vo = status_message.v_battery - BATT_EMPTY;
668.     status_message.batt_percentage = vo / delta * 100;
669.     if (status_message.batt_percentage > 100)
670.       status_message.batt_percentage = 100;
671.  
672.     mutex_enter_blocking(&mtx_status_message);
673.     sendMessage(&status_message, sizeof(struct ll_status));
674.     mutex_exit(&mtx_status_message);
675.  
676.     last_status_update_millis = now;
677. #ifdef USB_DEBUG
678.     DEBUG_SERIAL.print("status: 0b");
679.     DEBUG_SERIAL.print(status_message.status_bitmask, BIN);
680.     DEBUG_SERIAL.print("\t");
681.  
682.     DEBUG_SERIAL.print("vin: ");
683.     DEBUG_SERIAL.print(status_message.v_battery, 3);
684.     DEBUG_SERIAL.print(" V\t");
685.     DEBUG_SERIAL.print("vcharge: ");
686.     DEBUG_SERIAL.print(status_message.v_charge, 3);
687.     DEBUG_SERIAL.print(" V\t");
688.     DEBUG_SERIAL.print("charge_current: ");
689.     DEBUG_SERIAL.print(status_message.charging_current, 3);
690.     DEBUG_SERIAL.print(" A\t");
691.     DEBUG_SERIAL.print("emergency: 0b");
692.     DEBUG_SERIAL.print(status_message.emergency_bitmask, BIN);
693.     DEBUG_SERIAL.println();
694. #endif
695.   }
696.  
697.   if (now - last_UILED_millis > UI_SET_LED_CYCLETIME)
698.   {
699.     manageUILEDS();
700.     last_UILED_millis = now;
701. #ifdef ENABLE_SOUND_MODULE
702.     if (sound_available)
703.     {
704.       my_sound.processSounds();
705.     }
706. #endif
707.   }
708. }
709.  
710. void sendMessage(void *message, size_t size)
711. {
712.   // Only send messages, if ROS is running, else Raspi sometimes doesn't boot
713.   if (!ROS_running)
714.     return;
715.  
716.   // packages need to be at least 1 byte of type, 1 byte of data and 2 bytes of CRC
717.   if (size < 4)
718.   {
719.     return;
720.   }
721.   uint8_t *data_pointer = (uint8_t *)message;
722.  
723.   // calculate the CRC
724.   uint16_t crc = CRC16.ccitt((uint8_t *)message, size - 2);
725.   data_pointer[size - 1] = (crc >> 8) & 0xFF;
726.   data_pointer[size - 2] = crc & 0xFF;
727.  
728.   packetSerial.send((uint8_t *)message, size);
729. }
730.  
731. void sendUIMessage(void *message, size_t size)
732. {
733.   // packages need to be at least 1 byte of type, 1 byte of data and 2 bytes of CRC
734.   if (size < 4)
735.   {
736.     return;
737.   }
738.   uint8_t *data_pointer = (uint8_t *)message;
739.  
740.   // calculate the CRC
741.   uint16_t crc = CRC16.ccitt((uint8_t *)message, size - 2);
742.   data_pointer[size - 1] = (crc >> 8) & 0xFF;
743.   data_pointer[size - 2] = crc & 0xFF;
744.  
745.   UISerial.send((uint8_t *)message, size);
746. }
747.