zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: update sensor status error flags independently of sending a sys_status message 

Without this, there is no update to the sensor status flags in the Frsky
lib unless there's an active Mavlink connection configured to send
extended_status1
mission-4.1.18
floaledm 9 years ago committed by Andrew Tridgell
parent
6864864797
commit
34718b130a
5 changed files with 179 additions and 167 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      ArduCopter/ArduCopter.cpp
  2. 6
      ArduCopter/Copter.h
  3. 167
      ArduCopter/GCS_Mavlink.cpp
  4. 3
      ArduCopter/GCS_Mavlink.h
  5. 164
      ArduCopter/sensors.cpp

6
ArduCopter/ArduCopter.cpp
Unescape View File

@ -502,6 +502,12 @@ void Copter::one_hz_loop() @@ -502,6 +502,12 @@ void Copter::one_hz_loop()
terrain_logging();
adsb.set_is_flying(!ap.land_complete);
// update error mask of sensors and subsystems. The mask uses the
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
// indicates that the sensor or subsystem is present but not
// functioning correctly
update_sensor_status_flags();
}
// called at 50hz

6
ArduCopter/Copter.h
Unescape View File

@ -449,6 +449,11 @@ private: @@ -449,6 +449,11 @@ private:
AP_Frsky_Telem frsky_telemetry;
#endif
// Variables for extended status MAVLink messages
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
// Altitude
// The cm/s we are moving up or down based on filtered data - Positive = UP
int16_t climb_rate;
@ -949,6 +954,7 @@ private: @@ -949,6 +954,7 @@ private:
void failsafe_disable();
void fence_check();
void fence_send_mavlink_status(mavlink_channel_t chan);
void update_sensor_status_flags(void);
bool set_mode(control_mode_t mode, mode_reason_t reason);
bool gcs_set_mode(uint8_t mode) { return set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND); }
void update_flight_mode();

167
ArduCopter/GCS_Mavlink.cpp
Unescape View File

@ -3,9 +3,6 @@ @@ -3,9 +3,6 @@
#include "GCS_Mavlink.h"
// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | 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 | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
void Copter::gcs_send_heartbeat(void)
{
gcs_send_message(MSG_HEARTBEAT);
@ -111,130 +108,6 @@ NOINLINE void Copter::send_limits_status(mavlink_channel_t chan) @@ -111,130 +108,6 @@ NOINLINE void Copter::send_limits_status(mavlink_channel_t chan)
NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan)
{
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
// default sensors present
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
// first what sensors/controllers we have
if (g.compass_enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
}
if (gps.status() > AP_GPS::NO_GPS) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
}
#if OPTFLOW == ENABLED
if (optflow.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
#if PRECISION_LANDING == ENABLED
if (precland.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
if (ap.rc_receiver_present) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
if (copter.DataFlash.logging_present()) { // primary logging only (usually File)
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
}
#if PROXIMITY_ENABLED == ENABLED
if (copter.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
#endif
// all present sensors enabled by default except altitude and position control and motors which we will set individually
control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL &
~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL &
~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
~MAV_SYS_STATUS_LOGGING);
switch (control_mode) {
case AUTO:
case AVOID_ADSB:
case GUIDED:
case LOITER:
case RTL:
case CIRCLE:
case LAND:
case POSHOLD:
case BRAKE:
case THROW:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
break;
case ALT_HOLD:
case GUIDED_NOGPS:
case SPORT:
case AUTOTUNE:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
break;
default:
// stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual)
break;
}
// set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
}
if (copter.DataFlash.logging_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
}
// default to all healthy
control_sensors_health = control_sensors_present;
if (!barometer.all_healthy()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
}
if (!g.compass_enabled || !compass.healthy() || !ahrs.use_compass()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_MAG;
}
if (gps.status() == AP_GPS::NO_GPS) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_GPS;
}
if (!ap.rc_receiver_present || failsafe.radio) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
#if OPTFLOW == ENABLED
if (!optflow.healthy()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
#if PRECISION_LANDING == ENABLED
if (!precland.healthy()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
}
if (!ins.get_accel_health_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
}
if (ahrs.initialised() && !ahrs.healthy()) {
// AHRS subsystem is unhealthy
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
}
if (copter.DataFlash.logging_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
}
#if PROXIMITY_ENABLED == ENABLED
if (copter.g2.proximity.get_status() < AP_Proximity::Proximity_Good) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
#endif
int16_t battery_current = -1;
int8_t battery_remaining = -1;
@ -243,39 +116,6 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) @@ -243,39 +116,6 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan)
battery_current = battery.current_amps() * 100;
}
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
switch (terrain.status()) {
case AP_Terrain::TerrainStatusDisabled:
break;
case AP_Terrain::TerrainStatusUnhealthy:
// To-Do: restore unhealthy terrain status reporting once terrain is used in copter
//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 RANGEFINDER_ENABLED == ENABLED
if (rangefinder.num_sensors() > 0) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (rangefinder.has_data()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
#endif
if (!ap.initialised || ins.calibrating()) {
// while initialising the gyros and accels are not enabled
control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
mavlink_msg_sys_status_send(
chan,
control_sensors_present,
@ -288,13 +128,6 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) @@ -288,13 +128,6 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan)
0, // comm drops %,
0, // comm drops in pkts,
0, 0, 0, 0);
#if FRSKY_TELEM_ENABLED == ENABLED
// give mask of error flags to Frsky_Telemetry
uint32_t sensors_error_flags = (~control_sensors_health) & control_sensors_enabled & control_sensors_present;
frsky_telemetry.update_sensor_status_flags(sensors_error_flags);
#endif
}
void NOINLINE Copter::send_location(mavlink_channel_t chan)

3
ArduCopter/GCS_Mavlink.h
Unescape View File

@ -2,6 +2,9 @@ @@ -2,6 +2,9 @@
#include <GCS_MAVLink/GCS.h>
// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | 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 | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
class GCS_MAVLINK_Copter : public GCS_MAVLINK
{

164
ArduCopter/sensors.cpp
Unescape View File

@ -265,3 +265,167 @@ void Copter::update_proximity(void) @@ -265,3 +265,167 @@ void Copter::update_proximity(void)
#endif
}
// update error mask of sensors and subsystems. The mask
// uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set
// then it indicates that the sensor or subsystem is present but
// not functioning correctly.
void Copter::update_sensor_status_flags(void)
{
// default sensors present
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
// first what sensors/controllers we have
if (g.compass_enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
}
if (gps.status() > AP_GPS::NO_GPS) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
}
#if OPTFLOW == ENABLED
if (optflow.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
#if PRECISION_LANDING == ENABLED
if (precland.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
if (ap.rc_receiver_present) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
if (copter.DataFlash.logging_present()) { // primary logging only (usually File)
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
}
#if PROXIMITY_ENABLED == ENABLED
if (copter.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
#endif
// all present sensors enabled by default except altitude and position control and motors which we will set individually
control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL &
~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL &
~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
~MAV_SYS_STATUS_LOGGING);
switch (control_mode) {
case AUTO:
case AVOID_ADSB:
case GUIDED:
case LOITER:
case RTL:
case CIRCLE:
case LAND:
case POSHOLD:
case BRAKE:
case THROW:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
break;
case ALT_HOLD:
case GUIDED_NOGPS:
case SPORT:
case AUTOTUNE:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
break;
default:
// stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual)
break;
}
// set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
}
if (copter.DataFlash.logging_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
}
// default to all healthy
control_sensors_health = control_sensors_present;
if (!barometer.all_healthy()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
}
if (!g.compass_enabled || !compass.healthy() || !ahrs.use_compass()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_MAG;
}
if (gps.status() == AP_GPS::NO_GPS) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_GPS;
}
if (!ap.rc_receiver_present || failsafe.radio) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
#if OPTFLOW == ENABLED
if (!optflow.healthy()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
#if PRECISION_LANDING == ENABLED
if (!precland.healthy()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
}
if (!ins.get_accel_health_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
}
if (ahrs.initialised() && !ahrs.healthy()) {
// AHRS subsystem is unhealthy
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
}
if (copter.DataFlash.logging_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
}
#if PROXIMITY_ENABLED == ENABLED
if (copter.g2.proximity.get_status() < AP_Proximity::Proximity_Good) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
#endif
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
switch (terrain.status()) {
case AP_Terrain::TerrainStatusDisabled:
break;
case AP_Terrain::TerrainStatusUnhealthy:
// To-Do: restore unhealthy terrain status reporting once terrain is used in copter
//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 RANGEFINDER_ENABLED == ENABLED
if (rangefinder.num_sensors() > 0) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (rangefinder.has_data()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
#endif
if (!ap.initialised || ins.calibrating()) {
// while initialising the gyros and accels are not enabled
control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
#if FRSKY_TELEM_ENABLED == ENABLED
// give mask of error flags to Frsky_Telemetry
frsky_telemetry.update_sensor_status_flags(!control_sensors_health & control_sensors_enabled & control_sensors_present);
#endif
}

Write Preview
Loading…
Cancel
Save