Added support to detect when the robot falls using the accelerometer.

include/darwin_registers.h

@@ -272,8 +272,8 @@ typedef enum {
   DARWIN_IMU_ACCEL_Z_H             = 0x0149,
 
   DARWIN_MM_NUM_SERVOS             = 0x014A,
-  DARWIN_MM_CNTRL                  = 0x014B, //   bit 7  | bit 6 | bit 5 | bit 4 | bit 3 |     bit 2    |      bit 1      |    bit 0
-                                             // scanning |       |       |       |       | Enable power | Enable balance  | Enable manager
+  DARWIN_MM_CNTRL                  = 0x014B, //   bit 7  |   bit 6  |   bit 5  | bit 4 | bit 3 |     bit 2    |      bit 1      |    bit 0
+                                             // scanning | fwd fall | bwd fall |       |       | Enable power | Enable balance  | Enable manager
   DARWIN_MM_MODULE_EN0             = 0x014C, //      bit 7      | bit 6 | bit 5 | bit 4 |      bit 3      | bit 2 | bit 1 | bit 0i
                                              // Enable servo 0  |   assigned module     | Enable servo 1  |   assigned module
                                              //                 |     000 -> none       |
@@ -627,6 +627,8 @@ typedef enum {
 #define      MANAGER_EN_BAL          0x02
 #define      MANAGER_EN_PWR          0x04
 #define      MANAGER_SCANNING        0x80
+#define      MANAGER_FWD_FALL        0x40
+#define      MANAGER_BWD_FALL        0x20
 #define      MANAGER_EVEN_SER_EN     0x80
 #define      MANAGER_EVEN_SER_MOD    0x70
 #define      MANAGER_ODD_SER_EN      0x08

include/imu.h

@@ -14,6 +14,7 @@ void imu_stop(void);
 void imu_set_calibration_samples(uint16_t num_samples);
 void imu_start_gyro_cal(void);
 void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z);
+void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z);
 uint8_t imu_is_gyro_calibrated(void);
 // operation functions
 uint8_t imu_in_range(unsigned short int address,unsigned short int length);

include/motion_manager.h

@@ -38,6 +38,10 @@ typedef enum {MM_NONE          = 0,
               MM_JOINTS        = 3,
               MM_HEAD          = 4} TModules;
 
+typedef enum {MM_FWD_FALL      =0,
+              MM_BWD_FALL      =1, 
+              MM_STANDING      =2} TFall;
+
 typedef struct
 {
   void (*process_fnct)(void);
@@ -86,6 +90,8 @@ inline void manager_disable(void);
 inline uint8_t manager_is_enabled(void);
 void manager_enable_balance(void);
 void manager_disable_balance(void);
+inline uint8_t manager_has_fallen(void);
+TFall manager_get_fallen_position(void);
 inline uint8_t manager_get_num_servos(void);
 void manager_set_module(uint8_t servo_id,TModules module);
 inline TModules manager_get_module(uint8_t servo_id);

src/imu.c

@@ -31,11 +31,11 @@
 // accelerometer registers
 #define        ACCEL_ID               0x32
 #define        ACCEL_WHO_AM_I         0x0F
-#define        ACCEL_CNTRL_REG1        0x20
-#define        ACCEL_CNTRL_REG2        0x21
-#define        ACCEL_CNTRL_REG3        0x22
-#define        ACCEL_CNTRL_REG4        0x23
-#define        ACCEL_CNTRL_REG5        0x24
+#define        ACCEL_CNTRL_REG1       0x20
+#define        ACCEL_CNTRL_REG2       0x21
+#define        ACCEL_CNTRL_REG3       0x22
+#define        ACCEL_CNTRL_REG4       0x23
+#define        ACCEL_CNTRL_REG5       0x24
 #define        ACCEL_HP_FILTER_RESET  0x25
 #define        ACCEL_REFERENCE        0x26
 #define        ACCEL_STATUS_REG       0x27
@@ -58,11 +58,11 @@
 // gyroscope registers
 #define        GYRO_ID                0xD3
 #define        GYRO_WHO_AM_I          0x0F
-#define        GYRO_CNTRL_REG1         0x20
-#define        GYRO_CNTRL_REG2         0x21
-#define        GYRO_CNTRL_REG3         0x22
-#define        GYRO_CNTRL_REG4         0x23
-#define        GYRO_CNTRL_REG5         0x24
+#define        GYRO_CNTRL_REG1        0x20
+#define        GYRO_CNTRL_REG2        0x21
+#define        GYRO_CNTRL_REG3        0x22
+#define        GYRO_CNTRL_REG4        0x23
+#define        GYRO_CNTRL_REG5        0x24
 #define        GYRO_REFERENCE         0x25
 #define        GYRO_OUT_TEMP          0x26
 #define        GYRO_STATUS_REG        0x27
@@ -72,7 +72,7 @@
 #define        GYRO_OUT_Y_H           0x2B
 #define        GYRO_OUT_Z_L           0x2C
 #define        GYRO_OUT_Z_H           0x2D
-#define        GYRO_FIFO_CNTRL_REG     0x2E
+#define        GYRO_FIFO_CNTRL_REG    0x2E
 #define        GYRO_FIFO_SRC_REG      0x2F
 #define        GYRO_INT1_CFG          0x30
 #define        GYRO_INT1_SRC          0x31
@@ -110,6 +110,7 @@ uint16_t gyro_calibration_num_samples;
 const uint8_t accel_conf_reg=ACCEL_CNTRL_REG1;
 const uint8_t accel_conf_len=5;
 uint8_t accel_conf_data[5];
+int32_t accel_data[3];
 const uint8_t accel_data_reg=ACCEL_OUT_X_L;
 const uint8_t accel_data_len=6;
 
@@ -254,13 +255,12 @@ void imu_accel_get_data(void)
 void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out)
 {
   uint8_t i;
-  int16_t value;
 
   for(i=0;i<3;i++)
   {
-    value=(data_in[i*2]+(data_in[i*2+1]<<8));
-    data_out[i*2]=value%256;
-    data_out[i*2+1]=value/256;
+    accel_data[i]=(data_in[i*2]+(data_in[i*2+1]<<8));
+    data_out[i*2]=accel_data[i]%256;
+    data_out[i*2+1]=accel_data[i]/256;
   }
 }
 
@@ -603,6 +603,13 @@ void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z)
   *z=gyro_data[2];
 }
 
+void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z)
+{
+  *x=accel_data[0];
+  *y=accel_data[1];
+  *z=accel_data[2];
+}
+
 // operation functions
 uint8_t imu_in_range(unsigned short int address,unsigned short int length)
 {

src/motion_manager.c

@@ -12,6 +12,7 @@
 #include "smart_charger.h"
 #include "dyn_battery.h"
 #include "gpio.h"
+#include <stdlib.h>
 
 #define MANAGER_TIMER                   TIM5
 #define ENABLE_MANAGER_TIMER_CLK        __HAL_RCC_TIM5_CLK_ENABLE()
@@ -132,6 +133,7 @@ void manager_balance(void)
 {
   uint32_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain;
   int32_t gyro_x,gyro_y,gyro_z;
+  int32_t accel_x,accel_y,accel_z;
 
   if(manager_balance_enabled==0x01)// balance is enabled
   {
@@ -152,6 +154,26 @@ void manager_balance(void)
     manager_balance_offset[R_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
     manager_balance_offset[L_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
   }
+  // fall detection (using accelerometer)
+  imu_get_accel_data(&accel_x,&accel_y,&accel_z);
+  if(abs(accel_y)>abs(accel_z))
+  {
+    if(accel_y>0)
+    {
+      ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_FWD_FALL);
+      ram_data[DARWIN_MM_CNTRL]|=MANAGER_BWD_FALL;
+    }
+    else
+    {
+      ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_BWD_FALL);
+      ram_data[DARWIN_MM_CNTRL]|=MANAGER_FWD_FALL;
+    }
+  }
+  else
+  {
+    ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_FWD_FALL);
+    ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_BWD_FALL);
+  }
 }
 
 // interrupt handlers
@@ -509,6 +531,24 @@ void manager_disable_balance(void)
   ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_BAL);
 }
 
+uint8_t manager_has_fallen(void)
+{
+  if(ram_data[DARWIN_MM_CNTRL]&(MANAGER_FWD_FALL|MANAGER_BWD_FALL))
+    return 0x01;
+  else
+    return 0x00;
+}
+
+TFall manager_get_fallen_position(void)
+{
+  if(ram_data[DARWIN_MM_CNTRL]&MANAGER_FWD_FALL)
+    return MM_FWD_FALL;
+  else if(ram_data[DARWIN_MM_CNTRL]&MANAGER_BWD_FALL)
+    return MM_BWD_FALL;
+  else
+    return MM_STANDING;
+}
+
 inline uint8_t manager_get_num_servos(void)
 {
   return manager_num_servos;