merge Bugfix/wt901 via SerialPIO (ClemensElflein#96) integrated

Firmware/LowLevel/lib/JY901_SERIAL/JY901.cpp

@@ -9,28 +9,24 @@ CJY901 ::CJY901(SerialPIO *serial)
 {
 	this->serial = serial;
 }
-
-void CJY901::begin(int baudrate) {
+void CJY901::begin(unsigned long baudrate) {
 	serial->begin(baudrate);
 	delay(1000);
-	uint8_t unlock[] = {0xFF,0xF0,0xF0,0xF0,0xF0};
+	uint8_t unlock[] = {0xFF,0xF0,0xF0,0xF0,0xF0}; // undocumented but required magic unlock sequence
 	serial->write(unlock, 5);
 	serial->flush();
 	delay(10);
-	writeRegister(RSW, 0b0000000000010110);
+	writeRegister(RSW, 0b0000000000010110);  // Return data content
 	delay(10);
-	writeRegister(RRATE, 0x09);
+	writeRegister(RRATE, 0x08); // Return rate 0x06 = 10Hz (default), 0x08 = 50Hz, 0x09 = 100Hz
 	delay(10);
-	writeRegister(0x0b,0x00);
+	writeRegister(0x0b, 0x00);  // X axis Magnetic bias
 	delay(10);
-	writeRegister(0x0c,0x00);
+	writeRegister(0x0c, 0x00);  // Y axis Magnetic bias
 	delay(10);
-	writeRegister(0x0d,0x00);
+	writeRegister(0x0d, 0x00);  // Z axis Magnetic bias
 	delay(10);
 
-
-
-
 	ucRxCnt = 0;
 }
 
@@ -43,6 +39,7 @@ void CJY901 ::update()
 		if (ucRxBuffer[0] != 0x55)
 		{
 			ucRxCnt = 0;
+			commsError_ = true;
 			continue;
 		}
 		if (ucRxCnt < 11)
@@ -112,3 +109,12 @@ void CJY901::writeRegister(uint8_t address, uint16_t data) {
 	delay(100);
 
 }
+
+bool CJY901::commsError() {
+    bool hold = commsError_;
+#ifdef WT901
+    hold |= serial->overflow();
+#endif
+    commsError_ = false;
+    return hold;
+}

Firmware/LowLevel/lib/JY901_SERIAL/JY901.h

@@ -143,14 +143,15 @@ class CJY901
 
     #ifdef WT901_INSTEAD_OF_SOUND
 	CJY901 (HardwareSerial *serial); 
+	void begin(unsigned long baudrate = 9600UL);
 	#elif WT901
-	CJY901 (SerialPIO *serial); 
+	CJY901 (SerialPIO *serial);
+	void begin(unsigned long baudrate = 9700UL);  // YES, 9700 baud! See SerialPIO timing issue https://github.com/earlephilhower/arduino-pico/issues/1276 
 	#endif 
 
-	void begin(int baudrate = 9600);
 	// Call as often as possible to fetch data from serial
 	void update();
-
+	bool commsError();  // Get (and reset) communication error flag
 
 
   private: 
@@ -160,7 +161,8 @@ class CJY901
 	SerialPIO *serial;
 	#endif
 	unsigned char ucRxBuffer[250];
-	unsigned char ucRxCnt = 0;	
+	unsigned char ucRxCnt = 0;
+	bool commsError_ = false;  // Any kind of communication error	
 
 	void writeRegister(uint8_t address, uint16_t data);
 };

Firmware/LowLevel/src/imu.h

@@ -8,4 +8,8 @@ bool imu_read(float *acceleration_mss,float *gyro_rads,float *mag_uT);
 // call once per loop, used to process serial or do sensor fusion or do nothing.
 void imu_loop();
 
+#if defined(WT901) || defined(WT901_INSTEAD_OF_SOUND)
+bool imu_comms_error(); // get IMU communication error flag (and reset it)
+#endif
+
 #endif

Firmware/LowLevel/src/imu/WT901_SERIAL/imu.cpp

@@ -41,4 +41,9 @@ bool imu_read(float *acceleration_mss, float *gyro_rads, float *mag_uT)
 void imu_loop()
 {
     IMU.update();
+}
+
+bool imu_comms_error()
+{
+    return IMU.commsError();
 }

Firmware/LowLevel/src/main.cpp

@@ -44,6 +44,15 @@
 #define LIFT_EMERGENCY_MILLIS 100  // Time for both wheels to be lifted in order to count as emergency (0 disable). This is to filter uneven ground.
 #define BUTTON_EMERGENCY_MILLIS 20 // Time for button emergency to activate. This is to debounce the button.
 #define ANALOG_MEAN_COUNT 20       // size of array for calculation meanvalues
+
+// Define to stream debugging messages via USB
+// #define USB_DEBUG
+
+// Only define DEBUG_SERIAL if USB_DEBUG is actually enabled.
+// This enforces compile errors if it's used incorrectly.
+#ifdef USB_DEBUG
+#define DEBUG_SERIAL Serial
+#endif
 #define PACKET_SERIAL Serial1
 SerialPIO uiSerial(PIN_UI_TX, PIN_UI_RX, 250);
 
@@ -171,7 +180,7 @@ void updateEmergency() {
     uint8_t emergency_state = 0;
 
     // Handle emergency "Stop" buttons
-    if (emergency_read && LL_EMERGENCY_BITS_STOP) {
+    if (emergency_read & LL_EMERGENCY_BITS_STOP) {
         // If we just pressed, store the timestamp
         if (button_emergency_started == 0) {
             button_emergency_started = millis();
@@ -383,7 +392,9 @@ void setup() {
     //  Therefore, we pause the other core until setup() was a success
     rp2040.idleOtherCore();
 
-    DEBUG_BEGIN(9600);
+#ifdef USB_DEBUG
+    DEBUG_SERIAL.begin(9600);
+#endif  
 
     emergency_latch = true;
     ROS_running = false;
@@ -636,8 +647,10 @@ void onPacketReceived(const uint8_t *buffer, size_t size) {
         }
     } else if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_STATE && size == sizeof(struct ll_high_level_state)) {
         // copy the state
-        last_high_level_state = *((struct ll_high_level_state *)buffer);
-    } else if ((buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ || buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP) && size == sizeof(struct ll_high_level_config)) {
+        last_high_level_state = *((struct ll_high_level_state *) buffer);
+    }
+    else if ((buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ || buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP) && size == sizeof(struct ll_high_level_config)) 
+    {
         // Read and handle received config
         struct ll_high_level_config *pkt = (struct ll_high_level_config *)buffer;
         // Apply comms_version
@@ -721,17 +734,20 @@ void updateChargingEnabled() {
 
 void updateNeopixel() {
     led_blink_counter++;
-    // flash red on emergencies
-    if (emergency_latch && led_blink_counter & 0b10) {
-        p.neoPixelSetValue(0, 128, 0, 0, true);
+
+    if (emergency_latch && led_blink_counter & 0b100) {  // slow blink on emergencies
+        p.neoPixelSetValue(0, 128, 0, 0, true);          // 1/2 red
     } else {
         if (ROS_running) {
-            // Green, if ROS is running
-            p.neoPixelSetValue(0, 0, 255, 0, true);
+            p.neoPixelSetValue(0, 0, 255, 0, true);  // green
         } else {
-            // Yellow, if it's not running
-            p.neoPixelSetValue(0, 255, 50, 0, true);
+            p.neoPixelSetValue(0, 255, 50, 0, true);  // yellow
         }
+#if defined(WT901) || defined(WT901_INSTEAD_OF_SOUND)
+        if (led_blink_counter & 0b10 && imu_comms_error()) {  // fast blink on communication error (condition order matters -> short-circuit evaluation)
+            p.neoPixelSetValue(0, 255, 0, 255, true);         // magenta
+        }
+#endif
     }
 }