diff --git a/Rover/GCS_Rover.cpp b/Rover/GCS_Rover.cpp index 60c9215a25..9ad82f7e32 100644 --- a/Rover/GCS_Rover.cpp +++ b/Rover/GCS_Rover.cpp @@ -27,15 +27,7 @@ bool GCS_Rover::supersimple_input_active() const void GCS_Rover::update_vehicle_sensor_status_flags(void) { - // first what sensors/controllers we have -#if HAL_PROXIMITY_ENABLED - const AP_Proximity *proximity = AP_Proximity::get_singleton(); - if (proximity && proximity->get_status() > AP_Proximity::Status::NotConnected) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } -#endif - + // mode-specific: control_sensors_present |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | @@ -55,6 +47,17 @@ void GCS_Rover::update_vehicle_sensor_status_flags(void) control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control } +#if HAL_PROXIMITY_ENABLED + const AP_Proximity *proximity = AP_Proximity::get_singleton(); + if (proximity && proximity->get_status() > AP_Proximity::Status::NotConnected) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + if (proximity && proximity->get_status() != AP_Proximity::Status::NoData) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } +#endif + const RangeFinder *rangefinder = RangeFinder::get_singleton(); if (rangefinder && rangefinder->num_sensors() > 0) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; @@ -64,10 +67,4 @@ void GCS_Rover::update_vehicle_sensor_status_flags(void) control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } - -#if HAL_PROXIMITY_ENABLED - if (proximity && proximity->get_status() != AP_Proximity::Status::NoData) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } -#endif }