Implement PID and telemetry

main/motor.c

@@ -145,7 +145,7 @@ uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen){
  * Parse the KISS telemetry frame and check the crc8
  * TODO: Do more with the data than print it
  */
-void parse_telemetry(void) {
+bool parse_telemetry(esc_telemetry_t * telemetry) {
     uint8_t frame_size = 10;
     uint8_t data[128];
     // get data
@@ -153,28 +153,30 @@ void parse_telemetry(void) {
     
     uart_flush(ESC_UART_NUM);
 
-    if(length < 10) return;
-    
+    if(length < 10) return false;
+
     // chop out just the payload
     uint8_t payload[128];
     uint8_t payload_length = (frame_size - 1);
     for(uint8_t i = 0; i < payload_length; i++) {
         payload[i] = data[i];
     }
-    
+
     // calculate the crc8
     uint8_t expected_crc8 = get_crc8(payload, payload_length);
     uint8_t received_crc8 = (uint8_t) data[frame_size - 1];
     
-    if(expected_crc8 != received_crc8) return;
+    if(expected_crc8 != received_crc8) return false;
     
-    for(uint8_t i = 0; i < length; i++) {
-        printf("%d - %d\n", i, data[i]);
-    }
-    printf("--------------------\n");
-    printf("expected: %d\n", expected_crc8);
-    printf("received: %d\n", received_crc8);
-    printf("======================\n");
+    
+    telemetry->temperature = payload[0];
+    telemetry->voltage     = (payload[1] << 8) + payload[2];
+    telemetry->current     = (payload[3] << 8) + payload[4];
+    telemetry->consumption = (payload[5] << 8) + payload[6];
+    telemetry->rpm         = (payload[7] << 8) + payload[8];
+    
+
+    return true;
 }
 
 /**