2013-08-29 02:34:34 -03:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
2012-01-10 19:44:04 -04:00
# include "RangeFinder.h"
2014-06-26 23:56:50 -03:00
# include "AP_RangeFinder_analog.h"
2014-06-27 00:39:52 -03:00
# include "AP_RangeFinder_PulsedLightLRF.h"
2014-06-27 01:03:47 -03:00
# include "AP_RangeFinder_MaxsonarI2CXL.h"
2016-04-19 18:32:17 -03:00
# include "AP_RangeFinder_MaxsonarSerialLV.h"
2015-02-21 06:55:21 -04:00
# include "AP_RangeFinder_PX4_PWM.h"
2015-07-06 16:24:06 -03:00
# include "AP_RangeFinder_BBB_PRU.h"
2015-08-28 06:52:34 -03:00
# include "AP_RangeFinder_LightWareI2C.h"
2015-09-07 02:32:06 -03:00
# include "AP_RangeFinder_LightWareSerial.h"
2016-04-15 14:31:51 -03:00
# include "AP_RangeFinder_Bebop.h"
2016-05-03 23:57:07 -03:00
# include "AP_RangeFinder_MAVLink.h"
2016-09-13 00:24:41 -03:00
# include "AP_RangeFinder_LeddarOne.h"
2016-11-14 17:47:45 -04:00
# include "AP_RangeFinder_uLanding.h"
2017-07-28 09:46:52 -03:00
# include "AP_RangeFinder_TeraRangerI2C.h"
2017-03-09 19:38:28 -04:00
# include "AP_RangeFinder_VL53L0X.h"
2016-11-10 22:18:45 -04:00
# include <AP_BoardConfig/AP_BoardConfig.h>
2012-01-10 19:44:04 -04:00
2016-04-14 13:42:06 -03:00
extern const AP_HAL : : HAL & hal ;
2016-04-14 17:00:38 -03:00
2014-06-26 23:56:50 -03:00
// table of user settable parameters
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo RangeFinder : : var_info [ ] = {
2014-06-26 23:56:50 -03:00
// @Param: _TYPE
// @DisplayName: Rangefinder type
2014-07-16 10:52:46 -03:00
// @Description: What type of rangefinder device that is connected
2017-07-28 09:46:52 -03:00
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X
2015-10-06 08:06:53 -03:00
// @User: Standard
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _TYPE " , 0 , RangeFinder , state [ 0 ] . type , 0 ) ,
2012-01-10 19:44:04 -04:00
2014-06-26 23:56:50 -03:00
// @Param: _PIN
// @DisplayName: Rangefinder pin
// @Description: Analog pin that rangefinder is connected to. Set this to 0..9 for the APM2 analog pins. Set to 64 on an APM1 for the dedicated 'airspeed' port on the end of the board. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
2015-01-07 03:20:29 -04:00
// @Values: -1:Not Used, 0:APM2-A0, 1:APM2-A1, 2:APM2-A2, 3:APM2-A3, 4:APM2-A4, 5:APM2-A5, 6:APM2-A6, 7:APM2-A7, 8:APM2-A8, 9:APM2-A9, 11:PX4-airspeed port, 15:Pixhawk-airspeed port, 64:APM1-airspeed port
2015-10-06 08:06:53 -03:00
// @User: Standard
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _PIN " , 1 , RangeFinder , state [ 0 ] . pin , - 1 ) ,
2014-06-26 23:56:50 -03:00
// @Param: _SCALING
// @DisplayName: Rangefinder scaling
// @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts.
2017-05-02 10:48:03 -03:00
// @Units: m/V
2014-06-26 23:56:50 -03:00
// @Increment: 0.001
2015-10-06 08:06:53 -03:00
// @User: Standard
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _SCALING " , 2 , RangeFinder , state [ 0 ] . scaling , 3.0f ) ,
2014-06-26 23:56:50 -03:00
// @Param: _OFFSET
// @DisplayName: rangefinder offset
2015-04-04 20:06:37 -03:00
// @Description: Offset in volts for zero distance for analog rangefinders. Offset added to distance in centimeters for PWM and I2C Lidars
2017-05-02 10:48:03 -03:00
// @Units: V
2014-06-26 23:56:50 -03:00
// @Increment: 0.001
2015-10-06 08:06:53 -03:00
// @User: Standard
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _OFFSET " , 3 , RangeFinder , state [ 0 ] . offset , 0.0f ) ,
2014-06-26 23:56:50 -03:00
// @Param: _FUNCTION
// @DisplayName: Rangefinder function
// @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters.
// @Values: 0:Linear,1:Inverted,2:Hyperbolic
2015-10-06 08:06:53 -03:00
// @User: Standard
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _FUNCTION " , 4 , RangeFinder , state [ 0 ] . function , 0 ) ,
2014-06-26 23:56:50 -03:00
// @Param: _MIN_CM
// @DisplayName: Rangefinder minimum distance
2014-07-16 16:11:15 -03:00
// @Description: Minimum distance in centimeters that rangefinder can reliably read
2017-05-02 10:48:03 -03:00
// @Units: cm
2014-06-26 23:56:50 -03:00
// @Increment: 1
2015-10-06 08:06:53 -03:00
// @User: Standard
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _MIN_CM " , 5 , RangeFinder , state [ 0 ] . min_distance_cm , 20 ) ,
2014-06-26 23:56:50 -03:00
// @Param: _MAX_CM
// @DisplayName: Rangefinder maximum distance
2014-07-16 16:11:15 -03:00
// @Description: Maximum distance in centimeters that rangefinder can reliably read
2017-05-02 10:48:03 -03:00
// @Units: cm
2014-06-26 23:56:50 -03:00
// @Increment: 1
2015-10-06 08:06:53 -03:00
// @User: Standard
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _MAX_CM " , 6 , RangeFinder , state [ 0 ] . max_distance_cm , 700 ) ,
2014-06-26 23:56:50 -03:00
// @Param: _STOP_PIN
// @DisplayName: Rangefinder stop pin
2014-07-16 16:11:15 -03:00
// @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other.
2015-01-07 03:20:29 -04:00
// @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
2015-10-06 08:06:53 -03:00
// @User: Standard
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _STOP_PIN " , 7 , RangeFinder , state [ 0 ] . stop_pin , - 1 ) ,
2014-06-26 23:56:50 -03:00
2014-11-09 17:09:41 -04:00
// @Param: _SETTLE
2014-07-16 16:11:15 -03:00
// @DisplayName: Rangefinder settle time
// @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again.
2017-05-02 10:48:03 -03:00
// @Units: ms
2014-06-26 23:56:50 -03:00
// @Increment: 1
2015-10-06 08:06:53 -03:00
// @User: Standard
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _SETTLE " , 8 , RangeFinder , state [ 0 ] . settle_time_ms , 0 ) ,
2014-06-26 23:56:50 -03:00
2014-06-27 00:04:25 -03:00
// @Param: _RMETRIC
// @DisplayName: Ratiometric
2014-07-16 16:11:15 -03:00
// @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric.
2014-06-27 00:04:25 -03:00
// @Values: 0:No,1:Yes
2015-10-06 08:06:53 -03:00
// @User: Standard
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _RMETRIC " , 9 , RangeFinder , state [ 0 ] . ratiometric , 1 ) ,
2014-06-27 00:04:25 -03:00
2015-09-07 01:43:44 -03:00
// @Param: _PWRRNG
2015-02-21 06:55:21 -04:00
// @DisplayName: Powersave range
// @Description: This parameter sets the estimated terrain distance in meters above which the sensor will be put into a power saving mode (if available). A value of zero means power saving is not enabled
2017-05-02 10:48:03 -03:00
// @Units: m
2015-02-21 06:55:21 -04:00
// @Range: 0 32767
2015-10-06 08:06:53 -03:00
// @User: Standard
2015-02-21 06:55:21 -04:00
AP_GROUPINFO ( " _PWRRNG " , 10 , RangeFinder , _powersave_range , 0 ) ,
2015-04-12 23:38:25 -03:00
// @Param: _GNDCLEAR
// @DisplayName: Distance (in cm) from the range finder to the ground
// @Description: This parameter sets the expected range measurement(in cm) that the range finder should return when the vehicle is on the ground.
2017-05-02 10:48:03 -03:00
// @Units: cm
2017-02-14 16:46:43 -04:00
// @Range: 5 127
2015-04-12 23:38:25 -03:00
// @Increment: 1
2015-10-06 08:06:53 -03:00
// @User: Standard
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _GNDCLEAR " , 11 , RangeFinder , state [ 0 ] . ground_clearance_cm , RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT ) ,
2015-04-12 23:38:25 -03:00
2016-07-28 07:58:47 -03:00
// @Param: _ADDR
// @DisplayName: Bus address of sensor
// @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
// @Range: 0 127
// @Increment: 1
// @User: Standard
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _ADDR " , 23 , RangeFinder , state [ 0 ] . address , 0 ) ,
2016-07-28 07:58:47 -03:00
2016-10-14 19:22:08 -03:00
// @Param: _POS_X
2016-10-07 19:29:42 -03:00
// @DisplayName: X position offset
2016-10-18 19:38:58 -03:00
// @Description: X position of the first rangefinder in body frame. Positive X is forward of the origin. Use the zero range datum point if supplied.
2016-10-07 19:29:42 -03:00
// @Units: m
// @User: Advanced
2016-10-14 19:22:08 -03:00
// @Param: _POS_Y
2016-10-07 19:29:42 -03:00
// @DisplayName: Y position offset
2016-10-18 19:38:58 -03:00
// @Description: Y position of the first rangefinder in body frame. Positive Y is to the right of the origin. Use the zero range datum point if supplied.
2016-10-07 19:29:42 -03:00
// @Units: m
// @User: Advanced
2016-10-14 19:22:08 -03:00
// @Param: _POS_Z
2016-10-07 19:29:42 -03:00
// @DisplayName: Z position offset
2016-10-18 19:38:58 -03:00
// @Description: Z position of the first rangefinder in body frame. Positive Z is down from the origin. Use the zero range datum point if supplied.
2016-10-07 19:29:42 -03:00
// @Units: m
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _POS " , 49 , RangeFinder , state [ 0 ] . pos_offset , 0.0f ) ,
2016-10-07 19:29:42 -03:00
2017-02-09 06:26:57 -04:00
// @Param: _ORIENT
// @DisplayName: Rangefinder orientation
// @Description: Orientation of rangefinder
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " _ORIENT " , 53 , RangeFinder , state [ 0 ] . orientation , ROTATION_PITCH_270 ) ,
2017-02-09 06:26:57 -04:00
2016-10-16 19:15:06 -03:00
# if RANGEFINDER_MAX_INSTANCES > 1
2014-06-30 17:15:53 -03:00
// @Param: 2_TYPE
// @DisplayName: Second Rangefinder type
2014-07-16 10:52:46 -03:00
// @Description: What type of rangefinder device that is connected
2017-07-28 09:46:52 -03:00
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X
2015-10-06 08:06:53 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_TYPE " , 12 , RangeFinder , state [ 1 ] . type , 0 ) ,
2014-06-30 17:15:53 -03:00
2014-06-26 23:56:50 -03:00
// @Param: 2_PIN
// @DisplayName: Rangefinder pin
// @Description: Analog pin that rangefinder is connected to. Set this to 0..9 for the APM2 analog pins. Set to 64 on an APM1 for the dedicated 'airspeed' port on the end of the board. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
2015-01-07 03:20:29 -04:00
// @Values: -1:Not Used, 0:APM2-A0, 1:APM2-A1, 2:APM2-A2, 3:APM2-A3, 4:APM2-A4, 5:APM2-A5, 6:APM2-A6, 7:APM2-A7, 8:APM2-A8, 9:APM2-A9, 11:PX4-airspeed port, 15:Pixhawk-airspeed port, 64:APM1-airspeed port
2015-10-06 08:06:53 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_PIN " , 13 , RangeFinder , state [ 1 ] . pin , - 1 ) ,
2014-06-26 23:56:50 -03:00
// @Param: 2_SCALING
// @DisplayName: Rangefinder scaling
// @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts.
2017-05-02 10:48:03 -03:00
// @Units: m/V
2014-06-26 23:56:50 -03:00
// @Increment: 0.001
2015-10-06 08:06:53 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_SCALING " , 14 , RangeFinder , state [ 1 ] . scaling , 3.0f ) ,
2014-06-26 23:56:50 -03:00
// @Param: 2_OFFSET
// @DisplayName: rangefinder offset
// @Description: Offset in volts for zero distance
2017-05-02 10:48:03 -03:00
// @Units: V
2014-06-26 23:56:50 -03:00
// @Increment: 0.001
2015-10-06 08:06:53 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_OFFSET " , 15 , RangeFinder , state [ 1 ] . offset , 0.0f ) ,
2014-06-26 23:56:50 -03:00
// @Param: 2_FUNCTION
// @DisplayName: Rangefinder function
// @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters.
// @Values: 0:Linear,1:Inverted,2:Hyperbolic
2015-10-06 08:06:53 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_FUNCTION " , 16 , RangeFinder , state [ 1 ] . function , 0 ) ,
2014-06-26 23:56:50 -03:00
// @Param: 2_MIN_CM
// @DisplayName: Rangefinder minimum distance
2014-07-16 16:11:15 -03:00
// @Description: Minimum distance in centimeters that rangefinder can reliably read
2017-05-02 10:48:03 -03:00
// @Units: cm
2014-06-26 23:56:50 -03:00
// @Increment: 1
2015-10-06 08:06:53 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_MIN_CM " , 17 , RangeFinder , state [ 1 ] . min_distance_cm , 20 ) ,
2014-06-26 23:56:50 -03:00
// @Param: 2_MAX_CM
// @DisplayName: Rangefinder maximum distance
2014-07-16 16:11:15 -03:00
// @Description: Maximum distance in centimeters that rangefinder can reliably read
2017-05-02 10:48:03 -03:00
// @Units: cm
2014-06-26 23:56:50 -03:00
// @Increment: 1
2015-10-06 08:06:53 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_MAX_CM " , 18 , RangeFinder , state [ 1 ] . max_distance_cm , 700 ) ,
2012-01-10 19:44:04 -04:00
2014-06-26 23:56:50 -03:00
// @Param: 2_STOP_PIN
// @DisplayName: Rangefinder stop pin
2014-07-16 16:11:15 -03:00
// @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other.
2015-01-07 03:20:29 -04:00
// @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
2015-10-06 08:06:53 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_STOP_PIN " , 19 , RangeFinder , state [ 1 ] . stop_pin , - 1 ) ,
2012-01-10 19:44:04 -04:00
2014-11-09 17:09:41 -04:00
// @Param: 2_SETTLE
2014-06-26 23:56:50 -03:00
// @DisplayName: Sonar settle time
2014-07-16 16:11:15 -03:00
// @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again.
2017-05-02 10:48:03 -03:00
// @Units: ms
2014-06-26 23:56:50 -03:00
// @Increment: 1
2015-10-06 08:06:53 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_SETTLE " , 20 , RangeFinder , state [ 1 ] . settle_time_ms , 0 ) ,
2014-06-27 00:04:25 -03:00
// @Param: 2_RMETRIC
// @DisplayName: Ratiometric
2014-07-16 16:11:15 -03:00
// @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric.
2014-06-27 00:04:25 -03:00
// @Values: 0:No,1:Yes
2015-10-06 08:06:53 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_RMETRIC " , 21 , RangeFinder , state [ 1 ] . ratiometric , 1 ) ,
2015-04-12 23:38:25 -03:00
// @Param: 2_GNDCLEAR
// @DisplayName: Distance (in cm) from the second range finder to the ground
// @Description: This parameter sets the expected range measurement(in cm) that the second range finder should return when the vehicle is on the ground.
2017-05-02 10:48:03 -03:00
// @Units: cm
2015-04-12 23:38:25 -03:00
// @Range: 0 127
// @Increment: 1
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_GNDCLEAR " , 22 , RangeFinder , state [ 1 ] . ground_clearance_cm , RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT ) ,
2012-01-10 19:44:04 -04:00
2015-08-28 06:52:34 -03:00
// @Param: 2_ADDR
2016-07-31 19:38:34 -03:00
// @DisplayName: Bus address of second rangefinder
2015-08-28 06:52:34 -03:00
// @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
// @Range: 0 127
// @Increment: 1
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_ADDR " , 24 , RangeFinder , state [ 1 ] . address , 0 ) ,
2016-07-28 07:58:47 -03:00
2016-10-07 19:29:42 -03:00
// @Param: 2_POS_X
// @DisplayName: X position offset
2016-10-18 19:38:58 -03:00
// @Description: X position of the second rangefinder in body frame. Positive X is forward of the origin. Use the zero range datum point if supplied.
2016-10-07 19:29:42 -03:00
// @Units: m
// @User: Advanced
// @Param: 2_POS_Y
// @DisplayName: Y position offset
2016-10-18 19:38:58 -03:00
// @Description: Y position of the second rangefinder in body frame. Positive Y is to the right of the origin. Use the zero range datum point if supplied.
2016-10-07 19:29:42 -03:00
// @Units: m
// @User: Advanced
// @Param: 2_POS_Z
// @DisplayName: Z position offset
2016-10-18 19:38:58 -03:00
// @Description: Z position of the second rangefinder in body frame. Positive Z is down from the origin. Use the zero range datum point if supplied.
2016-10-07 19:29:42 -03:00
// @Units: m
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_POS " , 50 , RangeFinder , state [ 1 ] . pos_offset , 0.0f ) ,
2016-10-07 19:29:42 -03:00
2017-02-09 06:26:57 -04:00
// @Param: 2_ORIENT
// @DisplayName: Rangefinder 2 orientation
// @Description: Orientation of 2nd rangefinder
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 2_ORIENT " , 54 , RangeFinder , state [ 1 ] . orientation , ROTATION_PITCH_270 ) ,
2015-08-28 06:52:34 -03:00
# endif
2015-09-10 07:27:12 -03:00
# if RANGEFINDER_MAX_INSTANCES > 2
2016-07-26 02:47:25 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 3_TYPE
2016-07-31 19:38:34 -03:00
// @DisplayName: Third Rangefinder type
2015-09-10 07:27:12 -03:00
// @Description: What type of rangefinder device that is connected
2017-07-28 09:46:52 -03:00
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_TYPE " , 25 , RangeFinder , state [ 2 ] . type , 0 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 3_PIN
2015-09-10 07:27:12 -03:00
// @DisplayName: Rangefinder pin
// @Description: Analog pin that rangefinder is connected to. Set this to 0..9 for the APM2 analog pins. Set to 64 on an APM1 for the dedicated 'airspeed' port on the end of the board. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
// @Values: -1:Not Used, 0:APM2-A0, 1:APM2-A1, 2:APM2-A2, 3:APM2-A3, 4:APM2-A4, 5:APM2-A5, 6:APM2-A6, 7:APM2-A7, 8:APM2-A8, 9:APM2-A9, 11:PX4-airspeed port, 15:Pixhawk-airspeed port, 64:APM1-airspeed port
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_PIN " , 26 , RangeFinder , state [ 2 ] . pin , - 1 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 3_SCALING
2015-09-10 07:27:12 -03:00
// @DisplayName: Rangefinder scaling
// @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts.
2017-05-02 10:48:03 -03:00
// @Units: m/V
2015-09-10 07:27:12 -03:00
// @Increment: 0.001
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_SCALING " , 27 , RangeFinder , state [ 2 ] . scaling , 3.0f ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 3_OFFSET
2015-09-10 07:27:12 -03:00
// @DisplayName: rangefinder offset
// @Description: Offset in volts for zero distance
2017-05-02 10:48:03 -03:00
// @Units: V
2015-09-10 07:27:12 -03:00
// @Increment: 0.001
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_OFFSET " , 28 , RangeFinder , state [ 2 ] . offset , 0.0f ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 3_FUNCTION
2015-09-10 07:27:12 -03:00
// @DisplayName: Rangefinder function
// @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters.
// @Values: 0:Linear,1:Inverted,2:Hyperbolic
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_FUNCTION " , 29 , RangeFinder , state [ 2 ] . function , 0 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 3_MIN_CM
2015-09-10 07:27:12 -03:00
// @DisplayName: Rangefinder minimum distance
// @Description: Minimum distance in centimeters that rangefinder can reliably read
2017-05-02 10:48:03 -03:00
// @Units: cm
2015-09-10 07:27:12 -03:00
// @Increment: 1
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_MIN_CM " , 30 , RangeFinder , state [ 2 ] . min_distance_cm , 20 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 3_MAX_CM
2015-09-10 07:27:12 -03:00
// @DisplayName: Rangefinder maximum distance
// @Description: Maximum distance in centimeters that rangefinder can reliably read
2017-05-02 10:48:03 -03:00
// @Units: cm
2015-09-10 07:27:12 -03:00
// @Increment: 1
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_MAX_CM " , 31 , RangeFinder , state [ 2 ] . max_distance_cm , 700 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 3_STOP_PIN
2015-09-10 07:27:12 -03:00
// @DisplayName: Rangefinder stop pin
// @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other.
// @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_STOP_PIN " , 32 , RangeFinder , state [ 2 ] . stop_pin , - 1 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 3_SETTLE
2015-09-10 07:27:12 -03:00
// @DisplayName: Sonar settle time
// @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again.
2017-05-02 10:48:03 -03:00
// @Units: ms
2015-09-10 07:27:12 -03:00
// @Increment: 1
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_SETTLE " , 33 , RangeFinder , state [ 2 ] . settle_time_ms , 0 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 3_RMETRIC
2015-09-10 07:27:12 -03:00
// @DisplayName: Ratiometric
// @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric.
// @Values: 0:No,1:Yes
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_RMETRIC " , 34 , RangeFinder , state [ 2 ] . ratiometric , 1 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 3_GNDCLEAR
2016-07-31 19:38:34 -03:00
// @DisplayName: Distance (in cm) from the third range finder to the ground
// @Description: This parameter sets the expected range measurement(in cm) that the third range finder should return when the vehicle is on the ground.
2017-05-02 10:48:03 -03:00
// @Units: cm
2015-09-10 07:27:12 -03:00
// @Range: 0 127
// @Increment: 1
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_GNDCLEAR " , 35 , RangeFinder , state [ 2 ] . ground_clearance_cm , RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT ) ,
2015-09-10 07:27:12 -03:00
// @Param: 3_ADDR
2016-07-26 02:47:25 -03:00
// @DisplayName: Bus address of third rangefinder
2015-09-10 07:27:12 -03:00
// @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
// @Range: 0 127
// @Increment: 1
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_ADDR " , 36 , RangeFinder , state [ 2 ] . address , 0 ) ,
2016-07-26 02:47:25 -03:00
2016-10-07 19:29:42 -03:00
// @Param: 3_POS_X
// @DisplayName: X position offset
2016-10-18 19:38:58 -03:00
// @Description: X position of the third rangefinder in body frame. Positive X is forward of the origin. Use the zero range datum point if supplied.
2016-10-07 19:29:42 -03:00
// @Units: m
// @User: Advanced
// @Param: 3_POS_Y
// @DisplayName: Y position offset
2016-10-18 19:38:58 -03:00
// @Description: Y position of the third rangefinder in body frame. Positive Y is to the right of the origin. Use the zero range datum point if supplied.
2016-10-07 19:29:42 -03:00
// @Units: m
// @User: Advanced
// @Param: 3_POS_Z
// @DisplayName: Z position offset
2016-10-18 19:38:58 -03:00
// @Description: Z position of the third rangefinder in body frame. Positive Z is down from the origin. Use the zero range datum point if supplied.
2016-10-07 19:29:42 -03:00
// @Units: m
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_POS " , 51 , RangeFinder , state [ 2 ] . pos_offset , 0.0f ) ,
2016-10-07 19:29:42 -03:00
2017-02-09 06:26:57 -04:00
// @Param: 3_ORIENT
// @DisplayName: Rangefinder 3 orientation
// @Description: Orientation of 3rd rangefinder
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 3_ORIENT " , 55 , RangeFinder , state [ 2 ] . orientation , ROTATION_PITCH_270 ) ,
2016-07-26 02:47:25 -03:00
# endif
# if RANGEFINDER_MAX_INSTANCES > 3
2015-09-21 20:04:47 -03:00
// @Param: 4_TYPE
2016-07-31 19:38:34 -03:00
// @DisplayName: Fourth Rangefinder type
2015-09-10 07:27:12 -03:00
// @Description: What type of rangefinder device that is connected
2017-07-28 09:46:52 -03:00
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_TYPE " , 37 , RangeFinder , state [ 3 ] . type , 0 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 4_PIN
2015-09-10 07:27:12 -03:00
// @DisplayName: Rangefinder pin
// @Description: Analog pin that rangefinder is connected to. Set this to 0..9 for the APM2 analog pins. Set to 64 on an APM1 for the dedicated 'airspeed' port on the end of the board. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port.
// @Values: -1:Not Used, 0:APM2-A0, 1:APM2-A1, 2:APM2-A2, 3:APM2-A3, 4:APM2-A4, 5:APM2-A5, 6:APM2-A6, 7:APM2-A7, 8:APM2-A8, 9:APM2-A9, 11:PX4-airspeed port, 15:Pixhawk-airspeed port, 64:APM1-airspeed port
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_PIN " , 38 , RangeFinder , state [ 3 ] . pin , - 1 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 4_SCALING
2015-09-10 07:27:12 -03:00
// @DisplayName: Rangefinder scaling
// @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts.
2017-05-02 10:48:03 -03:00
// @Units: m/V
2015-09-10 07:27:12 -03:00
// @Increment: 0.001
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_SCALING " , 39 , RangeFinder , state [ 3 ] . scaling , 3.0f ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 4_OFFSET
2015-09-10 07:27:12 -03:00
// @DisplayName: rangefinder offset
// @Description: Offset in volts for zero distance
2017-05-02 10:48:03 -03:00
// @Units: V
2015-09-10 07:27:12 -03:00
// @Increment: 0.001
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_OFFSET " , 40 , RangeFinder , state [ 3 ] . offset , 0.0f ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 4_FUNCTION
2015-09-10 07:27:12 -03:00
// @DisplayName: Rangefinder function
// @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters.
// @Values: 0:Linear,1:Inverted,2:Hyperbolic
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_FUNCTION " , 41 , RangeFinder , state [ 3 ] . function , 0 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 4_MIN_CM
2015-09-10 07:27:12 -03:00
// @DisplayName: Rangefinder minimum distance
// @Description: Minimum distance in centimeters that rangefinder can reliably read
2017-05-02 10:48:03 -03:00
// @Units: cm
2015-09-10 07:27:12 -03:00
// @Increment: 1
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_MIN_CM " , 42 , RangeFinder , state [ 3 ] . min_distance_cm , 20 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 4_MAX_CM
2015-09-10 07:27:12 -03:00
// @DisplayName: Rangefinder maximum distance
// @Description: Maximum distance in centimeters that rangefinder can reliably read
2017-05-02 10:48:03 -03:00
// @Units: cm
2015-09-10 07:27:12 -03:00
// @Increment: 1
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_MAX_CM " , 43 , RangeFinder , state [ 3 ] . max_distance_cm , 700 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 4_STOP_PIN
2015-09-10 07:27:12 -03:00
// @DisplayName: Rangefinder stop pin
// @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other.
// @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_STOP_PIN " , 44 , RangeFinder , state [ 3 ] . stop_pin , - 1 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 4_SETTLE
2015-09-10 07:27:12 -03:00
// @DisplayName: Sonar settle time
// @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again.
2017-05-02 10:48:03 -03:00
// @Units: ms
2015-09-10 07:27:12 -03:00
// @Increment: 1
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_SETTLE " , 45 , RangeFinder , state [ 3 ] . settle_time_ms , 0 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 4_RMETRIC
2015-09-10 07:27:12 -03:00
// @DisplayName: Ratiometric
// @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric.
// @Values: 0:No,1:Yes
2016-07-26 02:48:22 -03:00
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_RMETRIC " , 46 , RangeFinder , state [ 3 ] . ratiometric , 1 ) ,
2015-09-10 07:27:12 -03:00
2015-09-21 20:04:47 -03:00
// @Param: 4_GNDCLEAR
2016-07-31 19:38:34 -03:00
// @DisplayName: Distance (in cm) from the fourth range finder to the ground
// @Description: This parameter sets the expected range measurement(in cm) that the fourth range finder should return when the vehicle is on the ground.
2017-05-02 10:48:03 -03:00
// @Units: cm
2015-09-10 07:27:12 -03:00
// @Range: 0 127
// @Increment: 1
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_GNDCLEAR " , 47 , RangeFinder , state [ 3 ] . ground_clearance_cm , RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT ) ,
2015-09-10 07:27:12 -03:00
// @Param: 4_ADDR
2016-07-31 19:38:34 -03:00
// @DisplayName: Bus address of fourth rangefinder
2015-09-10 07:27:12 -03:00
// @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
// @Range: 0 127
// @Increment: 1
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_ADDR " , 48 , RangeFinder , state [ 3 ] . address , 0 ) ,
2016-10-07 19:29:42 -03:00
// @Param: 4_POS_X
// @DisplayName: X position offset
// @Description: X position of the fourth rangefinder in body frame. Use the zero range datum point if supplied.
// @Units: m
// @User: Advanced
// @Param: 4_POS_Y
// @DisplayName: Y position offset
// @Description: Y position of the fourth rangefinder in body frame. Use the zero range datum point if supplied.
// @Units: m
// @User: Advanced
// @Param: 4_POS_Z
// @DisplayName: Z position offset
// @Description: Z position of the fourth rangefinder in body frame. Use the zero range datum point if supplied.
// @Units: m
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_POS " , 52 , RangeFinder , state [ 3 ] . pos_offset , 0.0f ) ,
2017-02-09 06:26:57 -04:00
// @Param: 4_ORIENT
// @DisplayName: Rangefinder 4 orientation
// @Description: Orientation of 4th range finder
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced
2017-08-07 00:04:56 -03:00
AP_GROUPINFO ( " 4_ORIENT " , 56 , RangeFinder , state [ 3 ] . orientation , ROTATION_PITCH_270 ) ,
2015-09-10 07:27:12 -03:00
# endif
2015-08-28 06:52:34 -03:00
2014-06-26 23:56:50 -03:00
AP_GROUPEND
} ;
2017-02-09 06:26:57 -04:00
RangeFinder : : RangeFinder ( AP_SerialManager & _serial_manager , enum Rotation orientation_default ) :
2015-06-08 00:10:43 -03:00
num_instances ( 0 ) ,
2015-09-07 02:32:06 -03:00
estimated_terrain_height ( 0 ) ,
serial_manager ( _serial_manager )
2015-06-08 00:10:43 -03:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
2017-02-09 06:26:57 -04:00
// set orientation defaults
for ( uint8_t i = 0 ; i < RANGEFINDER_MAX_INSTANCES ; i + + ) {
2017-08-07 00:04:56 -03:00
state [ i ] . orientation . set_default ( orientation_default ) ;
2017-02-09 06:26:57 -04:00
}
2015-06-08 00:10:43 -03:00
}
2014-06-26 23:56:50 -03:00
/*
initialise the RangeFinder class . We do detection of attached range
finders here . For now we won ' t allow for hot - plugging of
rangefinders .
*/
void RangeFinder : : init ( void )
{
if ( num_instances ! = 0 ) {
// init called a 2nd time?
return ;
}
for ( uint8_t i = 0 ; i < RANGEFINDER_MAX_INSTANCES ; i + + ) {
detect_instance ( i ) ;
2016-10-30 02:24:21 -03:00
if ( drivers [ i ] ! = nullptr ) {
2014-06-26 23:56:50 -03:00
// we loaded a driver for this instance, so it must be
// present (although it may not be healthy)
num_instances = i + 1 ;
}
2015-04-13 00:18:46 -03:00
// initialise pre-arm check variables
state [ i ] . pre_arm_check = false ;
state [ i ] . pre_arm_distance_min = 9999 ; // initialise to an arbitrary large value
state [ i ] . pre_arm_distance_max = 0 ;
2015-04-13 03:03:19 -03:00
// initialise status
state [ i ] . status = RangeFinder_NotConnected ;
state [ i ] . range_valid_count = 0 ;
2014-06-05 11:31:15 -03:00
}
2014-06-26 23:56:50 -03:00
}
/*
update RangeFinder state for all instances . This should be called at
around 10 Hz by main loop
*/
void RangeFinder : : update ( void )
{
for ( uint8_t i = 0 ; i < num_instances ; i + + ) {
2016-10-30 02:24:21 -03:00
if ( drivers [ i ] ! = nullptr ) {
2017-08-07 00:04:56 -03:00
if ( state [ i ] . type = = RangeFinder_TYPE_NONE ) {
2014-06-26 23:56:50 -03:00
// allow user to disable a rangefinder at runtime
2015-04-13 03:03:19 -03:00
state [ i ] . status = RangeFinder_NotConnected ;
state [ i ] . range_valid_count = 0 ;
2014-06-26 23:56:50 -03:00
continue ;
}
drivers [ i ] - > update ( ) ;
2017-08-07 01:20:46 -03:00
drivers [ i ] - > update_pre_arm_check ( ) ;
2014-06-26 23:56:50 -03:00
}
}
}
2016-04-14 16:59:31 -03:00
2016-11-11 17:58:46 -04:00
bool RangeFinder : : _add_backend ( AP_RangeFinder_Backend * backend )
2016-04-14 16:59:31 -03:00
{
if ( ! backend ) {
2016-11-11 17:58:46 -04:00
return false ;
2016-04-14 16:59:31 -03:00
}
if ( num_instances = = RANGEFINDER_MAX_INSTANCES ) {
AP_HAL : : panic ( " Too many RANGERS backends " ) ;
}
drivers [ num_instances + + ] = backend ;
2016-11-11 17:58:46 -04:00
return true ;
2016-04-14 16:59:31 -03:00
}
2016-11-10 22:18:45 -04:00
2014-06-26 23:56:50 -03:00
/*
detect if an instance of a rangefinder is connected .
*/
void RangeFinder : : detect_instance ( uint8_t instance )
{
2017-08-07 00:04:56 -03:00
enum RangeFinder_Type _type = ( enum RangeFinder_Type ) state [ instance ] . type . get ( ) ;
switch ( _type ) {
2016-12-03 07:02:03 -04:00
case RangeFinder_TYPE_PLI2C :
2017-03-01 00:22:32 -04:00
case RangeFinder_TYPE_PLI2CV3 :
2017-08-07 00:41:01 -03:00
if ( ! _add_backend ( AP_RangeFinder_PulsedLightLRF : : detect ( 1 , state [ instance ] , _type ) ) ) {
_add_backend ( AP_RangeFinder_PulsedLightLRF : : detect ( 0 , state [ instance ] , _type ) ) ;
2016-11-11 17:58:46 -04:00
}
2016-12-03 07:02:03 -04:00
break ;
case RangeFinder_TYPE_MBI2C :
2017-08-07 00:41:01 -03:00
_add_backend ( AP_RangeFinder_MaxsonarI2CXL : : detect ( state [ instance ] ) ) ;
2016-12-03 07:02:03 -04:00
break ;
case RangeFinder_TYPE_LWI2C :
2017-08-07 00:04:56 -03:00
if ( state [ instance ] . address ) {
2017-08-07 00:41:01 -03:00
_add_backend ( AP_RangeFinder_LightWareI2C : : detect ( state [ instance ] ,
2017-08-07 00:04:56 -03:00
hal . i2c_mgr - > get_device ( HAL_RANGEFINDER_LIGHTWARE_I2C_BUS , state [ instance ] . address ) ) ) ;
2015-08-28 06:52:34 -03:00
}
2016-12-03 07:02:03 -04:00
break ;
2017-07-28 09:46:52 -03:00
case RangeFinder_TYPE_TRI2C :
2017-07-28 09:20:10 -03:00
if ( state [ instance ] . address ) {
if ( ! _add_backend ( AP_RangeFinder_TeraRangerI2C : : detect ( state [ instance ] ,
hal . i2c_mgr - > get_device ( 1 , state [ instance ] . address ) ) ) ) {
_add_backend ( AP_RangeFinder_TeraRangerI2C : : detect ( state [ instance ] ,
hal . i2c_mgr - > get_device ( 0 , state [ instance ] . address ) ) ) ;
}
2016-12-15 20:53:30 -04:00
}
break ;
2017-03-09 19:38:28 -04:00
case RangeFinder_TYPE_VL53L0X :
2017-08-07 00:41:01 -03:00
if ( ! _add_backend ( AP_RangeFinder_VL53L0X : : detect ( state [ instance ] ,
2017-03-09 19:38:28 -04:00
hal . i2c_mgr - > get_device ( 1 , 0x29 ) ) ) ) {
2017-08-07 00:41:01 -03:00
_add_backend ( AP_RangeFinder_VL53L0X : : detect ( state [ instance ] ,
2017-03-09 19:38:28 -04:00
hal . i2c_mgr - > get_device ( 0 , 0x29 ) ) ) ;
}
break ;
2014-12-30 07:02:31 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
2016-12-03 07:02:03 -04:00
case RangeFinder_TYPE_PX4_PWM :
2017-08-07 00:41:01 -03:00
if ( AP_RangeFinder_PX4_PWM : : detect ( ) ) {
2015-02-21 06:55:21 -04:00
state [ instance ] . instance = instance ;
2017-08-07 00:41:01 -03:00
drivers [ instance ] = new AP_RangeFinder_PX4_PWM ( state [ instance ] , _powersave_range , estimated_terrain_height ) ;
2015-02-21 06:55:21 -04:00
}
2016-12-03 07:02:03 -04:00
break ;
2015-07-06 16:24:06 -03:00
# endif
# if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
2016-12-03 07:02:03 -04:00
case RangeFinder_TYPE_BBB_PRU :
2017-08-07 00:41:01 -03:00
if ( AP_RangeFinder_BBB_PRU : : detect ( ) ) {
2015-07-06 16:24:06 -03:00
state [ instance ] . instance = instance ;
2017-08-07 00:41:01 -03:00
drivers [ instance ] = new AP_RangeFinder_BBB_PRU ( state [ instance ] ) ;
2015-07-06 16:24:06 -03:00
}
2016-12-03 07:02:03 -04:00
break ;
2014-06-27 02:02:51 -03:00
# endif
2016-12-03 07:02:03 -04:00
case RangeFinder_TYPE_LWSER :
2017-08-07 00:41:01 -03:00
if ( AP_RangeFinder_LightWareSerial : : detect ( serial_manager ) ) {
2015-09-07 02:32:06 -03:00
state [ instance ] . instance = instance ;
2017-08-07 00:41:01 -03:00
drivers [ instance ] = new AP_RangeFinder_LightWareSerial ( state [ instance ] , serial_manager ) ;
2015-09-07 02:32:06 -03:00
}
2016-12-03 07:02:03 -04:00
break ;
case RangeFinder_TYPE_LEDDARONE :
2017-08-07 00:41:01 -03:00
if ( AP_RangeFinder_LeddarOne : : detect ( serial_manager ) ) {
2016-09-13 00:24:41 -03:00
state [ instance ] . instance = instance ;
2017-08-07 00:41:01 -03:00
drivers [ instance ] = new AP_RangeFinder_LeddarOne ( state [ instance ] , serial_manager ) ;
2016-09-13 00:24:41 -03:00
}
2016-12-03 07:02:03 -04:00
break ;
case RangeFinder_TYPE_ULANDING :
2017-08-07 00:41:01 -03:00
if ( AP_RangeFinder_uLanding : : detect ( serial_manager ) ) {
2016-11-14 17:47:45 -04:00
state [ instance ] . instance = instance ;
2017-08-07 00:41:01 -03:00
drivers [ instance ] = new AP_RangeFinder_uLanding ( state [ instance ] , serial_manager ) ;
2016-11-14 17:47:45 -04:00
}
2016-12-03 07:02:03 -04:00
break ;
2016-06-15 23:44:01 -03:00
# if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \
CONFIG_HAL_BOARD_SUBTYPE = = HAL_BOARD_SUBTYPE_LINUX_DISCO ) & & defined ( HAVE_LIBIIO )
2016-12-03 07:02:03 -04:00
case RangeFinder_TYPE_BEBOP :
2017-08-07 00:41:01 -03:00
if ( AP_RangeFinder_Bebop : : detect ( ) ) {
2016-04-15 14:31:51 -03:00
state [ instance ] . instance = instance ;
2017-08-07 00:41:01 -03:00
drivers [ instance ] = new AP_RangeFinder_Bebop ( state [ instance ] ) ;
2016-04-15 14:31:51 -03:00
}
2016-12-03 07:02:03 -04:00
break ;
2016-04-15 14:31:51 -03:00
# endif
2016-12-03 07:02:03 -04:00
case RangeFinder_TYPE_MAVLink :
2017-08-07 00:41:01 -03:00
if ( AP_RangeFinder_MAVLink : : detect ( ) ) {
2016-05-04 00:02:44 -03:00
state [ instance ] . instance = instance ;
2017-08-07 00:41:01 -03:00
drivers [ instance ] = new AP_RangeFinder_MAVLink ( state [ instance ] ) ;
2016-05-04 00:02:44 -03:00
}
2016-12-03 07:02:03 -04:00
break ;
case RangeFinder_TYPE_MBSER :
2017-08-07 00:41:01 -03:00
if ( AP_RangeFinder_MaxsonarSerialLV : : detect ( serial_manager ) ) {
2016-04-19 18:32:17 -03:00
state [ instance ] . instance = instance ;
2017-08-07 00:41:01 -03:00
drivers [ instance ] = new AP_RangeFinder_MaxsonarSerialLV ( state [ instance ] , serial_manager ) ;
2016-04-19 18:32:17 -03:00
}
2016-12-03 07:02:03 -04:00
break ;
case RangeFinder_TYPE_ANALOG :
2016-12-03 13:44:06 -04:00
// note that analog will always come back as present if the pin is valid
2017-08-07 00:41:01 -03:00
if ( AP_RangeFinder_analog : : detect ( state [ instance ] ) ) {
2014-06-26 23:56:50 -03:00
state [ instance ] . instance = instance ;
2017-08-07 00:41:01 -03:00
drivers [ instance ] = new AP_RangeFinder_analog ( state [ instance ] ) ;
2014-06-26 23:56:50 -03:00
}
2016-12-03 07:02:03 -04:00
break ;
default :
break ;
2014-06-05 11:31:15 -03:00
}
2012-01-10 19:44:04 -04:00
}
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * RangeFinder : : get_backend ( uint8_t id ) const {
if ( id > = num_instances ) {
return nullptr ;
2015-04-13 03:03:19 -03:00
}
2017-08-08 02:54:09 -03:00
if ( drivers [ id ] ! = nullptr ) {
if ( drivers [ id ] - > type ( ) = = RangeFinder_TYPE_NONE ) {
// pretend it isn't here; disabled at runtime?
return nullptr ;
}
2015-04-13 03:03:19 -03:00
}
2017-08-08 02:54:09 -03:00
return drivers [ id ] ;
} ;
2015-04-13 03:03:19 -03:00
2017-02-09 06:26:57 -04:00
RangeFinder : : RangeFinder_Status RangeFinder : : status_orient ( enum Rotation orientation ) const
{
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * backend = find_instance ( orientation ) ;
if ( backend = = nullptr ) {
return RangeFinder_NotConnected ;
2017-02-09 06:26:57 -04:00
}
2017-08-08 02:54:09 -03:00
return backend - > status ( ) ;
2017-02-09 06:26:57 -04:00
}
void RangeFinder : : handle_msg ( mavlink_message_t * msg )
{
uint8_t i ;
for ( i = 0 ; i < num_instances ; i + + ) {
2017-08-07 00:04:56 -03:00
if ( ( drivers [ i ] ! = nullptr ) & & ( state [ i ] . type ! = RangeFinder_TYPE_NONE ) ) {
2016-05-03 23:59:01 -03:00
drivers [ i ] - > handle_msg ( msg ) ;
2017-02-09 06:26:57 -04:00
}
}
}
// return true if we have a range finder with the specified orientation
bool RangeFinder : : has_orientation ( enum Rotation orientation ) const
{
2017-08-08 02:54:09 -03:00
return ( find_instance ( orientation ) ! = nullptr ) ;
2017-02-09 06:26:57 -04:00
}
// find first range finder instance with the specified orientation
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * RangeFinder : : find_instance ( enum Rotation orientation ) const
2017-02-09 06:26:57 -04:00
{
for ( uint8_t i = 0 ; i < num_instances ; i + + ) {
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * backend = get_backend ( i ) ;
if ( backend = = nullptr ) {
continue ;
2017-02-09 06:26:57 -04:00
}
2017-08-08 02:54:09 -03:00
if ( backend - > orientation ( ) ! = orientation ) {
continue ;
}
return backend ;
2017-02-09 06:26:57 -04:00
}
2017-08-08 02:54:09 -03:00
return nullptr ;
2017-02-09 06:26:57 -04:00
}
uint16_t RangeFinder : : distance_cm_orient ( enum Rotation orientation ) const
{
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * backend = find_instance ( orientation ) ;
if ( backend = = nullptr ) {
return 0 ;
2017-02-09 06:26:57 -04:00
}
2017-08-08 02:54:09 -03:00
return backend - > distance_cm ( ) ;
2017-02-09 06:26:57 -04:00
}
uint16_t RangeFinder : : voltage_mv_orient ( enum Rotation orientation ) const
{
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * backend = find_instance ( orientation ) ;
if ( backend = = nullptr ) {
return 0 ;
2017-02-09 06:26:57 -04:00
}
2017-08-08 02:54:09 -03:00
return backend - > voltage_mv ( ) ;
2017-02-09 06:26:57 -04:00
}
int16_t RangeFinder : : max_distance_cm_orient ( enum Rotation orientation ) const
{
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * backend = find_instance ( orientation ) ;
if ( backend = = nullptr ) {
return 0 ;
2017-02-09 06:26:57 -04:00
}
2017-08-08 02:54:09 -03:00
return backend - > max_distance_cm ( ) ;
2017-02-09 06:26:57 -04:00
}
int16_t RangeFinder : : min_distance_cm_orient ( enum Rotation orientation ) const
{
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * backend = find_instance ( orientation ) ;
if ( backend = = nullptr ) {
return 0 ;
2017-02-09 06:26:57 -04:00
}
2017-08-08 02:54:09 -03:00
return backend - > min_distance_cm ( ) ;
2017-02-09 06:26:57 -04:00
}
int16_t RangeFinder : : ground_clearance_cm_orient ( enum Rotation orientation ) const
{
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * backend = find_instance ( orientation ) ;
if ( backend = = nullptr ) {
return 0 ;
2015-04-13 03:03:19 -03:00
}
2017-08-08 02:54:09 -03:00
return backend - > ground_clearance_cm ( ) ;
2015-04-13 03:03:19 -03:00
}
2017-02-09 06:26:57 -04:00
bool RangeFinder : : has_data_orient ( enum Rotation orientation ) const
{
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * backend = find_instance ( orientation ) ;
if ( backend = = nullptr ) {
return false ;
2017-02-09 06:26:57 -04:00
}
2017-08-08 02:54:09 -03:00
return backend - > has_data ( ) ;
2017-02-09 06:26:57 -04:00
}
uint8_t RangeFinder : : range_valid_count_orient ( enum Rotation orientation ) const
{
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * backend = find_instance ( orientation ) ;
if ( backend = = nullptr ) {
return 0 ;
2017-02-09 06:26:57 -04:00
}
2017-08-08 02:54:09 -03:00
return backend - > range_valid_count ( ) ;
2017-02-09 06:26:57 -04:00
}
2015-04-13 00:18:46 -03:00
/*
returns true if pre - arm checks have passed for all range finders
these checks involve the user lifting or rotating the vehicle so that sensor readings between
the min and 2 m can be captured
*/
bool RangeFinder : : pre_arm_check ( ) const
{
for ( uint8_t i = 0 ; i < num_instances ; i + + ) {
// if driver is valid but pre_arm_check is false, return false
2017-08-07 00:04:56 -03:00
if ( ( drivers [ i ] ! = nullptr ) & & ( state [ i ] . type ! = RangeFinder_TYPE_NONE ) & & ! state [ i ] . pre_arm_check ) {
2015-04-13 00:18:46 -03:00
return false ;
}
}
return true ;
}
2017-02-09 06:26:57 -04:00
const Vector3f & RangeFinder : : get_pos_offset_orient ( enum Rotation orientation ) const
{
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * backend = find_instance ( orientation ) ;
if ( backend = = nullptr ) {
return pos_offset_zero ;
2017-02-09 06:26:57 -04:00
}
2017-08-08 02:54:09 -03:00
return backend - > get_pos_offset ( ) ;
2017-05-29 14:02:51 -03:00
}
2017-08-08 04:32:53 -03:00
MAV_DISTANCE_SENSOR RangeFinder : : get_mav_distance_sensor_type_orient ( enum Rotation orientation ) const
2017-05-29 14:02:51 -03:00
{
2017-08-08 02:54:09 -03:00
AP_RangeFinder_Backend * backend = find_instance ( orientation ) ;
if ( backend = = nullptr ) {
return MAV_DISTANCE_SENSOR_UNKNOWN ;
2017-05-29 14:02:51 -03:00
}
2017-08-08 04:32:53 -03:00
return backend - > get_mav_distance_sensor_type ( ) ;
2017-05-29 14:02:51 -03:00
}