How to utilize custom sonar for altitude estimation?

#1

Hello everyone.
I am trying to utilize a custom made sonar sensor for altitude estimation and eventually for precision hovering.
This sonar gives PWM signal per range.
I was able to write driver for crazyflie and have been worked fine, but we decided to move on to px4 project.
Is there any relevant documentations writing driver for similar sensors? I only can find LIDAR with i2C types.
Attached file is the driver I wrote for this project.
HCSR04.c.pdf (35.7 KB)
Hope someone can help me finding solutions.

Kyu

#2

PX4 support the LidarLite distance sensor. It has both and I2C and a PWM interface. Have a look if this can help you. Usually when I need to write a new driver from scratch I start with the closest existing driver to what I need to implement and modify it from there.

Here is the Lidar Lite driver: https://github.com/PX4/Firmware/tree/master/src/drivers/distance_sensor/ll40ls

Probably many of your lower level functions you have in your driver will already be implemented.

Hope this helps you get started.

#3

you can find more info in the px4 dev guide: https://dev.px4.io/en/middleware/drivers.html#creating-a-driver

#4

Thanks for your comment.
I already have been looking at the ll40ls sensor as you mentioned.
Glad to know it is working without much modifications.
Now I am figuring out how to fuse this info for altitude hold and terrain following.

#5

you need to publish to the distance_sensor uorb topic like is done in the LidarLite driver. The modules that need this measurement with automatically subscribe to this topic and use it (ex. EKF2). If you want to use this distance sensor directly to control altitude then you have to set the EKF2_HGT_MODE parameter to “Range Sensor”.

#6

Instead for the terrain following you need to set the EKF2_HGT_MODE parameter to “barometric pressure” and set the MPC_ALT_MODE parameter to “Terrain following”. In this way the barometer will be used for the altitude hold and the distance sensor will be taken for the terrain following.

#7

I tried to use sonar with px4flow for position hold but I couldn’t.
I found that feature needs Local Position Estimator (LPE) running.
And when I tried to build with LPE module, the build fails.
Following is the cmake I tried.

	#
	# Estimation modules
	#
	modules/attitude_estimator_q
	#modules/ekf2
    modules/local_position_estimator
	#modules/position_estimator_inav
	#modules/landing_target_estimator
	#modules/wind_estimator

When tried to build I got following errors

   [517/569] Building CXX object src/modules/navigator/CMakeFiles/modules__navigator.dir/loiter.cpp.obj
FAILED: /home/dronedev/gcc-arm-none-eabi-7-2017-q4-major/bin/arm-none-eabi-g++   -DCONFIG_ARCH_BOARD_PX4FMU_V2 -DMODULE_NAME=\"navigator\" -DPX4_MAIN=navigator_app_main -D__DF_NUTTX -D__PX4_NUTTX -D__STDC_FORMAT_MACROS -isystem NuttX/nuttx/include/cxx -isystem NuttX/nuttx/include -I. -Isrc -Isrc/lib -Isrc/modules -I../../src -I../../src/drivers/boards/px4fmu-v2 -I../../src/include -I../../src/lib -I../../src/lib/DriverFramework/framework/include -I../../src/lib/matrix -I../../src/modules -I../../src/platforms -INuttX/nuttx/arch/arm/src/armv7-m -INuttX/nuttx/arch/arm/src/chip -INuttX/nuttx/arch/arm/src/common -INuttX/apps/include -Iexternal/Install/include -I../../src/lib/ecl -fno-common -ffunction-sections -fdata-sections -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard -fno-common -ffunction-sections -fdata-sections -g -fno-exceptions -fno-rtti -std=gnu++11 -fno-threadsafe-statics -DCONFIG_WCHAR_BUILTIN -D__CUSTOM_FILE_IO__ -fcheck-new -Wall -Wextra -Werror -Warray-bounds -Wdisabled-optimization -Wdouble-promotion -Wfatal-errors -Wfloat-equal -Wformat-security -Winit-self -Wlogical-op -Wmissing-declarations -Wmissing-field-initializers -Wpointer-arith -Wshadow -Wuninitialized -Wunknown-pragmas -Wunused-variable -Wno-implicit-fallthrough -Wno-missing-include-dirs -Wno-unused-parameter -Wunused-but-set-variable -Wformat=1 -Wno-missing-field-initializers -Wno-overloaded-virtual -Wreorder -Wno-format-truncation -fvisibility=hidden -include /home/dronedev/workspace/px4/Firmware/src/include/visibility.h -fno-strict-aliasing -fomit-frame-pointer -fno-math-errno -funsafe-math-optimizations -ffunction-sections -fdata-sections -fno-strength-reduce -fno-builtin-printf -Os -DNDEBUG   -fno-strict-aliasing -fomit-frame-pointer -fno-math-errno -funsafe-math-optimizations -ffunction-sections -fdata-sections -fno-strength-reduce -fno-builtin-printf -Wframe-larger-than=1300 -MMD -MT src/modules/navigator/CMakeFiles/modules__navigator.dir/navigator_main.cpp.obj -MF src/modules/navigator/CMakeFiles/modules__navigator.dir/navigator_main.cpp.obj.d -o src/modules/navigator/CMakeFiles/modules__navigator.dir/navigator_main.cpp.obj -c ../../src/modules/navigator/navigator_main.cpp
In file included from ../../src/platforms/px4_param.h:42:0,
                 from ../../src/platforms/px4_module_params.h:44,
                 from ../../src/modules/navigator/datalinkloss.h:42,
                 from ../../src/modules/navigator/navigator.h:44,
                 from ../../src/modules/navigator/navigator_main.cpp:46:
../../src/modules/navigator/navigator.h:354:28: error: 'VT_B_DEC_MSS' is not a member of 'px4::params'
   (ParamFloat<px4::params::VT_B_DEC_MSS>) _param_back_trans_dec_mss,
                            ^
../../src/platforms/px4_param_macros.h:239:18: note: in definition of macro 'REM'
 #define REM(...) __VA_ARGS__
                  ^~~~~~~~~~~
../../src/platforms/px4_param.h:58:40: note: in expansion of macro 'PAIR'
  do_not_explicitly_use_this_namespace::PAIR(x);
                                        ^~~~
../../src/platforms/px4_param_macros.h:55:150: note: in expansion of macro '_DEFINE_SINGLE_PARAMETER'
 #define APPLY12(t, aaa, aab, aac, aad, aae, aaf, aag, aah, aai, aaj, aak, aal) t(aaa) t(aab) t(aac) t(aad) t(aae) t(aaf) t(aag) t(aah) t(aai) t(aaj) t(aak) t(aal)
                                                                                                                                                      ^
../../src/platforms/px4_param_macros.h:197:33: note: in expansion of macro 'APPLY12'
 #define APPLY_ALL_H3(t, n, ...) APPLY##n(t, __VA_ARGS__)
                                 ^~~~~
../../src/platforms/px4_param_macros.h:198:33: note: in expansion of macro 'APPLY_ALL_H3'
 #define APPLY_ALL_H2(t, n, ...) APPLY_ALL_H3(t, n, __VA_ARGS__)
                                 ^~~~~~~~~~~~
../../src/platforms/px4_param_macros.h:199:27: note: in expansion of macro 'APPLY_ALL_H2'
 #define APPLY_ALL(t, ...) APPLY_ALL_H2(t, NUM_ARGS(__VA_ARGS__), __VA_ARGS__)
                           ^~~~~~~~~~~~
../../src/platforms/px4_param.h:79:2: note: in expansion of macro 'APPLY_ALL'
  APPLY_ALL(_DEFINE_SINGLE_PARAMETER, __VA_ARGS__) \
  ^~~~~~~~~
../../src/modules/navigator/navigator.h:336:2: note: in expansion of macro 'DEFINE_PARAMETERS'
  DEFINE_PARAMETERS(
  ^~~~~~~~~~~~~~~~~
compilation terminated due to -Wfatal-errors.
FAILED: /home/dronedev/gcc-arm-none-eabi-7-2017-q4-major/bin/arm-none-eabi-g++   -DCONFIG_ARCH_BOARD_PX4FMU_V2 -DMODULE_NAME=\"navigator\" -DPX4_MAIN=navigator_app_main -D__DF_NUTTX -D__PX4_NUTTX -D__STDC_FORMAT_MACROS -isystem NuttX/nuttx/include/cxx -isystem NuttX/nuttx/include -I. -Isrc -Isrc/lib -Isrc/modules -I../../src -I../../src/drivers/boards/px4fmu-v2 -I../../src/include -I../../src/lib -I../../src/lib/DriverFramework/framework/include -I../../src/lib/matrix -I../../src/modules -I../../src/platforms -INuttX/nuttx/arch/arm/src/armv7-m -INuttX/nuttx/arch/arm/src/chip -INuttX/nuttx/arch/arm/src/common -INuttX/apps/include -Iexternal/Install/include -I../../src/lib/ecl -fno-common -ffunction-sections -fdata-sections -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard -fno-common -ffunction-sections -fdata-sections -g -fno-exceptions -fno-rtti -std=gnu++11 -fno-threadsafe-statics -DCONFIG_WCHAR_BUILTIN -D__CUSTOM_FILE_IO__ -fcheck-new -Wall -Wextra -Werror -Warray-bounds -Wdisabled-optimization -Wdouble-promotion -Wfatal-errors -Wfloat-equal -Wformat-security -Winit-self -Wlogical-op -Wmissing-declarations -Wmissing-field-initializers -Wpointer-arith -Wshadow -Wuninitialized -Wunknown-pragmas -Wunused-variable -Wno-implicit-fallthrough -Wno-missing-include-dirs -Wno-unused-parameter -Wunused-but-set-variable -Wformat=1 -Wno-missing-field-initializers -Wno-overloaded-virtual -Wreorder -Wno-format-truncation -fvisibility=hidden -include /home/dronedev/workspace/px4/Firmware/src/include/visibility.h -fno-strict-aliasing -fomit-frame-pointer -fno-math-errno -funsafe-math-optimizations -ffunction-sections -fdata-sections -fno-strength-reduce -fno-builtin-printf -Os -DNDEBUG   -fno-strict-aliasing -fomit-frame-pointer -fno-math-errno -funsafe-math-optimizations -ffunction-sections -fdata-sections -fno-strength-reduce -fno-builtin-printf -Wframe-larger-than=1300 -MMD -MT src/modules/navigator/CMakeFiles/modules__navigator.dir/loiter.cpp.obj -MF src/modules/navigator/CMakeFiles/modules__navigator.dir/loiter.cpp.obj.d -o src/modules/navigator/CMakeFiles/modules__navigator.dir/loiter.cpp.obj -c ../../src/modules/navigator/loiter.cpp
In file included from ../../src/platforms/px4_param.h:42:0,
                 from ../../src/platforms/px4_module_params.h:44,
                 from ../../src/modules/navigator/loiter.h:46,
                 from ../../src/modules/navigator/loiter.cpp:42:
../../src/modules/navigator/navigator.h:354:28: error: 'VT_B_DEC_MSS' is not a member of 'px4::params'
   (ParamFloat<px4::params::VT_B_DEC_MSS>) _param_back_trans_dec_mss,
                            ^
../../src/platforms/px4_param_macros.h:239:18: note: in definition of macro 'REM'
 #define REM(...) __VA_ARGS__
                  ^~~~~~~~~~~
../../src/platforms/px4_param.h:58:40: note: in expansion of macro 'PAIR'
  do_not_explicitly_use_this_namespace::PAIR(x);
                                        ^~~~
../../src/platforms/px4_param_macros.h:55:150: note: in expansion of macro '_DEFINE_SINGLE_PARAMETER'
 #define APPLY12(t, aaa, aab, aac, aad, aae, aaf, aag, aah, aai, aaj, aak, aal) t(aaa) t(aab) t(aac) t(aad) t(aae) t(aaf) t(aag) t(aah) t(aai) t(aaj) t(aak) t(aal)
                                                                                                                                                      ^
../../src/platforms/px4_param_macros.h:197:33: note: in expansion of macro 'APPLY12'
 #define APPLY_ALL_H3(t, n, ...) APPLY##n(t, __VA_ARGS__)
                                 ^~~~~
../../src/platforms/px4_param_macros.h:198:33: note: in expansion of macro 'APPLY_ALL_H3'
 #define APPLY_ALL_H2(t, n, ...) APPLY_ALL_H3(t, n, __VA_ARGS__)
                                 ^~~~~~~~~~~~
../../src/platforms/px4_param_macros.h:199:27: note: in expansion of macro 'APPLY_ALL_H2'
 #define APPLY_ALL(t, ...) APPLY_ALL_H2(t, NUM_ARGS(__VA_ARGS__), __VA_ARGS__)
                           ^~~~~~~~~~~~
../../src/platforms/px4_param.h:79:2: note: in expansion of macro 'APPLY_ALL'
  APPLY_ALL(_DEFINE_SINGLE_PARAMETER, __VA_ARGS__) \
  ^~~~~~~~~
../../src/modules/navigator/navigator.h:336:2: note: in expansion of macro 'DEFINE_PARAMETERS'
  DEFINE_PARAMETERS(
  ^~~~~~~~~~~~~~~~~
compilation terminated due to -Wfatal-errors.
FAILED: /home/dronedev/gcc-arm-none-eabi-7-2017-q4-major/bin/arm-none-eabi-g++   -DCONFIG_ARCH_BOARD_PX4FMU_V2 -DMODULE_NAME=\"navigator\" -DPX4_MAIN=navigator_app_main -D__DF_NUTTX -D__PX4_NUTTX -D__STDC_FORMAT_MACROS -isystem NuttX/nuttx/include/cxx -isystem NuttX/nuttx/include -I. -Isrc -Isrc/lib -Isrc/modules -I../../src -I../../src/drivers/boards/px4fmu-v2 -I../../src/include -I../../src/lib -I../../src/lib/DriverFramework/framework/include -I../../src/lib/matrix -I../../src/modules -I../../src/platforms -INuttX/nuttx/arch/arm/src/armv7-m -INuttX/nuttx/arch/arm/src/chip -INuttX/nuttx/arch/arm/src/common -INuttX/apps/include -Iexternal/Install/include -I../../src/lib/ecl -fno-common -ffunction-sections -fdata-sections -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard -fno-common -ffunction-sections -fdata-sections -g -fno-exceptions -fno-rtti -std=gnu++11 -fno-threadsafe-statics -DCONFIG_WCHAR_BUILTIN -D__CUSTOM_FILE_IO__ -fcheck-new -Wall -Wextra -Werror -Warray-bounds -Wdisabled-optimization -Wdouble-promotion -Wfatal-errors -Wfloat-equal -Wformat-security -Winit-self -Wlogical-op -Wmissing-declarations -Wmissing-field-initializers -Wpointer-arith -Wshadow -Wuninitialized -Wunknown-pragmas -Wunused-variable -Wno-implicit-fallthrough -Wno-missing-include-dirs -Wno-unused-parameter -Wunused-but-set-variable -Wformat=1 -Wno-missing-field-initializers -Wno-overloaded-virtual -Wreorder -Wno-format-truncation -fvisibility=hidden -include /home/dronedev/workspace/px4/Firmware/src/include/visibility.h -fno-strict-aliasing -fomit-frame-pointer -fno-math-errno -funsafe-math-optimizations -ffunction-sections -fdata-sections -fno-strength-reduce -fno-builtin-printf -Os -DNDEBUG   -fno-strict-aliasing -fomit-frame-pointer -fno-math-errno -funsafe-math-optimizations -ffunction-sections -fdata-sections -fno-strength-reduce -fno-builtin-printf -Wframe-larger-than=1300 -MMD -MT src/modules/navigator/CMakeFiles/modules__navigator.dir/mission.cpp.obj -MF src/modules/navigator/CMakeFiles/modules__navigator.dir/mission.cpp.obj.d -o src/modules/navigator/CMakeFiles/modules__navigator.dir/mission.cpp.obj -c ../../src/modules/navigator/mission.cpp
In file included from ../../src/platforms/px4_param.h:42:0,
                 from ../../src/platforms/px4_module_params.h:44,
                 from ../../src/modules/navigator/mission.h:55,
                 from ../../src/modules/navigator/mission.cpp:48:
../../src/modules/navigator/navigator.h:354:28: error: 'VT_B_DEC_MSS' is not a member of 'px4::params'
   (ParamFloat<px4::params::VT_B_DEC_MSS>) _param_back_trans_dec_mss,
                            ^
../../src/platforms/px4_param_macros.h:239:18: note: in definition of macro 'REM'
 #define REM(...) __VA_ARGS__
                  ^~~~~~~~~~~
../../src/platforms/px4_param.h:58:40: note: in expansion of macro 'PAIR'
  do_not_explicitly_use_this_namespace::PAIR(x);
                                        ^~~~
../../src/platforms/px4_param_macros.h:55:150: note: in expansion of macro '_DEFINE_SINGLE_PARAMETER'
 #define APPLY12(t, aaa, aab, aac, aad, aae, aaf, aag, aah, aai, aaj, aak, aal) t(aaa) t(aab) t(aac) t(aad) t(aae) t(aaf) t(aag) t(aah) t(aai) t(aaj) t(aak) t(aal)
                                                                                                                                                      ^
../../src/platforms/px4_param_macros.h:197:33: note: in expansion of macro 'APPLY12'
 #define APPLY_ALL_H3(t, n, ...) APPLY##n(t, __VA_ARGS__)
                                 ^~~~~
../../src/platforms/px4_param_macros.h:198:33: note: in expansion of macro 'APPLY_ALL_H3'
 #define APPLY_ALL_H2(t, n, ...) APPLY_ALL_H3(t, n, __VA_ARGS__)
                                 ^~~~~~~~~~~~
../../src/platforms/px4_param_macros.h:199:27: note: in expansion of macro 'APPLY_ALL_H2'
 #define APPLY_ALL(t, ...) APPLY_ALL_H2(t, NUM_ARGS(__VA_ARGS__), __VA_ARGS__)
                           ^~~~~~~~~~~~
../../src/platforms/px4_param.h:79:2: note: in expansion of macro 'APPLY_ALL'
  APPLY_ALL(_DEFINE_SINGLE_PARAMETER, __VA_ARGS__) \
  ^~~~~~~~~
../../src/modules/navigator/navigator.h:336:2: note: in expansion of macro 'DEFINE_PARAMETERS'
  DEFINE_PARAMETERS(
  ^~~~~~~~~~~~~~~~~
compilation terminated due to -Wfatal-errors.
ninja: build stopped: subcommand failed.
Makefile:154: recipe for target 'px4fmu-v2_default' failed
make: *** [px4fmu-v2_default] Error 1
#8

Never mind,
I figured out myself.
To enable LPE, following modules also need to be uncommented too.

modules/landing_target_estimator
modules/vtol_att_control
#9

@Kyuhyong_You Have you successfully tested it yet in for altitude hold and terrain following?

#10

why the for terrain following should set the EKF2_HGT_MODE parameter to “barometric pressure” ?if i set EKF2_HGT_MODE =2 and MPC_ALT_MODE = 1? Is that OK。
Looking forward to your reply,thanks.