ardupilot/ArduPlane/GCS_Plane.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

130 lines
4.3 KiB
C++
Raw Normal View History

2016-08-25 04:48:27 -03:00
#include "GCS_Plane.h"
#include "Plane.h"
uint8_t GCS_Plane::sysid_this_mav() const
{
return plane.g.sysid_this_mav;
}
void GCS_Plane::update_vehicle_sensor_status_flags(void)
{
// reverse thrust
if (plane.have_reverse_thrust()) {
control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
if (plane.have_reverse_thrust() && is_negative(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle))) {
control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;
control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
// flightmode-specific
control_sensors_present |=
MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
MAV_SYS_STATUS_SENSOR_YAW_POSITION |
MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL |
MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
bool rate_controlled = false;
bool attitude_stabilized = false;
switch (plane.control_mode->mode_number()) {
case Mode::Number::MANUAL:
break;
case Mode::Number::ACRO:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QACRO:
#endif
rate_controlled = true;
break;
case Mode::Number::STABILIZE:
case Mode::Number::FLY_BY_WIRE_A:
case Mode::Number::AUTOTUNE:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLAND:
case Mode::Number::QLOITER:
#if QAUTOTUNE_ENABLED
case Mode::Number::QAUTOTUNE:
#endif
#endif // HAL_QUADPLANE_ENABLED
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::CRUISE:
rate_controlled = true;
attitude_stabilized = true;
break;
case Mode::Number::TRAINING:
if (!plane.training_manual_roll || !plane.training_manual_pitch) {
rate_controlled = true;
attitude_stabilized = true;
}
break;
case Mode::Number::AUTO:
case Mode::Number::RTL:
case Mode::Number::LOITER:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
case Mode::Number::CIRCLE:
case Mode::Number::TAKEOFF:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QRTL:
case Mode::Number::LOITER_ALT_QLAND:
#endif
case Mode::Number::THERMAL:
rate_controlled = true;
attitude_stabilized = true;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION;
control_sensors_health |= MAV_SYS_STATUS_SENSOR_YAW_POSITION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
break;
case Mode::Number::INITIALISING:
break;
}
if (rate_controlled) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
}
if (attitude_stabilized) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION;
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION;
}
#if AP_TERRAIN_AVAILABLE
switch (plane.terrain.status()) {
case AP_Terrain::TerrainStatusDisabled:
break;
case AP_Terrain::TerrainStatusUnhealthy:
control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
break;
case AP_Terrain::TerrainStatusOK:
control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
control_sensors_health |= MAV_SYS_STATUS_TERRAIN;
break;
}
#endif
#if AP_RANGEFINDER_ENABLED
const RangeFinder *rangefinder = RangeFinder::get_singleton();
if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (plane.g.rangefinder_landing) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
if (rangefinder->has_data_orient(ROTATION_PITCH_270)) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
#endif
}