add function to configure run mode. switch gimbal ahrs i2c address

mrkcass · Oct 27, 2018 · 5acf317 · 5acf317
1 parent 1828fa9
commit 5acf317
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 4 deletions.
diff --git a/libhardware/src/ahrs.cpp b/libhardware/src/ahrs.cpp
@@ -44,6 +44,16 @@ void ahrs_info(ahrs_context ahrs)
  bno055_info(ahrs);
 }
 
+int ahrs_cfg_run_mode(ahrs_context ahrs, smx_byte mode)
+{
+ if (mode == AHRS_RUNMODE_IMU)
+ bno055_cfg_run_mode(ahrs, BNO_RUNMODE_ACCMAGGYRO);
+ else
+ bno055_cfg_run_mode(ahrs, BNO_RUNMODE_PANTILTROT);
+
+ return 0;
+}
+
 int ahrs_test(int output_format)
 {
  printf("starting test: \n");

diff --git a/libhardware/src/ahrs.h b/libhardware/src/ahrs.h
@@ -1,6 +1,8 @@
 #ifndef __ahrs_h__
 #define __ahrs_h__
 
+#include "somax.h"
+
 #define AHRS_NUM_DEVICES 2
 #define AHRS_ID_FRAME 1
 #define AHRS_ID_CAMD 2
@@ -18,6 +20,9 @@
 #define AHRS_OUTPUTFORMAT_QUATERNION 2
 #define AHRS_OUTPUTFORMAT_MAGNETOMETER 4
 
+#define AHRS_RUNMODE_AHRS 0
+#define AHRS_RUNMODE_IMU 1
+
 struct AHRS_CONTEXT;
 
 typedef int ahrs_id;
@@ -30,6 +35,7 @@ ahrs_context ahrs_open(ahrs_id id);
 void ahrs_close();
 void ahrs_info(ahrs_context ahrs);
 int ahrs_context_to_id(ahrs_context ahrs);
+int ahrs_cfg_run_mode(ahrs_context ahrs, smx_byte mode);
 int ahrs_test(int output_format);
 int ahrs_run(AHRS_EULER_CALLBACK euler_callbk, AHRS_QUATERNION_CALLBACK quaternion_callbk, AHRS_MAGNETOMETER_CALLBACK magnetometer_callbk);
 

diff --git a/libhardware/src/bno055.cpp b/libhardware/src/bno055.cpp
@@ -8,7 +8,7 @@
 #define BNO_CAMD_I2C_BUS 1
 #define BNO_CAMD_I2C_ADDRESS 0x29
 #define BNO_FRAME_I2C_BUS 0
-#define BNO_FRAME_I2C_ADDRESS 0x29
+#define BNO_FRAME_I2C_ADDRESS 0x28
 
 #define BNO_REG_PAGEID 0x00
 #define BNO_REG_CHIPID 0x00
@@ -192,6 +192,7 @@ struct BNO_CONTEXT
  AHRS_MAGNETOMETER_CALLBACK magnetometer_observer;
  int axis_map;
  int axis_sign;
+ smx_byte run_mode;
 };
 
 
@@ -274,6 +275,12 @@ void bno055_close()
 {
 }
 
+void bno055_cfg_run_mode(ahrs_context ahrs, smx_byte mode)
+{
+ BNO_CONTEXT *ctx = (BNO_CONTEXT *)ahrs;
+ ctx->run_mode = mode;
+}
+
 void bno055_configure_axis(ahrs_context ahrs, int axis, int mapped_axis, int mapped_sign)
 {
  BNO_CONTEXT* ctx = (BNO_CONTEXT*)ahrs;
@@ -498,7 +505,10 @@ static void bno055_run_configure_device(int dev_id)
  i2c_reg_write_byte(contexts[dev_id].i2c, BNO_REG_AXIS_MAP, contexts[dev_id].axis_map);
  i2c_reg_write_byte(contexts[dev_id].i2c, BNO_REG_AXIS_SIGN, contexts[dev_id].axis_sign);
 
- i2c_reg_write_byte(contexts[dev_id].i2c, BNO_REG_OPR_MODE, BNO_OPR_MODE_ACCMAGGYRO);
+ if (contexts[dev_id].run_mode == BNO_RUNMODE_ACCMAGGYRO)
+ i2c_reg_write_byte(contexts[dev_id].i2c, BNO_REG_OPR_MODE, BNO_OPR_MODE_ACCMAGGYRO);
+ else
+ i2c_reg_write_byte(contexts[dev_id].i2c, BNO_REG_OPR_MODE, BNO_OPR_MODE_NDOF);
 
  usleep(SWITCH_FROM_CONFIG_MS * 20 * U_MILLISECOND);
 

diff --git a/libhardware/src/bno055.h b/libhardware/src/bno055.h
@@ -1,6 +1,7 @@
 #ifndef __bno055_h__
 #define __bno055_h__
 
+#include "somax.h"
 #include "ahrs.h"
 
 #define BNO_NUM_DEVICES 2
@@ -13,12 +14,16 @@
 #define BNO_AXIS_Z 2
 
 #define BNO_SIGN_NEGATIVE -1
-#define BNO_SIGN_SAME 0
-#define BNO_SIGN_POSITIVE 1
+#define BNO_SIGN_SAME 0
+#define BNO_SIGN_POSITIVE 1
+
+#define BNO_RUNMODE_PANTILTROT 0
+#define BNO_RUNMODE_ACCMAGGYRO 1
 
 ahrs_context bno055_open(ahrs_id ahrs);
 void bno055_close();
 void bno055_configure_axis(ahrs_context ahrs, int axis, int mapped_axis, int mapped_sign);
+void bno055_cfg_run_mode(ahrs_context ahrs, smx_byte mode);
 int bno055_run();
 void bno055_info(ahrs_context ahrs);
 void bno055_output_callbk_euler(ahrs_context ahrs, AHRS_EULER_CALLBACK euler_callbk);