mirror of https://github.com/ArduPilot/ardupilot
Rover: make AP_RANGEFINDER_ENABLED remove more code
This commit is contained in:
parent
a5d7000ffd
commit
c40b1627dd
|
@ -58,6 +58,7 @@ void GCS_Rover::update_vehicle_sensor_status_flags(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#if AP_RANGEFINDER_ENABLED
|
||||
const RangeFinder *rangefinder = RangeFinder::get_singleton();
|
||||
if (rangefinder && rangefinder->num_sensors() > 0) {
|
||||
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
||||
|
@ -67,4 +68,5 @@ void GCS_Rover::update_vehicle_sensor_status_flags(void)
|
|||
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
|
@ -29,6 +29,7 @@ void Rover::Log_Write_Attitude()
|
|||
}
|
||||
}
|
||||
|
||||
#if AP_RANGEFINDER_ENABLED
|
||||
// Write a range finder depth message
|
||||
void Rover::Log_Write_Depth()
|
||||
{
|
||||
|
@ -83,6 +84,7 @@ void Rover::Log_Write_Depth()
|
|||
gcs().send_message(MSG_WATER_DEPTH);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
// guided mode logging
|
||||
struct PACKED log_GuidedTarget {
|
||||
|
|
|
@ -275,9 +275,11 @@ const AP_Param::Info Rover::var_info[] = {
|
|||
|
||||
// AP_SerialManager was here
|
||||
|
||||
#if AP_RANGEFINDER_ENABLED
|
||||
// @Group: RNGFND
|
||||
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp
|
||||
GOBJECT(rangefinder, "RNGFND", RangeFinder),
|
||||
#endif
|
||||
|
||||
// @Group: INS
|
||||
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
|
||||
|
|
|
@ -70,7 +70,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
|
|||
// Function name, Hz, us,
|
||||
SCHED_TASK(read_radio, 50, 200, 3),
|
||||
SCHED_TASK(ahrs_update, 400, 400, 6),
|
||||
#if AP_RANGEFINDER_ENABLED
|
||||
SCHED_TASK(read_rangefinders, 50, 200, 9),
|
||||
#endif
|
||||
#if AP_OPTICALFLOW_ENABLED
|
||||
SCHED_TASK_CLASS(AP_OpticalFlow, &rover.optflow, update, 200, 160, 11),
|
||||
#endif
|
||||
|
|
|
@ -206,8 +206,10 @@ private:
|
|||
// true if we have a position estimate from AHRS
|
||||
bool have_position;
|
||||
|
||||
#if AP_RANGEFINDER_ENABLED
|
||||
// range finder last update for each instance (used for DPTH logging)
|
||||
uint32_t rangefinder_last_reading_ms[RANGEFINDER_MAX_INSTANCES];
|
||||
#endif
|
||||
|
||||
// Ground speed
|
||||
// The amount current ground speed is below min ground speed. meters per second
|
||||
|
@ -372,7 +374,9 @@ private:
|
|||
void update_compass(void);
|
||||
void compass_save(void);
|
||||
void update_wheel_encoder();
|
||||
#if AP_RANGEFINDER_ENABLED
|
||||
void read_rangefinders(void);
|
||||
#endif
|
||||
|
||||
// Steering.cpp
|
||||
void set_servos(void);
|
||||
|
|
|
@ -87,6 +87,7 @@ void Rover::update_wheel_encoder()
|
|||
#endif
|
||||
}
|
||||
|
||||
#if AP_RANGEFINDER_ENABLED
|
||||
// read the rangefinders
|
||||
void Rover::read_rangefinders(void)
|
||||
{
|
||||
|
@ -95,3 +96,4 @@ void Rover::read_rangefinders(void)
|
|||
Log_Write_Depth();
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -40,9 +40,11 @@ void Rover::init_ardupilot()
|
|||
airspeed.set_log_bit(MASK_LOG_IMU);
|
||||
#endif
|
||||
|
||||
#if AP_RANGEFINDER_ENABLED
|
||||
// initialise rangefinder
|
||||
rangefinder.set_log_rfnd_bit(MASK_LOG_RANGEFINDER);
|
||||
rangefinder.init(ROTATION_NONE);
|
||||
#endif
|
||||
|
||||
#if HAL_PROXIMITY_ENABLED
|
||||
// init proximity sensor
|
||||
|
|
Loading…
Reference in New Issue