aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
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
This commit is contained in:
floaledm 2016-09-20 14:42:24 -05:00 committed by Andrew Tridgell
parent 6864864797
commit 34718b130a
5 changed files with 179 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/ArduCopter.cpp
View File

@ -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
View File

@ -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:
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
View File

@ -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)
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)
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)
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
View File

@ -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
View File

@ -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
}