AP_InertialSensor: fix code style problem, delete useless codes

libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp

@@ -13,22 +13,11 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 /*
-  This is an INS driver for the combination L3G4200D gyro and ADXL345 accelerometer.
-  This combination is available as a cheap 10DOF sensor on ebay
-
-  This sensor driver is an example only - it should not be used in any
-  serious autopilot as the latencies on I2C prevent good timing at
-  high sample rates. It is useful when doing an initial port of
-  ardupilot to a board where only I2C is available, and a cheap sensor
-  can be used.
-
-Datasheets:
-  ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
-  L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf
-*/
+  IMU driver for Robsense PhenixPro Devkit board including i3g4250d and iis328dq
+ */
 #include <AP_HAL/AP_HAL.h>
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ
 #include "AP_InertialSensor_RST.h"
 #include <AP_Math/AP_Math.h>
 
@@ -179,7 +168,8 @@ const extern AP_HAL::HAL &hal;
 #define GYRO_FIFO_CTRL_STREAM_TO_FIFO_MODE      (3<<5)
 #define GYRO_FIFO_CTRL_BYPASS_TO_STREAM_MODE    (1<<7)
 
-#define GYRO_DEFAULT_RATE               800 //data output frequency
+//data output frequency
+#define GYRO_DEFAULT_RATE               800
 #define GYRO_DEFAULT_RANGE_DPS          2000
 #define GYRO_DEFAULT_FILTER_FREQ        35
 #define GYRO_TEMP_OFFSET_CELSIUS        40
@@ -230,10 +220,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_RST::probe(AP_InertialSensor &imu,
  */
 bool AP_InertialSensor_RST::_init_gyro(void)
 {
+    uint8_t whoami;
 
-    _gyro_spi_sem = _dev_gyro->get_semaphore();
-
-    if (!_gyro_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    if (!_dev_gyro->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
         return false;
     }
 
@@ -242,10 +231,10 @@ bool AP_InertialSensor_RST::_init_gyro(void)
 
     _dev_gyro->set_speed(AP_HAL::Device::SPEED_HIGH);
 
-    _dev_gyro->read_registers(GYRO_WHO_AM_I, &_gyro_whoami, sizeof(_gyro_whoami));
-    if (_gyro_whoami != GYRO_WHO_I_AM) {
-        hal.console->printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)_gyro_whoami);
-        printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)_gyro_whoami);
+    _dev_gyro->read_registers(GYRO_WHO_AM_I, &whoami, sizeof(whoami));
+    if (whoami != GYRO_WHO_I_AM) {
+        hal.console->printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)whoami);
+        printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)whoami);
         goto fail_whoami;
     }
 
@@ -257,12 +246,19 @@ bool AP_InertialSensor_RST::_init_gyro(void)
     hal.scheduler->delay(100);
 
     _dev_gyro->write_register(GYRO_CTRL_REG1,
-                              GYRO_REG1_POWER_NORMAL | GYRO_REG1_Z_ENABLE | GYRO_REG1_X_ENABLE | GYRO_REG1_Y_ENABLE | RATE_800HZ_LP_50HZ);
+                              GYRO_REG1_POWER_NORMAL |
+                              GYRO_REG1_Z_ENABLE | GYRO_REG1_X_ENABLE | GYRO_REG1_Y_ENABLE |
+                              RATE_800HZ_LP_50HZ);
 
-	_dev_gyro->write_register(GYRO_CTRL_REG2, 0);		/* disable high-pass filters */
-	_dev_gyro->write_register(GYRO_CTRL_REG3, 0x0);        /* DRDY disable */
+    /* disable high-pass filters */
+    _dev_gyro->write_register(GYRO_CTRL_REG2, 0);
+
+    /* DRDY disable */
+    _dev_gyro->write_register(GYRO_CTRL_REG3, 0x0);
     _dev_gyro->write_register(GYRO_CTRL_REG4, RANGE_2000DPS);
-	_dev_gyro->write_register(GYRO_CTRL_REG5, GYRO_REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
+
+    /* disable wake-on-interrupt */
+    _dev_gyro->write_register(GYRO_CTRL_REG5, GYRO_REG5_FIFO_ENABLE);
 
     /* disable FIFO. This makes things simpler and ensures we
      * aren't getting stale data. It means we must run the hrt
@@ -273,15 +269,13 @@ bool AP_InertialSensor_RST::_init_gyro(void)
 
     hal.scheduler->delay(100);
 
-    _gyro_spi_sem->give();
+    _dev_gyro->get_semaphore()->give();
 
     return true;
 
 fail_whoami:
-    _gyro_spi_sem->give();
+    _dev_gyro->get_semaphore()->give();
     return false;
-
-
 }
 
 /*
@@ -289,9 +283,9 @@ fail_whoami:
  */
 bool AP_InertialSensor_RST::_init_accel(void)
 {
-    _accel_spi_sem = _dev_accel->get_semaphore();
+    uint8_t whoami;
 
-    if (!_accel_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    if (!_dev_accel->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
         return false;
     }
 
@@ -299,27 +293,34 @@ bool AP_InertialSensor_RST::_init_accel(void)
 
     _dev_accel->set_read_flag(0x80);
 
-    _dev_accel->read_registers(ACCEL_WHO_AM_I, &_accel_whoami, sizeof(_accel_whoami));
-    if (_accel_whoami != ACCEL_WHO_I_AM) {
-        hal.console->printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)_accel_whoami);
-        printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)_accel_whoami);
+    _dev_accel->read_registers(ACCEL_WHO_AM_I, &whoami, sizeof(whoami));
+    if (whoami != ACCEL_WHO_I_AM) {
+        hal.console->printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)whoami);
+        printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)whoami);
         goto fail_whoami;
     }
 
     _dev_accel->write_register(ACCEL_CTRL_REG1,
-                               ACCEL_REG1_POWER_NORMAL | ACCEL_REG1_Z_ENABLE | ACCEL_REG1_Y_ENABLE | ACCEL_REG1_X_ENABLE | RATE_1000HZ_LP_780HZ);
-	_dev_accel->write_register(ACCEL_CTRL_REG2, 0);		/* disable high-pass filters */
-	_dev_accel->write_register(ACCEL_CTRL_REG3, 0x02);        /* DRDY enable */
-	_dev_accel->write_register(ACCEL_CTRL_REG4, ACCEL_REG4_BDU | ACCEL_REG4_FULL_SCALE_8G);
+                               ACCEL_REG1_POWER_NORMAL |
+                               ACCEL_REG1_Z_ENABLE | ACCEL_REG1_Y_ENABLE | ACCEL_REG1_X_ENABLE |
+                               RATE_1000HZ_LP_780HZ);
+
+    /* disable high-pass filters */
+    _dev_accel->write_register(ACCEL_CTRL_REG2, 0);
+
+    /* DRDY enable */
+    _dev_accel->write_register(ACCEL_CTRL_REG3, 0x02);
+    _dev_accel->write_register(ACCEL_CTRL_REG4,
+                               ACCEL_REG4_BDU | ACCEL_REG4_FULL_SCALE_8G);
 
     _accel_scale = 0.244e-3f * ACCEL_ONE_G;
 
-    _accel_spi_sem->give();
+    _dev_accel->get_semaphore()->give();
 
     return true;
 
 fail_whoami:
-    _accel_spi_sem->give();
+    _dev_accel->get_semaphore()->give();
     return false;
 
 
@@ -327,8 +328,7 @@ fail_whoami:
 
 bool AP_InertialSensor_RST::_init_sensor(void)
 {
-    if(!_init_gyro() || !_init_accel())
-    {
+    if (!_init_gyro() || !_init_accel()) {
         return false;
     }
 
@@ -368,34 +368,19 @@ void AP_InertialSensor_RST::gyro_measure(void)
     Vector3f gyro;
     uint8_t status = 0;
     int16_t raw_data[3];
-    static uint64_t start = 0;
-    uint64_t now = 0;
-    static int count = 0;
 
     _dev_gyro->read_registers(GYRO_STATUS_REG, &status, sizeof(status));
 
-    if((status & GYRO_STATUS_ZYXDA) == 0)
+    if ((status & GYRO_STATUS_ZYXDA) == 0) {
         return;
-
-    count++;
-
-    now = AP_HAL::micros64();
-
-    if(now - start >= 1000000)
-    {
-        //printf("gyro count = %d\n", count);
-        count = 0;
-        start = now;
     }
 
-    if (_dev_gyro->read_registers(GYRO_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) 
-    {
+    if (_dev_gyro->read_registers(GYRO_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) {
         gyro = Vector3f(raw_data[0], raw_data[1], raw_data[2]);
         gyro *= _gyro_scale;
         _rotate_and_correct_gyro(_gyro_instance, gyro);
         _notify_new_gyro_raw_sample(_gyro_instance, gyro);
     }
-
 }
 
 // Accumulate values from accels
@@ -404,28 +389,14 @@ void AP_InertialSensor_RST::accel_measure(void)
     Vector3f accel;
     uint8_t status = 0;
     int16_t raw_data[3];
-    static uint64_t start = 0;
-    uint64_t now = 0;
-    static int count = 0;
 
     _dev_accel->read_registers(ACCEL_STATUS_REG, &status, sizeof(status));
 
-    if((status & ACCEL_STATUS_ZYXDA) == 0)
+    if ((status & ACCEL_STATUS_ZYXDA) == 0) {
         return;
-
-    count++;
-
-    now = AP_HAL::micros64();
-
-    if(now - start >= 1000000)
-    {
-        //printf("accel count = %d\n", count);
-        count = 0;
-        start = now;
     }
 
-    if (_dev_accel->read_registers(ACCEL_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) 
-    {
+    if (_dev_accel->read_registers(ACCEL_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data))) {
         accel = Vector3f(raw_data[0], raw_data[1], raw_data[2]);
         accel *= _accel_scale;
         _rotate_and_correct_accel(_accel_instance, accel);

libraries/AP_InertialSensor/AP_InertialSensor_RST.h

@@ -43,13 +43,6 @@ private:
 
     AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev_gyro;//i3g4250d
     AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev_accel;//iis328dq
-    AP_HAL::Semaphore *_gyro_spi_sem;
-    AP_HAL::Semaphore *_accel_spi_sem;
-
-    // gyro whoami
-    uint8_t _gyro_whoami;
-    // accel whoami
-    uint8_t _accel_whoami;
 
     float _gyro_scale;
     float _accel_scale;