#include <Arduino.h>

void ReceiveSerial2();
void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight);

//https://github.com/rogerclarkmelbourne/Arduino_STM32   in arduino/hardware
//Board: Generic STM32F103C series
//Upload method: serial
//20k RAM 64k Flash

// RX ist A10, TX ist A9   (3v3 level)
//to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately
//set boot0 back to 0 to run program on powerup

//Flashing the hoverbrett controller:
/*
 * connect uart adapter to serial port cable (the one with more red heatshrink)
 * (disconnect xt30 power connector)
 * set jumper on usb uart adapter to output 5V
 * hold boot0 button (black, the outermost) while powering up (or restarting with small button next to it)
 * flash
 */

//PA2 may be defective on my bluepill

#define SERIAL_CONTROL_BAUD   38400       // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
#define SERIAL_BAUD         115200      // [-] Baud rate for built-in Serial (used for the Serial Monitor)
#define START_FRAME         0xAAAA       // [-] Start frme definition for reliable serial communication

//#define DEBUG
//#define PARAMETEROUTPUT
uint8_t error = 0;
#define IMU_NO_CHANGE 2 //IMU values did not change for too long
uint8_t imu_no_change_counter = 0;

#define PIN_LED PC13

#define PIN_VBAT PA0 //battery voltage after voltage divider
//#define VBAT_DIV_FACTOR 0.010700 //how much voltage (V) equals one adc unit.  measured at 40V and averaged
#define VBAT_DIV_FACTOR 0.01399535423925667828 //how much voltage (V) equals one adc unit.  3444=48.2V
#define PIN_CURRENT PA1 //output of hall sensor for current measurement
#define CURRENT_OFFSET 2048 //adc reading at 0A, with CJMCU-758 typically at Vcc/2. measured with actual voltage supply in hoverbrett
#define CURRENT_FACTOR 0.38461538461538461538 //how much current (A) equals one adc unit.  2045-2032=13 at 5A
float vbat=0; //battery voltage
float ibat=0; //battery current
long last_adcupdated=0;
#define ADC_UPDATEPERIOD 10 //in ms


#define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial

//Status information sending
#define PARAMETERSENDPERIOD 50 //delay for sending stat data via nrf24
long last_parametersend=0;

#define CONTROLUPDATEPERIOD 10
long last_controlupdate = 0;

#define PIN_GAMETRAK_LENGTH PA1  //yellow (connector) / orange (gametrak module wires): length
#define PIN_GAMETRAK_VERTICAL PA3 //orange / red: vertical
#define PIN_GAMETRAK_HORIZONTAL PA4 //blue / yellow: horizontal


#define GT_LENGTH_OFFSET 4090 //adc offset value (rolled up value)
#define GT_LENGTH_MIN 220 //length in mm at which adc values start to change
#define GT_LENGTH_SCALE -0.73 //(adcvalue-offset)*scale = length[mm] (+length_min)
//2720 at 1000mm+220mm -> 1370 for 1000mm -> 
#define GT_LENGTH_MAXLENGTH 2500 //maximum length in [mm]. maximum string length is around 2m80
uint16_t gt_length=0; //0=rolled up, 1unit = 1mm

#define GT_VERTICAL_CENTER 2048 //adc value for center position
#define GT_VERTICAL_RANGE 2047 //adc value difference from center to maximum (30 deg)
int8_t gt_vertical=0; //0=center. joystick can rotate +-30 degrees. -127 = -30 deg
//left = -30 deg, right= 30deg

#define GT_HORIZONTAL_CENTER 2048 //adc value for center position
#define GT_HORIZONTAL_RANGE 2047 //adc value difference from center to maximum (30 deg)
int8_t gt_horizontal=0; //0=center

uint16_t gt_length_set=1000; //set length to keep [mm]
#define GT_LENGTH_MINDIFF 10 //[mm] threshold, do not move within gt_length_set-GT_LENGTH_MINDIFF and gt_length_set+GT_LENGTH_MINDIFF
float gt_speed_p=0.7; //value to multipy difference [mm] with -> out_speed
float gt_speedbackward_p=0.7;
float gt_steer_p=2.0;
#define GT_SPEED_LIMIT 300 //maximum out_speed value +
#define GT_SPEEDBACKWARD_LIMIT 100//maximum out_speed value (for backward driving) -
#define GT_STEER_LIMIT 300 //maximum out_steer value +-
#define GT_LENGTH_MAXIMUMDIFFBACKWARD -200 //[mm]. if gt_length_set=1000 and GT_LENGTH_MAXIMUMDIFFBACKWARD=-200 then only drives backward if lenght is greater 800


//#include <IMUGY85.h>
//https://github.com/fookingthg/GY85
//ITG3200 and ADXL345 from https://github.com/jrowberg/i2cdevlib/tree/master/Arduino
//https://github.com/mechasolution/Mecha_QMC5883L   //because QMC5883 on GY85 instead of HMC5883, source: https://circuitdigest.com/microcontroller-projects/digital-compass-with-arduino-and-hmc5883l-magnetometer
//in qmc5883L library read values changed from uint16_t to int16_t

#define IMUUPDATEPERIOD 10 //ms
//long last_imuupdated = 0;
//#define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered

/*
IMUGY85 imu;
double ax, ay, az, gx, gy, gz, roll, pitch, yaw, mx, my, mz, ma;
double old_ax, old_ay, old_az, old_gx, old_gy, old_gz, old_roll, old_pitch, old_yaw, old_mx, old_my, old_mz, old_ma;

double setYaw = 0;
float magalign_multiplier = 0; //how much the magnetometer should influence steering, 0=none, 1=stay aligned
*/

// Lenovo Trackpoint pinout
//from left to right. pins at bottom. chips on top
//1 GND (black)
//2 Data
//3 Clock
//4 Reset
//5 +5V (red)
//6 Right BTN
//7 Middle BTN
//8 Left BTN
//pinout: https://martin-prochnow.de/projects/thinkpad_keyboard
//see also https://github.com/feklee/usb-trackpoint/blob/master/code/code.ino

#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"

RF24 radio(PB0, PB1); //ce, cs
//SCK D13 (Pro mini), A5 (bluepill)
//Miso D12 (Pro mini), A6 (bluepill)
//Mosi D11 (Pro mini), A7 (bluepill)

// Radio pipe addresses for the 2 nodes to communicate.
const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };
#define NRF24CHANNEL 75

struct nrfdata {
  uint8_t steer;
  uint8_t speed;
  uint8_t commands; //bit 0 set = motor enable
  uint8_t checksum;
};

nrfdata lastnrfdata;
long last_nrfreceive = 0; //last time values were received and checksum ok
long nrf_delay = 0;
#define MAX_NRFDELAY 100 //ms. maximum time delay at which vehicle will disarm

boolean radiosendOk=false;


//command variables
boolean motorenabled = false; //set by nrfdata.commands




long last_send = 0;

int16_t out_speedl = 0; //between -1000 and 1000
int16_t out_speedr = 0;
int16_t lastsend_out_speedl = 0; //last value transmitted to motor controller
int16_t lastsend_out_speedr = 0;
int16_t set_speed = 0;
int16_t set_steer = 0;
uint8_t out_checksum = 0; //0= disable motors, 255=reserved,  1<=checksum<255
#define NRFDATA_CENTER 127

//boolean armed = false;
boolean lastpacketOK = false;

//Gametrak
//boolean armed_gt = false;

uint8_t controlmode=0;
#define MODE_DISARMED 0
#define MODE_RADIONRF 1
#define MODE_GAMETRAK 2



// Global variables
uint8_t idx = 0;                        // Index for new data pointer
uint16_t bufStartFrame;                 // Buffer Start Frame
byte *p;                                // Pointer declaration for the new received data
byte incomingByte;
byte incomingBytePrev;

typedef struct{
   uint16_t start;
   int16_t  speedLeft;
   int16_t  speedRight;
   uint16_t checksum;
} SerialCommand;
SerialCommand Command;

typedef struct{
   uint16_t start;
   int16_t  cmd1;
   int16_t  cmd2;
   int16_t  speedL;
   int16_t  speedR;
   int16_t  speedL_meas;
   int16_t  speedR_meas;
   int16_t  batVoltage;
   int16_t  boardTemp;
   int16_t  curL_DC;
   int16_t  curR_DC;
   uint16_t  checksum;
} SerialFeedback;
SerialFeedback Feedback;
SerialFeedback NewFeedback;




void setup() {

  Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1,  A10=RX1  (3v3 level)

  
  Serial2.begin(SERIAL_CONTROL_BAUD); //control.  B10=TX3, B11=RX3 (Serial2 is Usart 3)
  //Serial1 may be dead on my board?


  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED, HIGH);

  
  pinMode(PIN_VBAT,INPUT_ANALOG);
  pinMode(PIN_CURRENT,INPUT_ANALOG);

  pinMode(PIN_GAMETRAK_LENGTH,INPUT_ANALOG);
  pinMode(PIN_GAMETRAK_VERTICAL,INPUT_ANALOG);
  pinMode(PIN_GAMETRAK_HORIZONTAL,INPUT_ANALOG);
  

  #ifdef DEBUG 
  Serial.println("Initializing nrf24");
  #endif
  

  
  radio.begin();

  
  radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS
  //radio.setDataRate( RF24_1MBPS ); 

  radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive)

  radio.setRetries(15, 15); // optionally, increase the delay between retries & # of retries
  radio.setPayloadSize(8); // optionally, reduce the payload size.  seems to improve reliability

  radio.openWritingPipe(pipes[0]); //write on pipe 0
  radio.openReadingPipe(1, pipes[1]); //read on pipe 1

  radio.startListening();
  
  


  #ifdef DEBUG 
  Serial.println("Initialized"); 
  #endif


}

void loop() {


  ReceiveSerial2(); // Check for new received data

 
  if (millis() - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings
    vbat=analogRead(PIN_VBAT)*VBAT_DIV_FACTOR;
    ibat=(analogRead(PIN_CURRENT)-CURRENT_OFFSET)*CURRENT_FACTOR;

  
    gt_length = constrain((analogRead(PIN_GAMETRAK_LENGTH)-GT_LENGTH_OFFSET)*GT_LENGTH_SCALE +GT_LENGTH_MIN, 0,GT_LENGTH_MAXLENGTH);
    if (gt_length<=GT_LENGTH_MIN){
      gt_length=0; //if below minimum measurable length set to 0mm
    }
    gt_vertical = constrain(map(analogRead(PIN_GAMETRAK_VERTICAL)-GT_VERTICAL_CENTER, +GT_VERTICAL_RANGE,-GT_VERTICAL_RANGE,-127,127),-127,127); //left negative
    gt_horizontal = constrain(map(analogRead(PIN_GAMETRAK_HORIZONTAL)-GT_HORIZONTAL_CENTER, +GT_HORIZONTAL_RANGE,-GT_HORIZONTAL_RANGE,-127,127),-127,127); //down negative
    
    last_adcupdated = millis();

    /*
    Serial.print("gt_length=");
    Serial.print(gt_length);
    Serial.print(", gt_vertical=");
    Serial.print(gt_vertical);
    Serial.print(", gt_horizontal=");
    Serial.println(gt_horizontal);*/

    /*
    Serial.print("PIN_GAMETRAK_LENGTH=");
    Serial.print(analogRead(PIN_GAMETRAK_LENGTH));
    Serial.print(", PIN_GAMETRAK_VERTICAL=");
    Serial.print(analogRead(PIN_GAMETRAK_VERTICAL));
    Serial.print(", PIN_GAMETRAK_HORIZONTAL=");
    Serial.println(analogRead(PIN_GAMETRAK_HORIZONTAL));
    */
  }

  //NRF24
  nrf_delay = millis() - last_nrfreceive; //update nrf delay
  if ( radio.available() )
  {
    //Serial.println("radio available ...");

    lastpacketOK = false; //initialize with false, if checksum ok gets set to true
    digitalWrite(PIN_LED, !digitalRead(PIN_LED));
    radio.read( &lastnrfdata, sizeof(nrfdata) );

    if (lastnrfdata.speed == NRFDATA_CENTER && lastnrfdata.steer == NRFDATA_CENTER) { //arm only when centered
      controlmode = MODE_RADIONRF;//set radionrf mode at first received packet
    }




    uint8_t calcchecksum = (uint8_t)((lastnrfdata.steer + 3) * (lastnrfdata.speed + 13));
    if (lastnrfdata.checksum == calcchecksum) { //checksum ok?
      lastpacketOK = true;
      last_nrfreceive = millis();

      //parse commands
      motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0
    }

/*
#ifdef DEBUG
    Serial.print("Received:");
    Serial.print(" st=");
    Serial.print(lastnrfdata.steer);
    Serial.print(", sp=");
    Serial.print(lastnrfdata.speed);
    Serial.print(", c=");
    Serial.print(lastnrfdata.commands);
    Serial.print(", chks=");
    Serial.print(lastnrfdata.checksum);

    Serial.print("nrfdelay=");
    Serial.print(nrf_delay);
    Serial.println();
#endif
*/

    //y positive = forward
    //x positive = right

    /*
      setYaw+=((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*10/127;
      while (setYaw<0){
      setYaw+=360;
      }
      while (setYaw>=360){
      setYaw-=360;
      }*/

    /*
      Serial.print("setYaw=");
      Serial.print(setYaw);
      Serial.print(" Yaw=");
      Serial.println(yaw);*/

    
  }


  
  
  if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
    controlmode = MODE_DISARMED;
    #ifdef DEBUG
    Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
    #endif
  }
  if (controlmode == MODE_RADIONRF) { //is armed in nrf mode

    
    if (lastpacketOK) { //if lastnrfdata is valid
      if (millis() - last_controlupdate > CONTROLUPDATEPERIOD) {
        last_controlupdate = millis();

        //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
        //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
        set_speed = (int16_t)( ((int16_t)(lastnrfdata.speed) - NRFDATA_CENTER) * 1000 / 127 ); //-1000 to 1000
        set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 );

        //align to compass
        /*
        double yawdiff = (setYaw - 180) - (yaw - 180); //following angle difference works only for angles [-180,180]. yaw here is [0,360]
        yawdiff += (yawdiff > 180) ? -360 : (yawdiff < -180) ? 360 : 0;
        //yawdiff/=2;
        int yawdiffsign = 1;
        if (yawdiff < 0) {
          yawdiffsign = -1;
        }
        yawdiff = yawdiff * yawdiff; //square
        yawdiff = constrain(yawdiff * 1 , 0, 800);
        yawdiff *= yawdiffsign; //redo sign
        int16_t set_steer_mag = (int16_t)( yawdiff );

        float new_magalign_multiplier = map( abs((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER), 2, 10, 1.0, 0.0); //0=normal steering, 1=only mag steering
        new_magalign_multiplier = 0; //Force mag off
        new_magalign_multiplier = constrain(new_magalign_multiplier, 0.0, 1.0);

        magalign_multiplier = min(new_magalign_multiplier, min(1.0, magalign_multiplier + 0.01)); //go down fast, slowly increase
        magalign_multiplier = constrain(magalign_multiplier, 0.0, 1.0); //safety constrain again

        set_steer = set_steer * (1 - magalign_multiplier) + set_steer_mag * magalign_multiplier;
        */

        //calculate speed l and r from speed and steer
        #define SPEED_COEFFICIENT_NRF   1  // higher value == stronger
        #define STEER_COEFFICIENT_NRF   0.5  // higher value == stronger
        out_speedl = constrain(set_speed * SPEED_COEFFICIENT_NRF +  set_steer * STEER_COEFFICIENT_NRF, -1500, 1500);
        out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF -  set_steer * STEER_COEFFICIENT_NRF, -1500, 1500);

        /*
          Serial.print("Out steer=");
          Serial.println(out_steer);*/
      }
    }//if pastpacket not ok, keep last out_steer and speed values until disarmed


#ifdef DEBUG
    if (!lastpacketOK) {
      Serial.println("Armed but packet not ok");
    }
#endif

  }



  if (controlmode==MODE_DISARMED) { //check if gametrak can be armed
    if (gt_length>gt_length_set && gt_length<gt_length_set+10) { //is in trackable length
      controlmode=MODE_GAMETRAK; //enable gametrak mode
    }
  }else if (controlmode==MODE_GAMETRAK){ //gametrak control active and not remote active
    //Gametrak Control Code
    motorenabled=true;
    if (gt_length<=GT_LENGTH_MIN){ //let go
      controlmode=MODE_DISARMED;
      motorenabled=false;
    }
    int16_t _gt_length_diff = gt_length-gt_length_set; //positive if needs to drive forward
    if (_gt_length_diff>-GT_LENGTH_MINDIFF & _gt_length_diff<GT_LENGTH_MINDIFF){ //minimum difference to drive
      _gt_length_diff=0; //threshold
    }

    set_steer=constrain((int16_t)(-gt_horizontal*gt_steer_p),-GT_STEER_LIMIT,GT_STEER_LIMIT); //steer positive is left //gt_horizontal left is negative
    
    if (_gt_length_diff>0) { //needs to drive forward
      set_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),0,GT_SPEED_LIMIT);
    }else{ //drive backward
      if (_gt_length_diff > GT_LENGTH_MAXIMUMDIFFBACKWARD){ //only drive if not pulled back too much
        set_speed = constrain((int16_t)(_gt_length_diff*gt_speedbackward_p),-GT_SPEEDBACKWARD_LIMIT,0);
      }else{
        set_speed = 0; //stop
        set_steer = 0;
      }
    }
    
    

    //calculate speed l and r from speed and steer
    #define SPEED_COEFFICIENT_GT   1  // higher value == stronger
    #define STEER_COEFFICIENT_GT   0.5  // higher value == stronger
    out_speedl = constrain(set_speed * SPEED_COEFFICIENT_GT +  set_steer * STEER_COEFFICIENT_GT, -1000, 1000);
    out_speedr = constrain(set_speed * SPEED_COEFFICIENT_GT -  set_steer * STEER_COEFFICIENT_GT, -1000, 1000);
  }


  if (error > 0) { //disarm if error occured
    controlmode = MODE_DISARMED; //force disarmed
  }

  if (controlmode == MODE_DISARMED){ //all disarmed
    out_speedl = 0;
    out_speedr = 0;
  }


  
  if (millis() - last_send > SENDPERIOD) {
    //calculate checksum
    out_checksum = ((uint8_t) ((uint8_t)out_speedl) * ((uint8_t)out_speedr)); //simple checksum
    if (out_checksum == 0 || out_checksum == 255) {
      out_checksum = 1;  //cannot be 0 or 255 (special purpose)
    }
  
    if (!motorenabled) { //disable motors?
      out_checksum = 0; //checksum=0 disables motors
    }
  
    /*Serial2.write((uint8_t *) &out_speedl, sizeof(out_speedl));
    Serial2.write((uint8_t *) &out_speedr, sizeof(out_speedr));
    Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));*/
    if (motorenabled) { //motors enabled
      SendSerial2(out_speedl,out_speedr);
    } else { //motors disabled
      SendSerial2(0,0);
    }
    lastsend_out_speedl = out_speedl; //remember last transmittet values (for stat sending)
    lastsend_out_speedr = out_speedr;
    last_send = millis();
  
  
  #ifdef DEBUG
    Serial.print(" out_speedl=");
    Serial.print(out_speedl);
    Serial.print(" out_speedr=");
    Serial.print(out_speedr);
    Serial.print(" checksum=");
    Serial.print(out_checksum);

    Serial.print(" controlmode=");
    Serial.print(controlmode);
  
    Serial.println();
    
  #endif
  }

  
  //
  #ifdef PARAMETEROUTPUT
  if ( millis() - last_parametersend > PARAMETERSENDPERIOD) {
    //Serial.write((uint8_t *) &counter, sizeof(counter));//uint8_t, 1 byte
    //Serial.write((uint8_t *) &value1, sizeof(value1)); //uint16_t, 2 bytes
    //Serial.write((uint8_t *) &value2, sizeof(value2)); //int16_t, 2 bytes
    //Serial.write((uint8_t *) &floatvalue, sizeof(floatvalue)); //float, 4 bytes

    uint8_t booleanvalues=0; //reset
    booleanvalues |= motorenabled<<0; //bit 0
    booleanvalues |= (controlmode&0b00000011)<<1; //bit 1 and 2 (2bit number for controlmodes (3)
    
    Serial.write((uint8_t *) &out_speedl, sizeof(out_speedl)); //int16_t, 2 bytes
    Serial.write((uint8_t *) &out_speedr, sizeof(out_speedr)); //int16_t, 2 bytes
    Serial.write((uint8_t *) &booleanvalues, sizeof(booleanvalues)); //uint8_t, 1 byte //booleanvalues
    Serial.write((uint8_t *) &vbat, sizeof(vbat)); //float, 4 bytes
    //Serial.write((uint8_t *) &ibat, sizeof(ibat)); //float, 4 bytes
    float yaw_float=yaw;
    Serial.write((uint8_t *) &yaw_float, sizeof(yaw_float)); //float, 4 bytes
    Serial.write((uint8_t *) &gt_length, sizeof(gt_length)); //uint16_t, 2 bytes
    Serial.write((uint8_t *) &gt_horizontal, sizeof(gt_horizontal)); //int8_t, 1 byte
    Serial.write((uint8_t *) &gt_vertical, sizeof(gt_vertical)); //int8_t, 1 byte
    
    
    
    
    last_parametersend = millis();
  }
  #endif
  

}

/*
void sendRF(nrfstatdata senddata){
  #ifdef DEBUG
  Serial.println("Transmitting...");
  #endif
  
  radio.stopListening(); //stop listening to be able to transmit
  radiosendOk = radio.write( &senddata, sizeof(nrfstatdata) );
  if (!radiosendOk){
      #ifdef DEBUG
      Serial.println("send failed");
      #endif
  }
  radio.startListening(); //start listening again
}
*/




// ########################## SEND ##########################
void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight)
{
  // Create command
  Command.start    = (uint16_t)START_FRAME;
  Command.speedLeft    = (int16_t)uSpeedLeft;
  Command.speedRight    = (int16_t)uSpeedRight;
  Command.checksum = (uint16_t)(Command.start ^ Command.speedLeft ^ Command.speedRight);

  // Write to Serial
  Serial2.write((uint8_t *) &Command, sizeof(Command)); 
}

// ########################## RECEIVE ##########################
void ReceiveSerial2()
{
  // Check for new data availability in the Serial buffer
  if (Serial2.available()) {   
    incomingByte    = Serial2.read();                                 // Read the incoming byte
    bufStartFrame = ((uint16_t)(incomingBytePrev) << 8) +  incomingByte;  // Construct the start frame    
  }
  else {
    return;
  }

  // If DEBUG_RX is defined print all incoming bytes
  #ifdef DEBUG_RX
    Serial.print(incomingByte);
    return;
  #endif      
  
  // Copy received data
  if (bufStartFrame == START_FRAME) {                     // Initialize if new data is detected
    p     = (byte *)&NewFeedback;
    *p++  = incomingBytePrev;
    *p++  = incomingByte;   
    idx   = 2;    
  } else if (idx >= 2 && idx < sizeof(SerialFeedback)) {  // Save the new received data
    *p++  = incomingByte; 
    idx++;
  } 
  
  // Check if we reached the end of the package
  if (idx == sizeof(SerialFeedback)) {    
    uint16_t checksum;
    checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL
          ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC);
    // Check validity of the new data
    if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {

      // Copy the new data
      memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
      
      // Print data to built-in Serial
      /*Serial.print("1: ");   Serial.print(Feedback.cmd1);
      Serial.print(" 2: ");  Serial.print(Feedback.cmd2);
      Serial.print(" 3: ");  Serial.print(Feedback.speedR);
      Serial.print(" 4: ");  Serial.print(Feedback.speedL);
      Serial.print(" 5: ");  Serial.print(Feedback.speedR_meas);
      Serial.print(" 6: ");  Serial.print(Feedback.speedL_meas);
      //Serial.print(" 7: ");  Serial.print(Feedback.batVoltage);
      //Serial.print(" 8: ");  Serial.println(Feedback.boardTemp);
      Serial.print(" 9: ");  Serial.print(Feedback.curL_DC); //in A, in hoverbrett negative sign for forward
      Serial.print(" 10: ");  Serial.println(Feedback.curR_DC); //in A, in hoverbrett negative sign for forward
      */
     Serial.print(" 9: ");  Serial.println(Feedback.curL_DC); //in A, in hoverbrett negative sign for forward
    } else {
      Serial.println("Non-valid data skipped");
    }
    idx = 0;  // Reset the index (it prevents to enter in this if condition in the next cycle)
  }
  
  // Update previous states
  incomingBytePrev  = incomingByte;
}