Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: add dataflash as a bit for the mavlink SYS_STATUS message

Browse Source
This commit is contained in:
Peter Barker 2016-07-08 15:27:04 +10:00 committed by Lucas De Marchi
parent cb7686001f
commit 6d98dd5233
1 changed files with 14 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
APMrover2/GCS_Mavlink.cpp
View File

@ -105,8 +105,13 @@ void Rover::send_extended_status1(mavlink_channel_t chan)
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);
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:
@ -132,6 +137,10 @@ void Rover::send_extended_status1(mavlink_channel_t chan)
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;
@ -175,6 +184,10 @@ void Rover::send_extended_status1(mavlink_channel_t chan)
}
}
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);