diff options
Diffstat (limited to 'apps')
-rw-r--r-- | apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 2 | ||||
-rw-r--r-- | apps/commander/commander.c | 2 | ||||
-rw-r--r-- | apps/drivers/drv_orb_dev.h | 3 | ||||
-rw-r--r-- | apps/drivers/hmc5883/hmc5883.cpp | 2 | ||||
-rw-r--r-- | apps/drivers/mpu6000/mpu6000.cpp | 4 | ||||
-rw-r--r-- | apps/drivers/ms5611/ms5611.cpp | 2 | ||||
-rw-r--r-- | apps/examples/px4_simple_app/px4_simple_app.c | 4 | ||||
-rw-r--r-- | apps/fixedwing_control/fixedwing_control.c | 2 | ||||
-rw-r--r-- | apps/gps/mtk.c | 2 | ||||
-rw-r--r-- | apps/gps/nmea_helper.c | 2 | ||||
-rw-r--r-- | apps/gps/ubx.c | 2 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 14 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 4 | ||||
-rw-r--r-- | apps/multirotor_pos_control/multirotor_pos_control.c | 2 | ||||
-rw-r--r-- | apps/position_estimator/position_estimator_main.c | 2 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 2 | ||||
-rw-r--r-- | apps/px4/fmu/fmu.cpp | 6 | ||||
-rw-r--r-- | apps/px4/px4io/driver/px4io.cpp | 2 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 9 | ||||
-rw-r--r-- | apps/uORB/uORB.cpp | 88 | ||||
-rw-r--r-- | apps/uORB/uORB.h | 21 |
21 files changed, 110 insertions, 67 deletions
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index fdf6c9d91..09cbdd4e9 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -138,7 +138,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* advertise attitude */ - int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 149a662fd..55d3b82e7 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -96,7 +96,7 @@ static int buzzer; static int mavlink_fd; static bool commander_initialized = false; static struct vehicle_status_s current_status; /**< Main state machine */ -static int stat_pub; +static orb_advert_t stat_pub; static uint16_t nofix_counter = 0; static uint16_t gotfix_counter = 0; diff --git a/apps/drivers/drv_orb_dev.h b/apps/drivers/drv_orb_dev.h index b3fc01a5f..50288f690 100644 --- a/apps/drivers/drv_orb_dev.h +++ b/apps/drivers/drv_orb_dev.h @@ -81,4 +81,7 @@ /** Set the minimum interval at which the topic can be seen to be updated for this subscription */ #define ORBIOCSETINTERVAL _ORBIOC(12) +/** Get the global advertiser handle for the topic */ +#define ORBIOCGADVERTISER _ORBIOC(13) + #endif /* _DRV_UORB_H */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index a98b30efc..0fa1f365d 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -147,7 +147,7 @@ private: mag_scale _scale; bool _collect_phase; - int _mag_topic; + orb_advert_t _mag_topic; unsigned _reads; unsigned _measure_errors; diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 7576fc1d9..951e1a062 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -181,12 +181,12 @@ private: struct accel_report _accel_report; struct accel_scale _accel_scale; float _accel_range_scale; - int _accel_topic; + orb_advert_t _accel_topic; struct gyro_report _gyro_report; struct gyro_scale _gyro_scale; float _gyro_range_scale; - int _gyro_topic; + orb_advert_t _gyro_topic; unsigned _reads; perf_counter_t _sample_perf; diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index ee366f668..8f8f96217 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -125,7 +125,7 @@ private: int32_t _dT; int64_t _temp64; - int _baro_topic; + orb_advert_t _baro_topic; unsigned _reads; unsigned _measure_errors; diff --git a/apps/examples/px4_simple_app/px4_simple_app.c b/apps/examples/px4_simple_app/px4_simple_app.c index fd34a5dac..7f655964d 100644 --- a/apps/examples/px4_simple_app/px4_simple_app.c +++ b/apps/examples/px4_simple_app/px4_simple_app.c @@ -59,7 +59,7 @@ int px4_simple_app_main(int argc, char *argv[]) /* advertise attitude topic */ struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att); + orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); /* one could wait for multiple topics with this technique, just using one here */ struct pollfd fds[] = { @@ -103,7 +103,7 @@ int px4_simple_app_main(int argc, char *argv[]) att.roll = raw.accelerometer_m_s2[0]; att.pitch = raw.accelerometer_m_s2[1]; att.yaw = raw.accelerometer_m_s2[2]; - orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att); + orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } /* there could be more file descriptors here, in the form like: * if (fds[1..n].revents & POLLIN) {} diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index bad62edcf..7ea4101ab 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -675,7 +675,7 @@ int fixedwing_control_main(int argc, char *argv[]) /* Set up to publish fixed wing control messages */ struct fixedwing_control_s control; - int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control); + orb_advert_t fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control); /* Subscribe to global position, attitude and rc */ struct vehicle_global_position_s global_pos; diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c index cef70601b..0333c7100 100644 --- a/apps/gps/mtk.c +++ b/apps/gps/mtk.c @@ -311,7 +311,7 @@ void *mtk_loop(void *arg) /* advertise GPS topic */ struct vehicle_gps_position_s mtk_gps_d; mtk_gps = &mtk_gps_d; - int gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps); + orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps); while (1) { /* Parse a message from the gps receiver */ diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c index 4b520d403..54912b6d3 100644 --- a/apps/gps/nmea_helper.c +++ b/apps/gps/nmea_helper.c @@ -176,7 +176,7 @@ void *nmea_loop(void *arg) /* advertise GPS topic */ struct vehicle_gps_position_s nmea_gps_d = {0}; nmea_gps = &nmea_gps_d; - int gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps); + orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps); while (1) { /* Parse a message from the gps receiver */ diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index 771dfbd6c..a629e904d 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -842,7 +842,7 @@ void *ubx_loop(void *arg) ubx_gps = &ubx_gps_d; - int gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps); + orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps); while (1) { /* Parse a message from the gps receiver */ diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 7fba65cb7..97822510c 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -113,7 +113,7 @@ static struct vehicle_status_s v_status; static struct rc_channels_s rc; /* HIL publishers */ -static int pub_hil_attitude = -1; +static orb_advert_t pub_hil_attitude = -1; /** HIL attitude */ static struct vehicle_attitude_s hil_attitude; @@ -126,16 +126,16 @@ static struct ardrone_motors_setpoint_s ardrone_motors; static struct vehicle_command_s vcmd; -static int pub_hil_global_pos = -1; -static int ardrone_motors_pub = -1; -static int cmd_pub = -1; +static orb_advert_t pub_hil_global_pos = -1; +static orb_advert_t ardrone_motors_pub = -1; +static orb_advert_t cmd_pub = -1; static int sensor_sub = -1; static int att_sub = -1; static int global_pos_sub = -1; static int local_pos_sub = -1; -static int flow_pub = -1; -static int global_position_setpoint_pub = -1; -static int local_position_setpoint_pub = -1; +static orb_advert_t flow_pub = -1; +static orb_advert_t global_position_setpoint_pub = -1; +static orb_advert_t local_position_setpoint_pub = -1; static bool mavlink_hil_enabled = false; static char mavlink_message_string[51] = {0}; diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index baee507b2..df08ca75f 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -119,9 +119,9 @@ mc_thread_main(int argc, char *argv[]) /* publish actuator controls */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) actuators.control[i] = 0.0f; - int actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); armed.armed = true; - int armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index ff32fb460..3f9c72517 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -89,7 +89,7 @@ mpc_thread_main(int argc, char *argv[]) int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); /* publish attitude setpoint */ - int att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); while (1) { /* get a local copy of the vehicle state */ diff --git a/apps/position_estimator/position_estimator_main.c b/apps/position_estimator/position_estimator_main.c index 773cd87ff..17482dc0a 100644 --- a/apps/position_estimator/position_estimator_main.c +++ b/apps/position_estimator/position_estimator_main.c @@ -297,7 +297,7 @@ int position_estimator_main(int argc, char *argv[]) .lon = lon_current * 1e7, .alt = gps.alt }; - int global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + orb_advert_t global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index e56ff1c3a..fd3ed1137 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -154,7 +154,7 @@ int attitude_estimator_bm_main(int argc, char *argv[]) bool publishing = false; /* advertise attitude */ - int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); publishing = true; struct pollfd fds[] = { diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 83bc62509..3016cb01e 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -89,7 +89,7 @@ private: int _task; int _t_actuators; int _t_armed; - int _t_outputs; + orb_advert_t _t_outputs; unsigned _num_outputs; volatile bool _task_should_exit; @@ -566,15 +566,13 @@ fake(int argc, char *argv[]) ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; - int handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); + orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); if (handle < 0) { puts("advertise failed"); exit(1); } - close(handle); - exit(0); } diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp index a5def874d..c3371c138 100644 --- a/apps/px4/px4io/driver/px4io.cpp +++ b/apps/px4/px4io/driver/px4io.cpp @@ -99,7 +99,7 @@ protected: void set_channels(unsigned count, const servo_position_t *data); private: - int _publication; + orb_advert_t _publication; struct rc_input_values _input; }; diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index ef8ff798f..891167d1c 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -124,8 +124,8 @@ extern unsigned ppm_decoded_channels; extern uint64_t ppm_last_valid_decode; #endif -/* file handle that will be used for subscribing */ -static int sensor_pub; +/* ORB topic publishing our results */ +static orb_advert_t sensor_pub; /** * Sensor readout and publishing. @@ -404,7 +404,7 @@ int sensors_thread_main(int argc, char *argv[]) .yaw = 0.0f, .throttle = 0.0f }; - int manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + orb_advert_t manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); if (manual_control_pub < 0) { fprintf(stderr, "[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n"); @@ -413,7 +413,7 @@ int sensors_thread_main(int argc, char *argv[]) /* advertise the rc topic */ struct rc_channels_s rc; memset(&rc, 0, sizeof(rc)); - int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); + orb_advert_t rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); if (rc_pub < 0) { fprintf(stderr, "[sensors] ERROR: orb_advertise for topic rc_channels failed.\n"); @@ -466,7 +466,6 @@ int sensors_thread_main(int argc, char *argv[]) if (vstatus.flag_hil_enabled && !hil_enabled) { hil_enabled = true; publishing = false; - int ret = close(sensor_pub); printf("[sensors] Closing sensor pub: %i \n", ret); /* switching from HIL to non-HIL mode */ diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp index 1e7cdc8db..c4434e4ed 100644 --- a/apps/uORB/uORB.cpp +++ b/apps/uORB/uORB.cpp @@ -112,6 +112,8 @@ public: virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); + protected: virtual pollevent_t poll_state(struct file *filp); virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); @@ -298,6 +300,8 @@ ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen) * * Writes outside interrupt context will allocate the object * if it has not yet been allocated. + * + * Note that filp will usually be NULL. */ if (nullptr == _data) { if (!up_interrupt_context()) { @@ -353,12 +357,42 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) sd->update_interval = arg; return OK; + case ORBIOCGADVERTISER: + *(uintptr_t *)arg = (uintptr_t)this; + return OK; + default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); } } +ssize_t +ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) +{ + ORBDevNode *devnode = (ORBDevNode *)handle; + int ret; + + /* this is a bit risky, since we are trusting the handle in order to deref it */ + if (devnode->_meta != meta) { + errno = EINVAL; + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = devnode->write(nullptr, (const char *)data, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return OK; +} + pollevent_t ORBDevNode::poll_state(struct file *filp) { @@ -614,7 +648,7 @@ test() if (pfd < 0) return test_fail("advertise failed: %d", errno); - test_note("publish fd %d", pfd); + test_note("publish handle 0x%08x", pfd); sfd = orb_subscribe(ORB_ID(orb_test)); if (sfd < 0) @@ -877,29 +911,35 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve return ERROR; } - /* the advertiser must perform an initial publish to initialise the object */ - if (advertiser) { - ret = orb_publish(meta, fd, data); - - if (ret != OK) { - /* save errno across the close */ - ret = errno; - close(fd); - errno = ret; - return ERROR; - } - } - /* everything has been OK, we can return the handle now */ return fd; } } // namespace -int +orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { - return node_open(PUBSUB, meta, data, true); + int result, fd; + orb_advert_t advertiser; + + /* open the node as an advertiser */ + fd = node_open(PUBSUB, meta, data, true); + if (fd == ERROR) + return ERROR; + + /* get the advertiser handle and close the node */ + result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + close(fd); + if (result == ERROR) + return ERROR; + + /* the advertiser must perform an initial publish to initialise the object */ + result= orb_publish(meta, advertiser, data); + if (result == ERROR) + return ERROR; + + return advertiser; } int @@ -915,21 +955,9 @@ orb_unsubscribe(int handle) } int -orb_publish(const struct orb_metadata *meta, int handle, const void *data) +orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { - int ret; - - ret = write(handle, data, meta->o_size); - - if (ret < 0) - return ERROR; - - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } - - return OK; + return ORBDevNode::publish(meta, handle, data); } int diff --git a/apps/uORB/uORB.h b/apps/uORB/uORB.h index c36d0044f..eb068d2b7 100644 --- a/apps/uORB/uORB.h +++ b/apps/uORB/uORB.h @@ -106,11 +106,26 @@ struct orb_metadata { __BEGIN_DECLS /** + * ORB topic advertiser handle. + * + * Advertiser handles are global; once obtained they can be shared freely + * and do not need to be closed or released. + * + * This permits publication from interrupt context and other contexts where + * a file-descriptor-based handle would not otherwise be in scope for the + * publisher. + */ +typedef intptr_t orb_advert_t; + +/** * Advertise as the publisher of a topic. * * This performs the initial advertisement of a topic; it creates the topic * node in /obj if required and publishes the initial data. * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. * @param data A pointer to the initial data to be published. @@ -122,7 +137,7 @@ __BEGIN_DECLS * ORB_DEFINE with no corresponding ORB_DECLARE) * this function will return -1 and set errno to ENOENT. */ -extern int orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; +extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; /** * Publish new data to a topic. @@ -131,13 +146,13 @@ extern int orb_advertise(const struct orb_metadata *meta, const void *data) __EX * will be notified. Subscribers that are not waiting can check the topic * for updates using orb_check and/or orb_stat. * - * @handle The handle returned from orb_advertise. * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. + * @handle The handle returned from orb_advertise. * @param data A pointer to the data to be published. * @return OK on success, ERROR otherwise with errno set accordingly. */ -extern int orb_publish(const struct orb_metadata *meta, int handle, const void *data) __EXPORT; +extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT; /** * Subscribe to a topic. |