mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-28 23:00:35 +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
@@ -114,7 +114,7 @@ private:
|
||||
int _sensors_sub = -1;
|
||||
int _params_sub = -1;
|
||||
int _global_pos_sub = -1;
|
||||
orb_advert_t _att_pub = -1;
|
||||
orb_advert_t _att_pub = 0;
|
||||
|
||||
struct {
|
||||
param_t w_acc;
|
||||
@@ -327,7 +327,7 @@ void AttitudeEstimatorQ::task_main() {
|
||||
memcpy(&att.R[0], R.data, sizeof(att.R));
|
||||
att.R_valid = true;
|
||||
|
||||
if (_att_pub < 0) {
|
||||
if (_att_pub == 0) {
|
||||
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
||||
} else {
|
||||
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));
|
||||
|
||||
/* 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);
|
||||
|
||||
int loopcounter = 0;
|
||||
|
||||
@@ -180,7 +180,7 @@ BottleDrop::BottleDrop() :
|
||||
_command {},
|
||||
_global_pos {},
|
||||
ref {},
|
||||
_actuator_pub(-1),
|
||||
_actuator_pub(0),
|
||||
_actuators {},
|
||||
_drop_approval(false),
|
||||
_doors_opened(0),
|
||||
@@ -190,7 +190,7 @@ BottleDrop::BottleDrop() :
|
||||
_drop_position {},
|
||||
_drop_state(DROP_STATE_INIT),
|
||||
_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 */
|
||||
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("exiting.");
|
||||
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);
|
||||
|
||||
/* home position */
|
||||
orb_advert_t home_pub = -1;
|
||||
orb_advert_t home_pub = 0;
|
||||
struct home_position_s 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 */
|
||||
orb_advert_t mission_pub = -1;
|
||||
orb_advert_t mission_pub = 0;
|
||||
mission_s mission;
|
||||
|
||||
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(param_changed_sub);
|
||||
close(battery_sub);
|
||||
close(mission_pub);
|
||||
|
||||
thread_running = false;
|
||||
|
||||
|
||||
@@ -129,11 +129,11 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
|
||||
_armedSub(-1),
|
||||
|
||||
/* publications */
|
||||
_att_pub(-1),
|
||||
_global_pos_pub(-1),
|
||||
_local_pos_pub(-1),
|
||||
_estimator_status_pub(-1),
|
||||
_wind_pub(-1),
|
||||
_att_pub(0),
|
||||
_global_pos_pub(0),
|
||||
_local_pos_pub(0),
|
||||
_estimator_status_pub(0),
|
||||
_wind_pub(0),
|
||||
|
||||
_att({}),
|
||||
_gyro({}),
|
||||
|
||||
@@ -337,10 +337,10 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
||||
_vehicle_status_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_rate_sp_pub(-1),
|
||||
_attitude_sp_pub(-1),
|
||||
_actuators_0_pub(-1),
|
||||
_actuators_2_pub(-1),
|
||||
_rate_sp_pub(0),
|
||||
_attitude_sp_pub(0),
|
||||
_actuators_0_pub(0),
|
||||
_actuators_2_pub(0),
|
||||
|
||||
_rates_sp_id(0),
|
||||
_actuators_id(0),
|
||||
|
||||
@@ -434,9 +434,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||
_sensor_combined_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_attitude_sp_pub(-1),
|
||||
_tecs_status_pub(-1),
|
||||
_nav_capabilities_pub(-1),
|
||||
_attitude_sp_pub(0),
|
||||
_tecs_status_pub(0),
|
||||
_nav_capabilities_pub(0),
|
||||
|
||||
/* states */
|
||||
_att(),
|
||||
|
||||
@@ -44,7 +44,7 @@
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
LandDetector::LandDetector() :
|
||||
_landDetectedPub(-1),
|
||||
_landDetectedPub(0),
|
||||
_landDetected({0, false}),
|
||||
_taskShouldExit(false),
|
||||
_taskIsRunning(false)
|
||||
@@ -55,7 +55,6 @@ LandDetector::LandDetector() :
|
||||
LandDetector::~LandDetector()
|
||||
{
|
||||
_taskShouldExit = true;
|
||||
close(_landDetectedPub);
|
||||
}
|
||||
|
||||
void LandDetector::shutdown()
|
||||
|
||||
@@ -81,7 +81,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
|
||||
_transfer_partner_compid(0),
|
||||
_offboard_mission_sub(-1),
|
||||
_mission_result_sub(-1),
|
||||
_offboard_mission_pub(-1),
|
||||
_offboard_mission_pub(0),
|
||||
_slow_rate_limiter(_interval / 10.0f),
|
||||
_verbose(false)
|
||||
{
|
||||
@@ -93,7 +93,6 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
|
||||
|
||||
MavlinkMissionManager::~MavlinkMissionManager()
|
||||
{
|
||||
close(_offboard_mission_pub);
|
||||
close(_mission_result_sub);
|
||||
}
|
||||
|
||||
@@ -150,7 +149,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
|
||||
_current_seq = seq;
|
||||
|
||||
/* 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);
|
||||
|
||||
} else {
|
||||
|
||||
@@ -45,7 +45,7 @@
|
||||
|
||||
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||
_send_all_index(-1),
|
||||
_rc_param_map_pub(-1),
|
||||
_rc_param_map_pub(0),
|
||||
_rc_param_map()
|
||||
{
|
||||
}
|
||||
@@ -162,7 +162,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
|
||||
}
|
||||
_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);
|
||||
|
||||
} else {
|
||||
|
||||
@@ -95,34 +95,34 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||
hil_local_pos{},
|
||||
hil_land_detector{},
|
||||
_control_mode{},
|
||||
_global_pos_pub(-1),
|
||||
_local_pos_pub(-1),
|
||||
_attitude_pub(-1),
|
||||
_gps_pub(-1),
|
||||
_sensors_pub(-1),
|
||||
_gyro_pub(-1),
|
||||
_accel_pub(-1),
|
||||
_mag_pub(-1),
|
||||
_baro_pub(-1),
|
||||
_airspeed_pub(-1),
|
||||
_battery_pub(-1),
|
||||
_cmd_pub(-1),
|
||||
_flow_pub(-1),
|
||||
_distance_sensor_pub(-1),
|
||||
_offboard_control_mode_pub(-1),
|
||||
_actuator_controls_pub(-1),
|
||||
_global_vel_sp_pub(-1),
|
||||
_att_sp_pub(-1),
|
||||
_rates_sp_pub(-1),
|
||||
_force_sp_pub(-1),
|
||||
_pos_sp_triplet_pub(-1),
|
||||
_vicon_position_pub(-1),
|
||||
_vision_position_pub(-1),
|
||||
_telemetry_status_pub(-1),
|
||||
_rc_pub(-1),
|
||||
_manual_pub(-1),
|
||||
_land_detector_pub(-1),
|
||||
_time_offset_pub(-1),
|
||||
_global_pos_pub(0),
|
||||
_local_pos_pub(0),
|
||||
_attitude_pub(0),
|
||||
_gps_pub(0),
|
||||
_sensors_pub(0),
|
||||
_gyro_pub(0),
|
||||
_accel_pub(0),
|
||||
_mag_pub(0),
|
||||
_baro_pub(0),
|
||||
_airspeed_pub(0),
|
||||
_battery_pub(0),
|
||||
_cmd_pub(0),
|
||||
_flow_pub(0),
|
||||
_distance_sensor_pub(0),
|
||||
_offboard_control_mode_pub(0),
|
||||
_actuator_controls_pub(0),
|
||||
_global_vel_sp_pub(0),
|
||||
_att_sp_pub(0),
|
||||
_rates_sp_pub(0),
|
||||
_force_sp_pub(0),
|
||||
_pos_sp_triplet_pub(0),
|
||||
_vicon_position_pub(0),
|
||||
_vision_position_pub(0),
|
||||
_telemetry_status_pub(0),
|
||||
_rc_pub(0),
|
||||
_manual_pub(0),
|
||||
_land_detector_pub(0),
|
||||
_time_offset_pub(0),
|
||||
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
|
||||
_hil_frames(0),
|
||||
_old_timestamp(0),
|
||||
@@ -314,7 +314,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
|
||||
vcmd.source_component = msg->compid;
|
||||
vcmd.confirmation = cmd_mavlink.confirmation;
|
||||
|
||||
if (_cmd_pub < 0) {
|
||||
if (_cmd_pub == 0) {
|
||||
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||
|
||||
} else {
|
||||
@@ -370,7 +370,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
|
||||
vcmd.source_system = msg->sysid;
|
||||
vcmd.source_component = msg->compid;
|
||||
|
||||
if (_cmd_pub < 0) {
|
||||
if (_cmd_pub == 0) {
|
||||
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||
|
||||
} else {
|
||||
@@ -410,7 +410,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
|
||||
float zeroval = 0.0f;
|
||||
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);
|
||||
|
||||
} else {
|
||||
@@ -461,7 +461,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
|
||||
f.sensor_id = flow.sensor_id;
|
||||
f.gyro_temperature = flow.temperature;
|
||||
|
||||
if (_flow_pub < 0) {
|
||||
if (_flow_pub == 0) {
|
||||
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
|
||||
|
||||
} else {
|
||||
@@ -515,7 +515,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
|
||||
vcmd.source_component = msg->compid;
|
||||
vcmd.confirmation = 1;
|
||||
|
||||
if (_cmd_pub < 0) {
|
||||
if (_cmd_pub == 0) {
|
||||
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||
|
||||
} else {
|
||||
@@ -570,7 +570,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
|
||||
vicon_position.pitch = pos.pitch;
|
||||
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);
|
||||
|
||||
} else {
|
||||
@@ -605,7 +605,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
||||
|
||||
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);
|
||||
|
||||
} 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.z = set_position_target_local_ned.afz;
|
||||
//XXX: yaw
|
||||
if (_force_sp_pub < 0) {
|
||||
if (_force_sp_pub == 0) {
|
||||
_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
|
||||
} else {
|
||||
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)
|
||||
|
||||
|
||||
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);
|
||||
} else {
|
||||
@@ -740,7 +740,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
|
||||
|
||||
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);
|
||||
} else {
|
||||
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];
|
||||
}
|
||||
|
||||
if (_actuator_controls_pub < 0) {
|
||||
if (_actuator_controls_pub == 0) {
|
||||
_actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls);
|
||||
} else {
|
||||
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[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);
|
||||
|
||||
} else {
|
||||
@@ -857,7 +857,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
||||
|
||||
_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);
|
||||
|
||||
} else {
|
||||
@@ -892,7 +892,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
||||
_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);
|
||||
} else {
|
||||
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;
|
||||
}
|
||||
|
||||
if (_att_sp_pub < 0) {
|
||||
if (_att_sp_pub == 0) {
|
||||
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
|
||||
} else {
|
||||
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.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);
|
||||
|
||||
} 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);
|
||||
|
||||
if (_manual_pub < 0) {
|
||||
if (_manual_pub == 0) {
|
||||
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
|
||||
|
||||
} else {
|
||||
@@ -1001,7 +1001,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
||||
tstatus.timestamp = hrt_absolute_time();
|
||||
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);
|
||||
|
||||
} else {
|
||||
@@ -1105,7 +1105,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
|
||||
|
||||
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);
|
||||
|
||||
} else {
|
||||
@@ -1135,7 +1135,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
||||
airspeed.indicated_airspeed_m_s = ias;
|
||||
airspeed.true_airspeed_m_s = tas;
|
||||
|
||||
if (_airspeed_pub < 0) {
|
||||
if (_airspeed_pub == 0) {
|
||||
_airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed);
|
||||
|
||||
} else {
|
||||
@@ -1157,7 +1157,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
||||
gyro.z = imu.zgyro;
|
||||
gyro.temperature = imu.temperature;
|
||||
|
||||
if (_gyro_pub < 0) {
|
||||
if (_gyro_pub == 0) {
|
||||
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
|
||||
|
||||
} else {
|
||||
@@ -1179,7 +1179,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
||||
accel.z = imu.zacc;
|
||||
accel.temperature = imu.temperature;
|
||||
|
||||
if (_accel_pub < 0) {
|
||||
if (_accel_pub == 0) {
|
||||
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
|
||||
|
||||
} else {
|
||||
@@ -1200,7 +1200,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
||||
mag.y = imu.ymag;
|
||||
mag.z = imu.zmag;
|
||||
|
||||
if (_mag_pub < 0) {
|
||||
if (_mag_pub == 0) {
|
||||
/* publish to the first mag topic */
|
||||
_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.temperature = imu.temperature;
|
||||
|
||||
if (_baro_pub < 0) {
|
||||
if (_baro_pub == 0) {
|
||||
_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
|
||||
|
||||
} else {
|
||||
@@ -1275,7 +1275,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
||||
hil_sensors.differential_pressure_timestamp = timestamp;
|
||||
|
||||
/* publish combined sensor topic */
|
||||
if (_sensors_pub < 0) {
|
||||
if (_sensors_pub == 0) {
|
||||
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
|
||||
|
||||
} else {
|
||||
@@ -1294,7 +1294,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
||||
hil_battery_status.current_a = 10.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);
|
||||
|
||||
} else {
|
||||
@@ -1348,7 +1348,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
|
||||
hil_gps.fix_type = gps.fix_type;
|
||||
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);
|
||||
|
||||
} 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.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);
|
||||
|
||||
} else {
|
||||
@@ -1406,7 +1406,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
||||
hil_attitude.pitchspeed = hil_state.pitchspeed;
|
||||
hil_attitude.yawspeed = hil_state.yawspeed;
|
||||
|
||||
if (_attitude_pub < 0) {
|
||||
if (_attitude_pub == 0) {
|
||||
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
|
||||
|
||||
} else {
|
||||
@@ -1430,7 +1430,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
||||
hil_global_pos.eph = 2.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);
|
||||
|
||||
} else {
|
||||
@@ -1471,7 +1471,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
||||
hil_local_pos.xy_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);
|
||||
|
||||
} else {
|
||||
@@ -1486,7 +1486,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
||||
hil_land_detector.landed = landed;
|
||||
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);
|
||||
|
||||
} else {
|
||||
@@ -1509,7 +1509,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
||||
accel.z = hil_state.zacc;
|
||||
accel.temperature = 25.0f;
|
||||
|
||||
if (_accel_pub < 0) {
|
||||
if (_accel_pub == 0) {
|
||||
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
|
||||
|
||||
} 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.discharged_mah = -1.0f;
|
||||
|
||||
if (_battery_pub < 0) {
|
||||
if (_battery_pub == 0) {
|
||||
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
|
||||
|
||||
} else {
|
||||
|
||||
@@ -314,10 +314,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
||||
_vehicle_status_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_att_sp_pub(-1),
|
||||
_v_rates_sp_pub(-1),
|
||||
_actuators_0_pub(-1),
|
||||
_controller_status_pub(-1),
|
||||
_att_sp_pub(0),
|
||||
_v_rates_sp_pub(0),
|
||||
_actuators_0_pub(0),
|
||||
_controller_status_pub(0),
|
||||
_rates_sp_id(0),
|
||||
_actuators_id(0),
|
||||
|
||||
|
||||
@@ -307,9 +307,9 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
||||
_global_vel_sp_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_att_sp_pub(-1),
|
||||
_local_pos_sp_pub(-1),
|
||||
_global_vel_sp_pub(-1),
|
||||
_att_sp_pub(0),
|
||||
_local_pos_sp_pub(0),
|
||||
_global_vel_sp_pub(0),
|
||||
|
||||
_ref_alt(0.0f),
|
||||
_ref_timestamp(0),
|
||||
|
||||
@@ -69,7 +69,7 @@ static const int ERROR = -1;
|
||||
|
||||
Geofence::Geofence() :
|
||||
SuperBlock(NULL, "GF"),
|
||||
_fence_pub(-1),
|
||||
_fence_pub(0),
|
||||
_home_pos{},
|
||||
_home_pos_set(false),
|
||||
_last_horizontal_range_warning(0),
|
||||
@@ -317,7 +317,7 @@ Geofence::addPoint(int argc, char *argv[])
|
||||
void
|
||||
Geofence::publishFence(unsigned vertices)
|
||||
{
|
||||
if (_fence_pub == -1) {
|
||||
if (_fence_pub == 0) {
|
||||
_fence_pub = orb_advertise(ORB_ID(fence), &vertices);
|
||||
|
||||
} else {
|
||||
|
||||
@@ -108,10 +108,10 @@ Navigator::Navigator() :
|
||||
_onboard_mission_sub(-1),
|
||||
_offboard_mission_sub(-1),
|
||||
_param_update_sub(-1),
|
||||
_pos_sp_triplet_pub(-1),
|
||||
_mission_result_pub(-1),
|
||||
_geofence_result_pub(-1),
|
||||
_att_sp_pub(-1),
|
||||
_pos_sp_triplet_pub(0),
|
||||
_mission_result_pub(0),
|
||||
_geofence_result_pub(0),
|
||||
_att_sp_pub(0),
|
||||
_vstatus{},
|
||||
_control_mode{},
|
||||
_global_pos{},
|
||||
|
||||
@@ -343,7 +343,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
|
||||
/* advertise */
|
||||
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_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.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);
|
||||
|
||||
} else {
|
||||
|
||||
@@ -373,8 +373,7 @@ int sdlog2_main(int argc, char *argv[])
|
||||
cmd.param1 = -1;
|
||||
cmd.param2 = -1;
|
||||
cmd.param3 = 1;
|
||||
int fd = orb_advertise(ORB_ID(vehicle_command), &cmd);
|
||||
close(fd);
|
||||
orb_advertise(ORB_ID(vehicle_command), &cmd);
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -384,8 +383,7 @@ int sdlog2_main(int argc, char *argv[])
|
||||
cmd.param1 = -1;
|
||||
cmd.param2 = -1;
|
||||
cmd.param3 = 2;
|
||||
int fd = orb_advertise(ORB_ID(vehicle_command), &cmd);
|
||||
close(fd);
|
||||
orb_advertise(ORB_ID(vehicle_command), &cmd);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -517,13 +517,13 @@ Sensors::Sensors() :
|
||||
_manual_control_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_sensor_pub(-1),
|
||||
_manual_control_pub(-1),
|
||||
_actuator_group_3_pub(-1),
|
||||
_rc_pub(-1),
|
||||
_battery_pub(-1),
|
||||
_airspeed_pub(-1),
|
||||
_diff_pres_pub(-1),
|
||||
_sensor_pub(0),
|
||||
_manual_control_pub(0),
|
||||
_actuator_group_3_pub(0),
|
||||
_rc_pub(0),
|
||||
_battery_pub(0),
|
||||
_airspeed_pub(0),
|
||||
_diff_pres_pub(0),
|
||||
|
||||
/* performance counters */
|
||||
_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);
|
||||
|
||||
// publish message
|
||||
if(_sensor_combined_pub < 0) {
|
||||
if(_sensor_combined_pub == 0) {
|
||||
_sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor);
|
||||
} else {
|
||||
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);
|
||||
|
||||
// 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);
|
||||
} else {
|
||||
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);
|
||||
|
||||
/** 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);
|
||||
|
||||
@@ -233,7 +233,7 @@ param_notify_changes(void)
|
||||
* If we don't have a handle to our topic, create one now; otherwise
|
||||
* just publish.
|
||||
*/
|
||||
if (param_topic == -1) {
|
||||
if (param_topic == 0) {
|
||||
param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
|
||||
|
||||
} else {
|
||||
|
||||
@@ -135,7 +135,7 @@ __BEGIN_DECLS
|
||||
* a file-descriptor-based handle would not otherwise be in scope for the
|
||||
* publisher.
|
||||
*/
|
||||
typedef intptr_t orb_advert_t;
|
||||
typedef uintptr_t orb_advert_t;
|
||||
|
||||
/**
|
||||
* 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 */
|
||||
fd = node_open(PUBSUB, meta, data, true, instance, priority);
|
||||
if (fd == ERROR)
|
||||
return ERROR;
|
||||
return 0;
|
||||
|
||||
/* get the advertiser handle and close the node */
|
||||
result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
|
||||
close(fd);
|
||||
if (result == ERROR)
|
||||
return ERROR;
|
||||
return 0;
|
||||
|
||||
/* the advertiser must perform an initial publish to initialise the object */
|
||||
result = orb_publish(meta, advertiser, data);
|
||||
if (result == ERROR)
|
||||
return ERROR;
|
||||
return 0;
|
||||
|
||||
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);
|
||||
if (fd == ERROR) {
|
||||
warnx("node_open as advertiser failed.");
|
||||
return ERROR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* 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);
|
||||
if (result == ERROR) {
|
||||
warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd);
|
||||
return ERROR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* the advertiser must perform an initial publish to initialise the object */
|
||||
result = orb_publish(meta, advertiser, data);
|
||||
if (result == ERROR) {
|
||||
warnx("orb_publish failed");
|
||||
return ERROR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
return advertiser;
|
||||
|
||||
@@ -137,16 +137,17 @@ int uORBTest::UnitTest::pubsublatency_main(void)
|
||||
int uORBTest::UnitTest::test()
|
||||
{
|
||||
struct orb_test t, u;
|
||||
int pfd, sfd;
|
||||
int sfd;
|
||||
orb_advert_t ptopic;
|
||||
bool updated;
|
||||
|
||||
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);
|
||||
|
||||
test_note("publish handle 0x%08x", pfd);
|
||||
test_note("publish handle 0x%08x", ptopic);
|
||||
sfd = orb_subscribe(ORB_ID(orb_test));
|
||||
|
||||
if (sfd < 0)
|
||||
@@ -170,7 +171,7 @@ int uORBTest::UnitTest::test()
|
||||
t.val = 2;
|
||||
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");
|
||||
|
||||
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);
|
||||
|
||||
orb_unsubscribe(sfd);
|
||||
close(pfd);
|
||||
|
||||
/* this routine tests the multi-topic support */
|
||||
test_note("try multi-topic support");
|
||||
@@ -197,7 +197,7 @@ int uORBTest::UnitTest::test()
|
||||
test_note("advertised");
|
||||
|
||||
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)
|
||||
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.time = hrt_absolute_time();
|
||||
|
||||
int pfd0 = orb_advertise(T, &t);
|
||||
orb_advert_t pfd0 = orb_advertise(T, &t);
|
||||
|
||||
char * const args[1] = { NULL };
|
||||
|
||||
@@ -128,8 +128,6 @@ int uORBTest::UnitTest::latency_test(orb_id_t T, bool print)
|
||||
usleep(1000);
|
||||
}
|
||||
|
||||
px4_close(pfd0);
|
||||
|
||||
if (pubsub_task < 0) {
|
||||
return test_fail("failed launching task");
|
||||
}
|
||||
|
||||
@@ -48,7 +48,7 @@ const char *const UavcanGnssBridge::NAME = "gnss";
|
||||
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
|
||||
_node(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->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");
|
||||
(void)unregister_class_devname(_class_devname, class_instance);
|
||||
*channel = Channel();
|
||||
|
||||
@@ -230,10 +230,10 @@ VtolAttitudeControl::VtolAttitudeControl() :
|
||||
_battery_status_sub(-1),
|
||||
|
||||
//init publication handlers
|
||||
_actuators_0_pub(-1),
|
||||
_actuators_1_pub(-1),
|
||||
_vtol_vehicle_status_pub(-1),
|
||||
_v_rates_sp_pub(-1),
|
||||
_actuators_0_pub(0),
|
||||
_actuators_1_pub(0),
|
||||
_vtol_vehicle_status_pub(0),
|
||||
_v_rates_sp_pub(0),
|
||||
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")),
|
||||
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input"))
|
||||
|
||||
Reference in New Issue
Block a user