diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp
index 7b4ea77e..81a12e07 100644
--- a/src/sensors/MagneticSensorI2C.cpp
+++ b/src/sensors/MagneticSensorI2C.cpp
@@ -22,68 +22,71 @@ MagneticSensorI2CConfig_s AS5048_I2C = {
 //  @param _bit_resolution  bit resolution of the sensor
 //  @param _angle_register_msb  angle read register
 //  @param _bits_used_msb number of used bits in msb
-MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb){
-  // chip I2C address
-  chip_address = _chip_address;
-  // angle read register of the magnetic sensor
-  angle_register_msb = _angle_register_msb;
-  // register maximum value (counts per revolution)
-  cpr = _powtwo(_bit_resolution);
-
-  // depending on the sensor architecture there are different combinations of
-  // LSB and MSB register used bits
-  // AS5600 uses 0..7 LSB and 8..11 MSB
-  // AS5048 uses 0..5 LSB and 6..13 MSB
-  // used bits in LSB
-  lsb_used = _bit_resolution - _bits_used_msb;
-  // extraction masks
-  lsb_mask = (uint8_t)( (2 << lsb_used) - 1 );
-  msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 );
-  wire = &Wire;
+MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb) {
+    // chip I2C address
+    chip_address = _chip_address;
+    // angle read register of the magnetic sensor
+    angle_register_msb = _angle_register_msb;
+    // register maximum value (counts per revolution)
+    cpr = _powtwo(_bit_resolution);
+
+    // depending on the sensor architecture there are different combinations of
+    // LSB and MSB register used bits
+    // AS5600 uses 0..7 LSB and 8..11 MSB
+    // AS5048 uses 0..5 LSB and 6..13 MSB
+    // used bits in LSB
+    lsb_used = _bit_resolution - _bits_used_msb;
+    // extraction masks
+    lsb_mask = (uint8_t)((2 << lsb_used) - 1);
+    msb_mask = (uint8_t)((2 << _bits_used_msb) - 1);
+    wire = &Wire;
 }
 
-MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){
-  chip_address = config.chip_address; 
+MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config) {
+    chip_address = config.chip_address;
 
-  // angle read register of the magnetic sensor
-  angle_register_msb = config.angle_register;
-  // register maximum value (counts per revolution)
-  cpr = _powtwo(config.bit_resolution);
+    // angle read register of the magnetic sensor
+    angle_register_msb = config.angle_register;
+    // register maximum value (counts per revolution)
+    cpr = _powtwo(config.bit_resolution);
 
-  int bits_used_msb = config.data_start_bit - 7;
-  lsb_used = config.bit_resolution - bits_used_msb;
-  // extraction masks
-  lsb_mask = (uint8_t)( (2 << lsb_used) - 1 );
-  msb_mask = (uint8_t)( (2 << bits_used_msb) - 1 );
-  wire = &Wire;
+    int bits_used_msb = config.data_start_bit - 7;
+    lsb_used = config.bit_resolution - bits_used_msb;
+    // extraction masks
+    lsb_mask = (uint8_t)((2 << lsb_used) - 1);
+    msb_mask = (uint8_t)((2 << bits_used_msb) - 1);
+    wire = &Wire;
 }
 
 MagneticSensorI2C MagneticSensorI2C::AS5600() {
-  return {AS5600_I2C};
+    return { AS5600_I2C };
 }
 
-void MagneticSensorI2C::init(TwoWire* _wire){
+void MagneticSensorI2C::init(TwoWire* _wire) {
 
-  wire = _wire;
+    //PSW Code 
+    PSWCodeAS5600I2C();
 
-  // I2C communication begin
-  wire->begin();
+    wire = _wire;
 
-  this->Sensor::init(); // call base class init
+    // I2C communication begin
+    wire->begin();
+
+    this->Sensor::init(); // call base class init
 }
 
 //  Shaft angle calculation
 //  angle is in radians [rad]
-float MagneticSensorI2C::getSensorAngle(){
-  // (number of full rotations)*2PI + current sensor angle 
-  return  ( getRawCount() / (float)cpr) * _2PI ;
+float MagneticSensorI2C::getSensorAngle() {
+    // (number of full rotations)*2PI + current sensor angle 
+    return  (getRawCount() / (float)cpr) * _2PI;
 }
 
 
 
 // function reading the raw counter of the magnetic sensor
-int MagneticSensorI2C::getRawCount(){
-	return (int)MagneticSensorI2C::read(angle_register_msb);
+int MagneticSensorI2C::getRawCount() {
+    return (int)MagneticSensorI2C::read(angle_register_msb);
 }
 
 // I2C functions
@@ -93,27 +96,27 @@ int MagneticSensorI2C::getRawCount(){
 * Returns the value of the register
 */
 int MagneticSensorI2C::read(uint8_t angle_reg_msb) {
-  // read the angle register first MSB then LSB
-	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);
-  currWireError = wire->endTransmission(false);
-
-  // read the data msb and lsb
-	wire->requestFrom(chip_address, (uint8_t)2);
-	for (byte i=0; i < 2; i++) {
-		readArray[i] = wire->read();
-	}
-
-  // depending on the sensor architecture there are different combinations of
-  // LSB and MSB register used bits
-  // AS5600 uses 0..7 LSB and 8..11 MSB
-  // AS5048 uses 0..5 LSB and 6..13 MSB
-  readValue = ( readArray[1] &  lsb_mask );
-	readValue += ( ( readArray[0] & msb_mask ) << lsb_used );
-	return readValue;
+    // read the angle register first MSB then LSB
+    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);
+    currWireError = wire->endTransmission(false);
+
+    // read the data msb and lsb
+    wire->requestFrom(chip_address, (uint8_t)2);
+    for (byte i = 0; i < 2; i++) {
+        readArray[i] = wire->read();
+    }
+
+    // depending on the sensor architecture there are different combinations of
+    // LSB and MSB register used bits
+    // AS5600 uses 0..7 LSB and 8..11 MSB
+    // AS5048 uses 0..5 LSB and 6..13 MSB
+    readValue = (readArray[1] & lsb_mask);
+    readValue += ((readArray[0] & msb_mask) << lsb_used);
+    return readValue;
 }
 
 /*
@@ -125,36 +128,48 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) {
 */
 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(scl_pin, INPUT_PULLUP);
+    pinMode(sda_pin, INPUT_PULLUP);
+    delay(250);
+
+    if (digitalRead(scl_pin) == LOW) {
+        // Someone else has claimed master!");
+        return 1;
     }
-    pinMode(sda_pin, INPUT);
-    delayMicroseconds(20);
+
     if (digitalRead(sda_pin) == LOW) {
-      // SDA still blocked
-      return 2;
+        // 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);
     }
-    _delay(1000);
-  }
-  // SDA is clear (HIGH)
-  pinMode(sda_pin, INPUT);
-  pinMode(scl_pin, INPUT);
+    // SDA is clear (HIGH)
+    pinMode(sda_pin, INPUT);
+    pinMode(scl_pin, INPUT);
+
+    return 0;
+}
 
-  return 0;
+
+//PSW Code
+static void MagneticSensorI2C::PSWCodeAS5600I2C() {
+
+    Wire.beginTransmission(0x36); // I2C address of AS5600
+    Wire.write(0x07); // Address of CONF register (0x07)
+    Wire.write(0x00); // MSB of CONF register (default value)
+    Wire.write(0x00); // LSB of CONF register (OUTS = 10 for PWM output, PWMF set for high frequency)
+    Wire.endTransmission();
 }
+
diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h
index f8b189fa..4e4ea82d 100644
--- a/src/sensors/MagneticSensorI2C.h
+++ b/src/sensors/MagneticSensorI2C.h
@@ -77,6 +77,9 @@ class MagneticSensorI2C: public Sensor{
     /* the two wire instance for this sensor */
     TwoWire* wire;
 
+    //PSW Code 
+    static void PswMagicCodeAS5600I2C();
+
 
 };