diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 4ed272219dcbf6ba48627d0819c094e06e040c92..c91a8321d5c29ef6ede370b25527631cfc9128af 100644
--- a/include/darwin_registers.h
+++ b/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
diff --git a/include/imu.h b/include/imu.h
index 079304bf1b235660acb081ab1186a926366633c0..f3e1c3d203ca0ebe27e7a82ba3c569d4d5cddb11 100755
--- a/include/imu.h
+++ b/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);
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 10f053f73afb1aeef86c1b705e4e2f68b0feb327..641c39d8debe1b66a42cb213e7f7faff44104e63 100755
--- a/include/motion_manager.h
+++ b/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);
diff --git a/src/imu.c b/src/imu.c
index 3d3f0478946591baac10d9cfa0017ce762f82be9..e7925854577a476426157709ec5289cfbf2d830c 100755
--- a/src/imu.c
+++ b/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)
{
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 6e7768ab975482d88278925b992ee9e3de973a64..b6bb218c0f2ca6f328eddaa2d5a5401b8bcd01d5 100755
--- a/src/motion_manager.c
+++ b/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;