CAN and timing fixes

CHANGELOG.md

@@ -1,3 +1,8 @@
+### v1.17.1
+- Fixed CAN filters for extended IDs
+- VESC uses CAN filters
+- Reduce PendSV interrupts by delaying the idle loop by default
+
 ### v1.17.0
 - Added SPI speed selector to MagnTek encoders
 - Added "reg" and "save" commands to MagnTek encoder. Allows programming MT6835 encoders (debug=1 mode required!)

Firmware/FFBoard/Inc/CAN.h

@@ -153,6 +153,8 @@ class CANPort : public PersistentStorage { //  : public CanHandler if interrupt
 
 	virtual void setSilentMode(bool silent) = 0;
 
+	virtual bool isWaiting();
+
 
 	// ---------------------------------------------
 

Firmware/FFBoard/Src/CAN.cpp

@@ -30,6 +30,10 @@ CANPort* CANPort::handleToPort(void* handle){
 	return portInst;
 }
 
+bool CANPort::isWaiting(){
+	return isWaitingFlag;
+}
+
 
 // ------------------------
 

Firmware/FFBoard/Src/CANPort2B.cpp

@@ -267,17 +267,26 @@ int32_t CANPort_2B::addCanFilter(CAN_filter filter){
 	sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
 	sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
 	sFilterConfig.FilterFIFOAssignment = filter.buffer == 0 ? CAN_RX_FIFO0 : CAN_RX_FIFO1;
-	sFilterConfig.FilterIdHigh = ((filter.filter_id << 5)  | (filter.filter_id >> (32 - 5))) & 0xFFFF;
-	sFilterConfig.FilterIdLow = (filter.filter_id >> (11 - 3)) & 0xFFF8;
-	sFilterConfig.FilterMaskIdHigh = ((filter.filter_mask << 5)  | (filter.filter_mask >> (32 - 5))) & 0xFFFF;
-	sFilterConfig.FilterMaskIdLow = (filter.filter_mask >> (11 - 3)) & 0xFFF8;
 
 
-	if(filter.extid){
-		sFilterConfig.FilterIdLow |= 0x04;
-		sFilterConfig.FilterMaskIdLow |= 0x4; // Add IDE bit
+	if (!filter.extid) {
+	    // Standard 11-bit ID -> bits 31:21
+	    sFilterConfig.FilterIdHigh     = (filter.filter_id << 5) & 0xFFFF;
+	    sFilterConfig.FilterIdLow      = 0;
+	    sFilterConfig.FilterMaskIdHigh = (filter.filter_mask << 5) & 0xFFFF;
+	    sFilterConfig.FilterMaskIdLow  = 0;
+	} else {
+	    // Extended 29-bit ID -> bits 31:3
+	    sFilterConfig.FilterIdHigh     = (filter.filter_id >> 13) & 0xFFFF;
+	    sFilterConfig.FilterIdLow      = (filter.filter_id << 3) & 0xFFF8;
+	    sFilterConfig.FilterMaskIdHigh = (filter.filter_mask >> 13) & 0xFFFF;
+	    sFilterConfig.FilterMaskIdLow  = (filter.filter_mask << 3) & 0xFFF8;
+
+	    sFilterConfig.FilterIdLow     |= 0x04; // IDE = 1
+	    sFilterConfig.FilterMaskIdLow |= 0x04; // force IDE bit to match
 	}
 
+
 	configSem.Take();
 	int32_t lowestId = 0;// sFilterConfig.FilterFIFOAssignment == CAN_RX_FIFO0 ? 0 : slaveFilterStart;
 	int32_t highestId = slaveFilterStart;// sFilterConfig.FilterFIFOAssignment == CAN_RX_FIFO0 ? slaveFilterStart : 29;

Firmware/FFBoard/UserExtensions/Inc/FFBHIDMain.h

@@ -84,6 +84,8 @@ class FFBHIDMain: public FFBoardMain, public cpp_freertos::Thread, PersistentSto
 
 	float getCurFFBFreq();
 
+	void update() override;
+
 protected:
 	std::shared_ptr<EffectsControlItf> ffb;
 	std::shared_ptr<EffectsCalculator> effects_calc;

Firmware/FFBoard/UserExtensions/Inc/VescCAN.h

@@ -107,6 +107,8 @@ class VescCAN: public MotorDriver,
 	// Thread impl
 	void Run();
 
+	bool setCanFilter();
+
 private:
 
 	// Vesc interface and motor state
@@ -134,7 +136,8 @@ class VescCAN: public MotorDriver,
 	// CAN section
 
 	CANPort *port = &canport;
-	int32_t filterId = 0;
+	int32_t filterId = -1;
+	int32_t filterIdVesc = -1;
 	uint8_t OFFB_can_Id = 0x40; 			// Default OpenFFBoard CAN ID
 	uint8_t VESC_can_Id = 0xFF;				// Default VESC CAN id
 	uint8_t buffer_rx[BUFFER_RX_SIZE];		// Used to store multi-frame can message

Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp

@@ -371,6 +371,10 @@ void FFBHIDMain::errorCallback(const Error &error, bool cleared){
 	}
 }
 
+void FFBHIDMain::update(){
+	vTaskDelay(4); // Allow the idle task to run
+}
+
 #ifdef TIM_FFB
 void FFBHIDMain::timerElapsed(TIM_HandleTypeDef* htim){
 	if(htim == &TIM_FFB){

Firmware/FFBoard/UserExtensions/Src/FFBoardMain.cpp

@@ -69,7 +69,7 @@ void FFBoardMain::usbInit(){
  * Can be reimplemented by custom main classes but should not block
  */
 void FFBoardMain::update(){
-
+	vTaskDelay(1); // Wait at least one cycle if not overridden
 }
 
 /**

Firmware/FFBoard/UserExtensions/Src/VescCAN.cpp

@@ -61,13 +61,7 @@ VescCAN::VescCAN(uint8_t address) :
 	setAddress(address);
 	restoreFlash();
 
-	// Set up a filter to receive vesc commands
-	CAN_filter filterConf;
-	filterConf.extid = true;
-	filterConf.buffer = 0;
-	filterConf.filter_id = 0;
-	filterConf.filter_mask =  0;
-	this->filterId = this->port->addCanFilter(filterConf); // Receive all
+	setCanFilter();
 
 	if(port->getSpeedPreset() < 3){
 		port->setSpeedPreset(3); // Minimum 250k
@@ -83,6 +77,8 @@ VescCAN::~VescCAN() {
 	this->stopMotor();
 	this->port->freePort();
 	this->state = VescState::VESC_STATE_UNKNOWN;
+	this->port->removeCanFilter(filterId);
+	this->port->removeCanFilter(filterIdVesc);
 }
 
 void VescCAN::setAddress(uint8_t address) {
@@ -95,8 +91,40 @@ void VescCAN::setAddress(uint8_t address) {
 	}
 }
 
+bool VescCAN::setCanFilter(){
+	if(filterId != -1){
+		port->removeCanFilter(filterId);
+		filterId = -1;
+	}
+	// Set up a filter to receive vesc commands
+	CAN_filter filterConf;
+	filterConf.extid = true;
+	filterConf.buffer = 0;
+	filterConf.filter_id = this->OFFB_can_Id;
+	filterConf.filter_mask =  0xff;
+	this->filterId = this->port->addCanFilter(filterConf); // Receive messages for this id
+
+	if(filterIdVesc != -1){
+		filterIdVesc = -1;
+		port->removeCanFilter(filterIdVesc);
+	}
+
+	if(this->filterIdVesc == -1){
+		filterConf.extid = true;
+		filterConf.buffer = 0;
+		filterConf.filter_id = VESC_can_Id == 0xff ? ((uint8_t)VescCANMsg::CAN_PACKET_POLL_ROTOR_POS) << 8 : VESC_can_Id;
+		filterConf.filter_mask =  VESC_can_Id == 0xff ? 0xff00 : 0xff; // Filter pos update or vesc id
+		this->filterIdVesc = this->port->addCanFilter(filterConf);
+	}
+
+	return this->filterId != -1 && this->filterIdVesc != -1;
+}
+
 void VescCAN::turn(int16_t power) {
 	float torque = ((float) power / (float) 0x7fff);
+	if(fabs(torque-lastTorque) > 0.01){
+		pulseErrLed();
+	}
 	lastTorque = torque;
 	this->setTorque(torque);
 }
@@ -238,9 +266,17 @@ CommandStatus VescCAN::command(const ParsedCommand &cmd,
 	switch (static_cast<VescCAN_commands>(cmd.cmdId)) {
 
 	case VescCAN_commands::offbcanid:
-		return handleGetSet(cmd, replies, this->OFFB_can_Id);
+	{
+		CommandStatus status = handleGetSet(cmd, replies, this->OFFB_can_Id);
+		if(!setCanFilter()) status = CommandStatus::ERR;
+		return status;
+	}
 	case VescCAN_commands::vesccanid:
-		return handleGetSet(cmd, replies, this->VESC_can_Id);
+	{
+		CommandStatus status = handleGetSet(cmd, replies, this->VESC_can_Id);
+		if(!setCanFilter()) status = CommandStatus::ERR;
+		return status;
+	}
 	case VescCAN_commands::errorflags:
 		if (cmd.type == CMDtype::get)
 			replies.emplace_back(vescErrorFlag);
@@ -327,7 +363,7 @@ void VescCAN::Run() {
 		}
 
 		// if vesc is compatible, ready or in error => check the status
-		if (state >= VescState::VESC_STATE_COMPATIBLE) {
+		if (state >= VescState::VESC_STATE_COMPATIBLE && !port->isWaiting()) {
 			this->askGetValue();
 			// the return of askGetValue put the state in READY or ERROR is vesc respond to status
 		}
@@ -342,7 +378,7 @@ void VescCAN::Run() {
 		// when motor is active we call vesc each 500ms, to disable the vesc watchdog
 		// (if vesc not received message in 1sec, it cut off motor)
 		if (lastTorque != 0.0 && activeMotor
-				&& state == VescState::VESC_STATE_READY) {
+				&& state == VescState::VESC_STATE_READY && !port->isWaiting()) {
 			this->setTorque(lastTorque);
 		}
 
@@ -583,8 +619,9 @@ void VescCAN::canRxPendCallback(CANPort* port,CAN_rx_msg& msg) {
 			(this->VESC_can_Id != 0xFF) && // and the vescCanId is not the default one
 			(destCanID == this->VESC_can_Id); // we check that emiterId is the vescId for this axis
 
-	if (!messageIsForThisVescAxis)
+	if (!messageIsForThisVescAxis){
 		return;
+	}
 
 	// Process the CAN message received
 	switch (cmd) {