diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index fccb056792..199b7fa0c8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -2,13 +2,24 @@ #include -#include "AP_InertialSensor.h" - #include #include +#include #include #include -#include + +#include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" +#include "AP_InertialSensor_Flymaple.h" +#include "AP_InertialSensor_HIL.h" +#include "AP_InertialSensor_L3G4200D.h" +#include "AP_InertialSensor_LSM9DS0.h" +#include "AP_InertialSensor_MPU6000.h" +#include "AP_InertialSensor_MPU9250.h" +#include "AP_InertialSensor_PX4.h" +#include "AP_InertialSensor_QURT.h" +#include "AP_InertialSensor_SITL.h" +#include "AP_InertialSensor_qflight.h" /* enable TIMING_DEBUG to track down scheduling issues with the main @@ -45,7 +56,7 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { // @Param: PRODUCT_ID // @DisplayName: IMU Product ID - // @Description: Which type of IMU is installed (read-only). + // @Description: Which type of IMU is installed (read-only). // @User: Advanced // @Values: 0:Unknown,1:APM1-1280,2:APM1-2560,88:APM2,3:SITL,4:PX4v1,5:PX4v2,256:Flymaple,257:Linux AP_GROUPINFO("PRODUCT_ID", 0, AP_InertialSensor, _product_id, 0), @@ -339,7 +350,7 @@ AP_InertialSensor::AP_InertialSensor() : AP_HAL::panic("Too many inertial sensors"); } _s_instance = this; - AP_Param::setup_object_defaults(this, var_info); + AP_Param::setup_object_defaults(this, var_info); for (uint8_t i=0; i radians(10) || + if (fabsf(trim_roll) > radians(10) || fabsf(trim_pitch) > radians(10)) { hal.console->println("trim over maximum of 10 degrees"); return false; @@ -682,7 +693,7 @@ bool AP_InertialSensor::calibrate_trim(float &trim_roll, float &trim_pitch) failed: _calibrating = false; - return false; + return false; } /* @@ -918,7 +929,7 @@ void AP_InertialSensor::update(void) } // adjust health status if a sensor has a non-zero error count - // but another sensor doesn't. + // but another sensor doesn't. bool have_zero_accel_error_count = false; bool have_zero_gyro_error_count = false; for (uint8_t i=0; i _next_sample_usec+400) { - timing_printf("longsleep %u wait_usec=%u\n", + timing_printf("longsleep %u wait_usec=%u\n", (unsigned)(now2-_next_sample_usec), (unsigned)wait_usec); } @@ -1056,7 +1067,7 @@ check_sample: if (counter++ == 400) { counter = 0; hal.console->printf("now=%lu _delta_time_sum=%lu diff=%ld\n", - (unsigned long)now, + (unsigned long)now, (unsigned long)delta_time_sum, (long)(now - delta_time_sum)); } @@ -1070,7 +1081,7 @@ check_sample: /* get delta angles */ -bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle) const +bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle) const { if (_delta_angle_valid[i]) { delta_angle = _delta_angle[i]; @@ -1284,7 +1295,7 @@ void AP_InertialSensor::acal_init() } // update accel calibrator -void AP_InertialSensor::acal_update() +void AP_InertialSensor::acal_update() { if(_acal == NULL) { return; @@ -1346,10 +1357,10 @@ void AP_InertialSensor::_acal_save_calibrations() /* no break */ } - if (fabsf(_trim_roll) > radians(10) || + if (fabsf(_trim_roll) > radians(10) || fabsf(_trim_pitch) > radians(10)) { hal.console->print("ERR: Trim over maximum of 10 degrees!!"); - _new_trim = false; //we have either got faulty level during acal or highly misaligned accelerometers + _new_trim = false; //we have either got faulty level during acal or highly misaligned accelerometers } _accel_cal_requires_reboot = true; @@ -1364,7 +1375,7 @@ void AP_InertialSensor::_acal_event_failure() } /* - Returns true if new valid trim values are available and passes them to reference vars + Returns true if new valid trim values are available and passes them to reference vars */ bool AP_InertialSensor::get_new_trim(float& trim_roll, float &trim_pitch) { @@ -1377,7 +1388,7 @@ bool AP_InertialSensor::get_new_trim(float& trim_roll, float &trim_pitch) return false; } -/* +/* Returns body fixed accelerometer level data averaged during accel calibration's first step */ bool AP_InertialSensor::get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vector3f& ret) const @@ -1390,7 +1401,7 @@ bool AP_InertialSensor::get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vec return true; } -/* +/* Returns Primary accelerometer level data averaged during accel calibration's first step */ bool AP_InertialSensor::get_primary_accel_cal_sample_avg(uint8_t sample_num, Vector3f& ret) const diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 11ff2cc0df..114f656c01 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -1,7 +1,5 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef __AP_INERTIAL_SENSOR_H__ -#define __AP_INERTIAL_SENSOR_H__ +#pragma once // Gyro and Accelerometer calibration criteria #define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE 4.0f @@ -20,12 +18,12 @@ #define INS_VIBRATION_CHECK_INSTANCES 2 #include + +#include #include #include -#include -#include "AP_InertialSensor_UserInteract.h" -#include #include +#include class AP_InertialSensor_Backend; class AuxiliaryBus; @@ -101,7 +99,7 @@ public: float get_delta_angle_dt(uint8_t i) const; float get_delta_angle_dt() const { return get_delta_angle_dt(_primary_accel); } - + //get delta velocity if available bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const; bool get_delta_velocity(Vector3f &delta_velocity) const { return get_delta_velocity(_primary_accel, delta_velocity); } @@ -176,7 +174,7 @@ public: // return the main loop delta_t in seconds float get_loop_delta_t(void) const { return _loop_delta_t; } - + uint16_t error_count(void) const { return 0; } bool healthy(void) const { return get_gyro_health() && get_accel_health(); } @@ -283,7 +281,7 @@ private: // the selected sample rate uint16_t _sample_rate; float _loop_delta_t; - + // Most recent accelerometer reading Vector3f _accel[INS_MAX_INSTANCES]; Vector3f _delta_velocity[INS_MAX_INSTANCES]; @@ -301,7 +299,7 @@ private: Vector3f _gyro_filtered[INS_MAX_INSTANCES]; bool _new_accel_data[INS_MAX_INSTANCES]; bool _new_gyro_data[INS_MAX_INSTANCES]; - + // Most recent gyro reading Vector3f _gyro[INS_MAX_INSTANCES]; Vector3f _delta_angle[INS_MAX_INSTANCES]; @@ -371,7 +369,7 @@ private: // target time for next wait_for_sample() return uint32_t _next_sample_usec; - + // time between samples in microseconds uint32_t _sample_period_usec; @@ -427,19 +425,3 @@ private: bool _accel_cal_requires_reboot; }; - -#include "AP_InertialSensor_Backend.h" -#include "AP_InertialSensor_MPU6000.h" -#include "AP_InertialSensor_PX4.h" -#include "AP_InertialSensor_MPU9250.h" -#include "AP_InertialSensor_L3G4200D.h" -#include "AP_InertialSensor_Flymaple.h" -#include "AP_InertialSensor_LSM9DS0.h" -#include "AP_InertialSensor_HIL.h" -#include "AP_InertialSensor_SITL.h" -#include "AP_InertialSensor_qflight.h" -#include "AP_InertialSensor_QURT.h" -#include "AP_InertialSensor_UserInteract_Stream.h" -#include "AP_InertialSensor_UserInteract_MAVLink.h" - -#endif // __AP_INERTIAL_SENSOR_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 083a51f7de..d902b545f7 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -21,10 +21,16 @@ Note that drivers can implement just gyros or just accels, and can also provide multiple gyro/accel instances. */ -#ifndef __AP_INERTIALSENSOR_BACKEND_H__ -#define __AP_INERTIALSENSOR_BACKEND_H__ +#pragma once + +#include + +#include + +#include "AP_InertialSensor.h" class AuxiliaryBus; +class DataFlash_Class; class AP_InertialSensor_Backend { @@ -36,7 +42,7 @@ public: // override with a custom destructor if need be. virtual ~AP_InertialSensor_Backend(void) {} - /* + /* * Update the sensor data. Called by the frontend to transfer * accumulated sensor readings to the frontend structure via the * _publish_gyro() and _publish_accel() functions @@ -129,8 +135,8 @@ protected: uint16_t get_sample_rate_hz(void) const; // access to frontend dataflash - DataFlash_Class *get_dataflash(void) const { - return _imu._log_raw_data? _imu._dataflash : NULL; + DataFlash_Class *get_dataflash(void) const { + return _imu._log_raw_data? _imu._dataflash : NULL; } // common gyro update function for all backends @@ -138,14 +144,12 @@ protected: // common accel update function for all backends void update_accel(uint8_t instance); - + // support for updating filter at runtime int8_t _last_accel_filter_hz[INS_MAX_INSTANCES]; int8_t _last_gyro_filter_hz[INS_MAX_INSTANCES]; - + // note that each backend is also expected to have a static detect() // function which instantiates an instance of the backend sensor // driver if the sensor is available }; - -#endif // __AP_INERTIALSENSOR_BACKEND_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h index 43e590b77a..c166701e41 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h @@ -1,15 +1,15 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef __AP_INERTIAL_SENSOR_FLYMAPLE_H__ -#define __AP_INERTIAL_SENSOR_FLYMAPLE_H__ +#pragma once #include #if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE -#include "AP_InertialSensor.h" #include #include +#include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" + class AP_InertialSensor_Flymaple : public AP_InertialSensor_Backend { public: @@ -34,4 +34,3 @@ private: uint32_t _last_accel_timestamp; }; #endif -#endif // __AP_INERTIAL_SENSOR_FLYMAPLE_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h index 6ab5e15331..f3f326d23d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h @@ -1,9 +1,8 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef __AP_INERTIALSENSOR_HIL_H__ -#define __AP_INERTIALSENSOR_HIL_H__ +#pragma once #include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" class AP_InertialSensor_HIL : public AP_InertialSensor_Backend { @@ -19,5 +18,3 @@ public: private: bool _init_sensor(void); }; - -#endif // __AP_INERTIALSENSOR_HIL_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h index 66043842c7..2fe991e16a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h @@ -4,12 +4,11 @@ #include #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX -#include - #include #include #include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" class AP_InertialSensor_L3G4200D : public AP_InertialSensor_Backend { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h index bc5938b34a..59dbc18925 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h @@ -1,11 +1,11 @@ - -#ifndef __AP_INERTIAL_SENSOR_LSM9DS0_H__ -#define __AP_INERTIAL_SENSOR_LSM9DS0_H__ +#pragma once #define LSM9DS0_DEBUG 0 #include + #include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" class AP_InertialSensor_LSM9DS0 : public AP_InertialSensor_Backend { @@ -90,5 +90,3 @@ private: void _dump_registers(); #endif }; - -#endif /* __AP_INERTIAL_SENSOR_LSM9DS0_H__ */ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index d75d2f62d6..618483987f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -12,6 +12,7 @@ #include #include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" #include "AuxiliaryBus.h" // enable debug to see a register dump on startup diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h index 2122b49ed2..3e6a3868bf 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h @@ -1,13 +1,14 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef __AP_INERTIAL_SENSOR_MPU9250_H__ -#define __AP_INERTIAL_SENSOR_MPU9250_H__ +#pragma once #include + #include #include #include #include + +#include "AP_InertialSensor_Backend.h" #include "AP_InertialSensor.h" #include "AuxiliaryBus.h" @@ -164,5 +165,3 @@ private: uint8_t _ext_sens_data = 0; }; - -#endif // __AP_INERTIAL_SENSOR_MPU9250_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h index 845c968e97..150a5fe1f6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h @@ -1,12 +1,11 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef __AP_INERTIAL_SENSOR_PX4_H__ -#define __AP_INERTIAL_SENSOR_PX4_H__ +#pragma once #include #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" #include #include #include @@ -70,5 +69,4 @@ private: float _accel_dt_max[INS_MAX_INSTANCES]; #endif // AP_INERTIALSENSOR_PX4_DEBUG }; -#endif // CONFIG_HAL_BOARD -#endif // __AP_INERTIAL_SENSOR_PX4_H__ +#endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_QURT.h b/libraries/AP_InertialSensor/AP_InertialSensor_QURT.h index 8cc7874ef6..bec9b2896f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_QURT.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_QURT.h @@ -1,12 +1,15 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - #pragma once -#include "AP_InertialSensor.h" +#include #if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +#include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" + extern "C" { #undef DEG_TO_RAD -#include "mpu9x50.h" +#include } #include @@ -20,23 +23,23 @@ public: /* update accel and gyro state */ bool update() override; void accumulate(void) override; - + // detect the sensor static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); void data_ready(void); - + private: bool init_sensor(); void init_mpu9250(); uint64_t last_timestamp; uint32_t start_time_ms; - + uint8_t gyro_instance; uint8_t accel_instance; ObjectBuffer buf{100}; }; -#endif // CONFIG_HAL_BOARD +#endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h index 45304c3c6e..75fefc61f9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h @@ -1,10 +1,10 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#pragma once -#ifndef __AP_INERTIALSENSOR_SITL_H__ -#define __AP_INERTIALSENSOR_SITL_H__ +#include #include "AP_InertialSensor.h" -#include +#include "AP_InertialSensor_Backend.h" #define INS_SITL_INSTANCES 2 @@ -24,11 +24,9 @@ private: void timer_update(); float rand_float(void); float gyro_drift(void); - + SITL::SITL *sitl; uint8_t gyro_instance[INS_SITL_INSTANCES]; uint8_t accel_instance[INS_SITL_INSTANCES]; }; - -#endif // __AP_INERTIALSENSOR_SITL_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h index 5e54541b15..b2465fc3b9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h @@ -1,6 +1,4 @@ - -#ifndef __AP_INERTIAL_SENSOR_USER_INTERACT_H__ -#define __AP_INERTIAL_SENSOR_USER_INTERACT_H__ +#pragma once #include @@ -10,6 +8,3 @@ public: virtual bool blocking_read() = 0; virtual void printf(const char *, ...) FMT_PRINTF(2, 3) = 0; }; - -#endif // __AP_INERTIAL_SENSOR_USER_INTERACT_H__ - diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h index 6adbc63ec4..468a0c889d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h @@ -1,13 +1,12 @@ - -#ifndef __AP_INERTIAL_SENSOR_USER_INTERACT_MAVLINK_H__ -#define __AP_INERTIAL_SENSOR_USER_INTERACT_MAVLINK_H__ - -#include "AP_InertialSensor_UserInteract.h" +#pragma once #include #include #include + +#include "AP_InertialSensor_UserInteract.h" + class GCS_MAVLINK; /** @@ -23,6 +22,3 @@ public: private: GCS_MAVLINK *_gcs; }; - -#endif // __AP_INERTIAL_SENSOR_USER_INTERACT_MAVLINK_H__ - diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h index 1face45944..01569eba56 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h @@ -1,16 +1,14 @@ - -#ifndef __AP_INERTIAL_SENSOR_USER_INTERACT_STREAM_H__ -#define __AP_INERTIAL_SENSOR_USER_INTERACT_STREAM_H__ +#pragma once #include #include + #include "AP_InertialSensor_UserInteract.h" /** * AP_InertialSensor_UserInteract, implemented in terms of a BetterStream. */ -class AP_InertialSensor_UserInteractStream : - public AP_InertialSensor_UserInteract { +class AP_InertialSensor_UserInteractStream : public AP_InertialSensor_UserInteract { public: AP_InertialSensor_UserInteractStream(AP_HAL::BetterStream *s) : _s(s) {} @@ -20,6 +18,3 @@ public: private: AP_HAL::BetterStream *_s; }; - -#endif // __AP_INERTIAL_SENSOR_USER_INTERACT_STREAM_H__ - diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_qflight.h b/libraries/AP_InertialSensor/AP_InertialSensor_qflight.h index 610f983f14..317c51bb23 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_qflight.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_qflight.h @@ -1,12 +1,14 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - #pragma once -#include "AP_InertialSensor.h" +#include #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT #include +#include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" + class AP_InertialSensor_QFLIGHT : public AP_InertialSensor_Backend { public: @@ -27,4 +29,4 @@ private: DSPBuffer::IMU *imubuf; }; -#endif // CONFIG_HAL_BOARD_SUBTYPE +#endif