2017-02-13 21:30:32 -04:00
# include "GCS_Tracker.h"
# include "Tracker.h"
void GCS_Tracker : : request_datastream_position ( const uint8_t sysid , const uint8_t compid )
{
for ( uint8_t i = 0 ; i < num_gcs ( ) ; i + + ) {
if ( gcs ( ) . chan ( i ) . initialised ) {
// request position
if ( HAVE_PAYLOAD_SPACE ( ( mavlink_channel_t ) i , DATA_STREAM ) ) {
mavlink_msg_request_data_stream_send (
( mavlink_channel_t ) i ,
sysid ,
compid ,
MAV_DATA_STREAM_POSITION ,
tracker . g . mavlink_update_rate ,
1 ) ; // start streaming
}
}
}
}
void GCS_Tracker : : request_datastream_airpressure ( const uint8_t sysid , const uint8_t compid )
{
for ( uint8_t i = 0 ; i < num_gcs ( ) ; i + + ) {
if ( gcs ( ) . chan ( i ) . initialised ) {
// request air pressure
if ( HAVE_PAYLOAD_SPACE ( ( mavlink_channel_t ) i , DATA_STREAM ) ) {
mavlink_msg_request_data_stream_send (
( mavlink_channel_t ) i ,
sysid ,
compid ,
MAV_DATA_STREAM_RAW_SENSORS ,
tracker . g . mavlink_update_rate ,
1 ) ; // start streaming
}
}
}
}
2019-02-19 17:34:14 -04:00
// update sensors and subsystems present, enabled and healthy flags for reporting to GCS
void GCS_Tracker : : update_sensor_status_flags ( )
{
// default sensors present
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT ;
// first what sensors/controllers we have
if ( tracker . g . compass_enabled ) {
control_sensors_present | = MAV_SYS_STATUS_SENSOR_3D_MAG ; // compass present
}
const AP_GPS & gps = AP : : gps ( ) ;
if ( gps . status ( ) > AP_GPS : : NO_GPS ) {
control_sensors_present | = MAV_SYS_STATUS_SENSOR_GPS ;
}
const AP_Logger & logger = AP : : logger ( ) ;
if ( logger . 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_LOGGING &
~ MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
~ MAV_SYS_STATUS_SENSOR_BATTERY ) ;
if ( logger . 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 ;
}
const AP_BattMonitor & battery = AP : : battery ( ) ;
if ( battery . num_instances ( ) > 0 ) {
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_BATTERY ;
}
AP_AHRS & ahrs = AP : : ahrs ( ) ;
// 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 ) ;
const Compass & compass = AP : : compass ( ) ;
if ( tracker . g . compass_enabled & & compass . healthy ( 0 ) & & ahrs . use_compass ( ) ) {
control_sensors_health | = MAV_SYS_STATUS_SENSOR_3D_MAG ;
}
if ( gps . is_healthy ( ) ) {
control_sensors_health | = MAV_SYS_STATUS_SENSOR_GPS ;
}
const AP_InertialSensor & ins = AP : : ins ( ) ;
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 ( logger . logging_failed ( ) ) {
control_sensors_health & = ~ MAV_SYS_STATUS_LOGGING ;
}
if ( ! battery . healthy ( ) | | battery . has_failsafed ( ) ) {
control_sensors_enabled & = ~ MAV_SYS_STATUS_SENSOR_BATTERY ;
}
if ( 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 ) ;
}
}