i2c changed for motors

FlyPi/crazyflie-firmware-2021.06/src/modules/src/power_distribution_stock.c

@@ -35,7 +35,10 @@
 #include "motors.h"
 #include "debug.h"
 
+#include "i2c.h"
+
 static bool motorSetEnable = false;
+static uint8_t buffer[8];
 
 static struct {
   uint32_t m1;
@@ -75,10 +78,14 @@ bool powerDistributionTest(void)
 
 void powerStop()
 {
-  motorsSetRatio(MOTOR_M1, 0);
-  motorsSetRatio(MOTOR_M2, 0);
-  motorsSetRatio(MOTOR_M3, 0);
-  motorsSetRatio(MOTOR_M4, 0);
+  // motorsSetRatio(MOTOR_M1, 0);
+  // motorsSetRatio(MOTOR_M2, 0);
+  // motorsSetRatio(MOTOR_M3, 0);
+  // motorsSetRatio(MOTOR_M4, 0);
+  memset(&buffer, 0, 8);
+
+  i2c_send(0x4, buffer, 8);
+
 }
 
 void powerDistribution(const control_t *control)
@@ -103,10 +110,21 @@ void powerDistribution(const control_t *control)
 
   if (motorSetEnable)
   {
-    motorsSetRatio(MOTOR_M1, motorPowerSet.m1);
-    motorsSetRatio(MOTOR_M2, motorPowerSet.m2);
-    motorsSetRatio(MOTOR_M3, motorPowerSet.m3);
-    motorsSetRatio(MOTOR_M4, motorPowerSet.m4);
+    // motorsSetRatio(MOTOR_M1, motorPowerSet.m1);
+    // motorsSetRatio(MOTOR_M2, motorPowerSet.m2);
+    // motorsSetRatio(MOTOR_M3, motorPowerSet.m3);
+    // motorsSetRatio(MOTOR_M4, motorPowerSet.m4);
+
+    buffer[0] = motorPowerSet.m1 >> 8;
+    buffer[1] = motorPowerSet.m1 & 0xFF;
+    buffer[2] = motorPowerSet.m2 >> 8;
+    buffer[3] = motorPowerSet.m2 & 0xFF;
+    buffer[4] = motorPowerSet.m3 >> 8;
+    buffer[5] = motorPowerSet.m3 & 0xFF;
+    buffer[6] = motorPowerSet.m4 >> 8;
+    buffer[7] = motorPowerSet.m4 & 0xFF;
+
+    i2c_send(0x4, buffer, 8);
   }
   else
   {
@@ -123,10 +141,21 @@ void powerDistribution(const control_t *control)
       motorPower.m4 = idleThrust;
     }
 
-    motorsSetRatio(MOTOR_M1, motorPower.m1);
-    motorsSetRatio(MOTOR_M2, motorPower.m2);
-    motorsSetRatio(MOTOR_M3, motorPower.m3);
-    motorsSetRatio(MOTOR_M4, motorPower.m4);
+    // motorsSetRatio(MOTOR_M1, motorPower.m1);
+    // motorsSetRatio(MOTOR_M2, motorPower.m2);
+    // motorsSetRatio(MOTOR_M3, motorPower.m3);
+    // motorsSetRatio(MOTOR_M4, motorPower.m4);
+
+    buffer[0] = motorPowerSet.m1 >> 8;
+    buffer[1] = motorPowerSet.m1 & 0xFF;
+    buffer[2] = motorPowerSet.m2 >> 8;
+    buffer[3] = motorPowerSet.m2 & 0xFF;
+    buffer[4] = motorPowerSet.m3 >> 8;
+    buffer[5] = motorPowerSet.m3 & 0xFF;
+    buffer[6] = motorPowerSet.m4 >> 8;
+    buffer[7] = motorPowerSet.m4 & 0xFF;
+
+    i2c_send(0x4, buffer, 8);
   }
 }
 

FlyPi/crazyflie-firmware-2021.06/src/modules/src/system.c

@@ -71,6 +71,8 @@
 #include "cfassert.h"
 #include "i2cdev.h"
 
+#include "i2c.h"
+
 #ifndef START_DISARMED
 #define ARM_INIT true
 #else
@@ -132,6 +134,7 @@ void systemInit(void)
   storageInit();
   workerInit();
   adcInit();
+  i2c_init();
   ledseqInit();
   pmInit();
   buzzerInit();