Line data    Source code

       1             : #include "GCS_Rover.h"
       2             : 
       3             : #include "Rover.h"
       4             : 
       5             : #include <AP_RangeFinder/AP_RangeFinder_Backend.h>
       6             : 
       7           0 : bool GCS_Rover::simple_input_active() const
       8             : {
       9           0 :     if (rover.control_mode != &rover.mode_simple) {
      10           0 :         return false;
      11             :     }
      12           0 :     return (rover.g2.simple_type == ModeSimple::Simple_InitialHeading);
      13             : }
      14             : 
      15           0 : bool GCS_Rover::supersimple_input_active() const
      16             : {
      17           0 :     if (rover.control_mode != &rover.mode_simple) {
      18           0 :         return false;
      19             :     }
      20           0 :     return (rover.g2.simple_type == ModeSimple::Simple_CardinalDirections);
      21             : }
      22             : 
      23      125541 : void GCS_Rover::update_vehicle_sensor_status_flags(void)
      24             : {
      25             :     // mode-specific:
      26      125541 :     control_sensors_present |=
      27             :         MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
      28             :         MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
      29             :         MAV_SYS_STATUS_SENSOR_YAW_POSITION |
      30             :         MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
      31             : 
      32      125541 :     if (rover.control_mode->attitude_stabilized()) {
      33       52514 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
      34       52514 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
      35       52514 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control
      36       52514 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control
      37             :     }
      38      125541 :     if (rover.control_mode->is_autopilot_mode()) {
      39       50963 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position
      40       50963 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position
      41       50963 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control
      42       50963 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control
      43             :     }
      44             : 
      45             : #if HAL_PROXIMITY_ENABLED
      46      125541 :     const AP_Proximity *proximity = AP_Proximity::get_singleton();
      47      125541 :     if (proximity && proximity->get_status() > AP_Proximity::Status::NotConnected) {
      48        1340 :         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
      49        1340 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
      50             :     }
      51      125541 :     if (proximity && proximity->get_status() != AP_Proximity::Status::NoData) {
      52      125026 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
      53             :     }
      54             : #endif
      55             : 
      56             : #if AP_RANGEFINDER_ENABLED
      57      125541 :     const RangeFinder *rangefinder = RangeFinder::get_singleton();
      58      125541 :     if (rangefinder && rangefinder->num_sensors() > 0) {
      59        2308 :         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
      60        2308 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
      61        2308 :         AP_RangeFinder_Backend *s = rangefinder->get_backend(0);
      62        2308 :         if (s != nullptr && s->has_data()) {
      63        2287 :             control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
      64             :         }
      65             :     }
      66             : #endif
      67      125541 : }