Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: integrate HAL_PROXIMITY_ENABLED

Browse Source
This commit is contained in:
Randy Mackay 2021-03-25 17:47:29 +09:00
parent dbecf363f3
commit 1b69cf68ec
5 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Rover/GCS_Rover.cpp
View File

@ -28,11 +28,13 @@ bool GCS_Rover::supersimple_input_active() const
void GCS_Rover::update_vehicle_sensor_status_flags(void) void GCS_Rover::update_vehicle_sensor_status_flags(void)
{ {
// first what sensors/controllers we have // first what sensors/controllers we have
#if HAL_PROXIMITY_ENABLED
const AP_Proximity *proximity = AP_Proximity::get_singleton(); const AP_Proximity *proximity = AP_Proximity::get_singleton();
if (proximity && proximity->get_status() > AP_Proximity::Status::NotConnected) { if (proximity && proximity->get_status() > AP_Proximity::Status::NotConnected) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
} }
#endif
control_sensors_present |= control_sensors_present |=
MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
@ -62,7 +64,10 @@ void GCS_Rover::update_vehicle_sensor_status_flags(void)
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
} }
} }
#if HAL_PROXIMITY_ENABLED
if (proximity && proximity->get_status() != AP_Proximity::Status::NoData) { if (proximity && proximity->get_status() != AP_Proximity::Status::NoData) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
} }
#endif
} }

4
Rover/Parameters.cpp
View File

@ -496,9 +496,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AC_Fence/AC_Fence.cpp // @Path: ../libraries/AC_Fence/AC_Fence.cpp
AP_SUBGROUPINFO(fence, "FENCE_", 17, ParametersG2, AC_Fence), AP_SUBGROUPINFO(fence, "FENCE_", 17, ParametersG2, AC_Fence),
#if HAL_PROXIMITY_ENABLED
// @Group: PRX // @Group: PRX
// @Path: ../libraries/AP_Proximity/AP_Proximity.cpp // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp
AP_SUBGROUPINFO(proximity, "PRX", 18, ParametersG2, AP_Proximity), AP_SUBGROUPINFO(proximity, "PRX", 18, ParametersG2, AP_Proximity),
#endif
// @Group: AVOID_ // @Group: AVOID_
// @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
@ -701,7 +703,9 @@ ParametersG2::ParametersG2(void)
wheel_rate_control(wheel_encoder), wheel_rate_control(wheel_encoder),
attitude_control(rover.ahrs), attitude_control(rover.ahrs),
smart_rtl(), smart_rtl(),
#if HAL_PROXIMITY_ENABLED
proximity(), proximity(),
#endif
avoid(), avoid(),
follow(), follow(),
windvane(), windvane(),

2
Rover/Parameters.h
View File

@ -329,8 +329,10 @@ public:
// fence library // fence library
AC_Fence fence; AC_Fence fence;
#if HAL_PROXIMITY_ENABLED
// proximity library // proximity library
AP_Proximity proximity; AP_Proximity proximity;
#endif
// avoidance library // avoidance library
AC_Avoid avoid; AC_Avoid avoid;

4
Rover/Rover.cpp
View File

@ -56,7 +56,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_GPS, &rover.gps, update, 50, 300), SCHED_TASK_CLASS(AP_GPS, &rover.gps, update, 50, 300),
SCHED_TASK_CLASS(AP_Baro, &rover.barometer, update, 10, 200), SCHED_TASK_CLASS(AP_Baro, &rover.barometer, update, 10, 200),
SCHED_TASK_CLASS(AP_Beacon, &rover.g2.beacon, update, 50, 200), SCHED_TASK_CLASS(AP_Beacon, &rover.g2.beacon, update, 50, 200),
#if HAL_PROXIMITY_ENABLED
SCHED_TASK_CLASS(AP_Proximity, &rover.g2.proximity, update, 50, 200), SCHED_TASK_CLASS(AP_Proximity, &rover.g2.proximity, update, 50, 200),
#endif
SCHED_TASK_CLASS(AP_WindVane, &rover.g2.windvane, update, 20, 100), SCHED_TASK_CLASS(AP_WindVane, &rover.g2.windvane, update, 20, 100),
SCHED_TASK_CLASS(AC_Fence, &rover.g2.fence, update, 10, 100), SCHED_TASK_CLASS(AC_Fence, &rover.g2.fence, update, 10, 100),
SCHED_TASK(update_wheel_encoder, 50, 200), SCHED_TASK(update_wheel_encoder, 50, 200),
@ -300,9 +302,11 @@ void Rover::update_logging1(void)
Log_Write_Nav_Tuning(); Log_Write_Nav_Tuning();
} }
#if HAL_PROXIMITY_ENABLED
if (should_log(MASK_LOG_RANGEFINDER)) { if (should_log(MASK_LOG_RANGEFINDER)) {
logger.Write_Proximity(g2.proximity); logger.Write_Proximity(g2.proximity);
} }
#endif
} }
/* /*

2
Rover/system.cpp
View File

@ -67,8 +67,10 @@ void Rover::init_ardupilot()
// initialise rangefinder // initialise rangefinder
rangefinder.init(ROTATION_NONE); rangefinder.init(ROTATION_NONE);
#if HAL_PROXIMITY_ENABLED
// init proximity sensor // init proximity sensor
g2.proximity.init(); g2.proximity.init();
#endif
// init beacons used for non-gps position estimation // init beacons used for non-gps position estimation
g2.beacon.init(); g2.beacon.init();