ODrive disconnect detection and command added

Author: Ultrawipf

Configurator

@@ -29,7 +29,7 @@ enum class ODriveEncoderFlags : uint32_t {ERROR_NONE = 0,ERROR_UNSTABLE_GAIN = 0
 enum class ODriveAxisError : uint32_t {AXIS_ERROR_NONE = 0x00000000,AXIS_ERROR_INVALID_STATE  = 0x00000001, AXIS_ERROR_WATCHDOG_TIMER_EXPIRED = 0x00000800,AXIS_ERROR_MIN_ENDSTOP_PRESSED = 0x00001000, AXIS_ERROR_MAX_ENDSTOP_PRESSED = 0x00002000,AXIS_ERROR_ESTOP_REQUESTED = 0x00004000,AXIS_ERROR_HOMING_WITHOUT_ENDSTOP = 0x00020000,AXIS_ERROR_OVER_TEMP = 0x00040000,AXIS_ERROR_UNKNOWN_POSITION = 0x00080000};
 
 enum class ODriveCAN_commands : uint32_t{
-	canid,canspd,errors,state,maxtorque,vbus,anticogging
+	canid,canspd,errors,state,maxtorque,vbus,anticogging,connected
 };
 
 class ODriveCAN : public MotorDriver,public PersistentStorage, public Encoder, public CanHandler, public CommandHandler, cpp_freertos::Thread{
@@ -95,6 +95,7 @@ class ODriveCAN : public MotorDriver,public PersistentStorage, public Encoder, p
 	float posOffset = 0;
 	float lastVoltage = 0;
 	uint32_t lastVoltageUpdate = 0;
+	uint32_t lastCanMessage = 0;
 
 	int8_t nodeId = 0; // 6 bits can ID
 	int8_t motorId = 0;
@@ -107,14 +108,13 @@ class ODriveCAN : public MotorDriver,public PersistentStorage, public Encoder, p
 	volatile uint32_t odriveControllerFlags = 0;
 
 	float maxTorque = 1.0; // range how to scale the torque output
-	//bool ready = false;
 	volatile bool waitReady = true;
 
-	//volatile bool requestFirstRun = false;
 	bool active = false;
 
 	int32_t filterId = 0;
 	volatile ODriveLocalState state = ODriveLocalState::IDLE;
+	bool connected = false;
 
 	uint8_t baudrate = CANSPEEDPRESET_500; // 250000, 500000, 1M
 };

Firmware/FFBoard/UserExtensions/Inc/ODriveCAN.h

@@ -71,13 +71,14 @@ ODriveCAN::~ODriveCAN() {
 
 void ODriveCAN::registerCommands(){
 	CommandHandler::registerCommands();
-	registerCommand("canid", ODriveCAN_commands::canid, "CAN id of ODrive");
-	registerCommand("canspd", ODriveCAN_commands::canspd, "CAN baudrate");
-	registerCommand("errors", ODriveCAN_commands::errors, "ODrive error flags");
-	registerCommand("state", ODriveCAN_commands::state, "ODrive state");
-	registerCommand("maxtorque", ODriveCAN_commands::maxtorque, "Max torque to send for scaling");
-	registerCommand("vbus", ODriveCAN_commands::vbus, "ODrive Vbus");
-	registerCommand("anticogging", ODriveCAN_commands::anticogging, "Set 1 to start anticogging calibration");
+	registerCommand("canid", ODriveCAN_commands::canid, "CAN id of ODrive",CMDFLAG_GET | CMDFLAG_SET);
+	registerCommand("canspd", ODriveCAN_commands::canspd, "CAN baudrate",CMDFLAG_GET | CMDFLAG_SET);
+	registerCommand("errors", ODriveCAN_commands::errors, "ODrive error flags",CMDFLAG_GET);
+	registerCommand("state", ODriveCAN_commands::state, "ODrive state",CMDFLAG_GET);
+	registerCommand("maxtorque", ODriveCAN_commands::maxtorque, "Max torque to send for scaling",CMDFLAG_GET | CMDFLAG_SET);
+	registerCommand("vbus", ODriveCAN_commands::vbus, "ODrive Vbus",CMDFLAG_GET);
+	registerCommand("anticogging", ODriveCAN_commands::anticogging, "Set 1 to start anticogging calibration",CMDFLAG_SET);
+	registerCommand("connected", ODriveCAN_commands::connected, "ODrive connection state",CMDFLAG_GET);
 }
 
 void ODriveCAN::restoreFlash(){
@@ -174,6 +175,16 @@ void ODriveCAN::Run(){
 		if(HAL_GetTick() - lastVoltageUpdate > 1000){
 			requestMsg(0x17); // Update voltage
 		}
+
+		if(HAL_GetTick() - lastCanMessage > 2000){ // Timeout
+			odriveCurrentState = ODriveState::AXIS_STATE_UNDEFINED;
+			state = ODriveLocalState::IDLE;
+			waitReady = true;
+			connected = false;
+		}else{
+			connected = true;
+		}
+
 	}
 }
 
@@ -282,9 +293,7 @@ CommandStatus ODriveCAN::command(const ParsedCommand& cmd,std::vector<CommandRep
 		break;
 
 	case ODriveCAN_commands::canid:
-		if(cmd.type == CMDtype::get){
-			return handleGetSet(cmd, replies, this->nodeId);
-		}
+		return handleGetSet(cmd, replies, this->nodeId);
 		break;
 	case ODriveCAN_commands::state:
 		if(cmd.type == CMDtype::get){
@@ -321,6 +330,11 @@ CommandStatus ODriveCAN::command(const ParsedCommand& cmd,std::vector<CommandRep
 		}
 		break;
 	}
+	case ODriveCAN_commands::connected:
+		if(cmd.type == CMDtype::get){
+			replies.push_back(CommandReply(connected ? 1 : 0));
+		}
+		break;
 
 	default:
 		return CommandStatus::NOT_FOUND;
@@ -340,6 +354,8 @@ void ODriveCAN::canRxPendCallback(CAN_HandleTypeDef *hcan,uint8_t* rxBuf,CAN_RxH
 	uint64_t msg = *reinterpret_cast<uint64_t*>(rxBuf);
 	uint8_t cmd = rxHeader->StdId & 0x1F;
 
+	lastCanMessage = HAL_GetTick();
+
 	switch(cmd){
 	case 1:
 	{