mirror of https://github.com/ArduPilot/ardupilot
128 lines
4.2 KiB
C++
128 lines
4.2 KiB
C++
#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
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|