Line data    Source code

       1             : #include "GCS_Copter.h"
       2             : 
       3             : #include "Copter.h"
       4             : 
       5           6 : const char* GCS_Copter::frame_string() const
       6             : {
       7           6 :     if (copter.motors == nullptr) {
       8           5 :         return "MultiCopter";
       9             :     }
      10           1 :     return copter.motors->get_frame_string();
      11             : }
      12             : 
      13           0 : bool GCS_Copter::simple_input_active() const
      14             : {
      15           0 :     return copter.simple_mode == Copter::SimpleMode::SIMPLE;
      16             : }
      17             : 
      18           0 : bool GCS_Copter::supersimple_input_active() const
      19             : {
      20           0 :     return copter.simple_mode == Copter::SimpleMode::SUPERSIMPLE;
      21             : }
      22             : 
      23      386443 : void GCS_Copter::update_vehicle_sensor_status_flags(void)
      24             : {
      25      386443 :     control_sensors_present |=
      26             :         MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
      27             :         MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
      28             :         MAV_SYS_STATUS_SENSOR_YAW_POSITION;
      29             : 
      30      386443 :     control_sensors_enabled |=
      31             :         MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
      32             :         MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
      33             :         MAV_SYS_STATUS_SENSOR_YAW_POSITION;
      34             : 
      35      386443 :     control_sensors_health |=
      36             :         MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
      37             :         MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
      38             :         MAV_SYS_STATUS_SENSOR_YAW_POSITION;
      39             : 
      40             :     // Update position controller flags
      41      386443 :     if (copter.pos_control != nullptr) {
      42      386443 :         control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
      43      386443 :         control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
      44             : 
      45             :         // XY position controller
      46      386443 :         if (copter.pos_control->NE_is_active()) {
      47      221694 :             control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
      48      221694 :             control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
      49             :         }
      50             : 
      51             :         // Z altitude controller
      52      386443 :         if (copter.pos_control->D_is_active()) {
      53      277107 :             control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
      54      277107 :             control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
      55             :         }
      56             :     }
      57             : 
      58             :     // optional sensors, some of which are essentially always
      59             :     // available in the firmware:
      60             : #if HAL_PROXIMITY_ENABLED
      61      386443 :     if (copter.g2.proximity.sensor_present()) {
      62        3279 :         control_sensors_present |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
      63             :     }
      64      386443 :     if (copter.g2.proximity.sensor_enabled()) {
      65        3305 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
      66             :     }
      67      386443 :     if (!copter.g2.proximity.sensor_failed()) {
      68      385677 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
      69             :     }
      70             : #endif
      71             : 
      72             : #if AP_RANGEFINDER_ENABLED
      73      386443 :     const RangeFinder *rangefinder = RangeFinder::get_singleton();
      74      386443 :     if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) {
      75       24999 :         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
      76             :     }
      77      386443 :     if (copter.rangefinder_state.enabled) {
      78       25189 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
      79       25189 :         if (rangefinder && rangefinder->has_data_orient(ROTATION_PITCH_270)) {
      80       24551 :             control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
      81             :         }
      82             :     }
      83             : #endif
      84             : 
      85             : #if AC_PRECLAND_ENABLED
      86      386443 :     if (copter.precland.enabled()) {
      87        3625 :         control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
      88        3625 :         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
      89             :     }
      90      386443 :     if (copter.precland.enabled() && copter.precland.healthy()) {
      91        1798 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
      92             :     }
      93             : #endif
      94             : 
      95             : #if AP_TERRAIN_AVAILABLE
      96      386443 :     switch (copter.terrain.status()) {
      97        1154 :     case AP_Terrain::TerrainStatusDisabled:
      98        1154 :         break;
      99      385289 :     case AP_Terrain::TerrainStatusUnhealthy:
     100             :         // To-Do: restore unhealthy terrain status reporting once terrain is used in copter
     101             :         //control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
     102             :         //control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
     103             :         //break;
     104             :     case AP_Terrain::TerrainStatusOK:
     105      385289 :         control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
     106      385289 :         control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
     107      385289 :         control_sensors_health  |= MAV_SYS_STATUS_TERRAIN;
     108      385289 :         break;
     109             :     }
     110             : #endif
     111             : 
     112      386443 :     control_sensors_present |= MAV_SYS_STATUS_SENSOR_PROPULSION;
     113      386443 :     control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_PROPULSION;
     114             :     // only mark propulsion healthy if all of the motors are producing
     115             :     // nominal thrust
     116      386443 :     if (!copter.motors->get_thrust_boost()) {
     117      386361 :         control_sensors_health |= MAV_SYS_STATUS_SENSOR_PROPULSION;
     118             :     }
     119      386443 : }