zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

master
floaledm 8 years ago committed by Grant Morphett
parent
1b46a71596
commit
629af84ca1
5 changed files with 119 additions and 104 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
      APMrover2/APMrover2.cpp
  2. 105
      APMrover2/GCS_Mavlink.cpp
  3. 3
      APMrover2/GCS_Mavlink.h
  4. 5
      APMrover2/Rover.h
  5. 104
      APMrover2/sensors.cpp

6
APMrover2/APMrover2.cpp
Unescape View File

@ -330,6 +330,12 @@ void Rover::one_second_loop(void) @@ -330,6 +330,12 @@ void Rover::one_second_loop(void)
}
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
// 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();
}
void Rover::dataflash_periodic(void)

105
APMrover2/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, 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_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
void Rover::send_heartbeat(mavlink_channel_t chan)
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
@ -88,82 +85,6 @@ void Rover::send_attitude(mavlink_channel_t chan) @@ -88,82 +85,6 @@ void Rover::send_attitude(mavlink_channel_t chan)
void Rover::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 (rover.DataFlash.logging_present()) { // primary logging only (usually File)
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
}
// all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor output which we will set individually
control_sensors_enabled = 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_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_LOGGING);
switch (control_mode) {
case MANUAL:
case HOLD:
break;
case LEARNING:
case STEERING:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
break;
case AUTO:
case RTL:
case GUIDED:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control
break;
case INITIALISING:
break;
}
if (rover.DataFlash.logging_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
}
// 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;
}
// default to all healthy except compass and gps which we set individually
control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS);
if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
}
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
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;
}
int16_t battery_current = -1;
int8_t battery_remaining = -1;
@ -172,25 +93,7 @@ void Rover::send_extended_status1(mavlink_channel_t chan) @@ -172,25 +93,7 @@ void Rover::send_extended_status1(mavlink_channel_t chan)
battery_current = battery.current_amps() * 100;
}
if (sonar.num_sensors() > 0) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (g.sonar_trigger_cm > 0) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
if (sonar.has_data()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
if (rover.DataFlash.logging_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
}
if (AP_Notify::flags.initialising) {
// 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);
}
update_sensor_status_flags();
mavlink_msg_sys_status_send(
chan,
@ -204,12 +107,6 @@ void Rover::send_extended_status1(mavlink_channel_t chan) @@ -204,12 +107,6 @@ void Rover::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 Rover::send_location(mavlink_channel_t chan)

3
APMrover2/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, 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_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
class GCS_MAVLINK_Rover : public GCS_MAVLINK
{

5
APMrover2/Rover.h
Unescape View File

@ -290,6 +290,10 @@ private: @@ -290,6 +290,10 @@ private:
AP_Frsky_Telem frsky_telemetry;
#endif
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
// Navigation control variables
// The instantaneous desired lateral acceleration in m/s/s
float lateral_acceleration;
@ -402,6 +406,7 @@ private: @@ -402,6 +406,7 @@ private:
void update_navigation();
void send_heartbeat(mavlink_channel_t chan);
void send_attitude(mavlink_channel_t chan);
void update_sensor_status_flags(void);
void send_extended_status1(mavlink_channel_t chan);
void send_location(mavlink_channel_t chan);
void send_nav_controller_output(mavlink_channel_t chan);

104
APMrover2/sensors.cpp
Unescape View File

@ -125,3 +125,107 @@ void Rover::button_update(void) @@ -125,3 +125,107 @@ void Rover::button_update(void)
{
button.update();
}
// 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 Rover::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 (rover.DataFlash.logging_present()) { // primary logging only (usually File)
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
}
// all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor output which we will set individually
control_sensors_enabled = 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_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_LOGGING);
switch (control_mode) {
case MANUAL:
case HOLD:
break;
case LEARNING:
case STEERING:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
break;
case AUTO:
case RTL:
case GUIDED:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control
break;
case INITIALISING:
break;
}
if (rover.DataFlash.logging_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
}
// 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;
}
// default to all healthy except compass and gps which we set individually
control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS);
if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
}
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
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 (sonar.num_sensors() > 0) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (g.sonar_trigger_cm > 0) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
if (sonar.has_data()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
if (rover.DataFlash.logging_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
}
if (AP_Notify::flags.initialising) {
// 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