Odrive added option to store and reload encoder offset

Firmware/FFBoard/UserExtensions/Inc/ODriveCAN.h

@@ -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,connected
+	canid,canspd,errors,state,maxtorque,vbus,anticogging,connected,storepos
 };
 
 class ODriveCAN : public MotorDriver,public PersistentStorage, public Encoder, public CanHandler, public CommandHandler, cpp_freertos::Thread{
@@ -106,6 +106,7 @@ class ODriveCAN : public MotorDriver,public PersistentStorage, public Encoder, p
 
 	uint32_t lastPosTime = 0;
 	bool posWaiting = false;
+	bool reloadPosAfterStartup = false;
 
 	int8_t nodeId = 0; // 6 bits can ID
 	int8_t motorId = 0;

Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h

@@ -13,11 +13,11 @@
 
 #include "main.h"
 // Change this to the amount of currently registered variables
-#define NB_OF_VAR 148
+#define NB_OF_VAR 150
 extern const uint16_t VirtAddVarTab[NB_OF_VAR];
 
 // Amount of variables in exportable list
-#define NB_EXPORTABLE_ADR 133
+#define NB_EXPORTABLE_ADR 135
 extern const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR];
 
 
@@ -165,6 +165,8 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if
 #define ADR_ODRIVE_CANID 0x3D0 //0-6 ID M0, 7-12 ID M1, 13-15 can speed
 #define ADR_ODRIVE_SETTING1_M0 0x3D1
 #define ADR_ODRIVE_SETTING1_M1 0x3D2
+#define ADR_ODRIVE_OFS_M0 0x3D3 // Encoder offset position reload
+#define ADR_ODRIVE_OFS_M1 0x3D4
 // VESC Section
 #define ADR_VESC1_CANID 0x3E0 //0-7 AxisCanID, 8-16 VescCanId
 #define ADR_VESC1_DATA 0x3E1 //0-2 can speed, 3 useVescEncoder

Firmware/FFBoard/UserExtensions/Src/ODriveCAN.cpp

@@ -98,6 +98,7 @@ void ODriveCAN::registerCommands(){
 	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);
+	registerCommand("storepos", ODriveCAN_commands::storepos, "Store encoder offset",CMDFLAG_GET | CMDFLAG_SET);
 }
 
 void ODriveCAN::restoreFlash(){
@@ -118,6 +119,15 @@ void ODriveCAN::restoreFlash(){
 	uint16_t settings1 = 0;
 	if(Flash_Read(setting1addr, &settings1)){
 		maxTorque = (float)clip(settings1 & 0xfff, 0, 0xfff) / 100.0;
+		uint8_t settings1_2 = (settings1 >> 12) & 0xf;
+		reloadPosAfterStartup = (settings1_2 & 0x1) != 0;
+	}
+
+	if(reloadPosAfterStartup){
+		int16_t posOfs = 0;
+		if(Flash_Read(motorId == 0 ? ADR_ODRIVE_OFS_M0 : ADR_ODRIVE_OFS_M1, (uint16_t*)&posOfs)){
+			posOffset = (float)posOfs / getCpr();
+		}
 	}
 }
 
@@ -139,7 +149,15 @@ void ODriveCAN::saveFlash(){
 	Flash_Write(ADR_ODRIVE_CANID,canIds);
 
 	uint16_t settings1 = ((int32_t)(maxTorque*100) & 0xfff);
+	uint8_t settings1_2 = reloadPosAfterStartup ? 1 : 0; // 4 bits
+	settings1 |= (settings1_2 & 0xf) << 12;
+
 	Flash_Write(setting1addr, settings1);
+
+	if(reloadPosAfterStartup){
+		int32_t posOfs = posOffset * getCpr();
+		Flash_Write(motorId == 0 ? ADR_ODRIVE_OFS_M0 : ADR_ODRIVE_OFS_M1, posOfs);
+	}
 }
 
 void ODriveCAN::Run(){
@@ -171,7 +189,9 @@ void ODriveCAN::Run(){
 			// Odrive is active,
 			// enable torque mode
 			if(odriveCurrentState == ODriveState::AXIS_STATE_CLOSED_LOOP_CONTROL){
-				//this->setPos(0);
+				if(!reloadPosAfterStartup){
+					this->setPos(0); // Assume this as the zero position and let the user correct it
+				}
 				setMode(ODriveControlMode::CONTROL_MODE_TORQUE_CONTROL, ODriveInputMode::INPUT_MODE_PASSTHROUGH);
 			}
 
@@ -365,7 +385,13 @@ CommandStatus ODriveCAN::command(const ParsedCommand& cmd,std::vector<CommandRep
 			replies.emplace_back(connected ? 1 : 0);
 		}
 		break;
-
+	case ODriveCAN_commands::storepos:
+		if(cmd.type == CMDtype::get){
+			replies.emplace_back(reloadPosAfterStartup ? 1 : 0);
+		}else if(cmd.type == CMDtype::set){
+			reloadPosAfterStartup = cmd.val != 0;
+		}
+		break;
 	default:
 		return CommandStatus::NOT_FOUND;
 	}

Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c

@@ -147,6 +147,8 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] =
 	ADR_ODRIVE_CANID, //0-6 ID M0, 7-12 ID M1, 13-15 can speed
 	ADR_ODRIVE_SETTING1_M0,
 	ADR_ODRIVE_SETTING1_M1,
+	ADR_ODRIVE_OFS_M0, // Encoder offset position reload
+	ADR_ODRIVE_OFS_M1,
 // VESC Section
 	ADR_VESC1_CANID, //0-7 AxisCanID, 8-16 VescCanId
 	ADR_VESC1_DATA, //0-2 can speed, 3 useVescEncoder
@@ -328,6 +330,8 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] =
 	ADR_ODRIVE_CANID, //0-6 ID M0, 7-12 ID M1, 13-15 can speed
 	ADR_ODRIVE_SETTING1_M0,
 	ADR_ODRIVE_SETTING1_M1,
+	ADR_ODRIVE_OFS_M0, // Encoder offset position reload
+	ADR_ODRIVE_OFS_M1,
 // VESC Section
 	ADR_VESC1_CANID, //0-7 AxisCanID, 8-16 VescCanId
 	ADR_VESC1_DATA, //0-2 can speed, 3 useVescEncoder

Firmware/scripts/memory_map.csv

@@ -129,6 +129,8 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses
 ,ADR_ODRIVE_CANID,0x3D0,"//0-6 ID M0, 7-12 ID M1, 13-15 can speed",1,1
 ,ADR_ODRIVE_SETTING1_M0,0x3D1,,1,1
 ,ADR_ODRIVE_SETTING1_M1,0x3D2,,1,1
+,ADR_ODRIVE_OFS_M0,0x3D3,// Encoder offset position reload,1,1
+,ADR_ODRIVE_OFS_M1,0x3D4,,1,1
 // VESC Section,,,,,
 ,ADR_VESC1_CANID,0x3E0,"//0-7 AxisCanID, 8-16 VescCanId",1,1
 ,ADR_VESC1_DATA,0x3E1,"//0-2 can speed, 3 useVescEncoder",1,1