Line data    Source code

       1             : #include "GCS_Plane.h"
       2             : #include "Plane.h"
       3             : 
       4      555335 : void GCS_Plane::update_vehicle_sensor_status_flags(void)
       5             : {
       6             :     // reverse thrust
       7      555335 :     if (plane.have_reverse_thrust()) {
       8           0 :         control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;
       9             :     }
      10      555335 :     if (plane.have_reverse_thrust() && is_negative(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle))) {
      11           0 :         control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;
      12           0 :         control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR;
      13             :     }
      14             : 
      15             :     // flightmode-specific
      16      555335 :     control_sensors_present |=
      17             :         MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
      18             :         MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
      19             :         MAV_SYS_STATUS_SENSOR_YAW_POSITION |
      20             :         MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL |
      21             :         MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
      22             : 
      23      555335 :     bool rate_controlled = false;
      24      555335 :     bool attitude_stabilized = false;
      25      555335 :     switch (plane.control_mode->mode_number()) {
      26       88883 :     case Mode::Number::MANUAL:
      27       88883 :         break;
      28             : 
      29         648 :     case Mode::Number::ACRO:
      30             : #if HAL_QUADPLANE_ENABLED
      31             :     case Mode::Number::QACRO:
      32             : #endif
      33         648 :         rate_controlled = true;
      34         648 :         break;
      35             : 
      36      108827 :     case Mode::Number::STABILIZE:
      37             :     case Mode::Number::FLY_BY_WIRE_A:
      38             :     case Mode::Number::AUTOTUNE:
      39             : #if HAL_QUADPLANE_ENABLED
      40             :     case Mode::Number::QSTABILIZE:
      41             :     case Mode::Number::QHOVER:
      42             :     case Mode::Number::QLAND:
      43             :     case Mode::Number::QLOITER:
      44             : #if QAUTOTUNE_ENABLED
      45             :     case Mode::Number::QAUTOTUNE:
      46             : #endif
      47             : #endif  // HAL_QUADPLANE_ENABLED
      48             :     case Mode::Number::FLY_BY_WIRE_B:
      49             :     case Mode::Number::CRUISE:
      50      108827 :         rate_controlled = true;
      51      108827 :         attitude_stabilized = true;
      52      108827 :         break;
      53             : 
      54          46 :     case Mode::Number::TRAINING:
      55          46 :         if (!plane.training_manual_roll || !plane.training_manual_pitch) {
      56          40 :             rate_controlled = true;
      57          40 :             attitude_stabilized = true;
      58             :         }
      59          46 :         break;
      60             : 
      61      356672 :     case Mode::Number::AUTO:
      62             :     case Mode::Number::RTL:
      63             :     case Mode::Number::LOITER:
      64             :     case Mode::Number::AVOID_ADSB:
      65             :     case Mode::Number::GUIDED:
      66             :     case Mode::Number::CIRCLE:
      67             :     case Mode::Number::TAKEOFF:
      68             : #if MODE_AUTOLAND_ENABLED
      69             :     case Mode::Number::AUTOLAND:
      70             : #endif
      71             : #if HAL_QUADPLANE_ENABLED
      72             :     case Mode::Number::QRTL:
      73             :     case Mode::Number::LOITER_ALT_QLAND:
      74             : #endif
      75             :     case Mode::Number::THERMAL:
      76      356672 :         rate_controlled = true;
      77      356672 :         attitude_stabilized = true;
      78      356672 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION;
      79      356672 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_YAW_POSITION;
      80      356672 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
      81      356672 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
      82      356672 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
      83      356672 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
      84      356672 :         break;
      85             : 
      86         259 :     case Mode::Number::INITIALISING:
      87         259 :         break;
      88             :     }
      89             : 
      90      555335 :     if (rate_controlled) {
      91      466187 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
      92      466187 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
      93             :     }
      94      555335 :     if (attitude_stabilized) {
      95      465539 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION;
      96      465539 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION;
      97             :     }
      98             : 
      99             : #if AP_TERRAIN_AVAILABLE
     100      555335 :     switch (plane.terrain.status()) {
     101        3514 :     case AP_Terrain::TerrainStatusDisabled:
     102        3514 :         break;
     103       34001 :     case AP_Terrain::TerrainStatusUnhealthy:
     104       34001 :         control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
     105       34001 :         control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
     106       34001 :         break;
     107      517820 :     case AP_Terrain::TerrainStatusOK:
     108      517820 :         control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
     109      517820 :         control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
     110      517820 :         control_sensors_health  |= MAV_SYS_STATUS_TERRAIN;
     111      517820 :         break;
     112             :     }
     113             : #endif
     114             : 
     115             : #if AP_RANGEFINDER_ENABLED
     116      555335 :     const RangeFinder *rangefinder = RangeFinder::get_singleton();
     117      555335 :     if (rangefinder && rangefinder->has_orientation(plane.rangefinder_orientation())) {
     118       18633 :         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
     119       18633 :         if (uint16_t(plane.g.rangefinder_landing.get()) != 0) {
     120        5551 :             control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
     121             :         }
     122       18633 :         if (rangefinder->has_data_orient(plane.rangefinder_orientation())) {
     123       17606 :             control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;            
     124             :         }
     125             :     }
     126             : #endif
     127      555335 : }