From 08cd3f4a7776494ad07c7b9547b777bd501b224c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 7 Aug 2017 13:04:56 +1000 Subject: [PATCH] AP_RangeFinder: move instance parameters into instance state --- .../AP_RangeFinder_LightWareI2C.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp | 8 +- .../AP_RangeFinder/AP_RangeFinder_analog.cpp | 24 +-- libraries/AP_RangeFinder/RangeFinder.cpp | 146 +++++++++--------- libraries/AP_RangeFinder/RangeFinder.h | 45 +++--- .../AP_RangeFinder/RangeFinder_Backend.cpp | 4 +- .../examples/RFIND_test/RFIND_test.cpp | 2 +- 7 files changed, 116 insertions(+), 115 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp index 7eaba79145..9ca9e14c6d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp @@ -72,7 +72,7 @@ bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm) { be16_t val; - if (ranger._address[state.instance] == 0) { + if (state.address == 0) { return false; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp index 7c4af7a34f..73201ba174 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp @@ -109,7 +109,7 @@ void AP_RangeFinder_PX4_PWM::update(void) struct pwm_input_s pwm; float sum_cm = 0; uint16_t count = 0; - const float scaling = ranger._scaling[state.instance]; + const float scaling = state.scaling; uint32_t now = AP_HAL::millis(); while (::read(_fd, &pwm, sizeof(pwm)) == sizeof(pwm)) { @@ -120,7 +120,7 @@ void AP_RangeFinder_PX4_PWM::update(void) _last_pulse_time_ms = now; // setup for scaling in meters per millisecond - float distance_cm = pwm.pulse_width * 0.1f * scaling + ranger._offset[state.instance]; + float distance_cm = pwm.pulse_width * 0.1f * scaling + state.offset; float distance_delta_cm = fabsf(distance_cm - _last_sample_distance_cm); _last_sample_distance_cm = distance_cm; @@ -143,8 +143,8 @@ void AP_RangeFinder_PX4_PWM::update(void) // if we haven't received a pulse for 1 second then we may need to // reset the timer - int8_t stop_pin = ranger._stop_pin[state.instance]; - uint16_t settle_time_ms = (uint16_t)ranger._settle_time_ms[state.instance]; + int8_t stop_pin = state.stop_pin; + uint16_t settle_time_ms = (uint16_t)state.settle_time_ms; if (stop_pin != -1 && out_of_range()) { // we are above the power saving range. Disable the sensor diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp index 10a43942e5..47ac1f9414 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp @@ -34,14 +34,14 @@ extern const AP_HAL::HAL& hal; AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) : AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_UNKNOWN) { - source = hal.analogin->channel(ranger._pin[instance]); + source = hal.analogin->channel(_state.pin); if (source == nullptr) { // failed to allocate a ADC channel? This shouldn't happen set_status(RangeFinder::RangeFinder_NotConnected); return; } - source->set_stop_pin((uint8_t)ranger._stop_pin[instance]); - source->set_settle_time((uint16_t)ranger._settle_time_ms[instance]); + source->set_stop_pin((uint8_t)_state.stop_pin); + source->set_settle_time((uint16_t)_state.settle_time_ms); set_status(RangeFinder::RangeFinder_NoData); } @@ -52,7 +52,7 @@ AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder &_ranger, uint8_t insta */ bool AP_RangeFinder_analog::detect(RangeFinder &_ranger, uint8_t instance) { - if (_ranger._pin[instance] != -1) { + if (_ranger.state[instance].pin != -1) { return true; } return false; @@ -69,10 +69,10 @@ void AP_RangeFinder_analog::update_voltage(void) return; } // cope with changed settings - source->set_pin(ranger._pin[state.instance]); - source->set_stop_pin((uint8_t)ranger._stop_pin[state.instance]); - source->set_settle_time((uint16_t)ranger._settle_time_ms[state.instance]); - if (ranger._ratiometric[state.instance]) { + source->set_pin(state.pin); + source->set_stop_pin((uint8_t)state.stop_pin); + source->set_settle_time((uint16_t)state.settle_time_ms); + if (state.ratiometric) { state.voltage_mv = source->voltage_average_ratiometric() * 1000U; } else { state.voltage_mv = source->voltage_average() * 1000U; @@ -87,10 +87,10 @@ void AP_RangeFinder_analog::update(void) update_voltage(); float v = state.voltage_mv * 0.001f; float dist_m = 0; - float scaling = ranger._scaling[state.instance]; - float offset = ranger._offset[state.instance]; - RangeFinder::RangeFinder_Function function = (RangeFinder::RangeFinder_Function)ranger._function[state.instance].get(); - int16_t max_distance_cm = ranger._max_distance_cm[state.instance]; + float scaling = state.scaling; + float offset = state.offset; + RangeFinder::RangeFinder_Function function = (RangeFinder::RangeFinder_Function)state.function.get(); + int16_t max_distance_cm = state.max_distance_cm; switch (function) { case RangeFinder::FUNCTION_LINEAR: diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index e5b3c76bf6..b25748653c 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -39,14 +39,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Description: What type of rangefinder device that is connected // @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:TrOneI2C,15:LidarLiteV3-I2C,16:VL53L0X // @User: Standard - AP_GROUPINFO("_TYPE", 0, RangeFinder, _type[0], 0), + AP_GROUPINFO("_TYPE", 0, RangeFinder, state[0].type, 0), // @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. // @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 // @User: Standard - AP_GROUPINFO("_PIN", 1, RangeFinder, _pin[0], -1), + AP_GROUPINFO("_PIN", 1, RangeFinder, state[0].pin, -1), // @Param: _SCALING // @DisplayName: Rangefinder scaling @@ -54,7 +54,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: m/V // @Increment: 0.001 // @User: Standard - AP_GROUPINFO("_SCALING", 2, RangeFinder, _scaling[0], 3.0f), + AP_GROUPINFO("_SCALING", 2, RangeFinder, state[0].scaling, 3.0f), // @Param: _OFFSET // @DisplayName: rangefinder offset @@ -62,14 +62,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: V // @Increment: 0.001 // @User: Standard - AP_GROUPINFO("_OFFSET", 3, RangeFinder, _offset[0], 0.0f), + AP_GROUPINFO("_OFFSET", 3, RangeFinder, state[0].offset, 0.0f), // @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 // @User: Standard - AP_GROUPINFO("_FUNCTION", 4, RangeFinder, _function[0], 0), + AP_GROUPINFO("_FUNCTION", 4, RangeFinder, state[0].function, 0), // @Param: _MIN_CM // @DisplayName: Rangefinder minimum distance @@ -77,7 +77,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: cm // @Increment: 1 // @User: Standard - AP_GROUPINFO("_MIN_CM", 5, RangeFinder, _min_distance_cm[0], 20), + AP_GROUPINFO("_MIN_CM", 5, RangeFinder, state[0].min_distance_cm, 20), // @Param: _MAX_CM // @DisplayName: Rangefinder maximum distance @@ -85,14 +85,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: cm // @Increment: 1 // @User: Standard - AP_GROUPINFO("_MAX_CM", 6, RangeFinder, _max_distance_cm[0], 700), + AP_GROUPINFO("_MAX_CM", 6, RangeFinder, state[0].max_distance_cm, 700), // @Param: _STOP_PIN // @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 // @User: Standard - AP_GROUPINFO("_STOP_PIN", 7, RangeFinder, _stop_pin[0], -1), + AP_GROUPINFO("_STOP_PIN", 7, RangeFinder, state[0].stop_pin, -1), // @Param: _SETTLE // @DisplayName: Rangefinder settle time @@ -100,14 +100,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: ms // @Increment: 1 // @User: Standard - AP_GROUPINFO("_SETTLE", 8, RangeFinder, _settle_time_ms[0], 0), + AP_GROUPINFO("_SETTLE", 8, RangeFinder, state[0].settle_time_ms, 0), // @Param: _RMETRIC // @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 // @User: Standard - AP_GROUPINFO("_RMETRIC", 9, RangeFinder, _ratiometric[0], 1), + AP_GROUPINFO("_RMETRIC", 9, RangeFinder, state[0].ratiometric, 1), // @Param: _PWRRNG // @DisplayName: Powersave range @@ -124,7 +124,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Range: 5 127 // @Increment: 1 // @User: Standard - AP_GROUPINFO("_GNDCLEAR", 11, RangeFinder, _ground_clearance_cm[0], RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), + AP_GROUPINFO("_GNDCLEAR", 11, RangeFinder, state[0].ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), // @Param: _ADDR // @DisplayName: Bus address of sensor @@ -132,7 +132,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Range: 0 127 // @Increment: 1 // @User: Standard - AP_GROUPINFO("_ADDR", 23, RangeFinder, _address[0], 0), + AP_GROUPINFO("_ADDR", 23, RangeFinder, state[0].address, 0), // @Param: _POS_X // @DisplayName: X position offset @@ -151,14 +151,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @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. // @Units: m // @User: Advanced - AP_GROUPINFO("_POS", 49, RangeFinder, _pos_offset[0], 0.0f), + AP_GROUPINFO("_POS", 49, RangeFinder, state[0].pos_offset, 0.0f), // @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 - AP_GROUPINFO("_ORIENT", 53, RangeFinder, _orientation[0], ROTATION_PITCH_270), + AP_GROUPINFO("_ORIENT", 53, RangeFinder, state[0].orientation, ROTATION_PITCH_270), #if RANGEFINDER_MAX_INSTANCES > 1 // @Param: 2_TYPE @@ -166,14 +166,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Description: What type of rangefinder device that is connected // @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:TrOneI2C,15:LidarLiteV3-I2C,16:VL53L0X // @User: Advanced - AP_GROUPINFO("2_TYPE", 12, RangeFinder, _type[1], 0), + AP_GROUPINFO("2_TYPE", 12, RangeFinder, state[1].type, 0), // @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. // @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 // @User: Advanced - AP_GROUPINFO("2_PIN", 13, RangeFinder, _pin[1], -1), + AP_GROUPINFO("2_PIN", 13, RangeFinder, state[1].pin, -1), // @Param: 2_SCALING // @DisplayName: Rangefinder scaling @@ -181,7 +181,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: m/V // @Increment: 0.001 // @User: Advanced - AP_GROUPINFO("2_SCALING", 14, RangeFinder, _scaling[1], 3.0f), + AP_GROUPINFO("2_SCALING", 14, RangeFinder, state[1].scaling, 3.0f), // @Param: 2_OFFSET // @DisplayName: rangefinder offset @@ -189,14 +189,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: V // @Increment: 0.001 // @User: Advanced - AP_GROUPINFO("2_OFFSET", 15, RangeFinder, _offset[1], 0.0f), + AP_GROUPINFO("2_OFFSET", 15, RangeFinder, state[1].offset, 0.0f), // @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 // @User: Advanced - AP_GROUPINFO("2_FUNCTION", 16, RangeFinder, _function[1], 0), + AP_GROUPINFO("2_FUNCTION", 16, RangeFinder, state[1].function, 0), // @Param: 2_MIN_CM // @DisplayName: Rangefinder minimum distance @@ -204,7 +204,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: cm // @Increment: 1 // @User: Advanced - AP_GROUPINFO("2_MIN_CM", 17, RangeFinder, _min_distance_cm[1], 20), + AP_GROUPINFO("2_MIN_CM", 17, RangeFinder, state[1].min_distance_cm, 20), // @Param: 2_MAX_CM // @DisplayName: Rangefinder maximum distance @@ -212,14 +212,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: cm // @Increment: 1 // @User: Advanced - AP_GROUPINFO("2_MAX_CM", 18, RangeFinder, _max_distance_cm[1], 700), + AP_GROUPINFO("2_MAX_CM", 18, RangeFinder, state[1].max_distance_cm, 700), // @Param: 2_STOP_PIN // @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 // @User: Advanced - AP_GROUPINFO("2_STOP_PIN", 19, RangeFinder, _stop_pin[1], -1), + AP_GROUPINFO("2_STOP_PIN", 19, RangeFinder, state[1].stop_pin, -1), // @Param: 2_SETTLE // @DisplayName: Sonar settle time @@ -227,14 +227,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: ms // @Increment: 1 // @User: Advanced - AP_GROUPINFO("2_SETTLE", 20, RangeFinder, _settle_time_ms[1], 0), + AP_GROUPINFO("2_SETTLE", 20, RangeFinder, state[1].settle_time_ms, 0), // @Param: 2_RMETRIC // @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 // @User: Advanced - AP_GROUPINFO("2_RMETRIC", 21, RangeFinder, _ratiometric[1], 1), + AP_GROUPINFO("2_RMETRIC", 21, RangeFinder, state[1].ratiometric, 1), // @Param: 2_GNDCLEAR // @DisplayName: Distance (in cm) from the second range finder to the ground @@ -243,7 +243,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Range: 0 127 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("2_GNDCLEAR", 22, RangeFinder, _ground_clearance_cm[1], RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), + AP_GROUPINFO("2_GNDCLEAR", 22, RangeFinder, state[1].ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), // @Param: 2_ADDR // @DisplayName: Bus address of second rangefinder @@ -251,7 +251,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Range: 0 127 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("2_ADDR", 24, RangeFinder, _address[1], 0), + AP_GROUPINFO("2_ADDR", 24, RangeFinder, state[1].address, 0), // @Param: 2_POS_X // @DisplayName: X position offset @@ -270,14 +270,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @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. // @Units: m // @User: Advanced - AP_GROUPINFO("2_POS", 50, RangeFinder, _pos_offset[1], 0.0f), + AP_GROUPINFO("2_POS", 50, RangeFinder, state[1].pos_offset, 0.0f), // @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 - AP_GROUPINFO("2_ORIENT", 54, RangeFinder, _orientation[1], ROTATION_PITCH_270), + AP_GROUPINFO("2_ORIENT", 54, RangeFinder, state[1].orientation, ROTATION_PITCH_270), #endif #if RANGEFINDER_MAX_INSTANCES > 2 @@ -287,14 +287,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Description: What type of rangefinder device that is connected // @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:TrOneI2C,15:LidarLiteV3-I2C,16:VL53L0X // @User: Advanced - AP_GROUPINFO("3_TYPE", 25, RangeFinder, _type[2], 0), + AP_GROUPINFO("3_TYPE", 25, RangeFinder, state[2].type, 0), // @Param: 3_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. // @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 // @User: Advanced - AP_GROUPINFO("3_PIN", 26, RangeFinder, _pin[2], -1), + AP_GROUPINFO("3_PIN", 26, RangeFinder, state[2].pin, -1), // @Param: 3_SCALING // @DisplayName: Rangefinder scaling @@ -302,7 +302,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: m/V // @Increment: 0.001 // @User: Advanced - AP_GROUPINFO("3_SCALING", 27, RangeFinder, _scaling[2], 3.0f), + AP_GROUPINFO("3_SCALING", 27, RangeFinder, state[2].scaling, 3.0f), // @Param: 3_OFFSET // @DisplayName: rangefinder offset @@ -310,14 +310,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: V // @Increment: 0.001 // @User: Advanced - AP_GROUPINFO("3_OFFSET", 28, RangeFinder, _offset[2], 0.0f), + AP_GROUPINFO("3_OFFSET", 28, RangeFinder, state[2].offset, 0.0f), // @Param: 3_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 // @User: Advanced - AP_GROUPINFO("3_FUNCTION", 29, RangeFinder, _function[2], 0), + AP_GROUPINFO("3_FUNCTION", 29, RangeFinder, state[2].function, 0), // @Param: 3_MIN_CM // @DisplayName: Rangefinder minimum distance @@ -325,7 +325,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: cm // @Increment: 1 // @User: Advanced - AP_GROUPINFO("3_MIN_CM", 30, RangeFinder, _min_distance_cm[2], 20), + AP_GROUPINFO("3_MIN_CM", 30, RangeFinder, state[2].min_distance_cm, 20), // @Param: 3_MAX_CM // @DisplayName: Rangefinder maximum distance @@ -333,14 +333,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: cm // @Increment: 1 // @User: Advanced - AP_GROUPINFO("3_MAX_CM", 31, RangeFinder, _max_distance_cm[2], 700), + AP_GROUPINFO("3_MAX_CM", 31, RangeFinder, state[2].max_distance_cm, 700), // @Param: 3_STOP_PIN // @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 // @User: Advanced - AP_GROUPINFO("3_STOP_PIN", 32, RangeFinder, _stop_pin[2], -1), + AP_GROUPINFO("3_STOP_PIN", 32, RangeFinder, state[2].stop_pin, -1), // @Param: 3_SETTLE // @DisplayName: Sonar settle time @@ -348,14 +348,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: ms // @Increment: 1 // @User: Advanced - AP_GROUPINFO("3_SETTLE", 33, RangeFinder, _settle_time_ms[2], 0), + AP_GROUPINFO("3_SETTLE", 33, RangeFinder, state[2].settle_time_ms, 0), // @Param: 3_RMETRIC // @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 // @User: Advanced - AP_GROUPINFO("3_RMETRIC", 34, RangeFinder, _ratiometric[2], 1), + AP_GROUPINFO("3_RMETRIC", 34, RangeFinder, state[2].ratiometric, 1), // @Param: 3_GNDCLEAR // @DisplayName: Distance (in cm) from the third range finder to the ground @@ -364,7 +364,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Range: 0 127 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("3_GNDCLEAR", 35, RangeFinder, _ground_clearance_cm[2], RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), + AP_GROUPINFO("3_GNDCLEAR", 35, RangeFinder, state[2].ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), // @Param: 3_ADDR // @DisplayName: Bus address of third rangefinder @@ -372,7 +372,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Range: 0 127 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("3_ADDR", 36, RangeFinder, _address[2], 0), + AP_GROUPINFO("3_ADDR", 36, RangeFinder, state[2].address, 0), // @Param: 3_POS_X // @DisplayName: X position offset @@ -391,14 +391,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @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. // @Units: m // @User: Advanced - AP_GROUPINFO("3_POS", 51, RangeFinder, _pos_offset[2], 0.0f), + AP_GROUPINFO("3_POS", 51, RangeFinder, state[2].pos_offset, 0.0f), // @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 - AP_GROUPINFO("3_ORIENT", 55, RangeFinder, _orientation[2], ROTATION_PITCH_270), + AP_GROUPINFO("3_ORIENT", 55, RangeFinder, state[2].orientation, ROTATION_PITCH_270), #endif #if RANGEFINDER_MAX_INSTANCES > 3 @@ -408,14 +408,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Description: What type of rangefinder device that is connected // @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:TrOneI2C,15:LidarLiteV3-I2C,16:VL53L0X // @User: Advanced - AP_GROUPINFO("4_TYPE", 37, RangeFinder, _type[3], 0), + AP_GROUPINFO("4_TYPE", 37, RangeFinder, state[3].type, 0), // @Param: 4_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. // @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 // @User: Advanced - AP_GROUPINFO("4_PIN", 38, RangeFinder, _pin[3], -1), + AP_GROUPINFO("4_PIN", 38, RangeFinder, state[3].pin, -1), // @Param: 4_SCALING // @DisplayName: Rangefinder scaling @@ -423,7 +423,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: m/V // @Increment: 0.001 // @User: Advanced - AP_GROUPINFO("4_SCALING", 39, RangeFinder, _scaling[3], 3.0f), + AP_GROUPINFO("4_SCALING", 39, RangeFinder, state[3].scaling, 3.0f), // @Param: 4_OFFSET // @DisplayName: rangefinder offset @@ -431,14 +431,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: V // @Increment: 0.001 // @User: Advanced - AP_GROUPINFO("4_OFFSET", 40, RangeFinder, _offset[3], 0.0f), + AP_GROUPINFO("4_OFFSET", 40, RangeFinder, state[3].offset, 0.0f), // @Param: 4_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 // @User: Advanced - AP_GROUPINFO("4_FUNCTION", 41, RangeFinder, _function[3], 0), + AP_GROUPINFO("4_FUNCTION", 41, RangeFinder, state[3].function, 0), // @Param: 4_MIN_CM // @DisplayName: Rangefinder minimum distance @@ -446,7 +446,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: cm // @Increment: 1 // @User: Advanced - AP_GROUPINFO("4_MIN_CM", 42, RangeFinder, _min_distance_cm[3], 20), + AP_GROUPINFO("4_MIN_CM", 42, RangeFinder, state[3].min_distance_cm, 20), // @Param: 4_MAX_CM // @DisplayName: Rangefinder maximum distance @@ -454,14 +454,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: cm // @Increment: 1 // @User: Advanced - AP_GROUPINFO("4_MAX_CM", 43, RangeFinder, _max_distance_cm[3], 700), + AP_GROUPINFO("4_MAX_CM", 43, RangeFinder, state[3].max_distance_cm, 700), // @Param: 4_STOP_PIN // @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 // @User: Advanced - AP_GROUPINFO("4_STOP_PIN", 44, RangeFinder, _stop_pin[3], -1), + AP_GROUPINFO("4_STOP_PIN", 44, RangeFinder, state[3].stop_pin, -1), // @Param: 4_SETTLE // @DisplayName: Sonar settle time @@ -469,14 +469,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Units: ms // @Increment: 1 // @User: Advanced - AP_GROUPINFO("4_SETTLE", 45, RangeFinder, _settle_time_ms[3], 0), + AP_GROUPINFO("4_SETTLE", 45, RangeFinder, state[3].settle_time_ms, 0), // @Param: 4_RMETRIC // @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 // @User: Advanced - AP_GROUPINFO("4_RMETRIC", 46, RangeFinder, _ratiometric[3], 1), + AP_GROUPINFO("4_RMETRIC", 46, RangeFinder, state[3].ratiometric, 1), // @Param: 4_GNDCLEAR // @DisplayName: Distance (in cm) from the fourth range finder to the ground @@ -485,7 +485,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Range: 0 127 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("4_GNDCLEAR", 47, RangeFinder, _ground_clearance_cm[3], RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), + AP_GROUPINFO("4_GNDCLEAR", 47, RangeFinder, state[3].ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), // @Param: 4_ADDR // @DisplayName: Bus address of fourth rangefinder @@ -493,7 +493,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Range: 0 127 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("4_ADDR", 48, RangeFinder, _address[3], 0), + AP_GROUPINFO("4_ADDR", 48, RangeFinder, state[3].address, 0), // @Param: 4_POS_X // @DisplayName: X position offset @@ -512,14 +512,14 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Description: Z position of the fourth rangefinder in body frame. Use the zero range datum point if supplied. // @Units: m // @User: Advanced - AP_GROUPINFO("4_POS", 52, RangeFinder, _pos_offset[3], 0.0f), + AP_GROUPINFO("4_POS", 52, RangeFinder, state[3].pos_offset, 0.0f), // @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 - AP_GROUPINFO("4_ORIENT", 56, RangeFinder, _orientation[3], ROTATION_PITCH_270), + AP_GROUPINFO("4_ORIENT", 56, RangeFinder, state[3].orientation, ROTATION_PITCH_270), #endif AP_GROUPEND @@ -534,12 +534,8 @@ RangeFinder::RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orient // set orientation defaults for (uint8_t i=0; iget_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, _address[instance]))); + hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, state[instance].address))); } break; case RangeFinder_TYPE_TRONE: @@ -713,7 +709,7 @@ RangeFinder::RangeFinder_Status RangeFinder::status(uint8_t instance) const return RangeFinder_NotConnected; } - if (drivers[instance] == nullptr || _type[instance] == RangeFinder_TYPE_NONE) { + if (drivers[instance] == nullptr || state[instance].type == RangeFinder_TYPE_NONE) { return RangeFinder_NotConnected; } @@ -733,7 +729,7 @@ void RangeFinder::handle_msg(mavlink_message_t *msg) { uint8_t i; for (i=0; ihandle_msg(msg); } } @@ -750,7 +746,7 @@ bool RangeFinder::has_orientation(enum Rotation orientation) const bool RangeFinder::find_instance(enum Rotation orientation, uint8_t &instance) const { for (uint8_t i=0; i RANGEFINDER_PREARM_REQUIRED_CHANGE_CM) && (state[instance].pre_arm_distance_max < RANGEFINDER_PREARM_ALT_MAX_CM) && - ((int16_t)state[instance].pre_arm_distance_min < (MAX(_ground_clearance_cm[instance],min_distance_cm(instance)) + 10)) && - ((int16_t)state[instance].pre_arm_distance_min > (MIN(_ground_clearance_cm[instance],min_distance_cm(instance)) - 10))) { + ((int16_t)state[instance].pre_arm_distance_min < (MAX(state[instance].ground_clearance_cm,min_distance_cm(instance)) + 10)) && + ((int16_t)state[instance].pre_arm_distance_min > (MIN(state[instance].ground_clearance_cm,min_distance_cm(instance)) - 10))) { state[instance].pre_arm_check = true; } } @@ -888,7 +884,7 @@ MAV_DISTANCE_SENSOR RangeFinder::get_sensor_type(uint8_t instance) const { return MAV_DISTANCE_SENSOR_UNKNOWN; } - if (drivers[instance] == nullptr || _type[instance] == RangeFinder_TYPE_NONE) { + if (drivers[instance] == nullptr || state[instance].type == RangeFinder_TYPE_NONE) { return MAV_DISTANCE_SENSOR_UNKNOWN; } return drivers[instance]->get_sensor_type(); diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index f2dcfe294c..296e60c2b9 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -32,6 +32,7 @@ class RangeFinder { public: friend class AP_RangeFinder_Backend; + friend class AP_RangeFinder_analog; // bodgy detect function RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orientation_default); @@ -81,25 +82,26 @@ public: bool pre_arm_check; // true if sensor has passed pre-arm checks uint16_t pre_arm_distance_min; // min distance captured during pre-arm checks uint16_t pre_arm_distance_max; // max distance captured during pre-arm checks + + AP_Int8 type; + AP_Int8 pin; + AP_Int8 ratiometric; + AP_Int8 stop_pin; + AP_Int16 settle_time_ms; + AP_Float scaling; + AP_Float offset; + AP_Int8 function; + AP_Int16 min_distance_cm; + AP_Int16 max_distance_cm; + AP_Int8 ground_clearance_cm; + AP_Int8 address; + AP_Vector3f pos_offset; // position offset in body frame + AP_Int8 orientation; }; - // parameters for each instance - AP_Int8 _type[RANGEFINDER_MAX_INSTANCES]; - AP_Int8 _pin[RANGEFINDER_MAX_INSTANCES]; - AP_Int8 _ratiometric[RANGEFINDER_MAX_INSTANCES]; - AP_Int8 _stop_pin[RANGEFINDER_MAX_INSTANCES]; - AP_Int16 _settle_time_ms[RANGEFINDER_MAX_INSTANCES]; - AP_Float _scaling[RANGEFINDER_MAX_INSTANCES]; - AP_Float _offset[RANGEFINDER_MAX_INSTANCES]; - AP_Int8 _function[RANGEFINDER_MAX_INSTANCES]; - AP_Int16 _min_distance_cm[RANGEFINDER_MAX_INSTANCES]; - AP_Int16 _max_distance_cm[RANGEFINDER_MAX_INSTANCES]; - AP_Int8 _ground_clearance_cm[RANGEFINDER_MAX_INSTANCES]; - AP_Int8 _address[RANGEFINDER_MAX_INSTANCES]; AP_Int16 _powersave_range; - AP_Vector3f _pos_offset[RANGEFINDER_MAX_INSTANCES]; // position offset in body frame - AP_Int8 _orientation[RANGEFINDER_MAX_INSTANCES]; + // parameters for each instance static const struct AP_Param::GroupInfo var_info[]; // Return the number of range finder instances @@ -127,7 +129,7 @@ public: // get orientation of a given range finder enum Rotation get_orientation(uint8_t instance) const { - return (instance ranger._max_distance_cm[state.instance]) { + if ((int16_t)state.distance_cm > state.max_distance_cm) { set_status(RangeFinder::RangeFinder_OutOfRangeHigh); - } else if ((int16_t)state.distance_cm < ranger._min_distance_cm[state.instance]) { + } else if ((int16_t)state.distance_cm < state.min_distance_cm) { set_status(RangeFinder::RangeFinder_OutOfRangeLow); } else { set_status(RangeFinder::RangeFinder_Good); diff --git a/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp b/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp index 0345395e5e..d423cf6f91 100644 --- a/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp +++ b/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp @@ -36,7 +36,7 @@ void loop() sonar.update(); hal.console->printf("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n", - (int)sonar._type[0], (int)sonar.status(0), sonar.distance_cm(0), (int)sonar._type[1], (int)sonar.status(1), sonar.distance_cm(1)); + (int)sonar.type(0), (int)sonar.status(0), sonar.distance_cm(0), (int)sonar.type(1), (int)sonar.status(1), sonar.distance_cm(1)); } AP_HAL_MAIN();