ArduCopter/GCS_Copter.cpp

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