add setZero capability to AS5047U

Author: Richard Unger

src/encoders/as5047u/AS5047U.cpp

@@ -264,13 +264,39 @@ void AS5047U::enableUVW(bool enable){
 
 
 
+
+uint16_t AS5047U::getZero(){
+	uint16_t command = AS5047U_ZPOSM_REG | AS5047U_RW;
+	spi_transfer16(command);
+	command = AS5047U_ZPOSL_REG | AS5047U_RW;
+	uint16_t result = spi_transfer16(command);
+	AS5047UZPosL posL = {
+			.reg = nop()
+	};
+	return ((result&0x00FF)<<6) | posL.zposl;
+}
+
+
+
 uint16_t AS5047U::setZero(uint16_t value){
-	// TODO implement me!
-	return 0;
+	uint16_t command = AS5047U_ZPOSL_REG | AS5047U_RW;
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	command = AS5047U_ZPOSL_REG;
+	AS5047UZPosL posL = {
+			.reg = spi_transfer16(command)
+	};
+	posL.zposl = value&0x003F;
+	spi_transfer16(posL.reg);
+	command = AS5047U_ZPOSM_REG;
+	spi_transfer16(command);
+	spi_transfer16((value>>6)&0x00FF);
+	delayMicroseconds(50);
+	return getZero();
 }
 
 
 
+
 uint16_t AS5047U::nop(){
 	uint16_t result = spi_transfer16(0xFFFF); // using 0xFFFF as nop instead of 0x0000, then next call to fastAngle will return an angle
 	return result&AS5047U_RESULT_MASK;

src/encoders/as5047u/AS5047U.h

@@ -206,6 +206,7 @@ class AS5047U {
 	void enableUVW(bool enable);
 
 	uint16_t setZero(uint16_t);
+	uint16_t getZero();
 
 private: