diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index f1002ccbbd..28fedcac0c 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -266,6 +266,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan) } #endif +#if RANGEFINDER_ENABLED == ENABLED if (rangefinder.num_sensors() > 0) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; if (g.rangefinder_landing) { @@ -275,6 +276,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan) control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } +#endif if (AP_Notify::flags.initialising) { // while initialising the gyros and accels are not enabled @@ -456,6 +458,7 @@ void Plane::send_wind(mavlink_channel_t chan) void Plane::send_rangefinder(mavlink_channel_t chan) { +#if RANGEFINDER_ENABLED == ENABLED if (!rangefinder.has_data()) { // no sonar to report return; @@ -464,6 +467,7 @@ void Plane::send_rangefinder(mavlink_channel_t chan) chan, rangefinder.distance_cm() * 0.01f, rangefinder.voltage_mv()*0.001f); +#endif } void Plane::send_current_waypoint(mavlink_channel_t chan) diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 212b3d8b45..a51d299724 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -357,6 +357,7 @@ struct PACKED log_Sonar { // Write a sonar packet void Plane::Log_Write_Sonar() { +#if RANGEFINDER_ENABLED == ENABLED struct log_Sonar pkt = { LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG), timestamp : millis(), @@ -369,6 +370,7 @@ void Plane::Log_Write_Sonar() correction : rangefinder_state.correction }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); +#endif } struct PACKED log_Optflow { diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index af05440961..88e1f657d0 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -987,9 +987,11 @@ const AP_Param::Info Plane::var_info[] PROGMEM = { // @Path: ../libraries/AP_Relay/AP_Relay.cpp GOBJECT(relay, "RELAY_", AP_Relay), +#if RANGEFINDER_ENABLED == ENABLED // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp GOBJECT(rangefinder, "RNGFND", RangeFinder), +#endif // @Param: RNGFND_LANDING // @DisplayName: Enable rangefinder for landing diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 33ceb50523..65131d2c66 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -233,6 +233,7 @@ private: // a pin for reading the receiver RSSI voltage. AP_HAL::AnalogSource *rssi_analog_source; +#if RANGEFINDER_ENABLED == ENABLED // rangefinder RangeFinder rangefinder; @@ -242,6 +243,7 @@ private: uint32_t last_correction_time_ms; uint8_t in_range_count; } rangefinder_state; +#endif // Relay AP_Relay relay; diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index d08ed5a5a7..66ab26a1a3 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -525,6 +525,7 @@ float Plane::lookahead_adjustment(void) */ float Plane::rangefinder_correction(void) { +#if RANGEFINDER_ENABLED == ENABLED if (millis() - rangefinder_state.last_correction_time_ms > 5000) { // we haven't had any rangefinder data for 5s - don't use it return 0; @@ -540,9 +541,12 @@ float Plane::rangefinder_correction(void) } return rangefinder_state.correction; +#else + return 0; +#endif } - +#if RANGEFINDER_ENABLED == ENABLED /* update the offset between rangefinder height and terrain height */ @@ -591,3 +595,4 @@ void Plane::rangefinder_height_update(void) rangefinder_state.last_correction_time_ms = millis(); } } +#endif diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 265dc37188..1b321e8435 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -63,10 +63,10 @@ // #ifndef FRSKY_TELEM_ENABLED -#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 - # define FRSKY_TELEM_ENABLED DISABLED -#else +#if HAL_CPU_CLASS > HAL_CPU_CLASS_16 # define FRSKY_TELEM_ENABLED ENABLED +#else + # define FRSKY_TELEM_ENABLED DISABLED #endif #endif @@ -82,6 +82,12 @@ #endif #endif +#if HAL_CPU_CLASS > HAL_CPU_CLASS_16 +# define RANGEFINDER_ENABLED ENABLED +#else +# define RANGEFINDER_ENABLED DISABLED +#endif + ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // RADIO CONFIGURATION diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index aa7c4b7c48..c2992e9c06 100644 --- a/ArduPlane/landing.cpp +++ b/ArduPlane/landing.cpp @@ -52,9 +52,14 @@ bool Plane::verify_land() rangefinder data (to prevent us keeping throttle on after landing if we've had positive baro drift) */ +#if RANGEFINDER_ENABLED == ENABLED + bool rangefinder_in_range = rangefinder_state.in_range; +#else + bool rangefinder_in_range = false; +#endif if (height <= g.land_flare_alt || (aparm.land_flare_sec > 0 && height <= auto_state.land_sink_rate * aparm.land_flare_sec) || - (!rangefinder_state.in_range && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) || + (!rangefinder_in_range && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) || (fabsf(auto_state.land_sink_rate) < 0.2f && !is_flying())) { if (!auto_state.land_complete) { diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index 2c3f2631dc..0252c89856 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -12,7 +12,9 @@ void Plane::init_barometer(void) void Plane::init_rangefinder(void) { +#if RANGEFINDER_ENABLED == ENABLED rangefinder.init(); +#endif } /* @@ -20,12 +22,14 @@ void Plane::init_rangefinder(void) */ void Plane::read_rangefinder(void) { +#if RANGEFINDER_ENABLED == ENABLED rangefinder.update(); if (should_log(MASK_LOG_SONAR)) Log_Write_Sonar(); rangefinder_height_update(); +#endif } /*