From 840488909838d76daf6ba679102e2f6d97517881 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 20 Apr 2018 17:12:12 -0400 Subject: [PATCH] delete unused ADCSIM - set BOARD_NUMBER_BRICKS to 0 for boards without analog power bricks --- platforms/posix/include/system_config.h | 6 +- posix-configs/SITL/README.md | 1 - posix-configs/SITL/init/ekf2/hippocampus | 1 - posix-configs/SITL/init/ekf2/iris | 1 - posix-configs/SITL/init/ekf2/iris_1 | 1 - posix-configs/SITL/init/ekf2/iris_2 | 1 - posix-configs/SITL/init/ekf2/iris_irlock | 1 - posix-configs/SITL/init/ekf2/iris_opt_flow | 1 - posix-configs/SITL/init/ekf2/iris_rplidar | 1 - posix-configs/SITL/init/ekf2/multiple_iris | 1 - posix-configs/SITL/init/ekf2/plane | 1 - posix-configs/SITL/init/ekf2/rover | 1 - posix-configs/SITL/init/ekf2/solo | 1 - posix-configs/SITL/init/ekf2/standard_vtol | 1 - posix-configs/SITL/init/ekf2/tailsitter | 1 - posix-configs/SITL/init/ekf2/tiltrotor | 1 - posix-configs/SITL/init/ekf2/typhoon_h480 | 1 - posix-configs/SITL/init/inav/iris | 1 - posix-configs/SITL/init/inav/iris_opt_flow | 1 - posix-configs/SITL/init/lpe/hippocampus | 1 - posix-configs/SITL/init/lpe/iris | 1 - posix-configs/SITL/init/lpe/iris_1 | 1 - posix-configs/SITL/init/lpe/iris_2 | 1 - posix-configs/SITL/init/lpe/iris_irlock | 1 - posix-configs/SITL/init/lpe/iris_opt_flow | 1 - posix-configs/SITL/init/lpe/iris_rplidar | 1 - posix-configs/SITL/init/lpe/plane | 1 - posix-configs/SITL/init/lpe/rover | 1 - posix-configs/SITL/init/lpe/solo | 1 - posix-configs/SITL/init/lpe/standard_vtol | 1 - posix-configs/SITL/init/lpe/typhoon_h480 | 1 - posix-configs/SITL/init/rcS_gazebo_delta_wing | 1 - posix-configs/SITL/init/test/cmd_template.in | 1 - .../SITL/init/test/tests_template.in | 1 - src/drivers/boards/bebop/board_config.h | 5 +- src/drivers/boards/common/board_common.h | 4 +- src/drivers/boards/eagle/board_config.h | 5 +- src/drivers/boards/rpi/board_config.h | 5 + src/drivers/boards/sitl/board_config.h | 2 + src/modules/sensors/sensors.cpp | 36 ++- .../posix/drivers/adcsim/CMakeLists.txt | 42 --- src/platforms/posix/drivers/adcsim/adcsim.cpp | 255 ------------------ 42 files changed, 43 insertions(+), 350 deletions(-) delete mode 100644 src/platforms/posix/drivers/adcsim/CMakeLists.txt delete mode 100644 src/platforms/posix/drivers/adcsim/adcsim.cpp diff --git a/platforms/posix/include/system_config.h b/platforms/posix/include/system_config.h index ceee48cc8c..111434ebce 100644 --- a/platforms/posix/include/system_config.h +++ b/platforms/posix/include/system_config.h @@ -39,12 +39,8 @@ #pragma once -#define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) -#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 -#define ADCSIM0_DEVICE_PATH "/dev/adc0" - #define SIM_FORMATED_UUID "000000010000000200000003" + #define PX4_CPU_UUID_BYTE_LENGTH 12 #define PX4_CPU_UUID_WORD32_LENGTH 3 #define PX4_CPU_MFGUID_BYTE_LENGTH PX4_CPU_UUID_BYTE_LENGTH diff --git a/posix-configs/SITL/README.md b/posix-configs/SITL/README.md index 0accf8401e..0813477bc1 100644 --- a/posix-configs/SITL/README.md +++ b/posix-configs/SITL/README.md @@ -74,7 +74,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start commander start sensors start ekf_att_pos_estimator start diff --git a/posix-configs/SITL/init/ekf2/hippocampus b/posix-configs/SITL/init/ekf2/hippocampus index ebb279197a..c6efc8cf25 100644 --- a/posix-configs/SITL/init/ekf2/hippocampus +++ b/posix-configs/SITL/init/ekf2/hippocampus @@ -19,7 +19,6 @@ mixer load /dev/pwm_output0 ROMFS/px4fmu_test/mixers/uuv_quad_x.mix gyrosim start accelsim start barosim start -adcsim start gpssim start sleep 1 diff --git a/posix-configs/SITL/init/ekf2/iris b/posix-configs/SITL/init/ekf2/iris index c1536f10b9..91ebb0b3b8 100644 --- a/posix-configs/SITL/init/ekf2/iris +++ b/posix-configs/SITL/init/ekf2/iris @@ -51,7 +51,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/ekf2/iris_1 b/posix-configs/SITL/init/ekf2/iris_1 index 6079d6fbc1..80887475d7 100644 --- a/posix-configs/SITL/init/ekf2/iris_1 +++ b/posix-configs/SITL/init/ekf2/iris_1 @@ -53,7 +53,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/ekf2/iris_2 b/posix-configs/SITL/init/ekf2/iris_2 index 73f56db89a..f9e52518f7 100644 --- a/posix-configs/SITL/init/ekf2/iris_2 +++ b/posix-configs/SITL/init/ekf2/iris_2 @@ -53,7 +53,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/ekf2/iris_irlock b/posix-configs/SITL/init/ekf2/iris_irlock index 1bed0e6d8b..7d02ce9d04 100644 --- a/posix-configs/SITL/init/ekf2/iris_irlock +++ b/posix-configs/SITL/init/ekf2/iris_irlock @@ -52,7 +52,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/ekf2/iris_opt_flow b/posix-configs/SITL/init/ekf2/iris_opt_flow index 6ef121cccc..b3bcd2f42f 100644 --- a/posix-configs/SITL/init/ekf2/iris_opt_flow +++ b/posix-configs/SITL/init/ekf2/iris_opt_flow @@ -54,7 +54,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/ekf2/iris_rplidar b/posix-configs/SITL/init/ekf2/iris_rplidar index c1536f10b9..91ebb0b3b8 100644 --- a/posix-configs/SITL/init/ekf2/iris_rplidar +++ b/posix-configs/SITL/init/ekf2/iris_rplidar @@ -51,7 +51,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/ekf2/multiple_iris b/posix-configs/SITL/init/ekf2/multiple_iris index dca2c392f7..7c972ea3aa 100644 --- a/posix-configs/SITL/init/ekf2/multiple_iris +++ b/posix-configs/SITL/init/ekf2/multiple_iris @@ -51,7 +51,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/ekf2/plane b/posix-configs/SITL/init/ekf2/plane index b95e17307a..cfffadbbef 100644 --- a/posix-configs/SITL/init/ekf2/plane +++ b/posix-configs/SITL/init/ekf2/plane @@ -73,7 +73,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start measairspeedsim start pwm_out_sim start diff --git a/posix-configs/SITL/init/ekf2/rover b/posix-configs/SITL/init/ekf2/rover index 2ed44cef1b..e08a77a664 100644 --- a/posix-configs/SITL/init/ekf2/rover +++ b/posix-configs/SITL/init/ekf2/rover @@ -54,7 +54,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start measairspeedsim start pwm_out_sim start diff --git a/posix-configs/SITL/init/ekf2/solo b/posix-configs/SITL/init/ekf2/solo index 96aaa30630..8589dbf733 100644 --- a/posix-configs/SITL/init/ekf2/solo +++ b/posix-configs/SITL/init/ekf2/solo @@ -48,7 +48,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/ekf2/standard_vtol b/posix-configs/SITL/init/ekf2/standard_vtol index e3717f8a37..244e40b167 100644 --- a/posix-configs/SITL/init/ekf2/standard_vtol +++ b/posix-configs/SITL/init/ekf2/standard_vtol @@ -66,7 +66,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start measairspeedsim start pwm_out_sim start start diff --git a/posix-configs/SITL/init/ekf2/tailsitter b/posix-configs/SITL/init/ekf2/tailsitter index f8edcc9ca9..70aca9a37c 100644 --- a/posix-configs/SITL/init/ekf2/tailsitter +++ b/posix-configs/SITL/init/ekf2/tailsitter @@ -49,7 +49,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start measairspeedsim start pwm_out_sim start diff --git a/posix-configs/SITL/init/ekf2/tiltrotor b/posix-configs/SITL/init/ekf2/tiltrotor index 568897823c..89270954dd 100644 --- a/posix-configs/SITL/init/ekf2/tiltrotor +++ b/posix-configs/SITL/init/ekf2/tiltrotor @@ -83,7 +83,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start measairspeedsim start pwm_out_sim start diff --git a/posix-configs/SITL/init/ekf2/typhoon_h480 b/posix-configs/SITL/init/ekf2/typhoon_h480 index d9750fa8d7..a72a2c1ae8 100644 --- a/posix-configs/SITL/init/ekf2/typhoon_h480 +++ b/posix-configs/SITL/init/ekf2/typhoon_h480 @@ -54,7 +54,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/inav/iris b/posix-configs/SITL/init/inav/iris index 6fdebe6c4d..6f5350d7b6 100644 --- a/posix-configs/SITL/init/inav/iris +++ b/posix-configs/SITL/init/inav/iris @@ -49,7 +49,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/inav/iris_opt_flow b/posix-configs/SITL/init/inav/iris_opt_flow index 78283a2e9b..fc3df8467b 100644 --- a/posix-configs/SITL/init/inav/iris_opt_flow +++ b/posix-configs/SITL/init/inav/iris_opt_flow @@ -51,7 +51,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/lpe/hippocampus b/posix-configs/SITL/init/lpe/hippocampus index 226c113262..216b893511 100644 --- a/posix-configs/SITL/init/lpe/hippocampus +++ b/posix-configs/SITL/init/lpe/hippocampus @@ -19,7 +19,6 @@ mixer load /dev/pwm_output0 ROMFS/px4fmu_test/mixers/uuv_quad_x.mix gyrosim start accelsim start barosim start -adcsim start sleep 1 diff --git a/posix-configs/SITL/init/lpe/iris b/posix-configs/SITL/init/lpe/iris index b2d270d7a6..9e61b8809e 100644 --- a/posix-configs/SITL/init/lpe/iris +++ b/posix-configs/SITL/init/lpe/iris @@ -51,7 +51,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/lpe/iris_1 b/posix-configs/SITL/init/lpe/iris_1 index 62420a3867..7ad0f521e6 100644 --- a/posix-configs/SITL/init/lpe/iris_1 +++ b/posix-configs/SITL/init/lpe/iris_1 @@ -53,7 +53,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/lpe/iris_2 b/posix-configs/SITL/init/lpe/iris_2 index 9c7516a2fc..79df05e7a0 100644 --- a/posix-configs/SITL/init/lpe/iris_2 +++ b/posix-configs/SITL/init/lpe/iris_2 @@ -53,7 +53,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/lpe/iris_irlock b/posix-configs/SITL/init/lpe/iris_irlock index b10a556f3b..0337d09b4f 100644 --- a/posix-configs/SITL/init/lpe/iris_irlock +++ b/posix-configs/SITL/init/lpe/iris_irlock @@ -59,7 +59,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/lpe/iris_opt_flow b/posix-configs/SITL/init/lpe/iris_opt_flow index 8986c0d394..7ab67dc568 100644 --- a/posix-configs/SITL/init/lpe/iris_opt_flow +++ b/posix-configs/SITL/init/lpe/iris_opt_flow @@ -69,7 +69,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/lpe/iris_rplidar b/posix-configs/SITL/init/lpe/iris_rplidar index e0d014944f..36cbc6034e 100644 --- a/posix-configs/SITL/init/lpe/iris_rplidar +++ b/posix-configs/SITL/init/lpe/iris_rplidar @@ -50,7 +50,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sleep 1 diff --git a/posix-configs/SITL/init/lpe/plane b/posix-configs/SITL/init/lpe/plane index 7ad438a7c0..88c8da9990 100644 --- a/posix-configs/SITL/init/lpe/plane +++ b/posix-configs/SITL/init/lpe/plane @@ -56,7 +56,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start measairspeedsim start pwm_out_sim start diff --git a/posix-configs/SITL/init/lpe/rover b/posix-configs/SITL/init/lpe/rover index 2fce364e7d..52baafb8d0 100644 --- a/posix-configs/SITL/init/lpe/rover +++ b/posix-configs/SITL/init/lpe/rover @@ -50,7 +50,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start measairspeedsim start pwm_out_sim start diff --git a/posix-configs/SITL/init/lpe/solo b/posix-configs/SITL/init/lpe/solo index 25f8f8ae97..54400fe78d 100644 --- a/posix-configs/SITL/init/lpe/solo +++ b/posix-configs/SITL/init/lpe/solo @@ -51,7 +51,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/lpe/standard_vtol b/posix-configs/SITL/init/lpe/standard_vtol index 537ae1ba6e..b702e34a83 100644 --- a/posix-configs/SITL/init/lpe/standard_vtol +++ b/posix-configs/SITL/init/lpe/standard_vtol @@ -60,7 +60,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start measairspeedsim start pwm_out_sim start diff --git a/posix-configs/SITL/init/lpe/typhoon_h480 b/posix-configs/SITL/init/lpe/typhoon_h480 index 150a5459b0..fcfba46226 100644 --- a/posix-configs/SITL/init/lpe/typhoon_h480 +++ b/posix-configs/SITL/init/lpe/typhoon_h480 @@ -48,7 +48,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start pwm_out_sim start sensors start diff --git a/posix-configs/SITL/init/rcS_gazebo_delta_wing b/posix-configs/SITL/init/rcS_gazebo_delta_wing index 43b82509ca..88f8e11c7a 100644 --- a/posix-configs/SITL/init/rcS_gazebo_delta_wing +++ b/posix-configs/SITL/init/rcS_gazebo_delta_wing @@ -32,7 +32,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start measairspeedsim start pwm_out_sim start diff --git a/posix-configs/SITL/init/test/cmd_template.in b/posix-configs/SITL/init/test/cmd_template.in index 13245ff565..c142ed6c1a 100644 --- a/posix-configs/SITL/init/test/cmd_template.in +++ b/posix-configs/SITL/init/test/cmd_template.in @@ -10,7 +10,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start measairspeedsim start pwm_out_sim start diff --git a/posix-configs/SITL/init/test/tests_template.in b/posix-configs/SITL/init/test/tests_template.in index b6fff53ca6..78319f5cbc 100644 --- a/posix-configs/SITL/init/test/tests_template.in +++ b/posix-configs/SITL/init/test/tests_template.in @@ -10,7 +10,6 @@ tone_alarm start gyrosim start accelsim start barosim start -adcsim start gpssim start measairspeedsim start pwm_out_sim start diff --git a/src/drivers/boards/bebop/board_config.h b/src/drivers/boards/bebop/board_config.h index c7aa337834..ccea1c6d5b 100644 --- a/src/drivers/boards/bebop/board_config.h +++ b/src/drivers/boards/bebop/board_config.h @@ -43,11 +43,12 @@ #define BOARD_OVERRIDE_MFGUID BOARD_OVERRIDE_UUID #define BOARD_NAME "BEBOP" -#define BOARD_BATTERY1_V_DIV (10.177939394f) -#define BOARD_BATTERY1_A_PER_V (15.391030303f) + #define BOARD_HAS_NO_RESET #define BOARD_HAS_NO_BOOTLOADER +#define BOARD_NUMBER_BRICKS 0 + /* * I2C busses */ diff --git a/src/drivers/boards/common/board_common.h b/src/drivers/boards/common/board_common.h index 822b2b11b1..28a78ef18b 100644 --- a/src/drivers/boards/common/board_common.h +++ b/src/drivers/boards/common/board_common.h @@ -170,7 +170,9 @@ # endif #endif -#if BOARD_NUMBER_BRICKS == 1 +#if BOARD_NUMBER_BRICKS == 0 +/* allow SITL to disable all bricks */ +#elif BOARD_NUMBER_BRICKS == 1 # define BOARD_BATT_V_LIST {ADC_BATTERY_VOLTAGE_CHANNEL} # define BOARD_BATT_I_LIST {ADC_BATTERY_CURRENT_CHANNEL} # define BOARD_BRICK_VALID_LIST {BOARD_ADC_BRICK_VALID} diff --git a/src/drivers/boards/eagle/board_config.h b/src/drivers/boards/eagle/board_config.h index e8ce5674d1..a75a3ee5e3 100644 --- a/src/drivers/boards/eagle/board_config.h +++ b/src/drivers/boards/eagle/board_config.h @@ -43,11 +43,12 @@ #define BOARD_OVERRIDE_MFGUID BOARD_OVERRIDE_UUID #define BOARD_NAME "EAGLE" -#define BOARD_BATTERY1_V_DIV (10.177939394f) -#define BOARD_BATTERY1_A_PER_V (15.391030303f) + #define BOARD_HAS_NO_RESET #define BOARD_HAS_NO_BOOTLOADER +#define BOARD_NUMBER_BRICKS 0 + #define CONFIG_ARCH_BOARD_SITL 1 /* diff --git a/src/drivers/boards/rpi/board_config.h b/src/drivers/boards/rpi/board_config.h index b167f80530..b044398be5 100644 --- a/src/drivers/boards/rpi/board_config.h +++ b/src/drivers/boards/rpi/board_config.h @@ -43,8 +43,13 @@ #define BOARD_OVERRIDE_MFGUID BOARD_OVERRIDE_UUID #define BOARD_NAME "RPI" + +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 + #define BOARD_BATTERY1_V_DIV (10.177939394f) #define BOARD_BATTERY1_A_PER_V (15.391030303f) + #define BOARD_HAS_NO_RESET #define BOARD_HAS_NO_BOOTLOADER diff --git a/src/drivers/boards/sitl/board_config.h b/src/drivers/boards/sitl/board_config.h index f7e74a882d..fbd19865fd 100644 --- a/src/drivers/boards/sitl/board_config.h +++ b/src/drivers/boards/sitl/board_config.h @@ -54,5 +54,7 @@ #define PX4_I2C_BUS_ONBOARD 2 #define PX4_NUMBER_I2C_BUSES 1 +#define BOARD_NUMBER_BRICKS 0 + #include #include "../common/board_common.h" diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8a932dbda1..8f8d36804e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -173,14 +173,18 @@ private: orb_advert_t _sensor_pub{nullptr}; /**< combined sensor data topic */ orb_advert_t _airdata_pub{nullptr}; /**< combined sensor data topic */ orb_advert_t _magnetometer_pub{nullptr}; /**< combined sensor data topic */ + +#if BOARD_NUMBER_BRICKS > 0 orb_advert_t _battery_pub[BOARD_NUMBER_BRICKS] {}; /**< battery status */ + Battery _battery[BOARD_NUMBER_BRICKS]; /**< Helper lib to publish battery_status topic. */ +#endif /* BOARD_NUMBER_BRICKS > 0 */ + #if BOARD_NUMBER_BRICKS > 1 int _battery_pub_intance0ndx {0}; /**< track the index of instance 0 */ -#endif +#endif /* BOARD_NUMBER_BRICKS > 1 */ orb_advert_t _airspeed_pub{nullptr}; /**< airspeed */ - orb_advert_t _diff_pres_pub{nullptr}; /**< differential_pressure */ orb_advert_t _sensor_preflight{nullptr}; /**< sensor preflight topic */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -189,9 +193,9 @@ private: #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL differential_pressure_s _diff_pres {}; -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - Battery _battery[BOARD_NUMBER_BRICKS]; /**< Helper lib to publish battery_status topic. */ + orb_advert_t _diff_pres_pub{nullptr}; /**< differential_pressure */ +#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ Parameters _parameters{}; /**< local copies of interesting parameters */ ParameterHandles _parameter_handles{}; /**< handles for interesting parameters */ @@ -249,9 +253,13 @@ Sensors::Sensors(bool hil_enabled) : _airspeed_validator.set_timeout(300000); _airspeed_validator.set_equal_value_threshold(100); +#if BOARD_NUMBER_BRICKS > 0 + for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) { _battery[b].setParent(this); } + +#endif /* BOARD_NUMBER_BRICKS > 0 */ } int @@ -425,6 +433,7 @@ Sensors::adc_poll() /* read all channels available */ int ret = _h_adc.read(&buf_adc, sizeof(buf_adc)); +#if BOARD_NUMBER_BRICKS > 0 //todo:abosorb into new class Power /* For legacy support we publish the battery_status for the Battery that is @@ -433,19 +442,16 @@ Sensors::adc_poll() * Like in the FMUv4 */ - /* The ADC channela that are associated with each brick, in power controller + /* The ADC channels that are associated with each brick, in power controller * priority order highest to lowest, as defined by the board config. */ - int bat_voltage_v_chan[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST; int bat_voltage_i_chan[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST; /* The valid signals (HW dependent) are associated with each brick */ - bool valid_chan[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST; /* Per Brick readings with default unread channels at 0 */ - float bat_current_a[BOARD_NUMBER_BRICKS] = {0.0f}; float bat_voltage_v[BOARD_NUMBER_BRICKS] = {0.0f}; @@ -456,6 +462,8 @@ Sensors::adc_poll() int selected_source = -1; +#endif /* BOARD_NUMBER_BRICKS > 0 */ + if (ret >= (int)sizeof(buf_adc[0])) { /* Read add channels we got */ @@ -489,6 +497,9 @@ Sensors::adc_poll() } else #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ { + +#if BOARD_NUMBER_BRICKS > 0 + for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) { /* Once we have subscriptions, Do this once for the lowest (highest priority @@ -512,7 +523,7 @@ Sensors::adc_poll() _battery_pub_intance0ndx = selected_source; } -#endif +#endif /* BOARD_NUMBER_BRICKS > 1 */ } // todo:per brick scaling @@ -526,9 +537,13 @@ Sensors::adc_poll() - _parameters.battery_current_offset) * _parameters.battery_a_per_v; } } + +#endif /* BOARD_NUMBER_BRICKS > 0 */ } } +#if BOARD_NUMBER_BRICKS > 0 + if (_parameters.battery_source == 0) { for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) { @@ -557,8 +572,9 @@ Sensors::adc_poll() } } - _last_adc = t; +#endif /* BOARD_NUMBER_BRICKS > 0 */ + _last_adc = t; } } } diff --git a/src/platforms/posix/drivers/adcsim/CMakeLists.txt b/src/platforms/posix/drivers/adcsim/CMakeLists.txt deleted file mode 100644 index 52131f1446..0000000000 --- a/src/platforms/posix/drivers/adcsim/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ -px4_add_module( - MODULE platforms__posix__drivers__adcsim - MAIN adcsim - COMPILE_FLAGS - SRCS - adcsim.cpp - DEPENDS - git_driverframework - df_driver_framework -) diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp deleted file mode 100644 index f31ce495a7..0000000000 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ /dev/null @@ -1,255 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file adcsim.cpp - * - * Driver for the ADCSIM. - * - * This is a designed for simulating sampling things like voltages - * and so forth. - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include - -#include "SyncObj.hpp" -#include "VirtDevObj.hpp" - -using namespace DriverFramework; - -#define ADC_BASE_DEV_PATH "/dev/adc" - -class ADCSIM : public VirtDevObj -{ -public: - ADCSIM(uint32_t channels); - virtual ~ADCSIM(); - - virtual ssize_t devRead(void *buffer, size_t len); - -private: - perf_counter_t _sample_perf; - - unsigned _channel_count; - px4_adc_msg_t *_samples; /**< sample buffer */ - - /** worker function */ - virtual void _measure(); - - /** - * Sample a single channel and return the measured value. - * - * @param channel The channel to sample. - * @return The sampled value, or 0xffff if - * sampling failed. - */ - uint16_t _sample(unsigned channel); - - SyncObj m_lock; -}; - -ADCSIM::ADCSIM(uint32_t channels) : - VirtDevObj("adcsim", "/dev/adcsim", ADC_BASE_DEV_PATH, 10000), - _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), - _channel_count(0), - _samples(nullptr) -{ - /* always enable the temperature sensor */ - channels |= 1 << 16; - - /* allocate the sample array */ - for (unsigned i = 0; i < 32; i++) { - if (channels & (1 << i)) { - _channel_count++; - } - } - - _samples = new px4_adc_msg_t[_channel_count]; - - /* prefill the channel numbers in the sample array */ - if (_samples != nullptr) { - unsigned index = 0; - - for (unsigned i = 0; i < 32; i++) { - if (channels & (1 << i)) { - _samples[index].am_channel = i; - _samples[index].am_data = 0; - index++; - } - } - } -} - -ADCSIM::~ADCSIM() -{ - if (_samples != nullptr) { - delete _samples; - } -} - -ssize_t -ADCSIM::devRead(void *buffer, size_t len) -{ - const size_t maxsize = sizeof(px4_adc_msg_t) * _channel_count; - - if (len > maxsize) { - len = maxsize; - } - - /* block interrupts while copying samples to avoid racing with an update */ - m_lock.lock(); - memcpy(buffer, _samples, len); - m_lock.unlock(); - - return len; -} - -void -ADCSIM::_measure() -{ - m_lock.lock(); - - /* scan the channel set and sample each */ - for (unsigned i = 0; i < _channel_count; i++) { - _samples[i].am_data = _sample(_samples[i].am_channel); - } - - m_lock.unlock(); -} - -uint16_t -ADCSIM::_sample(unsigned channel) -{ - perf_begin(_sample_perf); - - uint16_t result = 1; - - perf_end(_sample_perf); - return result; -} - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int adcsim_main(int argc, char *argv[]); - -namespace -{ -ADCSIM *g_adc; - -int -test() -{ - DevHandle h; - DevMgr::getHandle(ADCSIM0_DEVICE_PATH, h); - - if (!h.isValid()) { - PX4_ERR("can't open ADCSIM device (%d)", h.getError()); - return 1; - } - - for (unsigned i = 0; i < 50; i++) { - px4_adc_msg_t data[PX4_MAX_ADC_CHANNELS]; - ssize_t count = h.read(data, sizeof(data)); - - if (count < 0) { - PX4_ERR("read error (%d)", h.getError()); - return 1; - } - - unsigned channels = count / sizeof(data[0]); - - for (unsigned j = 0; j < channels; j++) { - PX4_INFO("%d: %lu ", data[j].am_channel, (unsigned long)data[j].am_data); - } - - usleep(500000); - } - - DevMgr::releaseHandle(h); - return 0; -} -} - -int -adcsim_main(int argc, char *argv[]) -{ - int ret = 0; - - if (g_adc == nullptr) { - /* FIXME - this hardcodes the default channel set for SITL - should be configurable */ - g_adc = new ADCSIM((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); - - if (g_adc == nullptr) { - PX4_ERR("couldn't allocate the ADCSIM driver"); - return 1; - } - - ret = g_adc->init(); - - if (ret != 0) { - PX4_ERR("ADCSIM init failed (%d)", ret); - return 1; - } - } - - if (argc > 1 && g_adc) { - if (!strcmp(argv[1], "test")) { - ret = test(); - } - } - - return ret; -}