diff --git a/src/hoverboard-esc-serial-comm.cpp b/src/hoverboard-esc-serial-comm.cpp index 3b99bab..29755fe 100644 --- a/src/hoverboard-esc-serial-comm.cpp +++ b/src/hoverboard-esc-serial-comm.cpp @@ -35,6 +35,19 @@ bool ESCSerialComm::update(long millis) //returns true if something was sent or if (flag_received) { updateMotorparams(); } + + + if ( loopmillis > Motorparams.millis+FEEDBACKRECEIVETIMEOUT ) { //controller disconnected + if (controller_connected) { //just got disconnected + controller_connected=false; + Serial.println("Controller Front feedback timeout"); + } + }else if(!controller_connected && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before + controller_connected=true; + Serial.println("Controller Front connected"); + } + + if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers last_send=loopmillis;