yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

move subsystem_info entirely into commander and remove from uORB

Browse Source 
* HealthFlags: define bitfield using 1<<X

Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Daniel Agar
2020-10-25 10:08:15 -04:00
committed by GitHub
parent ea3d202370
commit 945c17bc3f
18 changed files with 44 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@@ -119,7 +119,6 @@ set(msg_files
sensor_preflight_mag.msg
sensor_selection.msg
sensors_status_imu.msg
subsystem_info.msg
system_power.msg
task_stack_info.msg
tecs_status.msg

39
msg/subsystem_info.msg
View File

@@ -1,39 +0,0 @@
uint64 timestamp # time since system start (microseconds)
uint64 SUBSYSTEM_TYPE_GYRO = 1
uint64 SUBSYSTEM_TYPE_ACC = 2
uint64 SUBSYSTEM_TYPE_MAG = 4
uint64 SUBSYSTEM_TYPE_ABSPRESSURE = 8
uint64 SUBSYSTEM_TYPE_DIFFPRESSURE = 16
uint64 SUBSYSTEM_TYPE_GPS = 32
uint64 SUBSYSTEM_TYPE_OPTICALFLOW = 64
uint64 SUBSYSTEM_TYPE_CVPOSITION = 128
uint64 SUBSYSTEM_TYPE_LASERPOSITION = 256
uint64 SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 512
uint64 SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1024
uint64 SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 2048
uint64 SUBSYSTEM_TYPE_YAWPOSITION = 4096
uint64 SUBSYSTEM_TYPE_ALTITUDECONTROL = 8192
uint64 SUBSYSTEM_TYPE_POSITIONCONTROL = 16384
uint64 SUBSYSTEM_TYPE_MOTORCONTROL = 32768
uint64 SUBSYSTEM_TYPE_RCRECEIVER = 65536
uint64 SUBSYSTEM_TYPE_GYRO2 = 131072
uint64 SUBSYSTEM_TYPE_ACC2 = 262144
uint64 SUBSYSTEM_TYPE_MAG2 = 524288
uint64 SUBSYSTEM_TYPE_GEOFENCE = 1048576
uint64 SUBSYSTEM_TYPE_AHRS = 2097152
uint64 SUBSYSTEM_TYPE_TERRAIN = 4194304
uint64 SUBSYSTEM_TYPE_REVERSEMOTOR = 8388608
uint64 SUBSYSTEM_TYPE_LOGGING = 16777216
uint64 SUBSYSTEM_TYPE_SENSORBATTERY = 33554432
uint64 SUBSYSTEM_TYPE_SENSORPROXIMITY = 67108864
uint64 SUBSYSTEM_TYPE_SATCOM = 134217728
uint64 SUBSYSTEM_TYPE_PREARM_CHECK = 268435456
uint64 SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 536870912
bool present
bool enabled
bool ok
uint64 subsystem_type
uint8 ORB_QUEUE_LENGTH = 8

2
msg/tools/uorb_rtps_message_ids.yaml
View File

@@ -150,8 +150,6 @@ rtps:
id: 70
- msg: px4io_status
id: 71
- msg: subsystem_info
id: 72
- msg: system_power
id: 73
- msg: task_stack_info

24
src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp
View File

@@ -359,8 +359,6 @@ void IridiumSBD::main_loop(int argc, char *argv[])
break;
}
publish_subsystem_status();
if (_new_state != _state) {
VERBOSE_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[_state], satcom_state_string[_new_state]);
_state = _new_state;
@@ -1116,31 +1114,13 @@ void IridiumSBD::publish_iridium_status()
}
}
void IridiumSBD::publish_subsystem_status()
{
const bool present = true;
const bool enabled = true;
const bool ok = _status.last_heartbeat > 0; // maybe at some point here an additional check should be made
if ((_info.present != present) || (_info.enabled != enabled) || (_info.ok != ok)) {
_info.timestamp = hrt_absolute_time();
_info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_SATCOM;
_info.present = present;
_info.enabled = enabled;
_info.ok = ok;
_subsystem_pub.publish(_info);
}
}
int IridiumSBD::open_first(struct file *filep)
int IridiumSBD::open_first(struct file *filep)
{
_cdev_used = true;
return CDev::open_first(filep);
}
int IridiumSBD::close_last(struct file *filep)
int IridiumSBD::close_last(struct file *filep)
{
_cdev_used = false;
return CDev::close_last(filep);

9
src/drivers/telemetry/iridiumsbd/IridiumSBD.h
View File

@@ -41,7 +41,6 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/subsystem_info.h>
typedef enum {
SATCOM_OK = 0,
@@ -255,9 +254,7 @@ private:
*/
pollevent_t poll_state(struct file *filp);
void publish_iridium_status(void);
void publish_subsystem_status();
void publish_iridium_status();
/**
* Notification of the first open of CDev.
@@ -305,7 +302,6 @@ private:
uint16_t _packet_length = 0;
uORB::Publication<iridiumsbd_status_s> _iridiumsbd_status_pub{ORB_ID(iridiumsbd_status)};
uORB::PublicationQueued<subsystem_info_s> _subsystem_pub{ORB_ID(subsystem_info)};
bool _test_pending = false;
char _test_command[32];
@@ -342,6 +338,5 @@ private:
bool _verbose = false;
iridiumsbd_status_s _status = {};
subsystem_info_s _info = {};
iridiumsbd_status_s _status{};
};

1
src/modules/commander/Arming/HealthFlags/HealthFlags.cpp
View File

@@ -40,7 +40,6 @@
*/
#include "HealthFlags.h"
#include <uORB/topics/subsystem_info.h>
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
{

35
src/modules/commander/Arming/HealthFlags/HealthFlags.h
View File

@@ -43,7 +43,40 @@
#include <px4_platform_common/log.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/subsystem_info.h>
struct subsystem_info_s {
// keep in sync with mavlink MAV_SYS_STATUS_SENSOR
static constexpr uint64_t SUBSYSTEM_TYPE_GYRO = 1 << 0;
static constexpr uint64_t SUBSYSTEM_TYPE_ACC = 1 << 1;
static constexpr uint64_t SUBSYSTEM_TYPE_MAG = 1 << 2;
static constexpr uint64_t SUBSYSTEM_TYPE_ABSPRESSURE = 1 << 3;
static constexpr uint64_t SUBSYSTEM_TYPE_DIFFPRESSURE = 1 << 4;
static constexpr uint64_t SUBSYSTEM_TYPE_GPS = 1 << 5;
static constexpr uint64_t SUBSYSTEM_TYPE_OPTICALFLOW = 1 << 6;
static constexpr uint64_t SUBSYSTEM_TYPE_CVPOSITION = 1 << 7;
static constexpr uint64_t SUBSYSTEM_TYPE_LASERPOSITION = 1 << 8;
static constexpr uint64_t SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 1 << 9;
static constexpr uint64_t SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1 << 10;
static constexpr uint64_t SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 1 << 11;
static constexpr uint64_t SUBSYSTEM_TYPE_YAWPOSITION = 1 << 12;
static constexpr uint64_t SUBSYSTEM_TYPE_ALTITUDECONTROL = 1 << 13;
static constexpr uint64_t SUBSYSTEM_TYPE_POSITIONCONTROL = 1 << 14;
static constexpr uint64_t SUBSYSTEM_TYPE_MOTORCONTROL = 1 << 15;
static constexpr uint64_t SUBSYSTEM_TYPE_RCRECEIVER = 1 << 16;
static constexpr uint64_t SUBSYSTEM_TYPE_GYRO2 = 1 << 17;
static constexpr uint64_t SUBSYSTEM_TYPE_ACC2 = 1 << 18;
static constexpr uint64_t SUBSYSTEM_TYPE_MAG2 = 1 << 19;
static constexpr uint64_t SUBSYSTEM_TYPE_GEOFENCE = 1 << 20;
static constexpr uint64_t SUBSYSTEM_TYPE_AHRS = 1 << 21;
static constexpr uint64_t SUBSYSTEM_TYPE_TERRAIN = 1 << 22;
static constexpr uint64_t SUBSYSTEM_TYPE_REVERSEMOTOR = 1 << 23;
static constexpr uint64_t SUBSYSTEM_TYPE_LOGGING = 1 << 24;
static constexpr uint64_t SUBSYSTEM_TYPE_SENSORBATTERY = 1 << 25;
static constexpr uint64_t SUBSYSTEM_TYPE_SENSORPROXIMITY = 1 << 26;
static constexpr uint64_t SUBSYSTEM_TYPE_SATCOM = 1 << 27;
static constexpr uint64_t SUBSYSTEM_TYPE_PREARM_CHECK = 1 << 28;
static constexpr uint64_t SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 1 << 29;
};
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status);
void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status);

1
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
View File

@@ -42,7 +42,6 @@
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/subsystem_info.h>
using namespace time_literals;

1
src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp
View File

@@ -41,7 +41,6 @@
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/subsystem_info.h>
using namespace time_literals;

1
src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp
View File

@@ -39,7 +39,6 @@
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/subsystem_info.h>
using namespace time_literals;

1
src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp
View File

@@ -39,7 +39,6 @@
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/subsystem_info.h>
using namespace time_literals;

1
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
View File

@@ -40,7 +40,6 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_states.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/subsystem_info.h>
bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
const bool report_fail, const bool enforce_gps_required)

1
src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp
View File

@@ -40,7 +40,6 @@
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/subsystem_info.h>
using namespace time_literals;

1
src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp
View File

@@ -39,7 +39,6 @@
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensors_status_imu.h>
#include <uORB/topics/subsystem_info.h>
bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
const bool report_status)

1
src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp
View File

@@ -39,7 +39,6 @@
#include <mathlib/mathlib.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/sensor_preflight_mag.h>
// return false if the magnetomer measurements are inconsistent

1
src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp
View File

@@ -40,7 +40,6 @@
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/subsystem_info.h>
using namespace time_literals;

14
src/modules/commander/Commander.cpp
View File

@@ -1722,14 +1722,6 @@ Commander::run()
battery_status_check();
/* update subsystem info which arrives from outside of commander*/
subsystem_info_s info;
while (_subsys_sub.update(&info)) {
set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status);
_status_changed = true;
}
/* If in INIT state, try to proceed to STANDBY state */
if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
@@ -3803,6 +3795,12 @@ void Commander::data_link_check()
_status_changed = true;
}
}
const bool present = true;
const bool enabled = true;
const bool ok = (iridium_status.last_heartbeat > 0); // maybe at some point here an additional check should be made
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SATCOM, present, enabled, ok, status);
}
break;

2
src/modules/commander/Commander.hpp
View File

@@ -73,7 +73,6 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/power_button_state.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_acceleration.h>
@@ -391,7 +390,6 @@ private:
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _subsys_sub{ORB_ID(subsystem_info)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};