FIX mege to dev

Author: askuric

examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino

@@ -10,9 +10,9 @@
 #include <SimpleFOC.h>
 
 // magnetic sensor instance - SPI
-MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
+MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
 // magnetic sensor instance - MagneticSensorI2C
-//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4);
+//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
 // magnetic sensor instance - analog output
 // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
 

examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino

@@ -9,9 +9,9 @@
 #include <SimpleFOC.h>
 
 // magnetic sensor instance - SPI
-// MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
+MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
 // magnetic sensor instance - I2C
-MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4);
+//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
 // magnetic sensor instance - analog output
 // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
 

examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino

@@ -12,11 +12,10 @@
  */
 #include <SimpleFOC.h>
 
-// magnetic sensor instance
-MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
-// magnetic sensor instance
-//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4);
-// magnetic sensor instance - analog output
+// magnetic sensor instance - SPI
+MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
+// magnetic sensor instance - MagneticSensorI2C
+//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
 // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
 
 // BLDC motor & driver instance

examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino

@@ -38,10 +38,10 @@
  */
 #include <SimpleFOC.h>
 
-// SPI magnetic sensor instance
-// MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
-// I2C magnetic sensor instance
-MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4);
+// magnetic sensor instance - SPI
+MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
+// magnetic sensor instance - MagneticSensorI2C
+//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
 // magnetic sensor instance - analog output
 // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
 

examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_dual_bus_example/magnetic_sensor_i2c_dual_bus_example.ino

@@ -0,0 +1,46 @@
+#include <SimpleFOC.h>
+
+/** Annoyingly some i2c sensors (e.g. AS5600) have a fixed chip address.  This means only one of these devices can be addressed on a single bus
+ * This example shows how a second i2c bus can be used to communicate with a second sensor.  
+ */ 
+
+MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);
+MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
+
+#if defined(_STM32_DEF_) // if stm chips
+  // example of stm32 defining 2nd bus
+  TwoWire Wire1(PB11, PB10);
+
+#elif defined(ESP_H) // if esp32
+  // esp32 defines a Wire1 but doesn't define pins! 
+  // nothing to do here for esp32! (See below)
+#else
+  // Wire constructors vary - you'll have to check what works for your chip
+  TwoWire Wire1(SDA1, SCL1);
+#endif
+
+void setup() {
+
+  Serial.begin(115200);
+  _delay(750);
+
+  Wire.setClock(400000);
+  Wire1.setClock(400000);
+
+  #if defined(ESP_H) // if esp32
+    // Normally SimpeFOC will call begin for i2c but with esp32 begin() is the only way to set pins!
+    // It seems safe to call begin multiple times
+    Wire1.begin(19,23,400000);
+  #endif
+
+  sensor0.init();
+  sensor1.init(&Wire1);
+}
+
+void loop() {
+  _delay(200);
+  Serial.print(sensor0.getAngle()); 
+  Serial.print(" - "); 
+  Serial.print(sensor1.getAngle());
+  Serial.println();
+}

examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c_example/magnetic_sensor_i2c_example.ino

@@ -9,15 +9,19 @@
 // make sure to read the chip address and the chip angle register msb value from the datasheet
 // also in most cases you will need external pull-ups on SDA and SCL lines!!!!!
 //
-// For AS5058B use MagneticSensorI2C(0x40, 14, 0xFE, 8)
+// For AS5058B
+// MagneticSensorI2C sensor = MagneticSensorI2C(0x40, 14, 0xFE, 8);
+
 // Example of AS5600 configuration 
-MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4);
+MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
 
 
 void setup() {
   // monitoring port
   Serial.begin(115200);
 
+  // configure i2C
+  Wire.setClock(400000);
   // initialise magnetic sensor hardware
   sensor.init();
 

examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_spi_example/magnetic_sensor_spi_example.ino

@@ -1,10 +1,12 @@
 #include <SimpleFOC.h>
 
-// MagneticSensorSPI(int cs, float _cpr, int _angle_register)
-//  cs              - SPI chip select pin 
-//  bit_resolution  - sensor resolution
-//  angle_register   - (optional) angle read register - default 0x3FFF
-MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
+// MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs)
+//  config  - SPI config
+//  cs      - SPI chip select pin 
+// magnetic sensor instance - SPI
+MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
+// alternative constructor (chipselsect, bit_resolution, angle_read_register, )
+// MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
 
 void setup() {
   // monitoring port

src/sensors/MagneticSensorI2C.cpp

@@ -3,7 +3,6 @@
 /** Typical configuration for the 12bit AMS AS5600 magnetic sensor over I2C interface */
 MagneticSensorI2CConfig_s AS5600_I2C = {
   .chip_address = 0x36,
-  .clock_speed = 1000000,
   .bit_resolution = 12,
   .angle_register = 0x0E,
   .data_start_bit = 11
@@ -12,7 +11,6 @@ MagneticSensorI2CConfig_s AS5600_I2C = {
 /** Typical configuration for the 12bit AMS AS5048 magnetic sensor over I2C interface */
 MagneticSensorI2CConfig_s AS5048_I2C = {
   .chip_address = 0x40,  // highly configurable.  if A1 and A2 are held low, this is probable value
-  .clock_speed = 1000000,
   .bit_resolution = 14,
   .angle_register = 0xFE,
   .data_start_bit = 15
@@ -41,10 +39,7 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution,
   // extraction masks
   lsb_mask = (uint8_t)( (2 << lsb_used) - 1 );
   msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 );
-  clock_speed = 400000;
-  sda_pin = SDA;
-  scl_pin = SCL;
-
+  wire = &Wire;
 }
 
 MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){
@@ -60,29 +55,16 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){
   // extraction masks
   lsb_mask = (uint8_t)( (2 << lsb_used) - 1 );
   msb_mask = (uint8_t)( (2 << bits_used_msb) - 1 );
-  clock_speed = 400000;
-  sda_pin = SDA;
-  scl_pin = SCL;
-
+  wire = &Wire;
 }
 
-void MagneticSensorI2C::init(){
+void MagneticSensorI2C::init(TwoWire* _wire){
+
+  wire = _wire;
 
-#if defined(_STM32_DEF_) // if stm chips
   // I2C communication begin
-  Wire.begin();
-  Wire.setClock(clock_speed);
-  Wire.setSCL(scl_pin);
-  Wire.setSDA(sda_pin);
-#elif defined(ESP_H) // if esp32
-	//I2C communication begin
-	Wire.begin(sda_pin, scl_pin, clock_speed);
-#else
-  // I2C communication begin
-  Wire.begin();
-  Wire.setClock(clock_speed);
-#endif
-  
+  wire->begin();
+
 	// velocity calculation init
 	angle_prev = 0;
 	velocity_calc_timestamp = _micros(); 
@@ -187,14 +169,14 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) {
 	byte readArray[2];
 	uint16_t readValue = 0;
   // notify the device that is aboout to be read
-	Wire.beginTransmission(chip_address);
-	Wire.write(angle_reg_msb);
-  Wire.endTransmission(false);
+	wire->beginTransmission(chip_address);
+	wire->write(angle_reg_msb);
+  wire->endTransmission(false);
 
   // read the data msb and lsb
-	Wire.requestFrom(chip_address, (uint8_t)2);
+	wire->requestFrom(chip_address, (uint8_t)2);
 	for (byte i=0; i < 2; i++) {
-		readArray[i] = Wire.read();
+		readArray[i] = wire->read();
 	}
 
   // depending on the sensor architecture there are different combinations of 
@@ -205,3 +187,46 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) {
 	readValue += ( ( readArray[0] & msb_mask ) << lsb_used );
 	return readValue;
 }
+
+/*
+* Checks whether other devices have locked the bus. Can clear SDA locks.
+* This should be called before sensor.init() on devices that suffer i2c slaves locking sda 
+* e.g some stm32 boards with AS5600 chips
+* Takes the sda_pin and scl_pin
+* Returns 0 for OK, 1 for other master and 2 for unfixable sda locked LOW
+*/
+int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) {
+
+  pinMode(scl_pin, INPUT_PULLUP);
+  pinMode(sda_pin, INPUT_PULLUP);
+  delay(250);  
+
+  if (digitalRead(scl_pin) == LOW) {
+    // Someone else has claimed master!");
+    return 1;
+  }
+
+  if(digitalRead(sda_pin) == LOW) {
+    // slave is communicating and awaiting clocks, we are blocked
+    pinMode(scl_pin, OUTPUT);
+    for (byte i = 0; i < 16; i++) {
+      // toggle clock for 2 bytes of data
+      digitalWrite(scl_pin, LOW);
+      delayMicroseconds(20);
+      digitalWrite(scl_pin, HIGH);
+      delayMicroseconds(20);
+    }
+    pinMode(sda_pin, INPUT);
+    delayMicroseconds(20);
+    if (digitalRead(sda_pin) == LOW) { 
+      // SDA still blocked
+      return 2; 
+    } 
+    _delay(1000);
+  }
+  // SDA is clear (HIGH)
+  pinMode(sda_pin, INPUT);
+  pinMode(scl_pin, INPUT);
+  
+  return 0; 
+}

src/sensors/MagneticSensorI2C.h

@@ -9,7 +9,6 @@
 
 struct MagneticSensorI2CConfig_s  {
   int chip_address;
-  long clock_speed;
   int bit_resolution;
   int angle_register;
   int data_start_bit; 
@@ -37,7 +36,7 @@ class MagneticSensorI2C: public Sensor{
     static MagneticSensorI2C AS5600();
 
     /** sensor initialise pins */
-    void init();
+    void init(TwoWire* _wire = &Wire);
 
     // implementation of abstract functions of the Sensor class
     /** get current angle (rad) */
@@ -60,14 +59,8 @@ class MagneticSensorI2C: public Sensor{
 
     int needsAbsoluteZeroSearch() override;
 
-    /* the speed of the i2c clock signal */
-    long clock_speed;
-
-    /* the pin used for i2c data */
-    int sda_pin;
-    
-    /* the pin used for i2c clock */
-    int scl_pin;
+    /** experimental function to check and fix SDA locked LOW issues */
+    int checkBus(byte sda_pin = SDA, byte scl_pin = SCL);
 
   private:
     float cpr; //!< Maximum range of the magnetic sensor
@@ -98,6 +91,10 @@ class MagneticSensorI2C: public Sensor{
     float angle_prev; //!< angle in previous velocity calculation step
     long velocity_calc_timestamp; //!< last velocity calculation timestamp
 
+    /* the two wire instance for this sensor */
+    TwoWire* wire;
+
+
 };