mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-27 17:23:33 +08:00
extensive orb_advert_t fixes
The calls to orb_advertise were being mishandled throughout the code. There were ::close() calls on memory pointers, there were checks against < 0 when it is a pointer to a object and values larger than 0x7ffffffff are valid. Some places orb_advert_t variables were being initialized as 0 other places as -1. The orb_advert_t type was changed to uintptr_t so the pointer value would not be wrapped as a negative number. This was causing a failure on ARM. Tests for < 0 were changed to == 0 since a null pointer is the valid representation for error, or uninitialized. Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
committed by
Lorenz Meier
parent
9a67303416
commit
a734fc96d1
@@ -86,8 +86,8 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha
|
|||||||
_measure_ticks(0),
|
_measure_ticks(0),
|
||||||
_collect_phase(false),
|
_collect_phase(false),
|
||||||
_diff_pres_offset(0.0f),
|
_diff_pres_offset(0.0f),
|
||||||
_airspeed_pub(-1),
|
_airspeed_pub(0),
|
||||||
_subsys_pub(-1),
|
_subsys_pub(0),
|
||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_conversion_interval(conversion_interval),
|
_conversion_interval(conversion_interval),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
|
||||||
@@ -146,7 +146,7 @@ Airspeed::init()
|
|||||||
/* measurement will have generated a report, publish */
|
/* measurement will have generated a report, publish */
|
||||||
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
|
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
|
||||||
|
|
||||||
if (_airspeed_pub < 0)
|
if (_airspeed_pub == 0)
|
||||||
warnx("uORB started?");
|
warnx("uORB started?");
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -367,7 +367,7 @@ Airspeed::update_status()
|
|||||||
SUBSYSTEM_TYPE_DIFFPRESSURE
|
SUBSYSTEM_TYPE_DIFFPRESSURE
|
||||||
};
|
};
|
||||||
|
|
||||||
if (_subsys_pub > 0) {
|
if (_subsys_pub == 0) {
|
||||||
orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
|
orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
|
||||||
} else {
|
} else {
|
||||||
_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
|
_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
|
||||||
|
|||||||
@@ -203,7 +203,7 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
|
|||||||
_enabled(false),
|
_enabled(false),
|
||||||
_work{},
|
_work{},
|
||||||
_reports(nullptr),
|
_reports(nullptr),
|
||||||
_batt_topic(-1),
|
_batt_topic(0),
|
||||||
_batt_orb_id(nullptr),
|
_batt_orb_id(nullptr),
|
||||||
_start_time(0),
|
_start_time(0),
|
||||||
_batt_capacity(0)
|
_batt_capacity(0)
|
||||||
@@ -427,13 +427,13 @@ BATT_SMBUS::cycle()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// publish to orb
|
// publish to orb
|
||||||
if (_batt_topic != -1) {
|
if (_batt_topic != 0) {
|
||||||
orb_publish(_batt_orb_id, _batt_topic, &new_report);
|
orb_publish(_batt_orb_id, _batt_topic, &new_report);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
_batt_topic = orb_advertise(_batt_orb_id, &new_report);
|
_batt_topic = orb_advertise(_batt_orb_id, &new_report);
|
||||||
|
|
||||||
if (_batt_topic < 0) {
|
if (_batt_topic == 0) {
|
||||||
errx(1, "ADVERT FAIL");
|
errx(1, "ADVERT FAIL");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -238,7 +238,7 @@ BMA180::BMA180(int bus, spi_dev_e device) :
|
|||||||
_reports(nullptr),
|
_reports(nullptr),
|
||||||
_accel_range_scale(0.0f),
|
_accel_range_scale(0.0f),
|
||||||
_accel_range_m_s2(0.0f),
|
_accel_range_m_s2(0.0f),
|
||||||
_accel_topic(-1),
|
_accel_topic(0),
|
||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_current_lowpass(0),
|
_current_lowpass(0),
|
||||||
_current_range(0),
|
_current_range(0),
|
||||||
@@ -733,7 +733,7 @@ BMA180::measure()
|
|||||||
poll_notify(POLLIN);
|
poll_notify(POLLIN);
|
||||||
|
|
||||||
/* publish for subscribers */
|
/* publish for subscribers */
|
||||||
if (_accel_topic > 0 && !(_pub_blocked))
|
if (_accel_topic != 0 && !(_pub_blocked))
|
||||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
|
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
|
||||||
|
|
||||||
/* stop the perf counter */
|
/* stop the perf counter */
|
||||||
|
|||||||
@@ -180,7 +180,7 @@ Gimbal::Gimbal() :
|
|||||||
_attitude_compensation_pitch(true),
|
_attitude_compensation_pitch(true),
|
||||||
_attitude_compensation_yaw(true),
|
_attitude_compensation_yaw(true),
|
||||||
_initialized(false),
|
_initialized(false),
|
||||||
_actuator_controls_2_topic(-1),
|
_actuator_controls_2_topic(0),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "gimbal_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "gimbal_read")),
|
||||||
_comms_errors(perf_alloc(PC_COUNT, "gimbal_comms_errors")),
|
_comms_errors(perf_alloc(PC_COUNT, "gimbal_comms_errors")),
|
||||||
_buffer_overflows(perf_alloc(PC_COUNT, "gimbal_buffer_overflows"))
|
_buffer_overflows(perf_alloc(PC_COUNT, "gimbal_buffer_overflows"))
|
||||||
@@ -197,7 +197,6 @@ Gimbal::~Gimbal()
|
|||||||
/* make sure we are truly inactive */
|
/* make sure we are truly inactive */
|
||||||
stop();
|
stop();
|
||||||
|
|
||||||
::close(_actuator_controls_2_topic);
|
|
||||||
::close(_vehicle_command_sub);
|
::close(_vehicle_command_sub);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -281,7 +280,7 @@ Gimbal::cycle()
|
|||||||
zero_report.timestamp = hrt_absolute_time();
|
zero_report.timestamp = hrt_absolute_time();
|
||||||
_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);
|
_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);
|
||||||
|
|
||||||
if (_actuator_controls_2_topic < 0) {
|
if (_actuator_controls_2_topic == 0) {
|
||||||
warnx("advert err");
|
warnx("advert err");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -175,9 +175,9 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) :
|
|||||||
_mode(GPS_DRIVER_MODE_UBX),
|
_mode(GPS_DRIVER_MODE_UBX),
|
||||||
_Helper(nullptr),
|
_Helper(nullptr),
|
||||||
_Sat_Info(nullptr),
|
_Sat_Info(nullptr),
|
||||||
_report_gps_pos_pub(-1),
|
_report_gps_pos_pub(0),
|
||||||
_p_report_sat_info(nullptr),
|
_p_report_sat_info(nullptr),
|
||||||
_report_sat_info_pub(-1),
|
_report_sat_info_pub(0),
|
||||||
_rate(0.0f),
|
_rate(0.0f),
|
||||||
_fake_gps(fake_gps)
|
_fake_gps(fake_gps)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -805,7 +805,7 @@ fake(int argc, char *argv[])
|
|||||||
|
|
||||||
orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
|
orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
|
||||||
|
|
||||||
if (handle < 0) {
|
if (handle == 0) {
|
||||||
puts("advertise failed");
|
puts("advertise failed");
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -360,7 +360,7 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota
|
|||||||
_collect_phase(false),
|
_collect_phase(false),
|
||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_orb_class_instance(-1),
|
_orb_class_instance(-1),
|
||||||
_mag_topic(-1),
|
_mag_topic(0),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
|
||||||
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
|
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
|
||||||
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
|
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
|
||||||
@@ -986,14 +986,14 @@ HMC5883::collect()
|
|||||||
|
|
||||||
if (!(_pub_blocked)) {
|
if (!(_pub_blocked)) {
|
||||||
|
|
||||||
if (_mag_topic != -1) {
|
if (_mag_topic != 0) {
|
||||||
/* publish it */
|
/* publish it */
|
||||||
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
|
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
|
||||||
} else {
|
} else {
|
||||||
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
|
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
|
||||||
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
|
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
|
||||||
|
|
||||||
if (_mag_topic < 0)
|
if (_mag_topic == 0)
|
||||||
debug("ADVERT FAIL");
|
debug("ADVERT FAIL");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -63,7 +63,7 @@ static int _sensor_sub = -1;
|
|||||||
static int _airspeed_sub = -1;
|
static int _airspeed_sub = -1;
|
||||||
static int _esc_sub = -1;
|
static int _esc_sub = -1;
|
||||||
|
|
||||||
static orb_advert_t _esc_pub;
|
static orb_advert_t _esc_pub = 0;
|
||||||
|
|
||||||
static bool _home_position_set = false;
|
static bool _home_position_set = false;
|
||||||
static double _home_lat = 0.0d;
|
static double _home_lat = 0.0d;
|
||||||
|
|||||||
@@ -411,7 +411,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
|
|||||||
_gyro_scale{},
|
_gyro_scale{},
|
||||||
_gyro_range_scale(0.0f),
|
_gyro_range_scale(0.0f),
|
||||||
_gyro_range_rad_s(0.0f),
|
_gyro_range_rad_s(0.0f),
|
||||||
_gyro_topic(-1),
|
_gyro_topic(0),
|
||||||
_orb_class_instance(-1),
|
_orb_class_instance(-1),
|
||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_current_rate(0),
|
_current_rate(0),
|
||||||
@@ -490,7 +490,7 @@ L3GD20::init()
|
|||||||
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
|
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
|
||||||
&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
|
&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
|
||||||
|
|
||||||
if (_gyro_topic < 0) {
|
if (_gyro_topic == 0) {
|
||||||
debug("failed to create sensor_gyro publication");
|
debug("failed to create sensor_gyro publication");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -563,7 +563,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
|
|||||||
_mag_range_ga(0.0f),
|
_mag_range_ga(0.0f),
|
||||||
_mag_range_scale(0.0f),
|
_mag_range_scale(0.0f),
|
||||||
_mag_samplerate(0),
|
_mag_samplerate(0),
|
||||||
_accel_topic(-1),
|
_accel_topic(0),
|
||||||
_accel_orb_class_instance(-1),
|
_accel_orb_class_instance(-1),
|
||||||
_accel_class_instance(-1),
|
_accel_class_instance(-1),
|
||||||
_accel_read(0),
|
_accel_read(0),
|
||||||
@@ -676,7 +676,7 @@ LSM303D::init()
|
|||||||
_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
|
_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
|
||||||
&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
|
&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
|
||||||
|
|
||||||
if (_mag->_mag_topic < 0) {
|
if (_mag->_mag_topic == 0) {
|
||||||
warnx("ADVERT ERR");
|
warnx("ADVERT ERR");
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -691,7 +691,7 @@ LSM303D::init()
|
|||||||
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
|
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
|
||||||
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
|
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
|
||||||
|
|
||||||
if (_accel_topic < 0) {
|
if (_accel_topic == 0) {
|
||||||
warnx("ADVERT ERR");
|
warnx("ADVERT ERR");
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1770,7 +1770,7 @@ LSM303D::test_error()
|
|||||||
LSM303D_mag::LSM303D_mag(LSM303D *parent) :
|
LSM303D_mag::LSM303D_mag(LSM303D *parent) :
|
||||||
CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
|
CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
|
||||||
_parent(parent),
|
_parent(parent),
|
||||||
_mag_topic(-1),
|
_mag_topic(0),
|
||||||
_mag_orb_class_instance(-1),
|
_mag_orb_class_instance(-1),
|
||||||
_mag_class_instance(-1)
|
_mag_class_instance(-1)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -211,7 +211,7 @@ MB12XX::MB12XX(int bus, int address) :
|
|||||||
_collect_phase(false),
|
_collect_phase(false),
|
||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_orb_class_instance(-1),
|
_orb_class_instance(-1),
|
||||||
_distance_sensor_topic(-1),
|
_distance_sensor_topic(nullptr),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
|
||||||
_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
|
_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
|
||||||
_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")),
|
_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")),
|
||||||
@@ -276,7 +276,7 @@ MB12XX::init()
|
|||||||
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
|
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
|
||||||
&_orb_class_instance, ORB_PRIO_LOW);
|
&_orb_class_instance, ORB_PRIO_LOW);
|
||||||
|
|
||||||
if (_distance_sensor_topic < 0) {
|
if (_distance_sensor_topic == nullptr) {
|
||||||
log("failed to create distance_sensor object. Did you start uOrb?");
|
log("failed to create distance_sensor object. Did you start uOrb?");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -588,7 +588,7 @@ MB12XX::collect()
|
|||||||
report.id = 0;
|
report.id = 0;
|
||||||
|
|
||||||
/* publish it, if we are the primary */
|
/* publish it, if we are the primary */
|
||||||
if (_distance_sensor_topic >= 0) {
|
if (_distance_sensor_topic != nullptr) {
|
||||||
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
|
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -641,7 +641,6 @@ MK::task_main()
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
::close(_t_esc_status);
|
|
||||||
::close(_t_actuators);
|
::close(_t_actuators);
|
||||||
::close(_t_actuator_armed);
|
::close(_t_actuator_armed);
|
||||||
|
|
||||||
|
|||||||
@@ -507,7 +507,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
|
|||||||
_accel_scale{},
|
_accel_scale{},
|
||||||
_accel_range_scale(0.0f),
|
_accel_range_scale(0.0f),
|
||||||
_accel_range_m_s2(0.0f),
|
_accel_range_m_s2(0.0f),
|
||||||
_accel_topic(-1),
|
_accel_topic(0),
|
||||||
_accel_orb_class_instance(-1),
|
_accel_orb_class_instance(-1),
|
||||||
_accel_class_instance(-1),
|
_accel_class_instance(-1),
|
||||||
_gyro_reports(nullptr),
|
_gyro_reports(nullptr),
|
||||||
@@ -658,7 +658,7 @@ MPU6000::init()
|
|||||||
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
|
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
|
||||||
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
|
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
|
||||||
|
|
||||||
if (_accel_topic < 0) {
|
if (_accel_topic == 0) {
|
||||||
warnx("ADVERT FAIL");
|
warnx("ADVERT FAIL");
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -670,7 +670,7 @@ MPU6000::init()
|
|||||||
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
|
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
|
||||||
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
|
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
|
||||||
|
|
||||||
if (_gyro->_gyro_topic < 0) {
|
if (_gyro->_gyro_topic == 0) {
|
||||||
warnx("ADVERT FAIL");
|
warnx("ADVERT FAIL");
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1847,7 +1847,7 @@ MPU6000::print_registers()
|
|||||||
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
|
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
|
||||||
CDev("MPU6000_gyro", path),
|
CDev("MPU6000_gyro", path),
|
||||||
_parent(parent),
|
_parent(parent),
|
||||||
_gyro_topic(-1),
|
_gyro_topic(0),
|
||||||
_gyro_orb_class_instance(-1),
|
_gyro_orb_class_instance(-1),
|
||||||
_gyro_class_instance(-1)
|
_gyro_class_instance(-1)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -224,7 +224,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char*
|
|||||||
_OFF(0),
|
_OFF(0),
|
||||||
_SENS(0),
|
_SENS(0),
|
||||||
_msl_pressure(101325),
|
_msl_pressure(101325),
|
||||||
_baro_topic(-1),
|
_baro_topic(0),
|
||||||
_orb_class_instance(-1),
|
_orb_class_instance(-1),
|
||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
|
||||||
@@ -322,7 +322,7 @@ MS5611::init()
|
|||||||
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
|
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
|
||||||
|
|
||||||
|
|
||||||
if (_baro_topic < 0) {
|
if (_baro_topic == 0) {
|
||||||
warnx("failed to create sensor_baro publication");
|
warnx("failed to create sensor_baro publication");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -223,7 +223,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char*
|
|||||||
_OFF(0),
|
_OFF(0),
|
||||||
_SENS(0),
|
_SENS(0),
|
||||||
_msl_pressure(101325),
|
_msl_pressure(101325),
|
||||||
_baro_topic(-1),
|
_baro_topic(0),
|
||||||
_orb_class_instance(-1),
|
_orb_class_instance(-1),
|
||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
|
||||||
@@ -325,7 +325,7 @@ MS5611::init()
|
|||||||
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
|
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
|
||||||
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
|
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
|
||||||
|
|
||||||
if (_baro_topic == (orb_advert_t)(-1)) {
|
if (_baro_topic == 0) {
|
||||||
warnx("failed to create sensor_baro publication");
|
warnx("failed to create sensor_baro publication");
|
||||||
}
|
}
|
||||||
//warnx("sensor_baro publication %ld", _baro_topic);
|
//warnx("sensor_baro publication %ld", _baro_topic);
|
||||||
|
|||||||
@@ -193,8 +193,8 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) :
|
|||||||
_collect_phase(false),
|
_collect_phase(false),
|
||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_orb_class_instance(-1),
|
_orb_class_instance(-1),
|
||||||
_px4flow_topic(-1),
|
_px4flow_topic(nullptr),
|
||||||
_distance_sensor_topic(-1),
|
_distance_sensor_topic(nullptr),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
|
||||||
_comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")),
|
_comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")),
|
||||||
_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")),
|
_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")),
|
||||||
@@ -517,7 +517,7 @@ PX4FLOW::collect()
|
|||||||
float zeroval = 0.0f;
|
float zeroval = 0.0f;
|
||||||
rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
|
rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
|
||||||
|
|
||||||
if (_px4flow_topic < 0) {
|
if (_px4flow_topic == 0) {
|
||||||
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
|
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -200,7 +200,7 @@ SF0X::SF0X(const char *port) :
|
|||||||
_last_read(0),
|
_last_read(0),
|
||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_orb_class_instance(-1),
|
_orb_class_instance(-1),
|
||||||
_distance_sensor_topic(-1),
|
_distance_sensor_topic(nullptr),
|
||||||
_consecutive_fail_count(0),
|
_consecutive_fail_count(0),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
|
||||||
_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
|
_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
|
||||||
@@ -293,6 +293,7 @@ SF0X::init()
|
|||||||
|
|
||||||
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
|
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
|
||||||
|
|
||||||
|
<<<<<<< HEAD
|
||||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||||
/* get a publish handle on the range finder topic */
|
/* get a publish handle on the range finder topic */
|
||||||
struct distance_sensor_s ds_report = {};
|
struct distance_sensor_s ds_report = {};
|
||||||
@@ -303,6 +304,10 @@ SF0X::init()
|
|||||||
if (_distance_sensor_topic < 0) {
|
if (_distance_sensor_topic < 0) {
|
||||||
log("failed to create distance_sensor object. Did you start uOrb?");
|
log("failed to create distance_sensor object. Did you start uOrb?");
|
||||||
}
|
}
|
||||||
|
=======
|
||||||
|
if (_range_finder_topic == 0) {
|
||||||
|
warnx("advert err");
|
||||||
|
>>>>>>> extensive orb_advert_t fixes
|
||||||
}
|
}
|
||||||
|
|
||||||
} while(0);
|
} while(0);
|
||||||
|
|||||||
@@ -238,7 +238,7 @@ TRONE::TRONE(int bus, int address) :
|
|||||||
_collect_phase(false),
|
_collect_phase(false),
|
||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_orb_class_instance(-1),
|
_orb_class_instance(-1),
|
||||||
_distance_sensor_topic(-1),
|
_distance_sensor_topic(0),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "trone_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "trone_read")),
|
||||||
_comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")),
|
_comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")),
|
||||||
_buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows"))
|
_buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows"))
|
||||||
@@ -301,7 +301,7 @@ TRONE::init()
|
|||||||
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
|
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
|
||||||
&_orb_class_instance, ORB_PRIO_LOW);
|
&_orb_class_instance, ORB_PRIO_LOW);
|
||||||
|
|
||||||
if (_distance_sensor_topic < 0) {
|
if (_distance_sensor_topic == 0) {
|
||||||
log("failed to create distance_sensor object. Did you start uOrb?");
|
log("failed to create distance_sensor object. Did you start uOrb?");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -587,7 +587,7 @@ TRONE::collect()
|
|||||||
report.id = 0;
|
report.id = 0;
|
||||||
|
|
||||||
/* publish it, if we are the primary */
|
/* publish it, if we are the primary */
|
||||||
if (_distance_sensor_topic >= 0) {
|
if (_distance_sensor_topic != nullptr) {
|
||||||
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
|
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -621,9 +621,9 @@ TRONE::start()
|
|||||||
true,
|
true,
|
||||||
SUBSYSTEM_TYPE_RANGEFINDER
|
SUBSYSTEM_TYPE_RANGEFINDER
|
||||||
};
|
};
|
||||||
static orb_advert_t pub = -1;
|
static orb_advert_t pub = nullptr;
|
||||||
|
|
||||||
if (pub > 0) {
|
if (pub != nullptr) {
|
||||||
orb_publish(ORB_ID(subsystem_info), pub, &info);
|
orb_publish(ORB_ID(subsystem_info), pub, &info);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -63,7 +63,7 @@ int ex_hwtest_main(int argc, char *argv[])
|
|||||||
|
|
||||||
struct actuator_controls_s actuators;
|
struct actuator_controls_s actuators;
|
||||||
memset(&actuators, 0, sizeof(actuators));
|
memset(&actuators, 0, sizeof(actuators));
|
||||||
orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators);
|
orb_advert_t actuator_pub_ptr = orb_advertise(ORB_ID(actuator_controls_0), &actuators);
|
||||||
|
|
||||||
struct actuator_armed_s arm;
|
struct actuator_armed_s arm;
|
||||||
memset(&arm, 0 , sizeof(arm));
|
memset(&arm, 0 , sizeof(arm));
|
||||||
@@ -71,8 +71,8 @@ int ex_hwtest_main(int argc, char *argv[])
|
|||||||
arm.timestamp = hrt_absolute_time();
|
arm.timestamp = hrt_absolute_time();
|
||||||
arm.ready_to_arm = true;
|
arm.ready_to_arm = true;
|
||||||
arm.armed = true;
|
arm.armed = true;
|
||||||
orb_advert_t arm_pub_fd = orb_advertise(ORB_ID(actuator_armed), &arm);
|
orb_advert_t arm_pub_ptr = orb_advertise(ORB_ID(actuator_armed), &arm);
|
||||||
orb_publish(ORB_ID(actuator_armed), arm_pub_fd, &arm);
|
orb_publish(ORB_ID(actuator_armed), arm_pub_ptr, &arm);
|
||||||
|
|
||||||
/* read back values to validate */
|
/* read back values to validate */
|
||||||
int arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
|
int arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
@@ -118,7 +118,7 @@ int ex_hwtest_main(int argc, char *argv[])
|
|||||||
}
|
}
|
||||||
|
|
||||||
actuators.timestamp = hrt_absolute_time();
|
actuators.timestamp = hrt_absolute_time();
|
||||||
orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators);
|
orb_publish(ORB_ID(actuator_controls_0), actuator_pub_ptr, &actuators);
|
||||||
usleep(10000);
|
usleep(10000);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -114,7 +114,7 @@ private:
|
|||||||
int _sensors_sub = -1;
|
int _sensors_sub = -1;
|
||||||
int _params_sub = -1;
|
int _params_sub = -1;
|
||||||
int _global_pos_sub = -1;
|
int _global_pos_sub = -1;
|
||||||
orb_advert_t _att_pub = -1;
|
orb_advert_t _att_pub = 0;
|
||||||
|
|
||||||
struct {
|
struct {
|
||||||
param_t w_acc;
|
param_t w_acc;
|
||||||
@@ -327,7 +327,7 @@ void AttitudeEstimatorQ::task_main() {
|
|||||||
memcpy(&att.R[0], R.data, sizeof(att.R));
|
memcpy(&att.R[0], R.data, sizeof(att.R));
|
||||||
att.R_valid = true;
|
att.R_valid = true;
|
||||||
|
|
||||||
if (_att_pub < 0) {
|
if (_att_pub == 0) {
|
||||||
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
|
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
|
||||||
|
|||||||
@@ -432,8 +432,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
|
|||||||
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
|
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||||
|
|
||||||
/* advertise attitude */
|
/* advertise attitude */
|
||||||
//orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
|
||||||
//orb_advert_t att_pub = -1;
|
|
||||||
orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
||||||
|
|
||||||
int loopcounter = 0;
|
int loopcounter = 0;
|
||||||
|
|||||||
@@ -180,7 +180,7 @@ BottleDrop::BottleDrop() :
|
|||||||
_command {},
|
_command {},
|
||||||
_global_pos {},
|
_global_pos {},
|
||||||
ref {},
|
ref {},
|
||||||
_actuator_pub(-1),
|
_actuator_pub(0),
|
||||||
_actuators {},
|
_actuators {},
|
||||||
_drop_approval(false),
|
_drop_approval(false),
|
||||||
_doors_opened(0),
|
_doors_opened(0),
|
||||||
@@ -190,7 +190,7 @@ BottleDrop::BottleDrop() :
|
|||||||
_drop_position {},
|
_drop_position {},
|
||||||
_drop_state(DROP_STATE_INIT),
|
_drop_state(DROP_STATE_INIT),
|
||||||
_onboard_mission {},
|
_onboard_mission {},
|
||||||
_onboard_mission_pub(-1)
|
_onboard_mission_pub(0)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -934,7 +934,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||||||
/* publish initial state */
|
/* publish initial state */
|
||||||
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
|
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
|
||||||
|
|
||||||
if (status_pub < 0) {
|
if (status_pub == 0) {
|
||||||
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
|
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
|
||||||
warnx("exiting.");
|
warnx("exiting.");
|
||||||
px4_task_exit(ERROR);
|
px4_task_exit(ERROR);
|
||||||
@@ -952,12 +952,12 @@ int commander_thread_main(int argc, char *argv[])
|
|||||||
armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
|
armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
|
||||||
|
|
||||||
/* home position */
|
/* home position */
|
||||||
orb_advert_t home_pub = -1;
|
orb_advert_t home_pub = 0;
|
||||||
struct home_position_s home;
|
struct home_position_s home;
|
||||||
memset(&home, 0, sizeof(home));
|
memset(&home, 0, sizeof(home));
|
||||||
|
|
||||||
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
|
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
|
||||||
orb_advert_t mission_pub = -1;
|
orb_advert_t mission_pub = 0;
|
||||||
mission_s mission;
|
mission_s mission;
|
||||||
|
|
||||||
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
|
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
|
||||||
@@ -2124,7 +2124,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||||||
close(diff_pres_sub);
|
close(diff_pres_sub);
|
||||||
close(param_changed_sub);
|
close(param_changed_sub);
|
||||||
close(battery_sub);
|
close(battery_sub);
|
||||||
close(mission_pub);
|
|
||||||
|
|
||||||
thread_running = false;
|
thread_running = false;
|
||||||
|
|
||||||
|
|||||||
@@ -129,11 +129,11 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
|
|||||||
_armedSub(-1),
|
_armedSub(-1),
|
||||||
|
|
||||||
/* publications */
|
/* publications */
|
||||||
_att_pub(-1),
|
_att_pub(0),
|
||||||
_global_pos_pub(-1),
|
_global_pos_pub(0),
|
||||||
_local_pos_pub(-1),
|
_local_pos_pub(0),
|
||||||
_estimator_status_pub(-1),
|
_estimator_status_pub(0),
|
||||||
_wind_pub(-1),
|
_wind_pub(0),
|
||||||
|
|
||||||
_att({}),
|
_att({}),
|
||||||
_gyro({}),
|
_gyro({}),
|
||||||
|
|||||||
@@ -337,10 +337,10 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
|||||||
_vehicle_status_sub(-1),
|
_vehicle_status_sub(-1),
|
||||||
|
|
||||||
/* publications */
|
/* publications */
|
||||||
_rate_sp_pub(-1),
|
_rate_sp_pub(0),
|
||||||
_attitude_sp_pub(-1),
|
_attitude_sp_pub(0),
|
||||||
_actuators_0_pub(-1),
|
_actuators_0_pub(0),
|
||||||
_actuators_2_pub(-1),
|
_actuators_2_pub(0),
|
||||||
|
|
||||||
_rates_sp_id(0),
|
_rates_sp_id(0),
|
||||||
_actuators_id(0),
|
_actuators_id(0),
|
||||||
|
|||||||
@@ -434,9 +434,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
|||||||
_sensor_combined_sub(-1),
|
_sensor_combined_sub(-1),
|
||||||
|
|
||||||
/* publications */
|
/* publications */
|
||||||
_attitude_sp_pub(-1),
|
_attitude_sp_pub(0),
|
||||||
_tecs_status_pub(-1),
|
_tecs_status_pub(0),
|
||||||
_nav_capabilities_pub(-1),
|
_nav_capabilities_pub(0),
|
||||||
|
|
||||||
/* states */
|
/* states */
|
||||||
_att(),
|
_att(),
|
||||||
|
|||||||
@@ -44,7 +44,7 @@
|
|||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
|
|
||||||
LandDetector::LandDetector() :
|
LandDetector::LandDetector() :
|
||||||
_landDetectedPub(-1),
|
_landDetectedPub(0),
|
||||||
_landDetected({0, false}),
|
_landDetected({0, false}),
|
||||||
_taskShouldExit(false),
|
_taskShouldExit(false),
|
||||||
_taskIsRunning(false)
|
_taskIsRunning(false)
|
||||||
@@ -55,7 +55,6 @@ LandDetector::LandDetector() :
|
|||||||
LandDetector::~LandDetector()
|
LandDetector::~LandDetector()
|
||||||
{
|
{
|
||||||
_taskShouldExit = true;
|
_taskShouldExit = true;
|
||||||
close(_landDetectedPub);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void LandDetector::shutdown()
|
void LandDetector::shutdown()
|
||||||
|
|||||||
@@ -81,7 +81,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
|
|||||||
_transfer_partner_compid(0),
|
_transfer_partner_compid(0),
|
||||||
_offboard_mission_sub(-1),
|
_offboard_mission_sub(-1),
|
||||||
_mission_result_sub(-1),
|
_mission_result_sub(-1),
|
||||||
_offboard_mission_pub(-1),
|
_offboard_mission_pub(0),
|
||||||
_slow_rate_limiter(_interval / 10.0f),
|
_slow_rate_limiter(_interval / 10.0f),
|
||||||
_verbose(false)
|
_verbose(false)
|
||||||
{
|
{
|
||||||
@@ -93,7 +93,6 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
|
|||||||
|
|
||||||
MavlinkMissionManager::~MavlinkMissionManager()
|
MavlinkMissionManager::~MavlinkMissionManager()
|
||||||
{
|
{
|
||||||
close(_offboard_mission_pub);
|
|
||||||
close(_mission_result_sub);
|
close(_mission_result_sub);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -150,7 +149,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
|
|||||||
_current_seq = seq;
|
_current_seq = seq;
|
||||||
|
|
||||||
/* mission state saved successfully, publish offboard_mission topic */
|
/* mission state saved successfully, publish offboard_mission topic */
|
||||||
if (_offboard_mission_pub < 0) {
|
if (_offboard_mission_pub == 0) {
|
||||||
_offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
|
_offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -45,7 +45,7 @@
|
|||||||
|
|
||||||
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
|
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||||
_send_all_index(-1),
|
_send_all_index(-1),
|
||||||
_rc_param_map_pub(-1),
|
_rc_param_map_pub(0),
|
||||||
_rc_param_map()
|
_rc_param_map()
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
@@ -162,7 +162,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
|
|||||||
}
|
}
|
||||||
_rc_param_map.timestamp = hrt_absolute_time();
|
_rc_param_map.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
if (_rc_param_map_pub < 0) {
|
if (_rc_param_map_pub == 0) {
|
||||||
_rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map);
|
_rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -95,34 +95,34 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
|||||||
hil_local_pos{},
|
hil_local_pos{},
|
||||||
hil_land_detector{},
|
hil_land_detector{},
|
||||||
_control_mode{},
|
_control_mode{},
|
||||||
_global_pos_pub(-1),
|
_global_pos_pub(0),
|
||||||
_local_pos_pub(-1),
|
_local_pos_pub(0),
|
||||||
_attitude_pub(-1),
|
_attitude_pub(0),
|
||||||
_gps_pub(-1),
|
_gps_pub(0),
|
||||||
_sensors_pub(-1),
|
_sensors_pub(0),
|
||||||
_gyro_pub(-1),
|
_gyro_pub(0),
|
||||||
_accel_pub(-1),
|
_accel_pub(0),
|
||||||
_mag_pub(-1),
|
_mag_pub(0),
|
||||||
_baro_pub(-1),
|
_baro_pub(0),
|
||||||
_airspeed_pub(-1),
|
_airspeed_pub(0),
|
||||||
_battery_pub(-1),
|
_battery_pub(0),
|
||||||
_cmd_pub(-1),
|
_cmd_pub(0),
|
||||||
_flow_pub(-1),
|
_flow_pub(0),
|
||||||
_distance_sensor_pub(-1),
|
_distance_sensor_pub(0),
|
||||||
_offboard_control_mode_pub(-1),
|
_offboard_control_mode_pub(0),
|
||||||
_actuator_controls_pub(-1),
|
_actuator_controls_pub(0),
|
||||||
_global_vel_sp_pub(-1),
|
_global_vel_sp_pub(0),
|
||||||
_att_sp_pub(-1),
|
_att_sp_pub(0),
|
||||||
_rates_sp_pub(-1),
|
_rates_sp_pub(0),
|
||||||
_force_sp_pub(-1),
|
_force_sp_pub(0),
|
||||||
_pos_sp_triplet_pub(-1),
|
_pos_sp_triplet_pub(0),
|
||||||
_vicon_position_pub(-1),
|
_vicon_position_pub(0),
|
||||||
_vision_position_pub(-1),
|
_vision_position_pub(0),
|
||||||
_telemetry_status_pub(-1),
|
_telemetry_status_pub(0),
|
||||||
_rc_pub(-1),
|
_rc_pub(0),
|
||||||
_manual_pub(-1),
|
_manual_pub(0),
|
||||||
_land_detector_pub(-1),
|
_land_detector_pub(0),
|
||||||
_time_offset_pub(-1),
|
_time_offset_pub(0),
|
||||||
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
|
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
|
||||||
_hil_frames(0),
|
_hil_frames(0),
|
||||||
_old_timestamp(0),
|
_old_timestamp(0),
|
||||||
@@ -314,7 +314,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
|
|||||||
vcmd.source_component = msg->compid;
|
vcmd.source_component = msg->compid;
|
||||||
vcmd.confirmation = cmd_mavlink.confirmation;
|
vcmd.confirmation = cmd_mavlink.confirmation;
|
||||||
|
|
||||||
if (_cmd_pub < 0) {
|
if (_cmd_pub == 0) {
|
||||||
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -370,7 +370,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
|
|||||||
vcmd.source_system = msg->sysid;
|
vcmd.source_system = msg->sysid;
|
||||||
vcmd.source_component = msg->compid;
|
vcmd.source_component = msg->compid;
|
||||||
|
|
||||||
if (_cmd_pub < 0) {
|
if (_cmd_pub == 0) {
|
||||||
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -410,7 +410,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
|
|||||||
float zeroval = 0.0f;
|
float zeroval = 0.0f;
|
||||||
rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval);
|
rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval);
|
||||||
|
|
||||||
if (_flow_pub < 0) {
|
if (_flow_pub == 0) {
|
||||||
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
|
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -461,7 +461,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
|
|||||||
f.sensor_id = flow.sensor_id;
|
f.sensor_id = flow.sensor_id;
|
||||||
f.gyro_temperature = flow.temperature;
|
f.gyro_temperature = flow.temperature;
|
||||||
|
|
||||||
if (_flow_pub < 0) {
|
if (_flow_pub == 0) {
|
||||||
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
|
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -515,7 +515,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
|
|||||||
vcmd.source_component = msg->compid;
|
vcmd.source_component = msg->compid;
|
||||||
vcmd.confirmation = 1;
|
vcmd.confirmation = 1;
|
||||||
|
|
||||||
if (_cmd_pub < 0) {
|
if (_cmd_pub == 0) {
|
||||||
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -570,7 +570,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
|
|||||||
vicon_position.pitch = pos.pitch;
|
vicon_position.pitch = pos.pitch;
|
||||||
vicon_position.yaw = pos.yaw;
|
vicon_position.yaw = pos.yaw;
|
||||||
|
|
||||||
if (_vicon_position_pub < 0) {
|
if (_vicon_position_pub == 0) {
|
||||||
_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
|
_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -605,7 +605,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
|||||||
|
|
||||||
offboard_control_mode.timestamp = hrt_absolute_time();
|
offboard_control_mode.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
if (_offboard_control_mode_pub < 0) {
|
if (_offboard_control_mode_pub == 0) {
|
||||||
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
|
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -630,7 +630,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
|||||||
force_sp.y = set_position_target_local_ned.afy;
|
force_sp.y = set_position_target_local_ned.afy;
|
||||||
force_sp.z = set_position_target_local_ned.afz;
|
force_sp.z = set_position_target_local_ned.afz;
|
||||||
//XXX: yaw
|
//XXX: yaw
|
||||||
if (_force_sp_pub < 0) {
|
if (_force_sp_pub == 0) {
|
||||||
_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
|
_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp);
|
orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp);
|
||||||
@@ -697,7 +697,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
|||||||
//XXX handle global pos setpoints (different MAV frames)
|
//XXX handle global pos setpoints (different MAV frames)
|
||||||
|
|
||||||
|
|
||||||
if (_pos_sp_triplet_pub < 0) {
|
if (_pos_sp_triplet_pub == 0) {
|
||||||
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet),
|
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet),
|
||||||
&pos_sp_triplet);
|
&pos_sp_triplet);
|
||||||
} else {
|
} else {
|
||||||
@@ -740,7 +740,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
|
|||||||
|
|
||||||
offboard_control_mode.timestamp = hrt_absolute_time();
|
offboard_control_mode.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
if (_offboard_control_mode_pub < 0) {
|
if (_offboard_control_mode_pub == 0) {
|
||||||
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
|
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
|
orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
|
||||||
@@ -763,7 +763,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
|
|||||||
actuator_controls.control[i] = set_actuator_control_target.controls[i];
|
actuator_controls.control[i] = set_actuator_control_target.controls[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_actuator_controls_pub < 0) {
|
if (_actuator_controls_pub == 0) {
|
||||||
_actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls);
|
_actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls);
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls);
|
orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls);
|
||||||
@@ -804,7 +804,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
|
|||||||
vision_position.q[2] = q(2);
|
vision_position.q[2] = q(2);
|
||||||
vision_position.q[3] = q(3);
|
vision_position.q[3] = q(3);
|
||||||
|
|
||||||
if (_vision_position_pub < 0) {
|
if (_vision_position_pub == 0) {
|
||||||
_vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position);
|
_vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -857,7 +857,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
|||||||
|
|
||||||
_offboard_control_mode.timestamp = hrt_absolute_time();
|
_offboard_control_mode.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
if (_offboard_control_mode_pub < 0) {
|
if (_offboard_control_mode_pub == 0) {
|
||||||
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode);
|
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -892,7 +892,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
|||||||
_att_sp.thrust = set_attitude_target.thrust;
|
_att_sp.thrust = set_attitude_target.thrust;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_att_sp_pub < 0) {
|
if (_att_sp_pub == 0) {
|
||||||
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
|
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
|
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
|
||||||
@@ -912,7 +912,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
|||||||
_rates_sp.thrust = set_attitude_target.thrust;
|
_rates_sp.thrust = set_attitude_target.thrust;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_att_sp_pub < 0) {
|
if (_att_sp_pub == 0) {
|
||||||
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
|
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp);
|
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp);
|
||||||
@@ -948,7 +948,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
|
|||||||
tstatus.system_id = msg->sysid;
|
tstatus.system_id = msg->sysid;
|
||||||
tstatus.component_id = msg->compid;
|
tstatus.component_id = msg->compid;
|
||||||
|
|
||||||
if (_telemetry_status_pub < 0) {
|
if (_telemetry_status_pub == 0) {
|
||||||
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
|
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -974,7 +974,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
|
|||||||
|
|
||||||
// warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
|
// warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
|
||||||
|
|
||||||
if (_manual_pub < 0) {
|
if (_manual_pub == 0) {
|
||||||
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
|
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1001,7 +1001,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
|||||||
tstatus.timestamp = hrt_absolute_time();
|
tstatus.timestamp = hrt_absolute_time();
|
||||||
tstatus.heartbeat_time = tstatus.timestamp;
|
tstatus.heartbeat_time = tstatus.timestamp;
|
||||||
|
|
||||||
if (_telemetry_status_pub < 0) {
|
if (_telemetry_status_pub == 0) {
|
||||||
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
|
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1105,7 +1105,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
|
|||||||
|
|
||||||
tsync_offset.offset_ns = _time_offset ;
|
tsync_offset.offset_ns = _time_offset ;
|
||||||
|
|
||||||
if (_time_offset_pub < 0) {
|
if (_time_offset_pub == 0) {
|
||||||
_time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset);
|
_time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1135,7 +1135,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||||||
airspeed.indicated_airspeed_m_s = ias;
|
airspeed.indicated_airspeed_m_s = ias;
|
||||||
airspeed.true_airspeed_m_s = tas;
|
airspeed.true_airspeed_m_s = tas;
|
||||||
|
|
||||||
if (_airspeed_pub < 0) {
|
if (_airspeed_pub == 0) {
|
||||||
_airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed);
|
_airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1157,7 +1157,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||||||
gyro.z = imu.zgyro;
|
gyro.z = imu.zgyro;
|
||||||
gyro.temperature = imu.temperature;
|
gyro.temperature = imu.temperature;
|
||||||
|
|
||||||
if (_gyro_pub < 0) {
|
if (_gyro_pub == 0) {
|
||||||
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
|
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1179,7 +1179,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||||||
accel.z = imu.zacc;
|
accel.z = imu.zacc;
|
||||||
accel.temperature = imu.temperature;
|
accel.temperature = imu.temperature;
|
||||||
|
|
||||||
if (_accel_pub < 0) {
|
if (_accel_pub == 0) {
|
||||||
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
|
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1200,7 +1200,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||||||
mag.y = imu.ymag;
|
mag.y = imu.ymag;
|
||||||
mag.z = imu.zmag;
|
mag.z = imu.zmag;
|
||||||
|
|
||||||
if (_mag_pub < 0) {
|
if (_mag_pub == 0) {
|
||||||
/* publish to the first mag topic */
|
/* publish to the first mag topic */
|
||||||
_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
|
_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
|
||||||
|
|
||||||
@@ -1219,7 +1219,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||||||
baro.altitude = imu.pressure_alt;
|
baro.altitude = imu.pressure_alt;
|
||||||
baro.temperature = imu.temperature;
|
baro.temperature = imu.temperature;
|
||||||
|
|
||||||
if (_baro_pub < 0) {
|
if (_baro_pub == 0) {
|
||||||
_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
|
_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1275,7 +1275,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||||||
hil_sensors.differential_pressure_timestamp = timestamp;
|
hil_sensors.differential_pressure_timestamp = timestamp;
|
||||||
|
|
||||||
/* publish combined sensor topic */
|
/* publish combined sensor topic */
|
||||||
if (_sensors_pub < 0) {
|
if (_sensors_pub == 0) {
|
||||||
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
|
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1294,7 +1294,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||||||
hil_battery_status.current_a = 10.0f;
|
hil_battery_status.current_a = 10.0f;
|
||||||
hil_battery_status.discharged_mah = -1.0f;
|
hil_battery_status.discharged_mah = -1.0f;
|
||||||
|
|
||||||
if (_battery_pub < 0) {
|
if (_battery_pub == 0) {
|
||||||
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
|
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1348,7 +1348,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
|
|||||||
hil_gps.fix_type = gps.fix_type;
|
hil_gps.fix_type = gps.fix_type;
|
||||||
hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used?
|
hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used?
|
||||||
|
|
||||||
if (_gps_pub < 0) {
|
if (_gps_pub == 0) {
|
||||||
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
|
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1373,7 +1373,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
|||||||
airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
|
airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
|
||||||
airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
|
airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
|
||||||
|
|
||||||
if (_airspeed_pub < 0) {
|
if (_airspeed_pub == 0) {
|
||||||
_airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed);
|
_airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1406,7 +1406,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
|||||||
hil_attitude.pitchspeed = hil_state.pitchspeed;
|
hil_attitude.pitchspeed = hil_state.pitchspeed;
|
||||||
hil_attitude.yawspeed = hil_state.yawspeed;
|
hil_attitude.yawspeed = hil_state.yawspeed;
|
||||||
|
|
||||||
if (_attitude_pub < 0) {
|
if (_attitude_pub == 0) {
|
||||||
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
|
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1430,7 +1430,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
|||||||
hil_global_pos.eph = 2.0f;
|
hil_global_pos.eph = 2.0f;
|
||||||
hil_global_pos.epv = 4.0f;
|
hil_global_pos.epv = 4.0f;
|
||||||
|
|
||||||
if (_global_pos_pub < 0) {
|
if (_global_pos_pub == 0) {
|
||||||
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
|
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1471,7 +1471,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
|||||||
hil_local_pos.xy_global = true;
|
hil_local_pos.xy_global = true;
|
||||||
hil_local_pos.z_global = true;
|
hil_local_pos.z_global = true;
|
||||||
|
|
||||||
if (_local_pos_pub < 0) {
|
if (_local_pos_pub == 0) {
|
||||||
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
|
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1486,7 +1486,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
|||||||
hil_land_detector.landed = landed;
|
hil_land_detector.landed = landed;
|
||||||
hil_land_detector.timestamp = hrt_absolute_time();
|
hil_land_detector.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
if (_land_detector_pub < 0) {
|
if (_land_detector_pub == 0) {
|
||||||
_land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector);
|
_land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1509,7 +1509,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
|||||||
accel.z = hil_state.zacc;
|
accel.z = hil_state.zacc;
|
||||||
accel.temperature = 25.0f;
|
accel.temperature = 25.0f;
|
||||||
|
|
||||||
if (_accel_pub < 0) {
|
if (_accel_pub == 0) {
|
||||||
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
|
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -1528,7 +1528,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
|||||||
hil_battery_status.current_a = 10.0f;
|
hil_battery_status.current_a = 10.0f;
|
||||||
hil_battery_status.discharged_mah = -1.0f;
|
hil_battery_status.discharged_mah = -1.0f;
|
||||||
|
|
||||||
if (_battery_pub < 0) {
|
if (_battery_pub == 0) {
|
||||||
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
|
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -314,10 +314,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|||||||
_vehicle_status_sub(-1),
|
_vehicle_status_sub(-1),
|
||||||
|
|
||||||
/* publications */
|
/* publications */
|
||||||
_att_sp_pub(-1),
|
_att_sp_pub(0),
|
||||||
_v_rates_sp_pub(-1),
|
_v_rates_sp_pub(0),
|
||||||
_actuators_0_pub(-1),
|
_actuators_0_pub(0),
|
||||||
_controller_status_pub(-1),
|
_controller_status_pub(0),
|
||||||
_rates_sp_id(0),
|
_rates_sp_id(0),
|
||||||
_actuators_id(0),
|
_actuators_id(0),
|
||||||
|
|
||||||
|
|||||||
@@ -307,9 +307,9 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
|||||||
_global_vel_sp_sub(-1),
|
_global_vel_sp_sub(-1),
|
||||||
|
|
||||||
/* publications */
|
/* publications */
|
||||||
_att_sp_pub(-1),
|
_att_sp_pub(0),
|
||||||
_local_pos_sp_pub(-1),
|
_local_pos_sp_pub(0),
|
||||||
_global_vel_sp_pub(-1),
|
_global_vel_sp_pub(0),
|
||||||
|
|
||||||
_ref_alt(0.0f),
|
_ref_alt(0.0f),
|
||||||
_ref_timestamp(0),
|
_ref_timestamp(0),
|
||||||
|
|||||||
@@ -69,7 +69,7 @@ static const int ERROR = -1;
|
|||||||
|
|
||||||
Geofence::Geofence() :
|
Geofence::Geofence() :
|
||||||
SuperBlock(NULL, "GF"),
|
SuperBlock(NULL, "GF"),
|
||||||
_fence_pub(-1),
|
_fence_pub(0),
|
||||||
_home_pos{},
|
_home_pos{},
|
||||||
_home_pos_set(false),
|
_home_pos_set(false),
|
||||||
_last_horizontal_range_warning(0),
|
_last_horizontal_range_warning(0),
|
||||||
@@ -317,7 +317,7 @@ Geofence::addPoint(int argc, char *argv[])
|
|||||||
void
|
void
|
||||||
Geofence::publishFence(unsigned vertices)
|
Geofence::publishFence(unsigned vertices)
|
||||||
{
|
{
|
||||||
if (_fence_pub == -1) {
|
if (_fence_pub == 0) {
|
||||||
_fence_pub = orb_advertise(ORB_ID(fence), &vertices);
|
_fence_pub = orb_advertise(ORB_ID(fence), &vertices);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -108,10 +108,10 @@ Navigator::Navigator() :
|
|||||||
_onboard_mission_sub(-1),
|
_onboard_mission_sub(-1),
|
||||||
_offboard_mission_sub(-1),
|
_offboard_mission_sub(-1),
|
||||||
_param_update_sub(-1),
|
_param_update_sub(-1),
|
||||||
_pos_sp_triplet_pub(-1),
|
_pos_sp_triplet_pub(0),
|
||||||
_mission_result_pub(-1),
|
_mission_result_pub(0),
|
||||||
_geofence_result_pub(-1),
|
_geofence_result_pub(0),
|
||||||
_att_sp_pub(-1),
|
_att_sp_pub(0),
|
||||||
_vstatus{},
|
_vstatus{},
|
||||||
_control_mode{},
|
_control_mode{},
|
||||||
_global_pos{},
|
_global_pos{},
|
||||||
|
|||||||
@@ -343,7 +343,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||||||
|
|
||||||
/* advertise */
|
/* advertise */
|
||||||
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
|
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
|
||||||
orb_advert_t vehicle_global_position_pub = -1;
|
orb_advert_t vehicle_global_position_pub = 0;
|
||||||
|
|
||||||
struct position_estimator_inav_params params;
|
struct position_estimator_inav_params params;
|
||||||
struct position_estimator_inav_param_handles pos_inav_param_handles;
|
struct position_estimator_inav_param_handles pos_inav_param_handles;
|
||||||
@@ -1158,7 +1158,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||||||
global_pos.eph = eph;
|
global_pos.eph = eph;
|
||||||
global_pos.epv = epv;
|
global_pos.epv = epv;
|
||||||
|
|
||||||
if (vehicle_global_position_pub < 0) {
|
if (vehicle_global_position_pub == 0) {
|
||||||
vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
|
vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -373,8 +373,7 @@ int sdlog2_main(int argc, char *argv[])
|
|||||||
cmd.param1 = -1;
|
cmd.param1 = -1;
|
||||||
cmd.param2 = -1;
|
cmd.param2 = -1;
|
||||||
cmd.param3 = 1;
|
cmd.param3 = 1;
|
||||||
int fd = orb_advertise(ORB_ID(vehicle_command), &cmd);
|
orb_advertise(ORB_ID(vehicle_command), &cmd);
|
||||||
close(fd);
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -384,8 +383,7 @@ int sdlog2_main(int argc, char *argv[])
|
|||||||
cmd.param1 = -1;
|
cmd.param1 = -1;
|
||||||
cmd.param2 = -1;
|
cmd.param2 = -1;
|
||||||
cmd.param3 = 2;
|
cmd.param3 = 2;
|
||||||
int fd = orb_advertise(ORB_ID(vehicle_command), &cmd);
|
orb_advertise(ORB_ID(vehicle_command), &cmd);
|
||||||
close(fd);
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -517,13 +517,13 @@ Sensors::Sensors() :
|
|||||||
_manual_control_sub(-1),
|
_manual_control_sub(-1),
|
||||||
|
|
||||||
/* publications */
|
/* publications */
|
||||||
_sensor_pub(-1),
|
_sensor_pub(0),
|
||||||
_manual_control_pub(-1),
|
_manual_control_pub(0),
|
||||||
_actuator_group_3_pub(-1),
|
_actuator_group_3_pub(0),
|
||||||
_rc_pub(-1),
|
_rc_pub(0),
|
||||||
_battery_pub(-1),
|
_battery_pub(0),
|
||||||
_airspeed_pub(-1),
|
_airspeed_pub(0),
|
||||||
_diff_pres_pub(-1),
|
_diff_pres_pub(0),
|
||||||
|
|
||||||
/* performance counters */
|
/* performance counters */
|
||||||
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
|
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
|
||||||
|
|||||||
@@ -157,7 +157,7 @@ void Simulator::handle_message(mavlink_message_t *msg) {
|
|||||||
fill_sensors_from_imu_msg(&_sensor, &imu);
|
fill_sensors_from_imu_msg(&_sensor, &imu);
|
||||||
|
|
||||||
// publish message
|
// publish message
|
||||||
if(_sensor_combined_pub < 0) {
|
if(_sensor_combined_pub == 0) {
|
||||||
_sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor);
|
_sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor);
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor);
|
orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor);
|
||||||
@@ -171,7 +171,7 @@ void Simulator::handle_message(mavlink_message_t *msg) {
|
|||||||
fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp);
|
fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp);
|
||||||
|
|
||||||
// publish message
|
// publish message
|
||||||
if(_manual_control_sp_pub < 0) {
|
if(_manual_control_sp_pub == 0) {
|
||||||
_manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp);
|
_manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp);
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp);
|
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp);
|
||||||
|
|||||||
@@ -130,7 +130,7 @@ const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
|
|||||||
ORB_DEFINE(parameter_update, struct parameter_update_s);
|
ORB_DEFINE(parameter_update, struct parameter_update_s);
|
||||||
|
|
||||||
/** parameter update topic handle */
|
/** parameter update topic handle */
|
||||||
static orb_advert_t param_topic = -1;
|
static orb_advert_t param_topic = 0;
|
||||||
|
|
||||||
static void param_set_used_internal(param_t param);
|
static void param_set_used_internal(param_t param);
|
||||||
|
|
||||||
@@ -233,7 +233,7 @@ param_notify_changes(void)
|
|||||||
* If we don't have a handle to our topic, create one now; otherwise
|
* If we don't have a handle to our topic, create one now; otherwise
|
||||||
* just publish.
|
* just publish.
|
||||||
*/
|
*/
|
||||||
if (param_topic == -1) {
|
if (param_topic == 0) {
|
||||||
param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
|
param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -135,7 +135,7 @@ __BEGIN_DECLS
|
|||||||
* a file-descriptor-based handle would not otherwise be in scope for the
|
* a file-descriptor-based handle would not otherwise be in scope for the
|
||||||
* publisher.
|
* publisher.
|
||||||
*/
|
*/
|
||||||
typedef intptr_t orb_advert_t;
|
typedef uintptr_t orb_advert_t;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Advertise as the publisher of a topic.
|
* Advertise as the publisher of a topic.
|
||||||
|
|||||||
@@ -89,18 +89,18 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
|
|||||||
/* open the node as an advertiser */
|
/* open the node as an advertiser */
|
||||||
fd = node_open(PUBSUB, meta, data, true, instance, priority);
|
fd = node_open(PUBSUB, meta, data, true, instance, priority);
|
||||||
if (fd == ERROR)
|
if (fd == ERROR)
|
||||||
return ERROR;
|
return 0;
|
||||||
|
|
||||||
/* get the advertiser handle and close the node */
|
/* get the advertiser handle and close the node */
|
||||||
result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
|
result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
|
||||||
close(fd);
|
close(fd);
|
||||||
if (result == ERROR)
|
if (result == ERROR)
|
||||||
return ERROR;
|
return 0;
|
||||||
|
|
||||||
/* the advertiser must perform an initial publish to initialise the object */
|
/* the advertiser must perform an initial publish to initialise the object */
|
||||||
result = orb_publish(meta, advertiser, data);
|
result = orb_publish(meta, advertiser, data);
|
||||||
if (result == ERROR)
|
if (result == ERROR)
|
||||||
return ERROR;
|
return 0;
|
||||||
|
|
||||||
return advertiser;
|
return advertiser;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -94,7 +94,7 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
|
|||||||
fd = node_open(PUBSUB, meta, data, true, instance, priority);
|
fd = node_open(PUBSUB, meta, data, true, instance, priority);
|
||||||
if (fd == ERROR) {
|
if (fd == ERROR) {
|
||||||
warnx("node_open as advertiser failed.");
|
warnx("node_open as advertiser failed.");
|
||||||
return ERROR;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* get the advertiser handle and close the node */
|
/* get the advertiser handle and close the node */
|
||||||
@@ -102,14 +102,14 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
|
|||||||
px4_close(fd);
|
px4_close(fd);
|
||||||
if (result == ERROR) {
|
if (result == ERROR) {
|
||||||
warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd);
|
warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd);
|
||||||
return ERROR;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* the advertiser must perform an initial publish to initialise the object */
|
/* the advertiser must perform an initial publish to initialise the object */
|
||||||
result = orb_publish(meta, advertiser, data);
|
result = orb_publish(meta, advertiser, data);
|
||||||
if (result == ERROR) {
|
if (result == ERROR) {
|
||||||
warnx("orb_publish failed");
|
warnx("orb_publish failed");
|
||||||
return ERROR;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
return advertiser;
|
return advertiser;
|
||||||
|
|||||||
@@ -137,16 +137,17 @@ int uORBTest::UnitTest::pubsublatency_main(void)
|
|||||||
int uORBTest::UnitTest::test()
|
int uORBTest::UnitTest::test()
|
||||||
{
|
{
|
||||||
struct orb_test t, u;
|
struct orb_test t, u;
|
||||||
int pfd, sfd;
|
int sfd;
|
||||||
|
orb_advert_t ptopic;
|
||||||
bool updated;
|
bool updated;
|
||||||
|
|
||||||
t.val = 0;
|
t.val = 0;
|
||||||
pfd = orb_advertise(ORB_ID(orb_test), &t);
|
ptopic = orb_advertise(ORB_ID(orb_test), &t);
|
||||||
|
|
||||||
if (pfd < 0)
|
if (ptopic == 0)
|
||||||
return test_fail("advertise failed: %d", errno);
|
return test_fail("advertise failed: %d", errno);
|
||||||
|
|
||||||
test_note("publish handle 0x%08x", pfd);
|
test_note("publish handle 0x%08x", ptopic);
|
||||||
sfd = orb_subscribe(ORB_ID(orb_test));
|
sfd = orb_subscribe(ORB_ID(orb_test));
|
||||||
|
|
||||||
if (sfd < 0)
|
if (sfd < 0)
|
||||||
@@ -170,7 +171,7 @@ int uORBTest::UnitTest::test()
|
|||||||
t.val = 2;
|
t.val = 2;
|
||||||
test_note("try publish");
|
test_note("try publish");
|
||||||
|
|
||||||
if (PX4_OK != orb_publish(ORB_ID(orb_test), pfd, &t))
|
if (PX4_OK != orb_publish(ORB_ID(orb_test), ptopic, &t))
|
||||||
return test_fail("publish failed");
|
return test_fail("publish failed");
|
||||||
|
|
||||||
if (PX4_OK != orb_check(sfd, &updated))
|
if (PX4_OK != orb_check(sfd, &updated))
|
||||||
@@ -186,7 +187,6 @@ int uORBTest::UnitTest::test()
|
|||||||
return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val);
|
return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val);
|
||||||
|
|
||||||
orb_unsubscribe(sfd);
|
orb_unsubscribe(sfd);
|
||||||
close(pfd);
|
|
||||||
|
|
||||||
/* this routine tests the multi-topic support */
|
/* this routine tests the multi-topic support */
|
||||||
test_note("try multi-topic support");
|
test_note("try multi-topic support");
|
||||||
@@ -197,7 +197,7 @@ int uORBTest::UnitTest::test()
|
|||||||
test_note("advertised");
|
test_note("advertised");
|
||||||
|
|
||||||
int instance1;
|
int instance1;
|
||||||
int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
|
orb_advert_t pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
|
||||||
|
|
||||||
if (instance0 != 0)
|
if (instance0 != 0)
|
||||||
return test_fail("mult. id0: %d", instance0);
|
return test_fail("mult. id0: %d", instance0);
|
||||||
|
|||||||
@@ -98,7 +98,7 @@ int uORBTest::UnitTest::latency_test(orb_id_t T, bool print)
|
|||||||
t.val = 308;
|
t.val = 308;
|
||||||
t.time = hrt_absolute_time();
|
t.time = hrt_absolute_time();
|
||||||
|
|
||||||
int pfd0 = orb_advertise(T, &t);
|
orb_advert_t pfd0 = orb_advertise(T, &t);
|
||||||
|
|
||||||
char * const args[1] = { NULL };
|
char * const args[1] = { NULL };
|
||||||
|
|
||||||
@@ -128,8 +128,6 @@ int uORBTest::UnitTest::latency_test(orb_id_t T, bool print)
|
|||||||
usleep(1000);
|
usleep(1000);
|
||||||
}
|
}
|
||||||
|
|
||||||
px4_close(pfd0);
|
|
||||||
|
|
||||||
if (pubsub_task < 0) {
|
if (pubsub_task < 0) {
|
||||||
return test_fail("failed launching task");
|
return test_fail("failed launching task");
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -48,7 +48,7 @@ const char *const UavcanGnssBridge::NAME = "gnss";
|
|||||||
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
|
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
|
||||||
_node(node),
|
_node(node),
|
||||||
_sub_fix(node),
|
_sub_fix(node),
|
||||||
_report_pub(-1)
|
_report_pub(0)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -119,7 +119,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
|
|||||||
channel->class_instance = class_instance;
|
channel->class_instance = class_instance;
|
||||||
|
|
||||||
channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH);
|
channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH);
|
||||||
if (channel->orb_advert < 0) {
|
if (channel->orb_advert == 0) {
|
||||||
log("ADVERTISE FAILED");
|
log("ADVERTISE FAILED");
|
||||||
(void)unregister_class_devname(_class_devname, class_instance);
|
(void)unregister_class_devname(_class_devname, class_instance);
|
||||||
*channel = Channel();
|
*channel = Channel();
|
||||||
|
|||||||
@@ -230,10 +230,10 @@ VtolAttitudeControl::VtolAttitudeControl() :
|
|||||||
_battery_status_sub(-1),
|
_battery_status_sub(-1),
|
||||||
|
|
||||||
//init publication handlers
|
//init publication handlers
|
||||||
_actuators_0_pub(-1),
|
_actuators_0_pub(0),
|
||||||
_actuators_1_pub(-1),
|
_actuators_1_pub(0),
|
||||||
_vtol_vehicle_status_pub(-1),
|
_vtol_vehicle_status_pub(0),
|
||||||
_v_rates_sp_pub(-1),
|
_v_rates_sp_pub(0),
|
||||||
|
|
||||||
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")),
|
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")),
|
||||||
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input"))
|
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input"))
|
||||||
|
|||||||
@@ -369,7 +369,7 @@ ACCELSIM::ACCELSIM(const char* path, enum Rotation rotation) :
|
|||||||
_mag_range_ga(0.0f),
|
_mag_range_ga(0.0f),
|
||||||
_mag_range_scale(0.0f),
|
_mag_range_scale(0.0f),
|
||||||
_mag_samplerate(0),
|
_mag_samplerate(0),
|
||||||
_accel_topic(-1),
|
_accel_topic(0),
|
||||||
_accel_orb_class_instance(-1),
|
_accel_orb_class_instance(-1),
|
||||||
_accel_class_instance(-1),
|
_accel_class_instance(-1),
|
||||||
_accel_read(0),
|
_accel_read(0),
|
||||||
@@ -496,7 +496,7 @@ ACCELSIM::init()
|
|||||||
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
|
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
|
||||||
&_accel_orb_class_instance, ORB_PRIO_DEFAULT);
|
&_accel_orb_class_instance, ORB_PRIO_DEFAULT);
|
||||||
|
|
||||||
if (_accel_topic == (orb_advert_t)(-1)) {
|
if (_accel_topic == 0) {
|
||||||
PX4_WARN("ADVERT ERR");
|
PX4_WARN("ADVERT ERR");
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1077,7 +1077,7 @@ ACCELSIM::measure()
|
|||||||
|
|
||||||
// The first call to measure() is from init() and _accel_topic is not
|
// The first call to measure() is from init() and _accel_topic is not
|
||||||
// yet initialized
|
// yet initialized
|
||||||
if (_accel_topic != (orb_advert_t)(-1)) {
|
if (_accel_topic != 0) {
|
||||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -83,8 +83,8 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha
|
|||||||
_measure_ticks(0),
|
_measure_ticks(0),
|
||||||
_collect_phase(false),
|
_collect_phase(false),
|
||||||
_diff_pres_offset(0.0f),
|
_diff_pres_offset(0.0f),
|
||||||
_airspeed_pub(-1),
|
_airspeed_pub(0),
|
||||||
_subsys_pub(-1),
|
_subsys_pub(0),
|
||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_conversion_interval(conversion_interval),
|
_conversion_interval(conversion_interval),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
|
||||||
@@ -143,7 +143,7 @@ Airspeed::init()
|
|||||||
/* measurement will have generated a report, publish */
|
/* measurement will have generated a report, publish */
|
||||||
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
|
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
|
||||||
|
|
||||||
if (_airspeed_pub < 0)
|
if (_airspeed_pub == 0)
|
||||||
PX4_WARN("uORB started?");
|
PX4_WARN("uORB started?");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -213,7 +213,7 @@ BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const cha
|
|||||||
_OFF(0),
|
_OFF(0),
|
||||||
_SENS(0),
|
_SENS(0),
|
||||||
_msl_pressure(101325),
|
_msl_pressure(101325),
|
||||||
_baro_topic(-1),
|
_baro_topic(0),
|
||||||
_orb_class_instance(-1),
|
_orb_class_instance(-1),
|
||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "barosim_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "barosim_read")),
|
||||||
@@ -315,7 +315,7 @@ BAROSIM::init()
|
|||||||
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
|
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
|
||||||
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
|
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
|
||||||
|
|
||||||
if (_baro_topic == (orb_advert_t)(-1)) {
|
if (_baro_topic == 0) {
|
||||||
PX4_ERR("failed to create sensor_baro publication");
|
PX4_ERR("failed to create sensor_baro publication");
|
||||||
}
|
}
|
||||||
//PX4_WARN("sensor_baro publication %ld", _baro_topic);
|
//PX4_WARN("sensor_baro publication %ld", _baro_topic);
|
||||||
@@ -722,7 +722,7 @@ BAROSIM::collect()
|
|||||||
|
|
||||||
/* publish it */
|
/* publish it */
|
||||||
if (!(_pub_blocked)) {
|
if (!(_pub_blocked)) {
|
||||||
if (_baro_topic != (orb_advert_t)(-1)) {
|
if (_baro_topic != 0) {
|
||||||
/* publish it */
|
/* publish it */
|
||||||
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
|
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -472,7 +472,7 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro
|
|||||||
_accel_scale{},
|
_accel_scale{},
|
||||||
_accel_range_scale(0.0f),
|
_accel_range_scale(0.0f),
|
||||||
_accel_range_m_s2(0.0f),
|
_accel_range_m_s2(0.0f),
|
||||||
_accel_topic(-1),
|
_accel_topic(0),
|
||||||
_accel_orb_class_instance(-1),
|
_accel_orb_class_instance(-1),
|
||||||
_accel_class_instance(-1),
|
_accel_class_instance(-1),
|
||||||
_gyro_reports(nullptr),
|
_gyro_reports(nullptr),
|
||||||
@@ -624,7 +624,7 @@ GYROSIM::init()
|
|||||||
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
|
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
|
||||||
&_accel_orb_class_instance, ORB_PRIO_HIGH);
|
&_accel_orb_class_instance, ORB_PRIO_HIGH);
|
||||||
|
|
||||||
if (_accel_topic < 0) {
|
if (_accel_topic == 0) {
|
||||||
PX4_WARN("ADVERT FAIL");
|
PX4_WARN("ADVERT FAIL");
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -636,7 +636,7 @@ GYROSIM::init()
|
|||||||
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
|
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
|
||||||
&_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH);
|
&_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH);
|
||||||
|
|
||||||
if (_gyro->_gyro_topic < 0) {
|
if (_gyro->_gyro_topic == 0) {
|
||||||
PX4_WARN("ADVERT FAIL");
|
PX4_WARN("ADVERT FAIL");
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1547,7 +1547,7 @@ GYROSIM::print_registers()
|
|||||||
GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) :
|
GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) :
|
||||||
VDev("GYROSIM_gyro", path),
|
VDev("GYROSIM_gyro", path),
|
||||||
_parent(parent),
|
_parent(parent),
|
||||||
_gyro_topic(-1),
|
_gyro_topic(0),
|
||||||
_gyro_orb_class_instance(-1),
|
_gyro_orb_class_instance(-1),
|
||||||
_gyro_class_instance(-1)
|
_gyro_class_instance(-1)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -59,7 +59,7 @@ __EXPORT int motor_test_main(int argc, char *argv[]);
|
|||||||
static void motor_test(unsigned channel, float value);
|
static void motor_test(unsigned channel, float value);
|
||||||
static void usage(const char *reason);
|
static void usage(const char *reason);
|
||||||
|
|
||||||
static orb_advert_t _test_motor_pub;
|
static orb_advert_t _test_motor_pub = 0;
|
||||||
|
|
||||||
void motor_test(unsigned channel, float value)
|
void motor_test(unsigned channel, float value)
|
||||||
{
|
{
|
||||||
|
|||||||
Reference in New Issue
Block a user