Browse Source

AP_RangeFinder: allow separate enable of MSP rangefinder

c415-sdk
Andrew Tridgell 5 years ago
parent
commit
84f7589f2d
  1. 8
      libraries/AP_RangeFinder/AP_RangeFinder.cpp
  2. 8
      libraries/AP_RangeFinder/AP_RangeFinder.h

8
libraries/AP_RangeFinder/AP_RangeFinder.cpp

@ -542,13 +542,13 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) @@ -542,13 +542,13 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
break;
#endif
#if HAL_MSP_ENABLED
#if HAL_MSP_RANGEFINDER_ENABLED
case Type::MSP:
if (AP_RangeFinder_MSP::detect()) {
drivers[instance] = new AP_RangeFinder_MSP(state[instance], params[instance]);
}
break;
#endif //HAL_MSP_ENABLED
#endif // HAL_MSP_RANGEFINDER_ENABLED
default:
break;
@ -596,7 +596,7 @@ void RangeFinder::handle_msg(const mavlink_message_t &msg) @@ -596,7 +596,7 @@ void RangeFinder::handle_msg(const mavlink_message_t &msg)
}
}
#if HAL_MSP_ENABLED
#if HAL_MSP_RANGEFINDER_ENABLED
void RangeFinder::handle_msp(const MSP::msp_rangefinder_sensor_t &pkt)
{
uint8_t i;
@ -606,7 +606,7 @@ void RangeFinder::handle_msp(const MSP::msp_rangefinder_sensor_t &pkt) @@ -606,7 +606,7 @@ void RangeFinder::handle_msp(const MSP::msp_rangefinder_sensor_t &pkt)
}
}
}
#endif //HAL_MSP_ENABLED
#endif // HAL_MSP_RANGEFINDER_ENABLED
// return true if we have a range finder with the specified orientation
bool RangeFinder::has_orientation(enum Rotation orientation) const

8
libraries/AP_RangeFinder/AP_RangeFinder.h

@ -34,6 +34,10 @@ @@ -34,6 +34,10 @@
#define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM 50
#endif
#ifndef HAL_MSP_RANGEFINDER_ENABLED
#define HAL_MSP_RANGEFINDER_ENABLED HAL_MSP_ENABLED && !HAL_MINIMIZE_FEATURES
#endif
class AP_RangeFinder_Backend;
class RangeFinder
@ -136,10 +140,10 @@ public: @@ -136,10 +140,10 @@ public:
// Handle an incoming DISTANCE_SENSOR message (from a MAVLink enabled range finder)
void handle_msg(const mavlink_message_t &msg);
#if HAL_MSP_ENABLED
#if HAL_MSP_RANGEFINDER_ENABLED
// Handle an incoming DISTANCE_SENSOR message (from a MSP enabled range finder)
void handle_msp(const MSP::msp_rangefinder_sensor_t &pkt);
#endif //HAL_MSP_ENABLED
#endif //HAL_MSP_RANGEFINDER_ENABLED
// return true if we have a range finder with the specified orientation
bool has_orientation(enum Rotation orientation) const;

Loading…
Cancel
Save