From acfd1ea51976300b5b89a7dc0f8c5158b3150604 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 1 Jun 2015 19:04:08 -0700 Subject: [PATCH 01/48] POSIX: added hrt_queue for handling fast periodic events The workqueues measure time in ticks which is typically 10ms. Some interrupt events in Nuttx occur at about 1ms so a more granular workqueue is needed for POSIX. Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/hrt_queue.c | 3 -- src/platforms/posix/px4_layer/hrt_work_lock.h | 54 +++++++++++++++++++ 2 files changed, 54 insertions(+), 3 deletions(-) create mode 100644 src/platforms/posix/px4_layer/hrt_work_lock.h diff --git a/src/platforms/posix/px4_layer/hrt_queue.c b/src/platforms/posix/px4_layer/hrt_queue.c index ab10714776..e45132253a 100644 --- a/src/platforms/posix/px4_layer/hrt_queue.c +++ b/src/platforms/posix/px4_layer/hrt_queue.c @@ -49,8 +49,6 @@ #include #include "hrt_work.h" -#ifdef CONFIG_SCHED_WORKQUEUE - /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ @@ -129,4 +127,3 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del return PX4_OK; } -#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/posix/px4_layer/hrt_work_lock.h b/src/platforms/posix/px4_layer/hrt_work_lock.h new file mode 100644 index 0000000000..86aa3913a2 --- /dev/null +++ b/src/platforms/posix/px4_layer/hrt_work_lock.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +#pragma once +extern sem_t _hrt_work_lock; + +inline void hrt_work_lock(); +inline void hrt_work_lock() +{ + PX4_INFO("hrt_work_lock"); + sem_wait(&_hrt_work_lock); +} + +inline void hrt_work_unlock(); +inline void hrt_work_unlock() +{ + PX4_INFO("hrt_work_unlock"); + sem_post(&_hrt_work_lock); +} + From db5530e1b59d08291a570b693cd11b869286faa3 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 2 Jun 2015 00:40:43 -0700 Subject: [PATCH 02/48] POSIX: Fixes for HRT implementation of simulated HW clock polling There is a race condition for the accel and mag polling rates. Whichever one gets set first, the other will be uninitialized. Set the mag polling rate to 1ms if uninitilized. Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/hrt_work_lock.h | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/src/platforms/posix/px4_layer/hrt_work_lock.h b/src/platforms/posix/px4_layer/hrt_work_lock.h index 86aa3913a2..69c5f204a3 100644 --- a/src/platforms/posix/px4_layer/hrt_work_lock.h +++ b/src/platforms/posix/px4_layer/hrt_work_lock.h @@ -33,22 +33,33 @@ #include #include -#include +#include #pragma once + +__BEGIN_DECLS + extern sem_t _hrt_work_lock; +extern struct wqueue_s g_hrt_work; + +void hrt_work_queue_init(void); +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); +int hrt_work_cancel(struct work_s *work); inline void hrt_work_lock(); +inline void hrt_work_unlock(); + inline void hrt_work_lock() { - PX4_INFO("hrt_work_lock"); + //PX4_INFO("hrt_work_lock"); sem_wait(&_hrt_work_lock); } -inline void hrt_work_unlock(); inline void hrt_work_unlock() { - PX4_INFO("hrt_work_unlock"); + //PX4_INFO("hrt_work_unlock"); sem_post(&_hrt_work_lock); } +__END_DECLS + From e7abd780518654f2cfc4b73e0df128646bf0dd4c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 2 Jun 2015 00:59:10 -0700 Subject: [PATCH 03/48] POSIX: Fixed output for list_topics, list_devices, etc Removed extra carriage returns in output strings Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/hrt_work_lock.h | 65 ------------------- 1 file changed, 65 deletions(-) delete mode 100644 src/platforms/posix/px4_layer/hrt_work_lock.h diff --git a/src/platforms/posix/px4_layer/hrt_work_lock.h b/src/platforms/posix/px4_layer/hrt_work_lock.h deleted file mode 100644 index 69c5f204a3..0000000000 --- a/src/platforms/posix/px4_layer/hrt_work_lock.h +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include - -#pragma once - -__BEGIN_DECLS - -extern sem_t _hrt_work_lock; -extern struct wqueue_s g_hrt_work; - -void hrt_work_queue_init(void); -int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); -int hrt_work_cancel(struct work_s *work); - -inline void hrt_work_lock(); -inline void hrt_work_unlock(); - -inline void hrt_work_lock() -{ - //PX4_INFO("hrt_work_lock"); - sem_wait(&_hrt_work_lock); -} - -inline void hrt_work_unlock() -{ - //PX4_INFO("hrt_work_unlock"); - sem_post(&_hrt_work_lock); -} - -__END_DECLS - From f0a3210e94a8385d0a1868695cf442c42c076d4f Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 31 May 2015 18:15:56 +0200 Subject: [PATCH 04/48] major simulator rework: - wait for first message from jMAVSim before sending data - publish raw rc data coming from PIXHAWK (temporary) - send some interesting messages to jMAVSim - prepare sensor data for sim drivers to read --- src/modules/simulator/simulator.cpp | 21 ++ src/modules/simulator/simulator.h | 65 ++++-- src/modules/simulator/simulator_mavlink.cpp | 217 ++++++++++++-------- 3 files changed, 199 insertions(+), 104 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 83e3fd8d76..da0b41589d 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -71,11 +71,32 @@ bool Simulator::getRawAccelReport(uint8_t *buf, int len) return _accel.copyData(buf, len); } +bool Simulator::getMagReport(uint8_t *buf, int len) +{ + return _mag.copyData(buf, len); +} + bool Simulator::getBaroSample(uint8_t *buf, int len) { return _baro.copyData(buf, len); } +void Simulator::write_MPU_data(void *buf) { + _mpu.writeData(buf); +} + +void Simulator::write_accel_data(void *buf) { + _accel.writeData(buf); +} + +void Simulator::write_mag_data(void *buf) { + _mag.writeData(buf); +} + +void Simulator::write_baro_data(void *buf) { + _baro.writeData(buf); +} + int Simulator::start(int argc, char *argv[]) { int ret = 0; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index a95fa15731..109c9f301c 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -61,31 +62,46 @@ namespace simulator { // FIXME - what is the endianness of these on actual device? #pragma pack(push, 1) struct RawAccelData { - int16_t x; - int16_t y; - int16_t z; + float temperature; + float x; + float y; + float z; +}; +#pragma pack(pop) + +#pragma pack(push, 1) +struct RawMagData { + float temperature; + float x; + float y; + float z; }; #pragma pack(pop) #pragma pack(push, 1) struct RawMPUData { - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; + float accel_x; + float accel_y; + float accel_z; + float temp; + float gyro_x; + float gyro_y; + float gyro_z; }; #pragma pack(pop) +#pragma pack(push, 1) struct RawBaroData { - uint8_t d[3]; + float pressure; + float altitude; + float temperature; }; +#pragma pack(pop) template class Report { public: Report(int readers) : + _readidx(0), _max_readers(readers), _report_len(sizeof(RType)) { @@ -158,23 +174,33 @@ public: static int start(int argc, char *argv[]); bool getRawAccelReport(uint8_t *buf, int len); + bool getMagReport(uint8_t *buf, int len); bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); + + void write_MPU_data(void *buf); + void write_accel_data(void *buf); + void write_mag_data(void *buf); + void write_baro_data(void *buf); + private: Simulator() : _accel(1), _mpu(1), _baro(1), + _mag(1), _sensor_combined_pub(nullptr) #ifndef __PX4_QURT , - _manual_control_sp_pub(nullptr), + _rc_channels_pub(nullptr), _actuator_outputs_sub(-1), _vehicle_attitude_sub(-1), + _manual_sub(-1), _sensor{}, - _manual_control_sp{}, + _rc_input{}, _actuators{}, - _attitude{} + _attitude{}, + _manual{} #endif {} ~Simulator() { _instance=NULL; } @@ -189,6 +215,7 @@ private: simulator::Report _accel; simulator::Report _mpu; simulator::Report _baro; + simulator::Report _mag; // uORB publisher handlers orb_advert_t _accel_pub; @@ -202,21 +229,26 @@ private: #ifndef __PX4_QURT // uORB publisher handlers - orb_advert_t _manual_control_sp_pub; + orb_advert_t _rc_channels_pub; // uORB subscription handlers int _actuator_outputs_sub; int _vehicle_attitude_sub; + int _manual_sub; // uORB data containers struct sensor_combined_s _sensor; - struct manual_control_setpoint_s _manual_control_sp; + struct rc_input_values _rc_input; struct actuator_outputs_s _actuators; struct vehicle_attitude_s _attitude; + struct manual_control_setpoint_s _manual; int _fd; unsigned char _buf[200]; hrt_abstime _time_last; + hrt_abstime _heartbeat_last; + hrt_abstime _attitude_last; + hrt_abstime _manual_last; struct sockaddr_in _srcaddr; socklen_t _addrlen = sizeof(_srcaddr); @@ -225,6 +257,7 @@ private: void send_data(); void pack_actuator_message(mavlink_hil_controls_t &actuator_msg); void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); + void update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu); static void *sending_trampoline(void *); void send(); #endif diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index de24fb8626..5c9ceae0cc 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -85,68 +85,107 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { } void Simulator::send_data() { - // check if it's time to send new data - hrt_abstime time_now = hrt_absolute_time(); - if (true) {//time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) { - _time_last = time_now; - mavlink_hil_controls_t msg; - pack_actuator_message(msg); - send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); - // can add more messages here, can also setup different timings + mavlink_hil_controls_t msg; + pack_actuator_message(msg); + send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); + + // send heartbeat + if (hrt_elapsed_time(&_heartbeat_last) >= 1e6) { + _heartbeat_last = hrt_absolute_time(); + mavlink_heartbeat_t msg; + msg.base_mode = 0; + msg.custom_mode = 0; + msg.autopilot = MAV_AUTOPILOT_PX4; + msg.mavlink_version = 3; + send_mavlink_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 200); + } + + // send attitude message + if (hrt_elapsed_time(&_attitude_last) > 20000) { + _attitude_last = hrt_absolute_time(); + mavlink_attitude_t msg; + msg.time_boot_ms = _attitude.timestamp / 1000; + msg.roll = _attitude.roll; + msg.pitch = _attitude.pitch; + msg.yaw = _attitude.yaw; + msg.rollspeed = _attitude.rollspeed; + msg.pitchspeed = _attitude.pitchspeed; + msg.yawspeed = _attitude.yawspeed; + send_mavlink_message(MAVLINK_MSG_ID_ATTITUDE, &msg, 200); + } + + // send manual control setpoint + if (hrt_elapsed_time(&_manual_last) > 200000) { + _manual_last = hrt_absolute_time(); + mavlink_manual_control_t msg; + msg.x = _manual.x * 1000; + msg.y = _manual.y * 1000; + msg.z = _manual.z * 1000; + msg.r = _manual.r * 1000; + msg.buttons = 0; + send_mavlink_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg, 200); } } -static void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg) { - manual->timestamp = hrt_absolute_time(); - manual->x = man_msg->x / 1000.0f; - manual->y = man_msg->y / 1000.0f; - manual->r = man_msg->r / 1000.0f; - manual->z = man_msg->z / 1000.0f; +static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels) { + rc->timestamp_publication = hrt_absolute_time(); + rc->timestamp_last_signal = hrt_absolute_time(); + rc->channel_count = rc_channels->chancount; + rc->rssi = rc_channels->rssi; + + rc->values[0] = rc_channels->chan1_raw; + rc->values[1] = rc_channels->chan2_raw; + rc->values[2] = rc_channels->chan3_raw; + rc->values[3] = rc_channels->chan4_raw; + rc->values[4] = rc_channels->chan5_raw; + rc->values[5] = rc_channels->chan6_raw; + rc->values[6] = rc_channels->chan7_raw; + rc->values[7] = rc_channels->chan8_raw; + rc->values[8] = rc_channels->chan9_raw; + rc->values[9] = rc_channels->chan10_raw; + rc->values[10] = rc_channels->chan11_raw; + rc->values[11] = rc_channels->chan12_raw; + rc->values[12] = rc_channels->chan13_raw; + rc->values[13] = rc_channels->chan14_raw; + rc->values[14] = rc_channels->chan15_raw; + rc->values[15] = rc_channels->chan16_raw; + rc->values[16] = rc_channels->chan17_raw; + rc->values[17] = rc_channels->chan18_raw; } -static void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) { - hrt_abstime timestamp = hrt_absolute_time(); - sensor->timestamp = timestamp; - sensor->gyro_raw[0] = imu->xgyro * 1000.0f; - sensor->gyro_raw[1] = imu->ygyro * 1000.0f; - sensor->gyro_raw[2] = imu->zgyro * 1000.0f; - sensor->gyro_rad_s[0] = imu->xgyro; - sensor->gyro_rad_s[1] = imu->ygyro; - sensor->gyro_rad_s[2] = imu->zgyro; +void Simulator::update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) { + // write sensor data to memory so that drivers can copy data from there + RawMPUData mpu; + mpu.accel_x = imu->xacc; + mpu.accel_y = imu->yacc; + mpu.accel_z = imu->zacc; + mpu.temp = imu->temperature; + mpu.gyro_x = imu->xgyro; + mpu.gyro_y = imu->ygyro; + mpu.gyro_z = imu->zgyro; - sensor->accelerometer_raw[0] = imu->xacc; // mg2ms2; - sensor->accelerometer_raw[1] = imu->yacc; // mg2ms2; - sensor->accelerometer_raw[2] = imu->zacc; // mg2ms2; - sensor->accelerometer_m_s2[0] = imu->xacc; - sensor->accelerometer_m_s2[1] = imu->yacc; - sensor->accelerometer_m_s2[2] = imu->zacc; - sensor->accelerometer_mode = 0; // TODO what is this? - sensor->accelerometer_range_m_s2 = 32.7f; // int16 - sensor->accelerometer_timestamp = timestamp; - sensor->timestamp = timestamp; + write_MPU_data((void *)&mpu); - sensor->adc_voltage_v[0] = 0.0f; - sensor->adc_voltage_v[1] = 0.0f; - sensor->adc_voltage_v[2] = 0.0f; + RawAccelData accel; + accel.x = imu->xacc; + accel.y = imu->yacc; + accel.z = imu->zacc; - sensor->magnetometer_raw[0] = imu->xmag * 1000.0f; - sensor->magnetometer_raw[1] = imu->ymag * 1000.0f; - sensor->magnetometer_raw[2] = imu->zmag * 1000.0f; - sensor->magnetometer_ga[0] = imu->xmag; - sensor->magnetometer_ga[1] = imu->ymag; - sensor->magnetometer_ga[2] = imu->zmag; - sensor->magnetometer_range_ga = 32.7f; // int16 - sensor->magnetometer_mode = 0; // TODO what is this - sensor->magnetometer_cuttoff_freq_hz = 50.0f; - sensor->magnetometer_timestamp = timestamp; + write_accel_data((void *)&accel); - sensor->baro_pres_mbar = imu->abs_pressure; - sensor->baro_alt_meter = imu->pressure_alt; - sensor->baro_temp_celcius = imu->temperature; - sensor->baro_timestamp = timestamp; + RawMagData mag; + mag.x = imu->xmag; + mag.y = imu->ymag; + mag.z = imu->zmag; - sensor->differential_pressure_pa = imu->diff_pressure * 1e2f; //from hPa to Pa - sensor->differential_pressure_timestamp = timestamp; + write_mag_data((void *)&mag); + + RawBaroData baro; + baro.pressure = imu->abs_pressure; + baro.altitude = imu->pressure_alt; + baro.temperature = imu->temperature; + + write_baro_data((void *)&baro); } void Simulator::handle_message(mavlink_message_t *msg) { @@ -154,27 +193,20 @@ void Simulator::handle_message(mavlink_message_t *msg) { case MAVLINK_MSG_ID_HIL_SENSOR: mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); - fill_sensors_from_imu_msg(&_sensor, &imu); - - // publish message - if(_sensor_combined_pub == nullptr) { - _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor); - } else { - orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor); - } + update_sensors(&_sensor, &imu); break; - case MAVLINK_MSG_ID_MANUAL_CONTROL: + case MAVLINK_MSG_ID_RC_CHANNELS: - mavlink_manual_control_t man_ctrl_sp; - mavlink_msg_manual_control_decode(msg, &man_ctrl_sp); - fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp); + mavlink_rc_channels_t rc_channels; + mavlink_msg_rc_channels_decode(msg, &rc_channels); + fill_rc_input_msg(&_rc_input, &rc_channels); // publish message - if(_manual_control_sp_pub == nullptr) { - _manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp); + if(_rc_channels_pub == nullptr) { + _rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input); } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp); + orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input); } break; } @@ -226,6 +258,12 @@ void Simulator::poll_topics() { if(updated) { orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude); } + + orb_check(_manual_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } } void *Simulator::sending_trampoline(void *) { @@ -238,11 +276,18 @@ void Simulator::send() { fds[0].fd = _actuator_outputs_sub; fds[0].events = POLLIN; - _time_last = hrt_absolute_time(); + _time_last = 0; + _heartbeat_last = 0; + _attitude_last = 0; + _manual_last = 0; + int pret = -1; + while(pret <= 0) { + pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); + } while(true) { // wait for up to 100ms for data - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); //timed out if (pret == 0) @@ -288,15 +333,10 @@ void Simulator::updateSamples() struct mag_report mag; memset(&mag, 0 ,sizeof(mag)); - // init publishers - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - // subscribe to topics _actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); // try to setup udp socket for communcation with simulator memset((char *)&_myaddr, 0, sizeof(_myaddr)); @@ -355,10 +395,23 @@ void Simulator::updateSamples() fds[1].events = POLLIN; int len = 0; + + // wait for first data from simulator and respond with first controls + // this is important for the UDP communication to work + int pret = -1; + while (pret <= 0) { + pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100); + } + + if (fds[0].revents & POLLIN) { + len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); + send_data(); + } + // wait for new mavlink messages to arrive while (true) { - int pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100); + pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100); //timed out if (pret == 0) @@ -405,17 +458,5 @@ void Simulator::updateSamples() } } } - - // publish these messages so that attitude estimator does not complain - hrt_abstime time_last = hrt_absolute_time(); - baro.timestamp = time_last; - accel.timestamp = time_last; - gyro.timestamp = time_last; - mag.timestamp = time_last; - // publish the sensor values - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro); - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro); - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); } } From aef3f37ae053584ce5c21b2bd9ab791a2d49fb0b Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 31 May 2015 18:29:28 +0200 Subject: [PATCH 05/48] enable reading sensor data from simulator module for SITL --- .../posix/drivers/accelsim/accelsim.cpp | 159 +++++++++++------- src/platforms/posix/drivers/barosim/baro.cpp | 109 +++--------- .../posix/drivers/barosim/baro_sim.cpp | 10 +- .../posix/drivers/gyrosim/gyrosim.cpp | 136 +++++---------- 4 files changed, 169 insertions(+), 245 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index bb9749577a..994b1bfb9b 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -54,6 +54,8 @@ #include #include +#include + #include #include @@ -86,6 +88,8 @@ static const int ERROR = -1; #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) +#define ACC_READ (0<<6) +#define MAG_READ (1<<6) extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); } @@ -512,6 +516,24 @@ ACCELSIM::reset() int ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) { + uint8_t cmd = send[0]; + + if (cmd & DIR_READ) { + // Get data from the simulator + Simulator *sim = Simulator::getInstance(); + if (sim == NULL) + return ENODEV; + + // FIXME - not sure what interrupt status should be + recv[1] = 0; + // skip cmd and status bytes + if (cmd & ACC_READ) { + sim->getRawAccelReport(&recv[2], len-2); + } else if (cmd & MAG_READ) { + sim->getMagReport(&recv[2], len-2); + } + } + return PX4_OK; } @@ -986,9 +1008,10 @@ ACCELSIM::measure() struct { uint8_t cmd; uint8_t status; - int16_t x; - int16_t y; - int16_t z; + float temperature; + float x; + float y; + float z; } raw_accel_report; #pragma pack(pop) @@ -999,8 +1022,11 @@ ACCELSIM::measure() /* fetch data from the sensor */ memset(&raw_accel_report, 0, sizeof(raw_accel_report)); - raw_accel_report.cmd = DIR_READ; - transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + raw_accel_report.cmd = DIR_READ | ACC_READ; + + if(OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) { + return; + } /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1029,54 +1055,58 @@ ACCELSIM::measure() // whether it has had failures accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - accel_report.x_raw = raw_accel_report.x; - accel_report.y_raw = raw_accel_report.y; - accel_report.z_raw = raw_accel_report.z; + accel_report.x_raw = (int16_t)(raw_accel_report.x/_accel_range_scale); + accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale); + accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale); - float xraw_f = raw_accel_report.x; - float yraw_f = raw_accel_report.y; - float zraw_f = raw_accel_report.z; + // float xraw_f = (int16_t)(raw_accel_report.x/_accel_range_scale); + // float yraw_f = (int16_t)(raw_accel_report.y / _accel_range_scale); + // float zraw_f = (int16_t)(raw_accel_report.z / _accel_range_scale); - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + // // apply user specified rotation + // rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + // float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + // float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + // float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; /* we have logs where the accelerometers get stuck at a fixed large value. We want to detect this and mark the sensor as being faulty */ - if (fabsf(_last_accel[0] - x_in_new) < 0.001f && - fabsf(_last_accel[1] - y_in_new) < 0.001f && - fabsf(_last_accel[2] - z_in_new) < 0.001f && - fabsf(x_in_new) > 20 && - fabsf(y_in_new) > 20 && - fabsf(z_in_new) > 20) { - _constant_accel_count += 1; - } else { - _constant_accel_count = 0; - } - if (_constant_accel_count > 100) { - // we've had 100 constant accel readings with large - // values. The sensor is almost certainly dead. We - // will raise the error_count so that the top level - // flight code will know to avoid this sensor, but - // we'll still give the data so that it can be logged - // and viewed - perf_count(_bad_values); - _constant_accel_count = 0; - } + // if (fabsf(_last_accel[0] - x_in_new) < 0.001f && + // fabsf(_last_accel[1] - y_in_new) < 0.001f && + // fabsf(_last_accel[2] - z_in_new) < 0.001f && + // fabsf(x_in_new) > 20 && + // fabsf(y_in_new) > 20 && + // fabsf(z_in_new) > 20) { + // _constant_accel_count += 1; + // } else { + // _constant_accel_count = 0; + // } + // if (_constant_accel_count > 100) { + // // we've had 100 constant accel readings with large + // // values. The sensor is almost certainly dead. We + // // will raise the error_count so that the top level + // // flight code will know to avoid this sensor, but + // // we'll still give the data so that it can be logged + // // and viewed + // perf_count(_bad_values); + // _constant_accel_count = 0; + // } - _last_accel[0] = x_in_new; - _last_accel[1] = y_in_new; - _last_accel[2] = z_in_new; + // _last_accel[0] = x_in_new; + // _last_accel[1] = y_in_new; + // _last_accel[2] = z_in_new; - accel_report.x = _accel_filter_x.apply(x_in_new); - accel_report.y = _accel_filter_y.apply(y_in_new); - accel_report.z = _accel_filter_z.apply(z_in_new); + // accel_report.x = _accel_filter_x.apply(x_in_new); + // accel_report.y = _accel_filter_y.apply(y_in_new); + // accel_report.z = _accel_filter_z.apply(z_in_new); + + accel_report.x = raw_accel_report.x; + accel_report.y = raw_accel_report.y; + accel_report.z = raw_accel_report.z; accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; @@ -1109,11 +1139,11 @@ ACCELSIM::mag_measure() #pragma pack(push, 1) struct { uint8_t cmd; - int16_t temperature; uint8_t status; - int16_t x; - int16_t y; - int16_t z; + float temperature; + float x; + float y; + float z; } raw_mag_report; #pragma pack(pop) @@ -1125,8 +1155,11 @@ ACCELSIM::mag_measure() /* fetch data from the sensor */ memset(&raw_mag_report, 0, sizeof(raw_mag_report)); - raw_mag_report.cmd = DIR_READ; - transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + raw_mag_report.cmd = DIR_READ | MAG_READ; + + if(OK != transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report))) { + return; + } /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1146,29 +1179,33 @@ ACCELSIM::mag_measure() mag_report.timestamp = hrt_absolute_time(); - mag_report.x_raw = raw_mag_report.x; - mag_report.y_raw = raw_mag_report.y; - mag_report.z_raw = raw_mag_report.z; + mag_report.x_raw = (int16_t)(raw_mag_report.x / _mag_range_scale); + mag_report.y_raw = (int16_t)(raw_mag_report.y / _mag_range_scale); + mag_report.z_raw = (int16_t)(raw_mag_report.z / _mag_range_scale); + + float xraw_f = (int16_t)(raw_mag_report.x / _mag_range_scale); + float yraw_f = (int16_t)(raw_mag_report.y / _mag_range_scale); + float zraw_f = (int16_t)(raw_mag_report.z / _mag_range_scale); - float xraw_f = mag_report.x_raw; - float yraw_f = mag_report.y_raw; - float zraw_f = mag_report.z_raw; /* apply user specified rotation */ rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; - mag_report.scaling = _mag_range_scale; - mag_report.range_ga = (float)_mag_range_ga; - mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); + // mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + // mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + // mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + // mag_report.scaling = _mag_range_scale; + // mag_report.range_ga = (float)_mag_range_ga; + // mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); /* remember the temperature. The datasheet isn't clear, but it * seems to be a signed offset from 25 degrees C in units of 0.125C */ - _last_temperature = 25 + (raw_mag_report.temperature * 0.125f); + _last_temperature = raw_mag_report.temperature; mag_report.temperature = _last_temperature; + mag_report.x = raw_mag_report.x; + mag_report.y = raw_mag_report.y; + mag_report.z = raw_mag_report.z; _mag_reports->force(&mag_report); diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 91bd149135..f7ea4f6949 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -275,6 +275,13 @@ BAROSIM::init() _measure_phase = 0; _reports->flush(); + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + + if (_baro_topic == (orb_advert_t)(-1)) { + PX4_ERR("failed to create sensor_baro publication"); + } + /* this do..while is goto without goto */ do { /* do temperature first */ @@ -312,12 +319,6 @@ BAROSIM::init() ret = OK; - _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); - - if (_baro_topic == nullptr) { - PX4_ERR("failed to create sensor_baro publication"); - } //PX4_WARN("sensor_baro publication %ld", _baro_topic); } while (0); @@ -622,7 +623,14 @@ int BAROSIM::collect() { int ret; - uint32_t raw; + +#pragma pack(push, 1) + struct { + float pressure; + float altitude; + float temperature; + } baro_report; +#pragma pack(pop) perf_begin(_sample_perf); @@ -632,7 +640,7 @@ BAROSIM::collect() report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ - ret = _interface->dev_read(0, (void *)&raw, 0); + ret = _interface->dev_read(0, (void *)&baro_report, 0); if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); @@ -641,84 +649,15 @@ BAROSIM::collect() /* handle a measurement */ if (_measure_phase == 0) { - - /* temperature offset (in ADC units) */ - int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8); - - /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ - _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23); - - /* base sensor scale/offset values */ - _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); - _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); - - /* temperature compensation */ - if (_TEMP < 2000) { - - int32_t T2 = POW2(dT) >> 31; - - int64_t f = POW2((int64_t)_TEMP - 2000); - int64_t OFF2 = 5 * f >> 1; - int64_t SENS2 = 5 * f >> 2; - - if (_TEMP < -1500) { - int64_t f2 = POW2(_TEMP + 1500); - OFF2 += 7 * f2; - SENS2 += 11 * f2 >> 1; - } - - _TEMP -= T2; - _OFF -= OFF2; - _SENS -= SENS2; - } - + report.pressure = baro_report.pressure; + report.altitude = baro_report.altitude; + report.temperature = baro_report.temperature; + report.timestamp = hrt_absolute_time(); } else { - - /* pressure calculation, result in Pa */ - int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; - _P = P * 0.01f; - _T = _TEMP * 0.01f; - - /* generate a new report */ - report.temperature = _TEMP / 100.0f; - report.pressure = P / 100.0f; /* convert to millibar */ - - /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ - - /* - * PERFORMANCE HINT: - * - * The single precision calculation is 50 microseconds faster than the double - * precision variant. It is however not obvious if double precision is required. - * Pending more inspection and tests, we'll leave the double precision variant active. - * - * Measurements: - * double precision: barosim_read: 992 events, 258641us elapsed, min 202us max 305us - * single precision: barosim_read: 963 events, 208066us elapsed, min 202us max 241us - */ - - /* tropospheric properties (0-11km) for standard atmosphere */ - const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ - const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ - const double g = 9.80665; /* gravity constant in m/s/s */ - const double R = 287.05; /* ideal gas constant in J/kg/K */ - - /* current pressure at MSL in kPa */ - double p1 = _msl_pressure / 1000.0; - - /* measured pressure in kPa */ - double p = P / 1000.0; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + report.pressure = baro_report.pressure; + report.altitude = baro_report.altitude; + report.temperature = baro_report.temperature; + report.timestamp = hrt_absolute_time(); /* publish it */ if (!(_pub_blocked)) { diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 734c2bc286..8b64962207 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -118,22 +118,26 @@ BARO_SIM::init() int BARO_SIM::dev_read(unsigned offset, void *data, unsigned count) { + /* union _cvt { uint8_t b[4]; uint32_t w; } *cvt = (_cvt *)data; - uint8_t buf[3]; + */ /* read the most recent measurement */ uint8_t cmd = 0; - int ret = transfer(&cmd, 1, &buf[0], 3); + int ret = transfer(&cmd, 1, static_cast(data), 3); + + /* if (ret == PX4_OK) { - /* fetch the raw value */ + // fetch the raw value cvt->b[0] = buf[2]; cvt->b[1] = buf[1]; cvt->b[2] = buf[0]; cvt->b[3] = 0; } + */ return ret; } diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 1ccaecf9a6..54e448cabb 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -379,13 +379,13 @@ private: struct MPUReport { uint8_t cmd; uint8_t status; - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; + float accel_x; + float accel_y; + float accel_z; + float temp; + float gyro_x; + float gyro_y; + float gyro_z; }; #pragma pack(pop) @@ -1194,15 +1194,6 @@ void GYROSIM::measure() { struct MPUReport mpu_report; - struct Report { - int16_t accel_x; - int16_t accel_y; - int16_t accel_z; - int16_t temp; - int16_t gyro_x; - int16_t gyro_y; - int16_t gyro_z; - } report; /* start measuring */ perf_begin(_sample_perf); @@ -1214,69 +1205,10 @@ GYROSIM::measure() // sensor transfer at high clock speed //set_frequency(GYROSIM_HIGH_BUS_SPEED); - - if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) - return; - - /* - * Convert from big to little endian - */ - - report.accel_x = int16_t_from_bytes(mpu_report.accel_x); - report.accel_y = int16_t_from_bytes(mpu_report.accel_y); - report.accel_z = int16_t_from_bytes(mpu_report.accel_z); - - report.temp = int16_t_from_bytes(mpu_report.temp); - - report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x); - report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); - report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); - - if (report.accel_x == 0 && - report.accel_y == 0 && - report.accel_z == 0 && - report.temp == 0 && - report.gyro_x == 0 && - report.gyro_y == 0 && - report.gyro_z == 0) { - // all zero data - probably a VDev bus error - perf_count(_bad_transfers); - perf_end(_sample_perf); - // note that we don't call reset() here as a reset() - // costs 20ms with interrupts disabled. That means if - // the mpu6k does go bad it would cause a FMU failure, - // regardless of whether another sensor is available, + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { return; } - perf_count(_good_transfers); - - if (_register_wait != 0) { - // we are waiting for some good transfers before using - // the sensor again. We still increment - // _good_transfers, but don't return any data yet - _register_wait--; - return; - } - - - /* - * Swap axes and negate y - */ - int16_t accel_xt = report.accel_y; - int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); - - int16_t gyro_xt = report.gyro_y; - int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); - - /* - * Apply the swap - */ - report.accel_x = accel_xt; - report.accel_y = accel_yt; - report.gyro_x = gyro_xt; - report.gyro_y = gyro_yt; - /* * Report buffers. */ @@ -1286,7 +1218,11 @@ GYROSIM::measure() /* * Adjust and scale results to m/s^2. */ - grb.timestamp = arb.timestamp = hrt_absolute_time(); + grb.timestamp = hrt_absolute_time(); + arb.timestamp = grb.timestamp; + + // this sleep is needed because the timing of the drivers is not yet working + usleep(1000); // report the error count as the sum of the number of bad // transfers and bad register reads. This allows the higher @@ -1312,13 +1248,13 @@ GYROSIM::measure() /* NOTE: Axes have been swapped to match the board a few lines above. */ - arb.x_raw = report.accel_x; - arb.y_raw = report.accel_y; - arb.z_raw = report.accel_z; - - float xraw_f = report.accel_x; - float yraw_f = report.accel_y; - float zraw_f = report.accel_z; + arb.x_raw = (int16_t)(mpu_report.accel_x / _accel_range_scale); + arb.y_raw = (int16_t)(mpu_report.accel_y / _accel_range_scale); + arb.z_raw = (int16_t)(mpu_report.accel_z / _accel_range_scale); +/* + float xraw_f = (int16_t)(mpu_report.accel_x / _accel_range_scale); + float yraw_f = (int16_t)(mpu_report.accel_y / _accel_range_scale); + float zraw_f = (int16_t)(mpu_report.accel_z / _accel_range_scale); // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); @@ -1330,22 +1266,26 @@ GYROSIM::measure() arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); - +*/ arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; - _last_temperature = (report.temp) / 361.0f + 35.0f; + _last_temperature = mpu_report.temp; - arb.temperature_raw = report.temp; + arb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f); arb.temperature = _last_temperature; - grb.x_raw = report.gyro_x; - grb.y_raw = report.gyro_y; - grb.z_raw = report.gyro_z; + arb.x = mpu_report.accel_x; + arb.y = mpu_report.accel_y; + arb.z = mpu_report.accel_z; - xraw_f = report.gyro_x; - yraw_f = report.gyro_y; - zraw_f = report.gyro_z; + grb.x_raw = (int16_t)(mpu_report.gyro_x/_gyro_range_scale); + grb.y_raw = (int16_t)(mpu_report.gyro_y/_gyro_range_scale); + grb.z_raw = (int16_t)(mpu_report.gyro_z/_gyro_range_scale); +/* + xraw_f = (int16_t)(mpu_report.gyro_x/_gyro_range_scale); + yraw_f = (int16_t)(mpu_report.gyro_y/_gyro_range_scale); + zraw_f = (int16_t)(mpu_report.gyro_z/_gyro_range_scale); // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); @@ -1357,20 +1297,24 @@ GYROSIM::measure() grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); - +*/ grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; - grb.temperature_raw = report.temp; + grb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f); grb.temperature = _last_temperature; + grb.x = mpu_report.gyro_x; + grb.y = mpu_report.gyro_y; + grb.z = mpu_report.gyro_z; + _accel_reports->force(&arb); _gyro_reports->force(&grb); + /* notify anyone waiting for data */ poll_notify(POLLIN); _gyro->parent_poll_notify(); - if (!(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); From fb778af8b3274be5ae5963382af0161ac6c7848e Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 31 May 2015 18:31:22 +0200 Subject: [PATCH 06/48] increase max file descriptors to 100 --- src/drivers/device/vdev.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 8618626ba8..89a3da3e0a 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -64,7 +64,7 @@ private: px4_dev_t() {} }; -#define PX4_MAX_DEV 50 +#define PX4_MAX_DEV 100 static px4_dev_t *devmap[PX4_MAX_DEV]; /* From 3d443847310015c7798ac5d458d4f0fc5c3487eb Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 31 May 2015 18:55:23 +0200 Subject: [PATCH 07/48] temporarily don't use multi advert because doesn't work --- src/drivers/hil/hil.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index be514eeb2b..bc7bf12571 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -228,8 +228,9 @@ HIL::init() ret = VDev::init(); #endif - if (ret != OK) + if (ret != OK) { return ret; + } // XXX already claimed with CDEV ///* try to claim the generic PWM output device node as well - it's OK if we fail at this */ @@ -347,9 +348,9 @@ HIL::task_main() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); /* advertise the mixed control outputs */ - int dummy; - _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), - &outputs, &dummy, ORB_PRIO_LOW); + //int dummy; + _t_outputs = orb_advertise(ORB_ID(actuator_outputs), + &outputs); px4_pollfd_struct_t fds[2]; fds[0].fd = _t_actuators; @@ -408,21 +409,18 @@ HIL::task_main() /* do we have a control update? */ if (fds[0].revents & POLLIN) { - /* get controls - must always do this to avoid spinning */ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { - /* do mixing */ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ if (i < outputs.noutputs && PX4_ISFINITE(outputs.output[i]) && From 45ee36234d5a3088d9f4f1eb1f848714db009dec Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 2 Jun 2015 17:39:11 +0200 Subject: [PATCH 08/48] activate sending thread only once got message from simulator --- src/modules/simulator/simulator_mavlink.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 5c9ceae0cc..9bcac4879c 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -280,10 +280,8 @@ void Simulator::send() { _heartbeat_last = 0; _attitude_last = 0; _manual_last = 0; - int pret = -1; - while(pret <= 0) { - pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); - } + + int pret; while(true) { // wait for up to 100ms for data @@ -368,8 +366,6 @@ void Simulator::updateSamples() /* low priority */ param.sched_priority = SCHED_PRIORITY_DEFAULT; (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); - pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); - pthread_attr_destroy(&sender_thread_attr); // setup serial connection to autopilot (used to get manual controls) int serial_fd = open(PIXHAWK_DEVICE, O_RDWR); @@ -396,8 +392,7 @@ void Simulator::updateSamples() int len = 0; - // wait for first data from simulator and respond with first controls - // this is important for the UDP communication to work + // wait for first data from simulator int pret = -1; while (pret <= 0) { pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100); @@ -405,9 +400,12 @@ void Simulator::updateSamples() if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); - send_data(); } + // got data from simulator, now activate the sending thread + pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); + pthread_attr_destroy(&sender_thread_attr); + // wait for new mavlink messages to arrive while (true) { From 909508f8f9cf1007efd24ab242045faaa16d9995 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Jun 2015 01:10:09 +0200 Subject: [PATCH 09/48] let mixer sleep a bit before loading to ensure device is set up --- src/systemcmds/mixer/mixer.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 41add4973a..8889f4531f 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -100,6 +100,9 @@ usage(const char *reason) static int load(const char *devname, const char *fname) { + // sleep a while to ensure device has been set up + usleep(20000); + int dev; char buf[2048]; From 9a4bee834de63408a79ce07868967a76a48db553 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Jun 2015 15:19:35 +0200 Subject: [PATCH 10/48] added gpssim driver --- src/platforms/posix/drivers/gpssim/gpssim.cpp | 682 ++++++++++++++++++ src/platforms/posix/drivers/gpssim/module.mk | 42 ++ .../posix/drivers/gpssim/ubx_sim.cpp | 98 +++ src/platforms/posix/drivers/gpssim/ubx_sim.h | 63 ++ 4 files changed, 885 insertions(+) create mode 100644 src/platforms/posix/drivers/gpssim/gpssim.cpp create mode 100644 src/platforms/posix/drivers/gpssim/module.mk create mode 100644 src/platforms/posix/drivers/gpssim/ubx_sim.cpp create mode 100644 src/platforms/posix/drivers/gpssim/ubx_sim.h diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp new file mode 100644 index 0000000000..db948b73f6 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -0,0 +1,682 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gps.cpp + * Driver for the GPS on a serial port + */ + +//#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +//#include +#include +#include +//#include +#include +//#include +//#include +#include +#include +#include +#include +#include + +#include + +//#include + +#define GPS_DRIVER_MODE_UBX_SIM +#define GPS_SIM_DEVICE_PATH "/dev/gps_sim" + +//#include "ubx.h" +//#include "mtk.h" +//#include "ashtech.h" + + +#define TIMEOUT_5HZ 500 +#define RATE_MEASUREMENT_PERIOD 5000000 + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/* class for dynamic allocation of satellite info data */ +class GPS_Sat_Info +{ +public: + struct satellite_info_s _data; +}; + + +class GPS_SIM : public device::VDev +{ +public: + GPS_SIM(const char *uart_path, bool fake_gps, bool enable_sat_info); + virtual ~GPS_SIM(); + + virtual int init(); + + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +private: + + bool _task_should_exit; ///< flag to make the main worker task exit + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; ///< current baudrate + char _port[20]; ///< device / serial port path + volatile int _task; ///< worker task + bool _healthy; ///< flag to signal if the GPS is ok + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _mode_changed; ///< flag that the GPS mode has changed + //gps_driver_mode_t _mode; ///< current mode + GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object + struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position + orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position + struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info + orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info + float _rate; ///< position update rate + bool _fake_gps; ///< fake gps output + + /** + * Try to configure the GPS, handle outgoing communication to the GPS + */ + void config(); + + /** + * Trampoline to the worker task + */ + static void task_main_trampoline(void *arg); + + + /** + * Worker task: main GPS thread that configures the GPS and parses incoming data, always running + */ + void task_main(void); + + /** + * Set the baudrate of the UART to the GPS + */ + int set_baudrate(unsigned baud); + + /** + * Send a reset command to the GPS + */ + void cmd_reset(); + + int receive(int timeout); + +}; + + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int gps_sim_main(int argc, char *argv[]); + +namespace +{ + +GPS_SIM *g_dev = nullptr; + +} + + +GPS_SIM::GPS_SIM(const char *uart_path, bool fake_gps, bool enable_sat_info) : + VDev("gps", GPS_SIM_DEVICE_PATH), + _task_should_exit(false), + //_healthy(false), + //_mode_changed(false), + //_mode(GPS_DRIVER_MODE_UBX), + //_Helper(nullptr), + _Sat_Info(nullptr), + _report_gps_pos_pub(nullptr), + _p_report_sat_info(nullptr), + _report_sat_info_pub(nullptr), + _rate(0.0f), + _fake_gps(fake_gps) +{ + // /* store port name */ + // strncpy(_port, uart_path, sizeof(_port)); + // /* enforce null termination */ + // _port[sizeof(_port) - 1] = '\0'; + + /* we need this potentially before it could be set in task_main */ + g_dev = this; + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + + /* create satellite info data object if requested */ + if (enable_sat_info) { + _Sat_Info = new(GPS_Sat_Info); + _p_report_sat_info = &_Sat_Info->_data; + memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); + } + + _debug_enabled = true; +} + +GPS_SIM::~GPS_SIM() +{ + /* tell the task we want it to go away */ + _task_should_exit = true; + + /* spin waiting for the task to stop */ + for (unsigned i = 0; (i < 10) && (_task != -1); i++) { + /* give it another 100ms */ + usleep(100000); + } + + /* well, kill it anyway, though this will probably crash */ + if (_task != -1) + px4_task_delete(_task); + + g_dev = nullptr; + +} + +int +GPS_SIM::init() +{ + int ret = ERROR; + + /* do regular cdev init */ + if (VDev::init() != OK) + goto out; + + /* start the GPS driver worker task */ + _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPS_SIM::task_main_trampoline, nullptr); + + if (_task < 0) { + warnx("task start failed: %d", errno); + return -errno; + } + + ret = OK; +out: + return ret; +} + +int +GPS_SIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + lock(); + + int ret = OK; + + switch (cmd) { + case SENSORIOCRESET: + cmd_reset(); + break; + + default: + /* give it to parent if no one wants it */ + ret = VDev::ioctl(filp, cmd, arg); + break; + } + + unlock(); + + return ret; +} + +void +GPS_SIM::task_main_trampoline(void *arg) +{ + g_dev->task_main(); +} + +int +GPS_SIM::receive(int timeout) { + Simulator *sim = Simulator::getInstance(); + simulator::RawGPSData gps; + sim->getGPSSample((uint8_t *)&gps, sizeof(gps)); + + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = gps.lat; + _report_gps_pos.lon = gps.lon; + _report_gps_pos.alt = gps.alt; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.eph = (float)gps.eph; + _report_gps_pos.epv = (float)gps.epv; + _report_gps_pos.vel_m_s = (float)(gps.vel)/100.0f; + _report_gps_pos.vel_n_m_s = (float)(gps.vn)/100.0f; + _report_gps_pos.vel_e_m_s = (float)(gps.ve)/100.0f; + _report_gps_pos.vel_d_m_s = (float)(gps.vd)/100.0f; + _report_gps_pos.cog_rad = (float)(gps.cog)*3.1415f/(100.0f * 180.0f); + _report_gps_pos.fix_type = gps.fix_type; + _report_gps_pos.satellites_used = gps.satellites_visible; + + usleep(200000); + return 1; +} + +void +GPS_SIM::task_main() +{ + + /* loop handling received serial bytes and also configuring in between */ + while (!_task_should_exit) { + + if (_fake_gps) { + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = (int32_t)47.378301e7f; + _report_gps_pos.lon = (int32_t)8.538777e7f; + _report_gps_pos.alt = (int32_t)1200e3f; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.s_variance_m_s = 10.0f; + _report_gps_pos.c_variance_rad = 0.1f; + _report_gps_pos.fix_type = 3; + _report_gps_pos.eph = 0.9f; + _report_gps_pos.epv = 1.8f; + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.vel_ned_valid = true; + + //no time and satellite information simulated + + + if (!(_pub_blocked)) { + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + } + + usleep(2e5); + + } else { + + // if (_Helper != nullptr) { + // delete(_Helper); + // set to zero to ensure parser is not used while not instantiated + // _Helper = nullptr; + // } + + // switch (_mode) { + // case GPS_DRIVER_MODE_UBX_SIM: + // _Helper = new UBX_SIM(_serial_fd, &_report_gps_pos, _p_report_sat_info); + // break; + + // default: + // break; + // } + + //if (_Helper->configure(_baudrate) == 0) { + + //Publish initial report that we have access to a GPS + //Make sure to clear any stale data in case driver is reset + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.timestamp_time = hrt_absolute_time(); + + if (!(_pub_blocked)) { + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + } + + // GPS is obviously detected successfully, reset statistics + //_Helper->reset_update_rates(); + + while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { + // lock(); + /* opportunistic publishing - else invalid data would end up on the bus */ + + if (!(_pub_blocked)) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (_p_report_sat_info) { + if (_report_sat_info_pub != nullptr) { + orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); + + } else { + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); + } + } + } + + //if (helper_ret & 1) { // consider only pos info updates for rate calculation */ + // last_rate_count++; + //} + + /* measure update rate every 5 seconds */ + //if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { + //_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + //last_rate_measurement = hrt_absolute_time(); + //last_rate_count = 0; + //_Helper->store_update_rates(); + //_Helper->reset_update_rates(); + //} + + // if (!_healthy) { + // const char *mode_str = "unknown"; + + // switch (_mode) { + // case GPS_DRIVER_MODE_UBX_SIM: + // mode_str = "UBX"; + // break; + + // default: + // break; + // } + + // warnx("module found: %s", mode_str); + // _healthy = true; + // } + } + + // if (_healthy) { + // warnx("module lost"); + // _healthy = false; + // _rate = 0.0f; + // } + + lock(); + //} + + // /* select next mode */ + // switch (_mode) { + // case GPS_DRIVER_MODE_UBX: + // _mode = GPS_DRIVER_MODE_MTK; + // break; + + // case GPS_DRIVER_MODE_MTK: + // _mode = GPS_DRIVER_MODE_ASHTECH; + // break; + + // case GPS_DRIVER_MODE_ASHTECH: + // _mode = GPS_DRIVER_MODE_UBX; + // break; + + // default: + // break; + // } + } + + } + + warnx("exiting"); + + //::close(_serial_fd); + + /* tell the dtor that we are exiting */ + _task = -1; + return; +} + + + +void +GPS_SIM::cmd_reset() +{ + +} + +void +GPS_SIM::print_info() +{ + //GPS Mode + if(_fake_gps) { + warnx("protocol: faked"); + } + + else { + warnx("protocol: SIM"); + } + + warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + warnx("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); + + if (_report_gps_pos.timestamp_position != 0) { + warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); + warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); + warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); + //warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); + //warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); + warnx("rate publication:\t%6.2f Hz", (double)_rate); + + } + + usleep(100000); +} + +/** + * Local functions in support of the shell command. + */ +namespace gps_sim +{ + +GPS_SIM *g_dev = nullptr; + +void start(const char *uart_path, bool fake_gps, bool enable_sat_info); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start(const char *uart_path, bool fake_gps, bool enable_sat_info) +{ + int fd; + + if (g_dev != nullptr) + warnx("already started"); + + /* create the driver */ + g_dev = new GPS_SIM(uart_path, fake_gps, enable_sat_info); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = px4_open(GPS_SIM_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + warnx("open: %s\n", GPS0_DEVICE_PATH); + goto fail; + } + + return; + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + warnx("start failed"); +} + +/** + * Stop the driver. + */ +void +stop() +{ + delete g_dev; + g_dev = nullptr; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + + warnx("PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = px4_open(GPS_SIM_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + warnx("failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + warnx("reset failed"); +} + +/** + * Print the status of the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "not running"); + + g_dev->print_info(); +} + +} // namespace + + +int +gps_sim_main(int argc, char *argv[]) +{ + + /* set to default */ + const char *device_name = GPS_DEFAULT_UART_PORT; + bool fake_gps = false; + bool enable_sat_info = false; + + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + /* work around getopt unreliability */ + if (argc > 3) { + if (!strcmp(argv[2], "-d")) { + device_name = argv[3]; + + } else { + goto out; + } + } + + /* Detect fake gps option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-f")) + fake_gps = true; + } + + /* Detect sat info option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-s")) + enable_sat_info = true; + } + + gps_sim::start(device_name, fake_gps, enable_sat_info); + } + + if (!strcmp(argv[1], "stop")) + gps_sim::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + gps_sim::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + gps_sim::reset(); + + /* + * Print driver status. + */ + if (!strcmp(argv[1], "status")) + gps_sim::info(); + + return 0; + +out: + warnx("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); + return 1; +} diff --git a/src/platforms/posix/drivers/gpssim/module.mk b/src/platforms/posix/drivers/gpssim/module.mk new file mode 100644 index 0000000000..630eaae377 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Simulated GPS driver +# + +MODULE_COMMAND = gps_sim + +SRCS = gpssim.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/drivers/gpssim/ubx_sim.cpp b/src/platforms/posix/drivers/gpssim/ubx_sim.cpp new file mode 100644 index 0000000000..a4e9043c96 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/ubx_sim.cpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ubx.cpp + * + * U-Blox protocol implementation. Following u-blox 6/7/8 Receiver Description + * including Prototol Specification. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * + * @author Hannes Delago + * (rework, add ubx7+ compatibility) + * + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "ubx_sim.h" +#include + +#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK +#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received +#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls +#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval + + +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) : + _fd(fd), + _gps_position(gps_position), + _satellite_info(satellite_info), +{ + +} + +UBX::~UBX() +{ +} + + +int UBX_SIM::configure(unsigned &baudrate) +{ + return 0; +} + +int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled +UBX_SIM::receive(const unsigned timeout) +{ + /* copy data from simulator here */ + usleep(1000000); + return 1; +} diff --git a/src/platforms/posix/drivers/gpssim/ubx_sim.h b/src/platforms/posix/drivers/gpssim/ubx_sim.h new file mode 100644 index 0000000000..5722822dad --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/ubx_sim.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ubx_sim.h + * + */ + +#ifndef UBX_SIM_H_ +#define UBX_SIM_H_ + + + + +class UBX_SIM +{ +public: + UBX_SIM(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info); + ~UBX_SIM(); + int receive(const unsigned timeout); + int configure(unsigned &baudrate); + +private: + + + + int _fd; + struct vehicle_gps_position_s *_gps_position; + struct satellite_info_s *_satellite_info; + bool _enable_sat_info; +}; + +#endif /* UBX_SIM_H_ */ From a77f637bc4dff210c7d558553af226d8b4ffc8d7 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Jun 2015 18:57:27 +0200 Subject: [PATCH 11/48] mavlink udp: - added option to stream messages over udp - still hardcoded stuff (port) --- src/modules/mavlink/mavlink_main.cpp | 68 ++++++++++++++++++++++------ src/modules/mavlink/mavlink_main.h | 24 ++++++++++ 2 files changed, 79 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7c739cfc34..dd1582c202 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -163,6 +163,7 @@ Mavlink::Mavlink() : mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), + _udp_initialised(false), _flow_control_enabled(true), _last_write_success_time(0), _last_write_try_time(0), @@ -173,6 +174,8 @@ Mavlink::Mavlink() : _rate_tx(0.0f), _rate_txerr(0.0f), _rate_rx(0.0f), + _socket_fd(-1), + _protocol(SERIAL), _rstatus {}, _message_buffer {}, _message_buffer_mutex {}, @@ -800,20 +803,21 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID pthread_mutex_lock(&_send_mutex); - unsigned buf_free = get_free_tx_buf(); - uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; _last_write_try_time = hrt_absolute_time(); - /* check if there is space in the buffer, let it overflow else */ - if (buf_free < packet_len) { - /* no enough space in buffer to send */ - count_txerr(); - count_txerrbytes(packet_len); - pthread_mutex_unlock(&_send_mutex); - return; + if (get_protocol() == SERIAL) { + /* check if there is space in the buffer, let it overflow else */ + unsigned buf_free = get_free_tx_buf(); + if (buf_free < packet_len) { + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); + return; + } } uint8_t buf[MAVLINK_MAX_PACKET_LEN]; @@ -839,11 +843,16 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); -#ifndef __PX4_POSIX +//#ifndef __PX4_POSIX /* send message to UART */ - ssize_t ret = ::write(_uart_fd, buf, packet_len); + size_t ret = -1; + if (get_protocol() == SERIAL) { + ret = ::write(_uart_fd, buf, packet_len); + } else if (get_protocol() == UDP) { + ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); + } - if (ret != (int) packet_len) { + if (ret != (size_t) packet_len) { count_txerr(); count_txerrbytes(packet_len); @@ -851,7 +860,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } -#endif +//#endif pthread_mutex_unlock(&_send_mutex); } @@ -908,6 +917,31 @@ Mavlink::resend_message(mavlink_message_t *msg) pthread_mutex_unlock(&_send_mutex); } +void +Mavlink::init_udp() +{ + memset((char *)&_myaddr, 0, sizeof(_myaddr)); + _myaddr.sin_family = AF_INET; + _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + _myaddr.sin_port = htons(14556); + + if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + PX4_WARN("create socket failed\n"); + return; + } + + if (bind(_socket_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { + PX4_WARN("bind failed\n"); + return; + } + + unsigned char inbuf[256]; + socklen_t addrlen = sizeof(_src_addr); + + // wait for client to connect to socket + recvfrom(_socket_fd,inbuf,sizeof(inbuf),0,(struct sockaddr *)&_src_addr,&addrlen); +} + void Mavlink::handle_message(const mavlink_message_t *msg) { @@ -1323,6 +1357,9 @@ Mavlink::task_main(int argc, char *argv[]) case 'd': _device_name = myoptarg; + if (strncmp(_device_name, "udp",3) == 0) { + set_protocol(UDP); + } break; // case 'e': @@ -1392,6 +1429,11 @@ Mavlink::task_main(int argc, char *argv[]) /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); + /* init socket if necessary */ + if (get_protocol() == UDP) { + init_udp(); + } + #ifndef __PX4_POSIX struct termios uart_config_original; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index a658ca20ca..852859508c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -46,6 +46,8 @@ #include #else #include +#include +#include #endif #include #include @@ -65,6 +67,12 @@ #include "mavlink_parameters.h" #include "mavlink_ftp.h" +enum Protocol { + SERIAL = 0, + UDP, + TCP, +}; + #ifdef __PX4_NUTTX class Mavlink #else @@ -190,6 +198,11 @@ public: */ void set_manual_input_mode_generation(bool generation_enabled) { _generate_rc = generation_enabled; } + /** + * Set communication protocol for this mavlink instance + */ + void set_protocol(Protocol p) {_protocol = p;}; + /** * Get the manual input generation mode * @@ -305,6 +318,8 @@ public: unsigned get_system_type() { return _system_type; } + Protocol get_protocol() { return _protocol; }; + protected: Mavlink *next; @@ -364,6 +379,7 @@ private: char *_subscribe_to_stream; float _subscribe_to_stream_rate; + bool _udp_initialised; bool _flow_control_enabled; uint64_t _last_write_success_time; @@ -377,6 +393,12 @@ private: float _rate_txerr; float _rate_rx; + int _socket_fd; + struct sockaddr_in _myaddr; + struct sockaddr_in _src_addr; + + Protocol _protocol; + struct telemetry_status_s _rstatus; ///< receive status struct mavlink_message_buffer { @@ -439,6 +461,8 @@ private: */ void update_rate_mult(); + void init_udp(); + #ifdef __PX4_NUTTX static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); #else From 4aa4038e270c33e36ba2f8db866db3c6abec6222 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Jun 2015 18:58:58 +0200 Subject: [PATCH 12/48] increase number of arguments passable to apps --- src/platforms/posix/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index f756749d52..475fa3e812 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -79,9 +79,9 @@ static void run_cmd(const vector &appargs) { static void process_line(string &line) { - vector appargs(5); + vector appargs(8); - stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4]; + stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> appargs[7]; run_cmd(appargs); } From 9da40a69ccc986babc96af286e98d734b661f4d3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Jun 2015 21:43:09 +0200 Subject: [PATCH 13/48] mavlink: do not send autopilot capabilities to avoid crash --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index dd1582c202..fe2289ff8f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1570,7 +1570,7 @@ Mavlink::task_main(int argc, char *argv[]) /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); - send_autopilot_capabilites(); + //send_autopilot_capabilites(); while (!_task_should_exit) { /* main loop */ From dc2dc9920f278f184322cd115f5361c23012ea0d Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Jun 2015 21:46:20 +0200 Subject: [PATCH 14/48] build gpssim --- makefiles/posix/config_posix_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk index 48de2636e0..a675e0644a 100644 --- a/makefiles/posix/config_posix_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -69,6 +69,7 @@ MODULES += platforms/posix/drivers/adcsim MODULES += platforms/posix/drivers/barosim MODULES += platforms/posix/drivers/tonealrmsim MODULES += platforms/posix/drivers/airspeedsim +MODULES += platforms/posix/drivers/gpssim # # Unit tests From 5c013af5742b68b8be8556bc37c27c3dcd476604 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Jun 2015 21:47:44 +0200 Subject: [PATCH 15/48] save gps data so driver can read --- src/modules/simulator/simulator.cpp | 9 ++++++++ src/modules/simulator/simulator.h | 22 ++++++++++++++++++ src/modules/simulator/simulator_mavlink.cpp | 25 +++++++++++++++++++++ 3 files changed, 56 insertions(+) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index da0b41589d..0c02da247f 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -81,6 +81,11 @@ bool Simulator::getBaroSample(uint8_t *buf, int len) return _baro.copyData(buf, len); } +bool Simulator::getGPSSample(uint8_t *buf, int len) +{ + return _gps.copyData(buf, len); +} + void Simulator::write_MPU_data(void *buf) { _mpu.writeData(buf); } @@ -97,6 +102,10 @@ void Simulator::write_baro_data(void *buf) { _baro.writeData(buf); } +void Simulator::write_gps_data(void *buf) { + _gps.writeData(buf); +} + int Simulator::start(int argc, char *argv[]) { int ret = 0; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 109c9f301c..39491d8c0d 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -98,6 +98,23 @@ struct RawBaroData { }; #pragma pack(pop) +#pragma pack(push, 1) +struct RawGPSData { + int32_t lat; + int32_t lon; + int32_t alt; + uint16_t eph; + uint16_t epv; + uint16_t vel; + int16_t vn; + int16_t ve; + int16_t vd; + uint16_t cog; + uint8_t fix_type; + uint8_t satellites_visible; +}; +#pragma pack(pop) + template class Report { public: Report(int readers) : @@ -177,11 +194,13 @@ public: bool getMagReport(uint8_t *buf, int len); bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); + bool getGPSSample(uint8_t *buf, int len); void write_MPU_data(void *buf); void write_accel_data(void *buf); void write_mag_data(void *buf); void write_baro_data(void *buf); + void write_gps_data(void *buf); private: Simulator() : @@ -189,6 +208,7 @@ private: _mpu(1), _baro(1), _mag(1), + _gps(1), _sensor_combined_pub(nullptr) #ifndef __PX4_QURT , @@ -216,6 +236,7 @@ private: simulator::Report _mpu; simulator::Report _baro; simulator::Report _mag; + simulator::Report _gps; // uORB publisher handlers orb_advert_t _accel_pub; @@ -258,6 +279,7 @@ private: void pack_actuator_message(mavlink_hil_controls_t &actuator_msg); void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); void update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu); + void update_gps(mavlink_hil_gps_t *gps_sim); static void *sending_trampoline(void *); void send(); #endif diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 9bcac4879c..befb249f4f 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -188,6 +188,25 @@ void Simulator::update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sen write_baro_data((void *)&baro); } +void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) { + RawGPSData gps; + gps.lat = gps_sim->lat; + gps.lon = gps_sim->lon; + gps.alt = gps_sim->alt; + gps.eph = gps_sim->eph; + gps.epv = gps_sim->epv; + gps.vel = gps_sim->vel; + gps.vn = gps_sim->vn; + gps.ve = gps_sim->ve; + gps.vd = gps_sim->vd; + gps.cog = gps_sim->cog; + gps.fix_type = gps_sim->fix_type; + gps.satellites_visible = gps_sim->satellites_visible; + + write_gps_data((void *)&gps); + +} + void Simulator::handle_message(mavlink_message_t *msg) { switch(msg->msgid) { case MAVLINK_MSG_ID_HIL_SENSOR: @@ -196,6 +215,12 @@ void Simulator::handle_message(mavlink_message_t *msg) { update_sensors(&_sensor, &imu); break; + case MAVLINK_MSG_ID_HIL_GPS: + mavlink_hil_gps_t gps_sim; + mavlink_msg_hil_gps_decode(msg, &gps_sim); + update_gps(&gps_sim); + break; + case MAVLINK_MSG_ID_RC_CHANNELS: mavlink_rc_channels_t rc_channels; From fd1effa4fef7a42710f102c773e5ab4ad56fcb5f Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 3 Jun 2015 14:30:35 -0700 Subject: [PATCH 16/48] Simulator: UART changes Some changes were needed to use the simulator and the UART for rc control. Signed-off-by: Mark Charlebois --- src/modules/simulator/simulator_mavlink.cpp | 112 +++++++++++++++++++- 1 file changed, 109 insertions(+), 3 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index befb249f4f..d72d4d473c 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -30,7 +30,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - +#include #include #include #include "simulator.h" @@ -46,6 +46,8 @@ using namespace simulator; static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; +static int openUart(const char *uart_name, int baud); + void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { float out[8]; @@ -393,7 +395,7 @@ void Simulator::updateSamples() (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); // setup serial connection to autopilot (used to get manual controls) - int serial_fd = open(PIXHAWK_DEVICE, O_RDWR); + int serial_fd = openUart(PIXHAWK_DEVICE, 115200); if (serial_fd < 0) { PX4_WARN("failed to open %s", PIXHAWK_DEVICE); @@ -417,7 +419,8 @@ void Simulator::updateSamples() int len = 0; - // wait for first data from simulator + // wait for first data from simulator and respond with first controls + // this is important for the UDP communication to work int pret = -1; while (pret <= 0) { pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100); @@ -425,6 +428,7 @@ void Simulator::updateSamples() if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); + send_data(); } // got data from simulator, now activate the sending thread @@ -483,3 +487,105 @@ void Simulator::updateSamples() } } } + +int openUart(const char *uart_name, int baud) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 0: speed = B0; break; + + case 50: speed = B50; break; + + case 75: speed = B75; break; + + case 110: speed = B110; break; + + case 134: speed = B134; break; + + case 150: speed = B150; break; + + case 200: speed = B200; break; + + case 300: speed = B300; break; + + case 600: speed = B600; break; + + case 1200: speed = B1200; break; + + case 1800: speed = B1800; break; + + case 2400: speed = B2400; break; + + case 4800: speed = B4800; break; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + case 460800: speed = B460800; break; + + case 921600: speed = B921600; break; + + default: + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", + baud); + return -EINVAL; + } + + /* open uart */ + int uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + + if (uart_fd < 0) { + return uart_fd; + } + + + /* Try to set baud rate */ + struct termios uart_config; + memset(&uart_config, 0, sizeof(uart_config)); + + int termios_state; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) { + warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); + ::close(uart_fd); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(uart_fd, &uart_config); + + /* USB serial is indicated by /dev/ttyACM0*/ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state); + ::close(uart_fd); + return -1; + } + + } + + // Make raw + cfmakeraw(&uart_config); + + if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR SET CONF %s\n", uart_name); + ::close(uart_fd); + return -1; + } + + return uart_fd; +} From 5cf11409440cddb912b0601908b4cc5c18c72125 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 3 Jun 2015 18:29:56 -0700 Subject: [PATCH 17/48] Add raw mode for UART to mavink_main.cpp Raw mode is not the default mode in Ubuntu 14.04. Disable echo and special character processing. Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_main.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index fe2289ff8f..b56771d376 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -675,6 +675,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * _is_usb_uart = true; } + /* Put in raw mode */ + cfmakeraw(&uart_config); + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { warnx("ERR SET CONF %s\n", uart_name); ::close(_uart_fd); From 99c066c39c1ebfd6c9a09d2883024c88eebe08d7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 4 Jun 2015 14:22:51 -0700 Subject: [PATCH 18/48] HIL: Cleanup creation and initialization Signed-off-by: Mark Charlebois --- src/drivers/hil/hil.cpp | 69 ++++++++++++++++------------------------- 1 file changed, 27 insertions(+), 42 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index bc7bf12571..d6dbe9a53f 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -108,6 +108,7 @@ public: int set_mode(Mode mode); int set_pwm_rate(unsigned rate); + int _task; private: static const unsigned _max_actuators = 4; @@ -115,7 +116,6 @@ private: Mode _mode; int _update_rate; int _current_update_rate; - int _task; int _t_actuators; int _t_armed; orb_advert_t _t_outputs; @@ -165,15 +165,15 @@ HIL *g_hil; HIL::HIL() : #ifdef __PX4_NUTTX - CDev( + CDev #else - VDev( + VDev #endif - "hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/), + ("hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/), + _task(-1), _mode(MODE_NONE), _update_rate(50), _current_update_rate(0), - _task(-1), _t_actuators(-1), _t_armed(-1), _t_outputs(0), @@ -466,9 +466,9 @@ HIL::task_main() int HIL::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -673,7 +673,7 @@ enum PortMode { PORT2_16PWM, }; -PortMode g_port_mode; +static PortMode g_port_mode = PORT_MODE_UNDEFINED; int hil_new_mode(PortMode new_mode) @@ -738,31 +738,6 @@ hil_new_mode(PortMode new_mode) return OK; } -int -hil_start(void) -{ - int ret = OK; - - if (g_hil == nullptr) { - - g_hil = new HIL; - - if (g_hil == nullptr) { - ret = -ENOMEM; - - } else { - ret = g_hil->init(); - - if (ret != OK) { - delete g_hil; - g_hil = nullptr; - } - } - } - - return ret; -} - int test(void) { @@ -828,17 +803,19 @@ hil_main(int argc, char *argv[]) const char *verb; int ret = OK; - if (hil_start() != OK) { - warnx("failed to start the HIL driver"); - return 1; - } - if (argc < 2) { usage(); return -EINVAL; } verb = argv[1]; + if (g_hil == nullptr) { + g_hil = new HIL; + if (g_hil == nullptr) { + return -ENOMEM; + } + } + /* * Mode switches. */ @@ -871,10 +848,9 @@ hil_main(int argc, char *argv[]) return OK; /* switch modes */ - return hil_new_mode(new_mode); + ret = hil_new_mode(new_mode); } - - if (!strcmp(verb, "test")) { + else if (!strcmp(verb, "test")) { ret = test(); } @@ -886,5 +862,14 @@ hil_main(int argc, char *argv[]) usage(); ret = -EINVAL; } + + if ( ret == OK && g_hil->_task == -1 ) { + ret = g_hil->init(); + if (ret != OK) { + warnx("failed to start the HIL driver"); + delete g_hil; + g_hil = nullptr; + } + } return ret; } From 6cb26de74c5a08d9a6112121f88e343ab6311d5d Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 4 Jun 2015 16:10:20 -0700 Subject: [PATCH 19/48] Multi-uORB support changes - part 1 This adds support for a dynamic build for QuRT and initial Multi-uORB changes to enable communication between the DSP and the application processor. This part of the changes do not affect the POSIX build. This is enablement for the QuRT build using Multi-uORB. The second part of the changes will be added in a new module under src/modules. Signed-off-by: Mark Charlebois --- .../posix/DISABLE_config_posix_muorb_test.mk | 78 +++ makefiles/posix/config_posix_default.mk | 4 + .../qurt/DISABLE_config_qurt_muorb_test.mk | 77 +++ makefiles/qurt/config_qurt_default.mk | 6 +- makefiles/qurt_elf.mk | 12 +- makefiles/toolchain_gnu-arm-eabi.mk | 9 +- makefiles/toolchain_hexagon.mk | 52 +- makefiles/toolchain_native.mk | 1 + src/drivers/device/device_posix.cpp | 17 +- src/drivers/device/i2c_posix.cpp | 13 +- src/modules/uORB/module.mk | 6 +- src/modules/uORB/uORBCommunicator.hpp | 167 ++++++ src/modules/uORB/uORBDevices_nuttx.cpp | 116 ++++- src/modules/uORB/uORBDevices_nuttx.hpp | 48 ++ src/modules/uORB/uORBDevices_posix.cpp | 116 ++++- src/modules/uORB/uORBDevices_posix.hpp | 52 +- src/modules/uORB/uORBMain.cpp | 7 +- src/modules/uORB/uORBManager.hpp | 76 ++- src/modules/uORB/uORBManager_nuttx.cpp | 102 +++- src/modules/uORB/uORBManager_posix.cpp | 102 ++++ src/modules/uORB/uORBUtils.cpp | 18 + src/modules/uORB/uORBUtils.hpp | 7 + src/platforms/posix/px4_layer/module.mk | 1 + src/platforms/posix/px4_layer/work_lock.c | 19 + src/platforms/posix/px4_layer/work_lock.h | 22 +- src/platforms/posix/tests/muorb/module.mk | 47 ++ .../posix/tests/muorb/muorb_test_example.cpp | 118 +++++ .../posix/tests/muorb/muorb_test_example.h | 59 +++ .../posix/tests/muorb/muorb_test_main.cpp | 66 +++ .../tests/muorb/muorb_test_start_posix.cpp | 101 ++++ src/platforms/px4_log.h | 18 +- src/platforms/qurt/dspal/dspal_stub.c | 32 ++ .../qurt/px4_layer/commands_muorb_test.c | 47 ++ src/platforms/qurt/px4_layer/main.cpp | 13 +- src/platforms/qurt/px4_layer/module.mk | 7 +- .../qurt/px4_layer/px4_qurt_impl.cpp | 5 +- src/platforms/qurt/px4_layer/qurt_stubs.c | 57 +++ src/platforms/qurt/px4_layer/work_lock.c | 19 + src/platforms/qurt/px4_layer/work_lock.h | 22 +- src/platforms/qurt/tests/muorb/module.mk | 43 ++ .../qurt/tests/muorb/muorb_test_example.cpp | 60 +++ .../qurt/tests/muorb/muorb_test_example.h | 53 ++ .../qurt/tests/muorb/muorb_test_main.cpp | 56 ++ .../tests/muorb/muorb_test_start_qurt.cpp | 101 ++++ unittests/CMakeLists.txt | 79 ++- .../uorb_unittests/uORBCommunicatorMock.cpp | 206 ++++++++ .../uorb_unittests/uORBCommunicatorMock.hpp | 135 +++++ .../uORBCommunicatorMockLoopback.cpp | 133 +++++ .../uORBCommunicatorMockLoopback.hpp | 127 +++++ .../uORBCommunicator_gtests.cpp | 481 ++++++++++++++++++ unittests/uorb_unittests/uORBGtestTopics.hpp | 54 ++ unittests/uorb_unittests/uORB_test.cpp | 206 ++++++++ 52 files changed, 3387 insertions(+), 86 deletions(-) create mode 100644 makefiles/posix/DISABLE_config_posix_muorb_test.mk create mode 100644 makefiles/qurt/DISABLE_config_qurt_muorb_test.mk create mode 100644 src/modules/uORB/uORBCommunicator.hpp create mode 100644 src/platforms/posix/px4_layer/work_lock.c create mode 100644 src/platforms/posix/tests/muorb/module.mk create mode 100644 src/platforms/posix/tests/muorb/muorb_test_example.cpp create mode 100644 src/platforms/posix/tests/muorb/muorb_test_example.h create mode 100644 src/platforms/posix/tests/muorb/muorb_test_main.cpp create mode 100644 src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp create mode 100644 src/platforms/qurt/dspal/dspal_stub.c create mode 100644 src/platforms/qurt/px4_layer/commands_muorb_test.c create mode 100644 src/platforms/qurt/px4_layer/qurt_stubs.c create mode 100644 src/platforms/qurt/px4_layer/work_lock.c create mode 100644 src/platforms/qurt/tests/muorb/module.mk create mode 100644 src/platforms/qurt/tests/muorb/muorb_test_example.cpp create mode 100644 src/platforms/qurt/tests/muorb/muorb_test_example.h create mode 100644 src/platforms/qurt/tests/muorb/muorb_test_main.cpp create mode 100644 src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp create mode 100644 unittests/uorb_unittests/uORBCommunicatorMock.cpp create mode 100644 unittests/uorb_unittests/uORBCommunicatorMock.hpp create mode 100644 unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp create mode 100644 unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp create mode 100644 unittests/uorb_unittests/uORBCommunicator_gtests.cpp create mode 100644 unittests/uorb_unittests/uORBGtestTopics.hpp create mode 100644 unittests/uorb_unittests/uORB_test.cpp diff --git a/makefiles/posix/DISABLE_config_posix_muorb_test.mk b/makefiles/posix/DISABLE_config_posix_muorb_test.mk new file mode 100644 index 0000000000..34fa623d86 --- /dev/null +++ b/makefiles/posix/DISABLE_config_posix_muorb_test.mk @@ -0,0 +1,78 @@ +# +# Makefile for the Foo *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +#MODULES += drivers/blinkm +#MODULES += drivers/hil +#MODULES += drivers/led +#MODULES += drivers/rgbled +#MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +MODULES += systemcmds/param + +# +# General system control +# +#MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +#MODULES += modules/ekf_att_pos_estimator + +# +# Vehicle Control +# +#MODULES += modules/mc_att_control + +# +# Library modules +# +MODULES += modules/systemlib +#MODULES += modules/systemlib/mixer +MODULES += modules/uORB +#MODULES += modules/dataman +#MODULES += modules/sdlog2 +#MODULES += modules/simulator +#MODULES += modules/commander + +# +# Libraries +# +#MODULES += lib/mathlib +#MODULES += lib/mathlib/math/filter +#MODULES += lib/geo +#MODULES += lib/geo_lookup +#MODULES += lib/conversion + +# +# posix port +# +MODULES += platforms/posix/px4_layer +#MODULES += platforms/posix/drivers/accelsim +#MODULES += platforms/posix/drivers/gyrosim +#MODULES += platforms/posix/drivers/adcsim +#MODULES += platforms/posix/drivers/barosim + +# +# muorb fastrpc changes. +# +#MODULES += $(PX4_BASE)../muorb_krait + +# +# Unit tests +# +MODULES += platforms/posix/tests/muorb +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk index a675e0644a..30dada7bbd 100644 --- a/makefiles/posix/config_posix_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -79,3 +79,7 @@ MODULES += platforms/posix/drivers/gpssim #MODULES += platforms/posix/tests/hrt_test #MODULES += platforms/posix/tests/wqueue +# +# muorb fastrpc changes. +# +#MODULES += $(PX4_BASE)../muorb_krait diff --git a/makefiles/qurt/DISABLE_config_qurt_muorb_test.mk b/makefiles/qurt/DISABLE_config_qurt_muorb_test.mk new file mode 100644 index 0000000000..1afa954f5b --- /dev/null +++ b/makefiles/qurt/DISABLE_config_qurt_muorb_test.mk @@ -0,0 +1,77 @@ +# +# Makefile for the Foo *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +#MODULES += drivers/blinkm +#MODULES += drivers/hil +#MODULES += drivers/led +#MODULES += drivers/rgbled +#MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +#MODULES += systemcmds/param + +# +# General system control +# +#MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +#MODULES += modules/ekf_att_pos_estimator + +# +# Vehicle Control +# +#MODULES += modules/mc_att_control + +# +# Library modules +# +#MODULES += modules/systemlib +#MODULES += modules/systemlib/mixer +MODULES += modules/uORB +#MODULES += modules/dataman +#MODULES += modules/sdlog2 +#MODULES += modules/simulator +#MODULES += modules/commander + +# +# Libraries +# +#MODULES += lib/mathlib +#MODULES += lib/mathlib/math/filter +#MODULES += lib/geo +#MODULES += lib/geo_lookup +#MODULES += lib/conversion + +# +# QuRT port +# +MODULES += platforms/qurt/px4_layer +#MODULES += platforms/posix/drivers/accelsim +#MODULES += platforms/posix/drivers/gyrosim +#MODULES += platforms/posix/drivers/adcsim +#MODULES += platforms/posix/drivers/barosim + +# +# Unit tests +# +MODULES += platforms/qurt/tests/muorb +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + +# +# sources for muorb over fastrpc +# +MODULES += $(PX4_BASE)/../muorb_qurt/ diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 8285ef28a8..398fe05194 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -15,7 +15,7 @@ MODULES += drivers/blinkm MODULES += drivers/hil MODULES += drivers/led MODULES += drivers/rgbled -#MODULES += modules/sensors +MODULES += modules/sensors #MODULES += drivers/ms5611 # @@ -76,3 +76,7 @@ MODULES += platforms/posix/tests/vcdev_test MODULES += platforms/posix/tests/hrt_test MODULES += platforms/posix/tests/wqueue +# +# sources for muorb over fastrpc +# +#MODULES += $(PX4_BASE)/../muorb_qurt/ diff --git a/makefiles/qurt_elf.mk b/makefiles/qurt_elf.mk index 53cc8d8287..281a1603d2 100644 --- a/makefiles/qurt_elf.mk +++ b/makefiles/qurt_elf.mk @@ -40,11 +40,13 @@ # # What we're going to build. # + +EXTRALDFLAGS = -Wl,-soname=libdspal_client.so PRODUCT_SHARED_LIB = $(WORK_DIR)firmware.a PRODUCT_SHARED_PRELINK = $(WORK_DIR)firmware.o .PHONY: firmware -firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)mainapp +firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)libdspal_client.so $(WORK_DIR)mainapp # # Built product rules @@ -63,7 +65,13 @@ $(WORK_DIR)apps.o: $(WORK_DIR)apps.cpp $(call COMPILEXX,$<, $@) mv $(WORK_DIR)apps.cpp $(WORK_DIR)apps.cpp_sav -$(WORK_DIR)mainapp: $(WORK_DIR)apps.o $(PRODUCT_SHARED_LIB) +$(WORK_DIR)libdspal_client.so: $(WORK_DIR)apps.o $(PRODUCT_SHARED_LIB) + $(call LINK_SO,$@, $^) + +$(WORK_DIR)dspal_stub.o: $(PX4_BASE)/src/platforms/qurt/dspal/dspal_stub.c + $(call COMPILENOSHARED,$^, $@) + +$(WORK_DIR)mainapp: $(WORK_DIR)dspal_stub.o $(call LINK,$@, $^) # diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 6c4e64f54c..143b15536c 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -125,6 +125,9 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ # note - requires corresponding support in NuttX INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) +LIBSTDCXX := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libstdc++.a) +LIBC := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libc.a) + # Language-specific flags # ARCHCFLAGS = -std=gnu99 @@ -262,7 +265,8 @@ endef define PRELINK @$(ECHO) "PRELINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1 + $(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1 + #$(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1 endef # Update the archive $1 with the files in $2 @@ -278,7 +282,8 @@ endef define LINK @$(ECHO) "LINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group + $(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) ${LIBSTDCXX} $(LIBGCC) --end-group + #$(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) ${LIBSTDCXX} $(LIBGCC) --end-group endef # Convert $1 from a linked object to a raw binary in $2 diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index 7135158aa8..4574574999 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -38,7 +38,8 @@ # Toolchain commands. Normally only used inside this file. # HEXAGON_TOOLS_ROOT = /opt/6.4.05 -HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 +#HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 +HEXAGON_SDK_ROOT = /prj/atlanticus/users/rkintada/Hexagon_SDK/2.0 #V_ARCH = v4 V_ARCH = v5 CROSSDEV = hexagon- @@ -60,8 +61,10 @@ AR = $(HEXAGON_BIN)/$(CROSSDEV)ar rcs NM = $(HEXAGON_BIN)/$(CROSSDEV)nm OBJCOPY = $(HEXAGON_BIN)/$(CROSSDEV)objcopy OBJDUMP = $(HEXAGON_BIN)/$(CROSSDEV)objdump +HEXAGON_GCC = $(HEXAGON_BIN)/$(CROSSDEV)gcc QURTLIBS = \ + $(QCTOOLSLIB)/libdl.a \ $(TOOLSLIB)/init.o \ $(TOOLSLIB)/libc.a \ $(TOOLSLIB)/libqcc.a \ @@ -77,7 +80,8 @@ QURTLIBS = \ $(QCTOOLSLIB)/libhexagon.a \ $(TOOLSLIB)/fini.o - +DYNAMIC_LIBS = \ + -Wl,$(TOOLSLIB)/pic/libstdc++.a # Check if the right version of the toolchain is available @@ -96,7 +100,7 @@ MAXOPTIMIZATION ?= -O0 # Base CPU flags for each of the supported architectures. # -ARCHCPUFLAGS = -m$(V_ARCH) +ARCHCPUFLAGS = -m$(V_ARCH) -G0 # Set the board flags @@ -117,6 +121,8 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -I$(PX4_BASE)/../dspal/sys \ -I$(PX4_BASE)/mavlink/include/mavlink \ -I$(QURTLIB)/..//include \ + -I$(HEXAGON_SDK_ROOT)/inc \ + -I$(HEXAGON_SDK_ROOT)/inc/stddef \ -Wno-error=shadow # optimisation flags @@ -127,7 +133,8 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ -fomit-frame-pointer \ -funsafe-math-optimizations \ -ffunction-sections \ - -fdata-sections + -fdata-sections \ + -fpic # enable precise stack overflow tracking # note - requires corresponding support in NuttX @@ -198,10 +205,6 @@ CXXFLAGS = $(ARCHCXXFLAGS) \ $(EXTRACXXFLAGS) \ $(addprefix -I,$(INCLUDE_DIRS)) -ifeq (1,$(V_dynamic)) -CXX_FLAGS += -fpic -D__V_DYNAMIC__ -endif - # Flags we pass to the assembler # AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ @@ -211,7 +214,16 @@ AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ LDSCRIPT = $(PX4_BASE)/posix-configs/posixtest/scripts/ld.script # Flags we pass to the linker # -LDFLAGS += -g -nostdlib --section-start .start=0x1d000000\ +LDFLAGS += -g -mv5 -nostdlib -mG0lib -G0 -fpic -shared \ + -nostartfiles \ + -Wl,-Bsymbolic \ + -Wl,--wrap=malloc \ + -Wl,--wrap=calloc \ + -Wl,--wrap=free \ + -Wl,--wrap=realloc \ + -Wl,--wrap=memalign \ + -Wl,--wrap=__stack_chk_fail \ + -lc \ $(EXTRALDFLAGS) \ $(addprefix -L,$(LIB_DIRS)) @@ -230,21 +242,31 @@ DEP_INCLUDES = $(subst .o,.d,$(OBJS)) # Compile C source $1 to object $2 # as a side-effect, generate a dependency file # -define COMPILE +define COMPILENOSHARED @$(ECHO) "CC: $1" @$(MKDIR) -p $(dir $2) @echo $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 endef -# Compile C++ source $1 to $2 +# Compile C source $1 to object $2 for use in shared library +# as a side-effect, generate a dependency file +# +define COMPILE + @$(ECHO) "CC: $1" + @$(MKDIR) -p $(dir $2) + @echo $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 + $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2 +endef + +# Compile C++ source $1 to $2 for use in shared library # as a side-effect, generate a dependency file # define COMPILEXX @$(ECHO) "CXX: $1" @$(MKDIR) -p $(dir $2) @echo $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 - $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 + $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2 endef # Assemble $1 into $2 @@ -299,8 +321,7 @@ endef define LINK_SO @$(ECHO) "LINK_SO: $1" @$(MKDIR) -p $(dir $1) - echo "$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) $(EXTRA_LIBS)" - $(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) -pthread -lc + $(HEXAGON_GCC) $(LDFLAGS) -fPIC -shared -nostartfiles -o $1 -Wl,--whole-archive $2 -Wl,--no-whole-archive $(LIBS) $(DYNAMIC_LIBS) endef # Link the objects in $2 into the application $1 @@ -308,7 +329,6 @@ endef define LINK @$(ECHO) "LINK: $1" @$(MKDIR) -p $(dir $1) - @echo $(Q) $(LD) $(LDFLAGS) -o $1 --start-group $2 $(EXTRA_LIBS) $(QURTLIBS) --end-group - $(Q) $(LD) $(LDFLAGS) -o $1 --start-group $2 $(EXTRA_LIBS) $(QURTLIBS) --end-group + $(LD) --section-start .start=0x1d000000 -o $1 --start-group $2 $(EXTRA_LIBS) $(QURTLIBS) --end-group --dynamic-linker= -E --force-dynamic endef diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 0b8223bfd0..fb730cd9fa 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -210,6 +210,7 @@ ARCHWARNINGSXX = $(ARCHWARNINGS) \ # pull in *just* libm from the toolchain ... this is grody LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) #EXTRA_LIBS += $(LIBM) +#EXTRA_LIBS += ${PX4_BASE}../muorb_krait/lib/libmuorb.so EXTRA_LIBS += -pthread -lm -lrt # Flags we pass to the C compiler diff --git a/src/drivers/device/device_posix.cpp b/src/drivers/device/device_posix.cpp index 771bee2471..7d83842266 100644 --- a/src/drivers/device/device_posix.cpp +++ b/src/drivers/device/device_posix.cpp @@ -85,10 +85,11 @@ Device::log(const char *fmt, ...) PX4_INFO("[%s] ", _name); va_start(ap, fmt); - vprintf(fmt, ap); + PX4_INFO( fmt, ap ); + //vprintf(fmt, ap); va_end(ap); - printf("\n"); - fflush(stdout); + //printf("\n"); + //fflush(stdout); } void @@ -97,12 +98,14 @@ Device::debug(const char *fmt, ...) va_list ap; if (_debug_enabled) { - printf("<%s> ", _name); + PX4_INFO("<%s> ", _name); + //printf("<%s> ", _name); va_start(ap, fmt); - vprintf(fmt, ap); + //vprintf(fmt, ap); + PX4_INFO(fmt, ap); va_end(ap); - printf("\n"); - fflush(stdout); + //printf("\n"); + //fflush(stdout); } } diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index 473307b1cc..ddf7a0db4c 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -81,7 +81,9 @@ I2C::I2C(const char *name, I2C::~I2C() { if (_fd >= 0) { +#ifndef __PX4_QURT ::close(_fd); +#endif _fd = -1; } } @@ -116,6 +118,7 @@ I2C::init() _fd = 10000; } else { +#ifndef __PX4_QURT // Open the actual I2C device and map to the virtual dev name _fd = ::open(get_devname(), O_RDWR); if (_fd < 0) { @@ -123,6 +126,7 @@ I2C::init() px4_errno = errno; return PX4_ERROR; } +#endif } return ret; @@ -246,8 +250,11 @@ ssize_t I2C::read(file_t *filp, char *buffer, size_t buflen) warnx ("2C SIM I2C::read"); return 0; } - +#ifndef __PX4_QURT return ::read(_fd, buffer, buflen); +#else + return 0; +#endif } ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen) @@ -257,7 +264,11 @@ ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen) return buflen; } +#ifndef __PX4_QURT return ::write(_fd, buffer, buflen); +#else + return buflen; +#endif } } // namespace device diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 31a2b24e4a..bb6d4ea98b 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -46,9 +46,13 @@ SRCS = uORBDevices_nuttx.cpp \ else SRCS = uORBDevices_posix.cpp \ - uORBTest_UnitTest.cpp \ uORBManager_posix.cpp endif + +ifeq ($(PX4_TARGET_OS),posix) +SRCS += uORBTest_UnitTest.cpp +endif + SRCS += objects_common.cpp \ Publication.cpp \ Subscription.cpp \ diff --git a/src/modules/uORB/uORBCommunicator.hpp b/src/modules/uORB/uORBCommunicator.hpp new file mode 100644 index 0000000000..ad407bd7d8 --- /dev/null +++ b/src/modules/uORB/uORBCommunicator.hpp @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef _uORBCommunicator_hpp_ +#define _uORBCommunicator_hpp_ + +#include +#include + +namespace uORBCommunicator +{ + class IChannel; + class IChannelRxHandler; +} + +/** + * Interface to enable remote subscriptions. The implementor of this interface + * shall manage the communication channel. It can be fastRPC or tcp or ip. + */ + +class uORBCommunicator::IChannel +{ +public: + + //========================================================================= + // INTERFACES FOR Control messages over a channel. + //========================================================================= + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription( const std::string& messageName, int32_t msgRateInHz ) = 0; + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription( const std::string& messageName ) = 0; + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler( uORBCommunicator::IChannelRxHandler* handler ) = 0; + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message( const std::string& messageName, int32_t length, uint8_t* data) = 0; +}; + +/** + * Class passed to the communication link implement to provide callback for received + * messages over a channel. + */ +class uORBCommunicator::IChannelRxHandler +{ +public: + + /** + * Interface to process a received AddSubscription from remote. + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_add_subscription( const std::string& messageName, int32_t msgRateInHz ) = 0; + + /** + * Interface to process a received control msg to remove subscription + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_remove_subscription( const std::string& messageName ) = 0; + + /** + * Interface to process the received data message. + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_received_message( const std::string& messageName, int32_t length, uint8_t* data ) = 0; +}; + +#endif /* _uORBCommunicator_hpp_ */ diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index d3c30f0508..319f724a67 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -36,10 +36,15 @@ #include #include #include +#include #include "uORBDevices_nuttx.hpp" #include "uORBUtils.hpp" +#include "uORBManager.hpp" +#include "uORBCommunicator.hpp" #include +std::map uORB::DeviceMaster::_node_map; + uORB::DeviceNode::DeviceNode ( const struct orb_metadata *meta, @@ -53,7 +58,9 @@ uORB::DeviceNode::DeviceNode _last_update(0), _generation(0), _publisher(0), - _priority(priority) + _priority(priority), + _IsRemoteSubscriberPresent( false ), + _subscriber_count(0) { // enable debug() calls _debug_enabled = true; @@ -120,6 +127,8 @@ uORB::DeviceNode::open(struct file *filp) ret = CDev::open(filp); + add_internal_subscriber(); + if (ret != OK) delete sd; @@ -142,7 +151,9 @@ uORB::DeviceNode::close(struct file *filp) if (sd != nullptr) { hrt_cancel(&sd->update_call); + remove_internal_subscriber(); delete sd; + sd = nullptr; } } @@ -295,7 +306,19 @@ uORB::DeviceNode::publish errno = EIO; return ERROR; } - + /* + * if the write is successful, send the data over the Multi-ORB link + */ + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( ch != nullptr ) + { + if( ch->send_message( meta->o_name, meta->o_size, (uint8_t*)data ) != 0 ) + { + warnx( "[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel", + __LINE__, meta->o_name ); + return ERROR; + } + } return OK; } @@ -421,6 +444,79 @@ uORB::DeviceNode::update_deferred_trampoline(void *arg) node->update_deferred(); } +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::DeviceNode::add_internal_subscriber() +{ + _subscriber_count++; + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( ch != nullptr && _subscriber_count > 0 ) + { + ch->add_subscription( _meta->o_name, 1 ); + } +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::DeviceNode::remove_internal_subscriber() +{ + _subscriber_count--; + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( ch != nullptr && _subscriber_count == 0 ) + { + ch->remove_subscription( _meta->o_name ); + } +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz ) +{ + // if there is already data in the node, send this out to + // the remote entity. + // send the data to the remote entity. + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( _data != nullptr && ch != nullptr ) // _data will not be null if there is a publisher. + { + ch->send_message( _meta->o_name, _meta->o_size, _data); + } + + return OK; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_remove_subscription() +{ + return OK; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t* data) +{ + int16_t ret = -1; + if( length != (int32_t)(_meta->o_size) ) + { + warnx( "[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]", + __LINE__, _meta->o_name, (int)length, (int)_meta->o_size ); + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = 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; +} + uORB::DeviceMaster::DeviceMaster(Flavor f) : CDev((f == PUBSUB) ? "obj_master" : "param_master", (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), @@ -509,6 +605,11 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) free((void *)objname); free((void *)devpath); } + else + { + // add to the node map;. + _node_map[std::string(nodepath)] = node; + } group_tries++; @@ -529,3 +630,14 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) return CDev::ioctl(filp, cmd, arg); } } + +uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const std::string& nodepath ) +{ + uORB::DeviceNode* rc = nullptr; + if( _node_map.find( nodepath ) != _node_map.end() ) + { + rc = _node_map[nodepath]; + } + return rc; +} + diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp index 3d8c0f90ad..e26598531a 100644 --- a/src/modules/uORB/uORBDevices_nuttx.hpp +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -35,6 +35,8 @@ #define _uORBDevices_nuttx_hpp_ #include +#include +#include #include "uORBCommon.hpp" @@ -119,6 +121,43 @@ public: const void *data ); + /** + * processes a request for add subscription from remote + * @param rateInHz + * Specifies the desired rate for the message. + * @return + * 0 = success + * otherwise failure. + */ + int16_t process_add_subscription( int32_t rateInHz ); + + /** + * processes a request to remove a subscription from remote. + */ + int16_t process_remove_subscription(); + + /** + * processed the received data message from remote. + */ + int16_t process_received_message( int32_t length, uint8_t* data ); + + /** + * Add the subscriber to the node's list of subscriber. If there is + * remote proxy to which this subscription needs to be sent, it will + * done via uORBCommunicator::IChannel interface. + * @param sd + * the subscriber to be added. + */ + void add_internal_subscriber(); + + /** + * Removes the subscriber from the list. Also notifies the remote + * if there a uORBCommunicator::IChannel instance. + * @param sd + * the Subscriber to be removed. + */ + void remove_internal_subscriber(); + protected: virtual pollevent_t poll_state(struct file *filp); virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); @@ -147,6 +186,9 @@ private: // private class methods. return sd; } + bool _IsRemoteSubscriberPresent; + int32_t _subscriber_count; + /** * Perform a deferred update for a rate-limited subscriber. */ @@ -166,6 +208,10 @@ private: // private class methods. * @return True if the topic should appear updated to the subscriber */ bool appears_updated(SubscriberData *sd); + + // disable copy and assignment operators + DeviceNode( const DeviceNode& ); + DeviceNode& operator=( const DeviceNode& ); }; /** @@ -180,9 +226,11 @@ public: DeviceMaster(Flavor f); virtual ~DeviceMaster(); + static uORB::DeviceNode* GetDeviceNode( const std::string& node_name ); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: Flavor _flavor; + static std::map _node_map; }; diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index 9035a32bdc..037ff395b3 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -35,11 +35,16 @@ #include #include #include +#include #include "uORBDevices_posix.hpp" #include "uORBUtils.hpp" +#include "uORBManager.hpp" +#include "uORBCommunicator.hpp" #include +std::map uORB::DeviceMaster::_node_map; + uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(device::file_t *filp) { @@ -60,7 +65,8 @@ uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, _last_update(0), _generation(0), _publisher(0), - _priority(priority) + _priority(priority), + _subscriber_count(0) { // enable debug() calls //_debug_enabled = true; @@ -127,6 +133,8 @@ uORB::DeviceNode::open(device::file_t *filp) ret = VDev::open(filp); + add_internal_subscriber(); + if (ret != PX4_OK) { warnx("ERROR: VDev::open failed\n"); delete sd; @@ -153,7 +161,9 @@ uORB::DeviceNode::close(device::file_t *filp) if (sd != nullptr) { hrt_cancel(&sd->update_call); + remove_internal_subscriber(); delete sd; + sd = nullptr; } } @@ -280,7 +290,7 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg) } ssize_t -uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) +uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data ) { //warnx("uORB::DeviceNode::publish meta = %p", meta); @@ -310,6 +320,19 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v return ERROR; } + /* + * if the write is successful, send the data over the Multi-ORB link + */ + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( ch != nullptr ) + { + if( ch->send_message( meta->o_name, meta->o_size, (uint8_t*)data ) != 0 ) + { + warnx( "[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel", + __LINE__, meta->o_name ); + return ERROR; + } + } return PX4_OK; } @@ -437,6 +460,80 @@ uORB::DeviceNode::update_deferred_trampoline(void *arg) node->update_deferred(); } +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::DeviceNode::add_internal_subscriber() +{ + _subscriber_count++; + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( ch != nullptr && _subscriber_count > 0 ) + { + ch->add_subscription( _meta->o_name, 1 ); + } +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::DeviceNode::remove_internal_subscriber() +{ + _subscriber_count--; + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( ch != nullptr && _subscriber_count == 0 ) + { + ch->remove_subscription( _meta->o_name ); + } +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz ) +{ + // if there is already data in the node, send this out to + // the remote entity. + // send the data to the remote entity. + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( _data != nullptr && ch != nullptr ) // _data will not be null if there is a publisher. + { + ch->send_message( _meta->o_name, _meta->o_size, _data); + } + + return 0; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_remove_subscription() +{ + return 0; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t* data) +{ + int16_t ret = -1; + if( length != (int32_t)(_meta->o_size) ) + { + warnx( "[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]", + __LINE__, _meta->o_name, (int)length, (int)_meta->o_size ); + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = write(nullptr, (const char *)data, _meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)_meta->o_size) { + errno = EIO; + return ERROR; + } + + return PX4_OK; +} + + uORB::DeviceMaster::DeviceMaster(Flavor f) : VDev((f == PUBSUB) ? "obj_master" : "param_master", (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), @@ -528,6 +625,12 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) free((void *)objname); free((void *)devpath); } + else + { + // add to the node map;. + _node_map[std::string(nodepath)] = node; + } + group_tries++; @@ -549,3 +652,12 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) } } +uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const std::string& nodepath ) +{ + uORB::DeviceNode* rc = nullptr; + if( _node_map.find( nodepath ) != _node_map.end() ) + { + rc = _node_map[nodepath]; + } + return rc; +} diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index ce22b84321..11e333d83f 100644 --- a/src/modules/uORB/uORBDevices_posix.hpp +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -35,9 +35,10 @@ #define _uORBDevices_posix_hpp_ #include +#include +#include #include "uORBCommon.hpp" - namespace uORB { class DeviceNode; @@ -56,7 +57,44 @@ public: virtual ssize_t write(device::file_t *filp, const char *buffer, size_t buflen); virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); + static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data ); + + /** + * processes a request for add subscription from remote + * @param rateInHz + * Specifies the desired rate for the message. + * @return + * 0 = success + * otherwise failure. + */ + int16_t process_add_subscription( int32_t rateInHz ); + + /** + * processes a request to remove a subscription from remote. + */ + int16_t process_remove_subscription(); + + /** + * processed the received data message from remote. + */ + int16_t process_received_message( int32_t length, uint8_t* data ); + + /** + * Add the subscriber to the node's list of subscriber. If there is + * remote proxy to which this subscription needs to be sent, it will + * done via uORBCommunicator::IChannel interface. + * @param sd + * the subscriber to be added. + */ + void add_internal_subscriber(); + + /** + * Removes the subscriber from the list. Also notifies the remote + * if there a uORBCommunicator::IChannel instance. + * @param sd + * the Subscriber to be removed. + */ + void remove_internal_subscriber(); protected: virtual pollevent_t poll_state(device::file_t *filp); @@ -81,6 +119,8 @@ private: SubscriberData *filp_to_sd(device::file_t *filp); + int32_t _subscriber_count; + /** * Perform a deferred update for a rate-limited subscriber. */ @@ -100,6 +140,11 @@ private: * @return True if the topic should appear updated to the subscriber */ bool appears_updated(SubscriberData *sd); + + + // disable copy and assignment operators + DeviceNode( const DeviceNode& ); + DeviceNode& operator=( const DeviceNode& ); }; /** @@ -114,9 +159,12 @@ public: DeviceMaster(Flavor f); ~DeviceMaster(); + static uORB::DeviceNode* GetDeviceNode( const std::string& node_name ); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); private: Flavor _flavor; + static std::map _node_map; }; #endif /* _uORBDeviceNode_posix.hpp */ diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp index 147ce5fbb3..8b2930eaef 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/src/modules/uORB/uORBMain.cpp @@ -34,9 +34,12 @@ #include #include "uORBDevices.hpp" #include "uORB.h" -#include "uORBTest_UnitTest.hpp" #include "uORBCommon.hpp" +#ifndef __PX4_QURT +#include "uORBTest_UnitTest.hpp" +#endif + extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } static uORB::DeviceMaster *g_dev = nullptr; @@ -85,6 +88,7 @@ uorb_main(int argc, char *argv[]) return OK; } +#ifndef __PX4_QURT /* * Test the driver/device. */ @@ -108,6 +112,7 @@ uorb_main(int argc, char *argv[]) return t.latency_test(ORB_ID(orb_test), true); } } +#endif /* * Print driver information. diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index 05626de723..89b9d7c13f 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -35,7 +35,13 @@ #define _uORBManager_hpp_ #include "uORBCommon.hpp" +#include "uORBDevices.hpp" +#include +#include #include +#include + +#include "uORBCommunicator.hpp" namespace uORB { @@ -47,7 +53,7 @@ namespace uORB * uORB nodes for each uORB topics and also implements the behavor of the * uORB Api's. */ -class uORB::Manager +class uORB::Manager : public uORBCommunicator::IChannelRxHandler { public: // public interfaces for this class. @@ -281,6 +287,27 @@ class uORB::Manager */ int orb_set_interval(int handle, unsigned interval) ; + /** + * Method to set the uORBCommunicator::IChannel instance. + * @param comm_channel + * The IChannel instance to talk to remote proxies. + * @note: + * Currently this call only supports the use of one IChannel + * Future extensions may include more than one IChannel's. + */ + void set_uorb_communicator(uORBCommunicator::IChannel* comm_channel); + + /** + * Gets the uORB Communicator instance. + */ + uORBCommunicator::IChannel* get_uorb_communicator( void ); + + /** + * Utility method to check if there is a remote subscriber present + * for a given topic + */ + bool is_remote_subscriber_present( const std::string& messageName ); + private: // class methods /** * Advertise a node; don't consider it an error if the node has @@ -316,10 +343,57 @@ class uORB::Manager private: // data members static Manager _Instance; + // the communicator channel instance. + uORBCommunicator::IChannel* _comm_channel; + std::set _remote_subscriber_topics; private: //class methods Manager(); + /** + * Interface to process a received AddSubscription from remote. + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_add_subscription(const std::string& messageName, + int32_t msgRateInHz); + + /** + * Interface to process a received control msg to remove subscription + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_remove_subscription(const std::string& messageName); + + /** + * Interface to process the received data message. + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_received_message(const std::string& messageName, + int32_t length, uint8_t* data); + }; #endif /* _uORBManager_hpp_ */ diff --git a/src/modules/uORB/uORBManager_nuttx.cpp b/src/modules/uORB/uORBManager_nuttx.cpp index e8784d593e..3a188c1646 100644 --- a/src/modules/uORB/uORBManager_nuttx.cpp +++ b/src/modules/uORB/uORBManager_nuttx.cpp @@ -39,7 +39,6 @@ #include #include "uORBUtils.hpp" #include "uORBManager.hpp" -#include "uORBDevices.hpp" //========================= Static initializations ================= @@ -55,6 +54,7 @@ uORB::Manager* uORB::Manager::get_instance() //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- uORB::Manager::Manager() +: _comm_channel( nullptr ) { } @@ -280,3 +280,103 @@ int uORB::Manager::node_open /* everything has been OK, we can return the handle now */ return fd; } + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::Manager::set_uorb_communicator( uORBCommunicator::IChannel* channel) +{ + _comm_channel = channel; + if (_comm_channel != nullptr) { + _comm_channel->register_handler(this); + } +} + +uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void ) +{ + return _comm_channel; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_add_subscription(const std::string& messageName, + int32_t msgRateInHz) +{ + warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", + __LINE__, messageName.c_str() ); + int16_t rc = 0; + _remote_subscriber_topics.insert( messageName ); + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName ); + if (ret == OK) { + // get the node name. + uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); + if ( node == nullptr) { + warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet", + __LINE__, messageName.c_str() ); + } + else{ + // node is present. + node->process_add_subscription(msgRateInHz); + } + } else { + rc = -1; + } + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_remove_subscription( + const std::string& messageName) +{ + warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s", + __LINE__, messageName.c_str() ); + int16_t rc = -1; + _remote_subscriber_topics.erase( messageName ); + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName ); + if (ret == OK) { + uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); + // get the node name. + if ( node == nullptr) { + warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", + __LINE__, messageName.c_str()); + } else { + // node is present. + node->process_remove_subscription(); + rc = 0; + } + } + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_received_message(const std::string& messageName, + int32_t length, uint8_t* data) +{ + //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName.c_str() ); + + int16_t rc = -1; + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName ); + if (ret == OK) { + uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); + // get the node name. + if ( node == nullptr) { + warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]", + __LINE__, messageName.c_str(), nodepath ); + + } else { + // node is present. + node->process_received_message( length, data ); + rc = 0; + } + } + return rc; +} + +bool uORB::Manager::is_remote_subscriber_present( const std::string& messageName ) +{ + return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() ); +} diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp index 21187bf359..1954de2784 100644 --- a/src/modules/uORB/uORBManager_posix.cpp +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -39,6 +39,7 @@ #include #include "uORBUtils.hpp" #include "uORBManager.hpp" +#include "px4_config.h" #include "uORBDevices.hpp" @@ -55,6 +56,7 @@ uORB::Manager* uORB::Manager::get_instance() //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- uORB::Manager::Manager() +: _comm_channel( nullptr ) { } @@ -292,3 +294,103 @@ int uORB::Manager::node_open /* everything has been OK, we can return the handle now */ return fd; } + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::Manager::set_uorb_communicator( uORBCommunicator::IChannel* channel) +{ + _comm_channel = channel; + if (_comm_channel != nullptr) { + _comm_channel->register_handler(this); + } +} + +uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void ) +{ + return _comm_channel; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_add_subscription(const std::string& messageName, + int32_t msgRateInHz) +{ + warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", + __LINE__, messageName.c_str() ); + int16_t rc = 0; + _remote_subscriber_topics.insert( messageName ); + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName ); + if (ret == OK) { + // get the node name. + uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); + if ( node == nullptr) { + warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet", + __LINE__, messageName.c_str() ); + } + else{ + // node is present. + node->process_add_subscription(msgRateInHz); + } + } else { + rc = -1; + } + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_remove_subscription( + const std::string& messageName) +{ + warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s", + __LINE__, messageName.c_str() ); + int16_t rc = -1; + _remote_subscriber_topics.erase( messageName ); + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName ); + if (ret == OK) { + uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); + // get the node name. + if ( node == nullptr) { + warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", + __LINE__, messageName.c_str()); + } else { + // node is present. + node->process_remove_subscription(); + rc = 0; + } + } + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_received_message(const std::string& messageName, + int32_t length, uint8_t* data) +{ + //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName.c_str() ); + + int16_t rc = -1; + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName ); + if (ret == OK) { + uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); + // get the node name. + if ( node == nullptr) { + warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]", + __LINE__, messageName.c_str(), nodepath ); + + } else { + // node is present. + node->process_received_message( length, data ); + rc = 0; + } + } + return rc; +} + +bool uORB::Manager::is_remote_subscriber_present( const std::string& messageName ) +{ + return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() ); +} diff --git a/src/modules/uORB/uORBUtils.cpp b/src/modules/uORB/uORBUtils.cpp index c9d49222bc..3bedc5ecd9 100644 --- a/src/modules/uORB/uORBUtils.cpp +++ b/src/modules/uORB/uORBUtils.cpp @@ -61,3 +61,21 @@ int uORB::Utils::node_mkpath return OK; } + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int uORB::Utils::node_mkpath(char *buf, Flavor f, + const std::string& orbMsgName ) +{ + unsigned len; + + unsigned index = 0; + + len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param", + orbMsgName.c_str(), index ); + + if (len >= orb_maxpath) + return -ENAMETOOLONG; + + return OK; +} diff --git a/src/modules/uORB/uORBUtils.hpp b/src/modules/uORB/uORBUtils.hpp index dc2fb9b78e..f9df35eee8 100644 --- a/src/modules/uORB/uORBUtils.hpp +++ b/src/modules/uORB/uORBUtils.hpp @@ -34,6 +34,7 @@ #define _uORBUtils_hpp_ #include "uORBCommon.hpp" +#include namespace uORB { @@ -50,6 +51,12 @@ public: const struct orb_metadata *meta, int *instance = nullptr ); + + /** + * same as above except this generators the path based on the string. + */ + static int node_mkpath(char *buf, Flavor f, const std::string& orbMsgName); + }; #endif // _uORBUtils_hpp_ diff --git a/src/platforms/posix/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk index b2bd0da4bb..73b6ce9e00 100644 --- a/src/platforms/posix/px4_layer/module.mk +++ b/src/platforms/posix/px4_layer/module.mk @@ -42,6 +42,7 @@ SRCS = \ hrt_queue.c \ hrt_work_cancel.c \ work_thread.c \ + work_lock.c \ work_queue.c \ work_cancel.c \ lib_crc32.c \ diff --git a/src/platforms/posix/px4_layer/work_lock.c b/src/platforms/posix/px4_layer/work_lock.c new file mode 100644 index 0000000000..b2ad307d7c --- /dev/null +++ b/src/platforms/posix/px4_layer/work_lock.c @@ -0,0 +1,19 @@ +//#pragma once +#include +#include +#include "work_lock.h" + + +extern sem_t _work_lock[]; + +void work_lock(int id) +{ + //printf("work_lock %d\n", id); + sem_wait(&_work_lock[id]); +} + +void work_unlock(int id) +{ + //printf("work_unlock %d\n", id); + sem_post(&_work_lock[id]); +} diff --git a/src/platforms/posix/px4_layer/work_lock.h b/src/platforms/posix/px4_layer/work_lock.h index f77f228b36..ad2e5b4a01 100644 --- a/src/platforms/posix/px4_layer/work_lock.h +++ b/src/platforms/posix/px4_layer/work_lock.h @@ -31,23 +31,13 @@ * ****************************************************************************/ -#include -#include +#ifndef _work_lock_h_ +#define _work_lock_h_ -#pragma once -extern sem_t _work_lock[]; -inline void work_lock(int id); -inline void work_lock(int id) -{ - //printf("work_lock %d\n", id); - sem_wait(&_work_lock[id]); -} +//#pragma once -inline void work_unlock(int id); -inline void work_unlock(int id) -{ - //printf("work_unlock %d\n", id); - sem_post(&_work_lock[id]); -} +void work_lock(int id); +void work_unlock(int id); +#endif // _work_lock_h_ diff --git a/src/platforms/posix/tests/muorb/module.mk b/src/platforms/posix/tests/muorb/module.mk new file mode 100644 index 0000000000..15bf4824e9 --- /dev/null +++ b/src/platforms/posix/tests/muorb/module.mk @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = muorb_test + +INCLUDE_DIRS += ${PX4_BASE}../muorb_krait \ + ${PX4_BASE}../muorb_krait/lib/include \ + ${PX4_BASE}../muorb_krait/Pal/lib + +SRCS = muorb_test_main.cpp \ + muorb_test_start_posix.cpp \ + muorb_test_example.cpp + diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.cpp b/src/platforms/posix/tests/muorb/muorb_test_example.cpp new file mode 100644 index 0000000000..37dfec5ed3 --- /dev/null +++ b/src/platforms/posix/tests/muorb/muorb_test_example.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hello_example.cpp + * Example for Linux + * + * @author Mark Charlebois + */ + +#include "muorb_test_example.h" +#include +#include +#include +#include + +px4::AppState MuorbTestExample::appState; + +int MuorbTestExample::main() +{ + appState.setRunning(true); + + int i=0; + orb_advert_t pub_id = orb_advertise( ORB_ID( esc_status ), & m_esc_status ); + if( pub_id == 0 ) + { + PX4_ERR( "error publishing esc_status" ); + return -1; + } + orb_advert_t pub_id_vc = orb_advertise( ORB_ID( vehicle_command ), & m_vc ); + if( pub_id_vc == 0 ) + { + PX4_ERR( "error publishing vehicle_command" ); + return -1; + } + if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) + { + PX4_ERR( "[%d]Error publishing the vechile command message", i ); + return -1; + } + int sub_vc = orb_subscribe( ORB_ID( vehicle_command ) ); + if ( sub_vc == PX4_ERROR ) + { + PX4_ERR( "Error subscribing to vehicle_command topic" ); + return -1; + } + + while (!appState.exitRequested() && i<100) { + + PX4_DEBUG("[%d] Doing work...", i ); + if( orb_publish( ORB_ID( esc_status ), pub_id, &m_esc_status ) == PX4_ERROR ) + { + PX4_ERR( "[%d]Error publishing the esc status message for iter", i ); + break; + } + bool updated = false; + if( orb_check( sub_vc, &updated ) == 0 ) + { + if( updated ) + { + PX4_DEBUG( "[%d]Vechicle Status is updated... reading new value", i ); + if( orb_copy( ORB_ID( vehicle_command ), sub_vc, &m_vc ) != 0 ) + { + PX4_ERR( "[%d]Error calling orb copy for vechivle status... ", i ); + break; + } + if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) + { + PX4_ERR( "[%d]Error publishing the vechile command message", i ); + break; + } + } + else + { + PX4_DEBUG( "[%d] VC topic is not updated", i ); + } + } + else + { + PX4_ERR( "[%d]Error checking the updated status for vechile command... ", i ); + break; + } + + ++i; + } + + return 0; +} diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.h b/src/platforms/posix/tests/muorb/muorb_test_example.h new file mode 100644 index 0000000000..a3625167c7 --- /dev/null +++ b/src/platforms/posix/tests/muorb/muorb_test_example.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hello_example.h + * Example app for Linux + * + * @author Mark Charlebois + */ +#pragma once + +#include +#include "uORB/topics/esc_status.h" +#include "uORB/topics/vehicle_command.h" + +class MuorbTestExample { +public: + MuorbTestExample() {}; + + ~MuorbTestExample() {}; + + int main(); + + static px4::AppState appState; /* track requests to terminate app */ +private: + struct esc_status_s m_esc_status; + struct vehicle_command_s m_vc; + +}; diff --git a/src/platforms/posix/tests/muorb/muorb_test_main.cpp b/src/platforms/posix/tests/muorb/muorb_test_main.cpp new file mode 100644 index 0000000000..6ebc0ae92c --- /dev/null +++ b/src/platforms/posix/tests/muorb/muorb_test_main.cpp @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hello_main.cpp + * Example for Linux + * + * @author Mark Charlebois + */ +#include +#include +#include +#include "muorb_test_example.h" +#include +#include "uORB/uORBManager.hpp" +#include "uORBKraitFastRpcChannel.hpp" + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "muorb_test"); + + PX4_DEBUG("muorb_test"); + + // register the fast rpc channel with UORB. + uORB::Manager::get_instance()->set_uorb_communicator( uORB::KraitFastRpcChannel::GetInstance() ); + + // start the KaitFastRPC channel thread. + uORB::KraitFastRpcChannel::GetInstance()->Start(); + + MuorbTestExample hello; + hello.main(); + + uORB::KraitFastRpcChannel::GetInstance()->Stop(); + PX4_DEBUG("goodbye"); + return 0; +} diff --git a/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp b/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp new file mode 100644 index 0000000000..943605f531 --- /dev/null +++ b/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hello_start_linux.cpp + * + * @author Thomas Gubler + * @author Mark Charlebois + */ +#include "muorb_test_example.h" +#include +#include +#include +#include +#include +#include + +static int daemon_task; /* Handle of deamon task / thread */ + +//using namespace px4; + +extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]); + +static void usage() +{ + PX4_DEBUG("usage: muorb_test {start|stop|status}"); +} +int muorb_test_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("already running"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("muorb_test", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char* const*)argv); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + MuorbTestExample::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("is running"); + + } else { + PX4_DEBUG("not started"); + } + + return 0; + } + + usage(); + return 1; +} diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index a0d7f07bce..2adfd40f9f 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -52,11 +52,21 @@ } #if defined(__PX4_QURT) #include +#include "HAP_farf.h" -#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__); -#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__); -#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__); -#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__); +//#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__); +//#define PX4_DEBUG(...) __px4_log("DEBUG", __VA_ARGS__); +//#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__); +//#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__); +//#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__); +#define PX4_DEBUG(...) FARF(HIGH, __VA_ARGS__); +#define PX4_INFO(...) FARF(HIGH, __VA_ARGS__); +#define PX4_WARN(...) FARF(HIGH, __VA_ARGS__); +#define PX4_ERR(...) FARF(HIGH, __VA_ARGS__); + +//#define PX4_INFO(...) __px4_log_omit("INFO", __VA_ARGS__); +//#define PX4_WARN(...) __px4_log_omit("WARN", __VA_ARGS__); +//#define PX4_ERR(...) __px4_log_omit("ERROR", __VA_ARGS__); #elif defined(__PX4_LINUX) #include diff --git a/src/platforms/qurt/dspal/dspal_stub.c b/src/platforms/qurt/dspal/dspal_stub.c new file mode 100644 index 0000000000..311a352909 --- /dev/null +++ b/src/platforms/qurt/dspal/dspal_stub.c @@ -0,0 +1,32 @@ +#include +#include + +#define STACK_SIZE 0x8000 +static char __attribute__ ((aligned (16))) stack1[STACK_SIZE]; + +int main(int argc, char* argv[]) +{ + int ret = 0; + char *builtin[]={"libgcc.so", "libc.so", "libstdc++.so"}; + void *handle; + char *error; + void (*entry_function)() = NULL; + + printf("In DSPAL main\n"); + dlinit(3, builtin); +#if 0 + handle = dlopen ("libdspal_client.so", RTLD_LAZY); + if (!handle) { + printf("Error opening libdspal_client.so\n"); + return 1; + } + entry_function = dlsym(handle, "dspal_entry"); + if (((error = dlerror()) != NULL) || (entry_function == NULL)) { + printf("Error dlsym for dspal_entry"); + ret = 2; + } + dlclose(handle); +#endif + return ret; +} + diff --git a/src/platforms/qurt/px4_layer/commands_muorb_test.c b/src/platforms/qurt/px4_layer/commands_muorb_test.c new file mode 100644 index 0000000000..e594b9dad8 --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_muorb_test.c @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file commands_muorb_test.c + * Commands to run for the "qurt_muorb_test" config + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + static const char *commands = + "uorb start\n" + "muorb_test start\n"; + + return commands; +} diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 538b64229f..864f1d22c2 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -131,14 +132,18 @@ namespace px4 { extern void init_once(void); }; -int main(int argc, char **argv) +__BEGIN_DECLS +void dspal_entry() { + const char *argv[2] = { "dspal_client", NULL }; + int argc = 1; + printf("In main\n"); map apps; init_app_map(apps); px4::init_once(); - px4::init(argc, argv, "mainapp"); + px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); - for (;;) {} + for (;;) { sleep(100000); } } - +__END_DECLS diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index 714263edcb..aa1a3b7d61 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -42,6 +42,7 @@ SRCS = \ px4_qurt_tasks.cpp \ work_thread.c \ work_queue.c \ + work_lock.c \ work_cancel.c \ lib_crc32.c \ drv_hrt.c \ @@ -52,12 +53,16 @@ SRCS = \ sq_remfirst.c \ sq_addafter.c \ dq_rem.c \ - main.cpp + main.cpp \ + qurt_stubs.c ifeq ($(CONFIG),qurt_hello) SRCS += commands_hello.c endif ifeq ($(CONFIG),qurt_default) SRCS += commands_default.c endif +ifeq ($(CONFIG),qurt_muorb_test) +SRCS += commands_muorb_test.c +endif MAXOPTIMIZATION = -Os diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 3ff09b3d4f..0987c6feb1 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -51,6 +51,7 @@ #include #include "systemlib/param/param.h" + __BEGIN_DECLS // FIXME - sysconf(_SC_CLK_TCK) not supported @@ -79,7 +80,7 @@ void qurt_log(const char *fmt, ...) } #endif -extern int _posix_init(void); +//extern int _posix_init(void); __END_DECLS @@ -94,7 +95,7 @@ void init_once(void); void init_once(void) { // Required for QuRT - _posix_init(); + //_posix_init(); work_queues_init(); hrt_init(); diff --git a/src/platforms/qurt/px4_layer/qurt_stubs.c b/src/platforms/qurt/px4_layer/qurt_stubs.c new file mode 100644 index 0000000000..67955e5121 --- /dev/null +++ b/src/platforms/qurt/px4_layer/qurt_stubs.c @@ -0,0 +1,57 @@ + +//extern "C" { + +void _Read_uleb( void ) +{ +} + +void _Parse_fde_instr( void ) +{ +} + +void _Parse_csd( void ) +{ +} + +void _Locksyslock( void ) +{ +} + +void _Unlocksyslock( void ) +{ +} + +void _Valbytes( void ) +{ +} + +void _Get_eh_data( void ) +{ +} + +void _Parse_lsda( void ) +{ +} + +void __cxa_guard_release( void ) +{ +} + +void _Read_enc_ptr( void ) +{ +} + +void _Read_sleb( void ) +{ +} + +void __cxa_guard_acquire( void ) +{ +} + +void __cxa_pure_virtual() +{ + while (1); +} + +//} diff --git a/src/platforms/qurt/px4_layer/work_lock.c b/src/platforms/qurt/px4_layer/work_lock.c new file mode 100644 index 0000000000..b2ad307d7c --- /dev/null +++ b/src/platforms/qurt/px4_layer/work_lock.c @@ -0,0 +1,19 @@ +//#pragma once +#include +#include +#include "work_lock.h" + + +extern sem_t _work_lock[]; + +void work_lock(int id) +{ + //printf("work_lock %d\n", id); + sem_wait(&_work_lock[id]); +} + +void work_unlock(int id) +{ + //printf("work_unlock %d\n", id); + sem_post(&_work_lock[id]); +} diff --git a/src/platforms/qurt/px4_layer/work_lock.h b/src/platforms/qurt/px4_layer/work_lock.h index f77f228b36..ad2e5b4a01 100644 --- a/src/platforms/qurt/px4_layer/work_lock.h +++ b/src/platforms/qurt/px4_layer/work_lock.h @@ -31,23 +31,13 @@ * ****************************************************************************/ -#include -#include +#ifndef _work_lock_h_ +#define _work_lock_h_ -#pragma once -extern sem_t _work_lock[]; -inline void work_lock(int id); -inline void work_lock(int id) -{ - //printf("work_lock %d\n", id); - sem_wait(&_work_lock[id]); -} +//#pragma once -inline void work_unlock(int id); -inline void work_unlock(int id) -{ - //printf("work_unlock %d\n", id); - sem_post(&_work_lock[id]); -} +void work_lock(int id); +void work_unlock(int id); +#endif // _work_lock_h_ diff --git a/src/platforms/qurt/tests/muorb/module.mk b/src/platforms/qurt/tests/muorb/module.mk new file mode 100644 index 0000000000..128d894f87 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = muorb_test + +SRCS = muorb_test_main.cpp \ + muorb_test_start_qurt.cpp \ + muorb_test_example.cpp + diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp new file mode 100644 index 0000000000..fdf46c6260 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hello_example.cpp + * Example for Linux + * + * @author Mark Charlebois + */ + +#include "muorb_test_example.h" +#include +#include +#include + +px4::AppState MuorbTestExample::appState; + +int MuorbTestExample::main() +{ + appState.setRunning(true); + + int i=0; + while (!appState.exitRequested() && i<5) { + + PX4_DEBUG(" Doing work..."); + ++i; + } + + return 0; +} diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.h b/src/platforms/qurt/tests/muorb/muorb_test_example.h new file mode 100644 index 0000000000..d330233b37 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hello_example.h + * Example app for Linux + * + * @author Mark Charlebois + */ +#pragma once + +#include + +class MuorbTestExample { +public: + MuorbTestExample() {}; + + ~MuorbTestExample() {}; + + int main(); + + static px4::AppState appState; /* track requests to terminate app */ +}; diff --git a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp new file mode 100644 index 0000000000..5e3ad9bb22 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hello_main.cpp + * Example for Linux + * + * @author Mark Charlebois + */ +#include +#include +#include +#include "muorb_test_example.h" +#include + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "muorb_test"); + + PX4_DEBUG("muorb_test"); + MuorbTestExample hello; + hello.main(); + + PX4_DEBUG("goodbye"); + return 0; +} diff --git a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp new file mode 100644 index 0000000000..943605f531 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hello_start_linux.cpp + * + * @author Thomas Gubler + * @author Mark Charlebois + */ +#include "muorb_test_example.h" +#include +#include +#include +#include +#include +#include + +static int daemon_task; /* Handle of deamon task / thread */ + +//using namespace px4; + +extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]); + +static void usage() +{ + PX4_DEBUG("usage: muorb_test {start|stop|status}"); +} +int muorb_test_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("already running"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("muorb_test", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char* const*)argv); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + MuorbTestExample::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("is running"); + + } else { + PX4_DEBUG("not started"); + } + + return 0; + } + + usage(); + return 1; +} diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index ae87cfca7a..810e0a2600 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -1,18 +1,34 @@ cmake_minimum_required(VERSION 2.8) + +include( CMakeForceCompiler ) +#set( CMAKE_SYSTEM_NAME px4_posix_clang ) +CMAKE_FORCE_C_COMPILER( clang Clang ) +CMAKE_FORCE_CXX_COMPILER( clang++ Clang ) +#set( CMAKE_C_COMPILER /opt/clang-3.4.2/bin/clang ) +#set( CMAKE_CXX_COMPILER /opt/clang-3.4.2/bin/clang++ ) +#set( CMAKE_FIND_ROOT_PATH /opt/clang-3.4.2/ ) +#set( CMAKE_FIND_ROOT_PATH_MODE_PROGRAM_NEVER ) +#set( CMAKE_FIND_ROOT_PATH_MODE_LIBARARY_ONLY ) +#set( CMAKE_FIND_ROOT_PATH_MODE_INCLUDE_ONLY ) + project(unittests) enable_testing() + include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g -Wall") elseif(COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g -Wall") else() message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") endif() -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g") + +#set(CMAKE_INCLUDE_SYSTEM_FLAG_C "-isystem" ) +#set(CMAKE_INCLUDE_SYSTEM_FLAG_CXX "-isystem" ) set(GTEST_DIR gtest) add_subdirectory(${GTEST_DIR}) @@ -22,9 +38,14 @@ set(PX_SRC ${CMAKE_SOURCE_DIR}/../src) include_directories(${CMAKE_SOURCE_DIR}) include_directories(${PX_SRC}) include_directories(${PX_SRC}/modules) +include_directories(${PX_SRC}/modules/uORB) include_directories(${PX_SRC}/lib) include_directories(${PX_SRC}/drivers) include_directories(${PX_SRC}/platforms) +include_directories(${PX_SRC}/platforms/posix/include) +include_directories(${PX_SRC}/platforms/posix/px4_layer) +include_directories(${PX_SRC}/drivers/device ) + add_definitions(-D__EXPORT=) add_definitions(-D__PX4_TESTS) @@ -35,18 +56,48 @@ add_definitions(-DOK=0) add_definitions(-D_UNIT_TEST=) add_definitions(-D__PX4_POSIX) add_definitions(-D__PX4_LINUX) +add_definitions(-D__PX4_POSIX) # check add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) function(add_gtest) foreach(test_name ${ARGN}) - target_link_libraries(${test_name} gtest_main) + target_link_libraries(${test_name} gtest_main pthread rt ) add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) add_dependencies(unittests ${test_name}) endforeach() endfunction() +add_library( px4_platform +# ${PX_SRC}/platforms/common/px4_getopt.c + ${PX_SRC}/platforms/posix/px4_layer/px4_posix_impl.cpp + ${PX_SRC}/platforms/posix/px4_layer/px4_posix_tasks.cpp + ${PX_SRC}/platforms/posix/px4_layer/work_lock.c + ${PX_SRC}/platforms/posix/px4_layer/work_queue.c + ${PX_SRC}/platforms/posix/px4_layer/dq_rem.c + ${PX_SRC}/platforms/posix/px4_layer/sq_addlast.c + ${PX_SRC}/platforms/posix/px4_layer/lib_crc32.c + ${PX_SRC}/platforms/posix/px4_layer/sq_addafter.c + ${PX_SRC}/platforms/posix/px4_layer/queue.c + ${PX_SRC}/platforms/posix/px4_layer/work_cancel.c + ${PX_SRC}/platforms/posix/px4_layer/dq_remfirst.c + ${PX_SRC}/platforms/posix/px4_layer/work_thread.c + ${PX_SRC}/platforms/posix/px4_layer/drv_hrt.c + ${PX_SRC}/platforms/posix/px4_layer/sq_remfirst.c + ${PX_SRC}/platforms/posix/px4_layer/dq_addlast.c + ${PX_SRC}/drivers/device/device_posix.cpp + ${PX_SRC}/drivers/device/vdev.cpp + ${PX_SRC}/drivers/device/vfile.cpp + ${PX_SRC}/drivers/device/vdev_posix.cpp + ${PX_SRC}/drivers/device/i2c_posix.cpp + ${PX_SRC}/drivers/device/sim.cpp + ${PX_SRC}/drivers/device/ringbuffer.cpp + ) + +#target_include_directories( px4_platform PUBLIC ${PX_SRC}/platforms ) + + # add each test add_executable(autodeclination_test autodeclination_test.cpp ${PX_SRC}/lib/geo_lookup/geo_mag_declination.c) @@ -64,6 +115,10 @@ add_executable(mixer_test mixer_test.cpp hrt.cpp ${PX_SRC}/modules/systemlib/mixer/mixer_simple.cpp ${PX_SRC}/modules/systemlib/pwm_limit/pwm_limit.c ${PX_SRC}/systemcmds/tests/test_mixer.cpp) + +target_link_libraries( mixer_test LINK_PUBLIC px4_platform ) + + add_gtest(mixer_test) # conversion_test @@ -93,4 +148,20 @@ add_executable(param_test param_test.cpp ${PX_SRC}/modules/systemlib/param/param.c ${PX_SRC}/modules/systemlib/bson/tinybson.c ) +target_link_libraries( param_test px4_platform ) + add_gtest(param_test) + +# uorb test +add_executable(uorb_tests uorb_unittests/uORBCommunicator_gtests.cpp + uorb_unittests/uORBCommunicatorMock.cpp + uorb_unittests/uORBCommunicatorMockLoopback.cpp + ${PX_SRC}/modules/uORB/uORBDevices_posix.cpp + ${PX_SRC}/modules/uORB/uORBManager_posix.cpp + ${PX_SRC}/modules/uORB/objects_common.cpp + ${PX_SRC}/modules/uORB/uORBUtils.cpp + ${PX_SRC}/modules/uORB/uORB.cpp + ) +target_link_libraries( uorb_tests px4_platform ) + +add_gtest(uorb_tests) diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.cpp b/unittests/uorb_unittests/uORBCommunicatorMock.cpp new file mode 100644 index 0000000000..118bd95202 --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicatorMock.cpp @@ -0,0 +1,206 @@ +//============================================================================= +// File: uORB_test.cpp +// +// @@-COPYRIGHT-START-@@ +// +// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. +// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") +// +// The party receiving this software directly from QTI (the "Recipient") +// may use this software as reasonably necessary solely for the purposes +// set forth in the agreement between the Recipient and QTI (the +// "Agreement"). The software may be used in source code form solely by +// the Recipient's employees (if any) authorized by the Agreement. Unless +// expressly authorized in the Agreement, the Recipient may not sublicense, +// assign, transfer or otherwise provide the source code to any third +// party. Qualcomm Technologies, Inc. retains all ownership rights in and +// to the software +// +// This notice supersedes any other QTI notices contained within the software +// except copyright notices indicating different years of publication for +// different portions of the software. This notice does not supersede the +// application of any third party copyright notice to that third party's +// code. +// +// @@-COPYRIGHT-END-@@ +// +//============================================================================= + +#include "uORBCommunicatorMock.hpp" +#include "uORB/uORB.h" +#include "uORBGtestTopics.hpp" +#include "px4_log.h" +#include +#include "uORBManager.hpp" + +#define LOG_TAG "uORBCommunicatorMock.cpp" + +uORB_test::uORBCommunicatorMock::uORBCommunicatorMock() +: _rx_handler( nullptr ) +{ +/* + _sub_topicA_copy_fd = orb_subscribe( ORB_ID( topicA_copy ), (void*)&_sub_semaphore ); + _sub_topicB_copy_fd = orb_subscribe( ORB_ID( topicB_copy), nullptr ); +*/ + _topic_translation_map[ "topicA" ] = "topicA_copy"; + _topic_translation_map[ "topicB" ] = "topicB_copy"; + _topic_translation_map[ "topicA_copy" ] = "topicA"; + _topic_translation_map[ "topicB_copy" ] = "topicB"; +} + +int16_t uORB_test::uORBCommunicatorMock::add_subscription +( + const std::string& messageName, + int32_t msgRateInHz +) +{ + + int16_t rc = 0; + PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName.c_str(), msgRateInHz ); + _msgCounters[messageName]._add_subscriptionCount++; + + /* + int16_t rc = -1; + if( _rx_handler ) + { + if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) + { + rc = _rx_handler->process_add_subscription + ( + _topic_translation_map[messageName], + msgRateInHz + ); + } + } + */ + return rc; +} + +int16_t uORB_test::uORBCommunicatorMock::remove_subscription +( + const std::string& messageName +) +{ + int16_t rc = 0; + PX4_INFO( "got remove_subscription for msg[%s]", messageName.c_str() ); + _msgCounters[messageName]._remove_subscriptionCount++; +/* + int16_t rc = -1; + if( _rx_handler ) + { + if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) + { + rc = _rx_handler->process_remove_subscription + ( + _topic_translation_map[messageName] + ); + } + } +*/ + return rc; +} + +int16_t uORB_test::uORBCommunicatorMock::register_handler +( + uORBCommunicator::IChannelRxHandler* handler +) +{ + int16_t rc = 0; + _rx_handler = handler; + return rc; +} + + +int16_t uORB_test::uORBCommunicatorMock::send_message +( + const std::string& messageName, + int32_t length, + uint8_t* data +) +{ + int16_t rc = 0; + PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName.c_str(), length ); + if( uORB::Manager::get_instance()->is_remote_subscriber_present( messageName ) ) + { + _msgCounters[messageName]._send_messageCount++; + if( messageName == "topicA" ) + { + memcpy( &_topicAData, (void*)data, length ); + } + else if( messageName == "topicB" ) + { + memcpy( &_topicBData, (void*)data, length ); + } + else + { + //EPRINTF( "error messageName[%s] is not supported", messageName.c_str() ); + } + } +/* + int16_t rc = -1; + if( _rx_handler ) + { + if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) + { + rc = _rx_handler->process_received_message + ( _topic_translation_map[messageName], length, data ); + } + } +*/ + return rc; +} + + +bool uORB_test::uORBCommunicatorMock::get_remote_topicA_data +( + struct orb_topic_A* data +) +{ + bool rc = false; + memcpy( data, &_topicAData, sizeof(_topicAData) ); + rc = true; +/* + if( orb_copy(ORB_ID(topicA_copy), _sub_topicA_copy_fd, data) == OK ) + { + rc = true; + } +*/ + return rc; +} + +bool uORB_test::uORBCommunicatorMock::get_remote_topicB_data +( + struct orb_topic_B* data +) +{ + bool rc = false; + memcpy( data, &_topicBData, sizeof(_topicBData) ); + rc = true; +/* + + if( orb_copy(ORB_ID(topicB_copy), _sub_topicB_copy_fd, data) == OK ) + { + rc = true; + } +*/ + return rc; +} + +void uORB_test::uORBCommunicatorMock::reset_counters() +{ + InterfaceCounters resetCounter; + resetCounter._add_subscriptionCount = 0; + resetCounter._remove_subscriptionCount = 0; + resetCounter._send_messageCount = 0; + + std::map::iterator it; + for( it = _msgCounters.begin(); it != _msgCounters.end(); ++it ) + { + it->second = resetCounter; + } +} + +uORB_test::uORBCommunicatorMock::InterfaceCounters uORB_test::uORBCommunicatorMock::get_interface_counters(const std::string& messageName ) +{ + return _msgCounters[ messageName ]; +} diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.hpp b/unittests/uorb_unittests/uORBCommunicatorMock.hpp new file mode 100644 index 0000000000..9279d6446d --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicatorMock.hpp @@ -0,0 +1,135 @@ +//============================================================================= +// File: uORB_test.cpp +// +// @@-COPYRIGHT-START-@@ +// +// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. +// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") +// +// The party receiving this software directly from QTI (the "Recipient") +// may use this software as reasonably necessary solely for the purposes +// set forth in the agreement between the Recipient and QTI (the +// "Agreement"). The software may be used in source code form solely by +// the Recipient's employees (if any) authorized by the Agreement. Unless +// expressly authorized in the Agreement, the Recipient may not sublicense, +// assign, transfer or otherwise provide the source code to any third +// party. Qualcomm Technologies, Inc. retains all ownership rights in and +// to the software +// +// This notice supersedes any other QTI notices contained within the software +// except copyright notices indicating different years of publication for +// different portions of the software. This notice does not supersede the +// application of any third party copyright notice to that third party's +// code. +// +// @@-COPYRIGHT-END-@@ +// +//============================================================================= + +#ifndef _uORBCommunicatorMock_hpp_ +#define _uORBCommunicatorMock_hpp_ + +#include "uORB/uORBCommunicator.hpp" +#include "uORBGtestTopics.hpp" +#include +#include + +namespace uORB_test +{ + class uORBCommunicatorMock; +} + +class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel +{ + public: + + //counters to track how many times the iterface is called from + // uorb. + typedef struct + { + int64_t _add_subscriptionCount; + int64_t _remove_subscriptionCount; + int64_t _send_messageCount; + }InterfaceCounters; + + uORBCommunicatorMock(); + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription( const std::string& messageName, int32_t msgRateInHz ); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription( const std::string& messageName ); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler( uORBCommunicator::IChannelRxHandler* handler ); + + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message( const std::string& messageName, int32_t length, uint8_t* data); + + uORBCommunicator::IChannelRxHandler* get_rx_handler() + { + return _rx_handler; + } + + bool get_remote_topicA_data( struct orb_topic_A* data ); + bool get_remote_topicB_data( struct orb_topic_B* data ); + + void reset_counters(); + + InterfaceCounters get_interface_counters( const std::string& messageName ); + + + private: + uORBCommunicator::IChannelRxHandler* _rx_handler; + int _sub_topicA_copy_fd; + int _sub_topicB_copy_fd; + + std::map _topic_translation_map; + + struct orb_topic_A _topicAData; + struct orb_topic_B _topicBData; + + std::map _msgCounters; +}; + +#endif /* _uORBCommunicatorMock_test_hpp_ */ diff --git a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp new file mode 100644 index 0000000000..e52d1d58f8 --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp @@ -0,0 +1,133 @@ +//============================================================================= +// File: uORB_test.cpp +// +// @@-COPYRIGHT-START-@@ +// +// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. +// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") +// +// The party receiving this software directly from QTI (the "Recipient") +// may use this software as reasonably necessary solely for the purposes +// set forth in the agreement between the Recipient and QTI (the +// "Agreement"). The software may be used in source code form solely by +// the Recipient's employees (if any) authorized by the Agreement. Unless +// expressly authorized in the Agreement, the Recipient may not sublicense, +// assign, transfer or otherwise provide the source code to any third +// party. Qualcomm Technologies, Inc. retains all ownership rights in and +// to the software +// +// This notice supersedes any other QTI notices contained within the software +// except copyright notices indicating different years of publication for +// different portions of the software. This notice does not supersede the +// application of any third party copyright notice to that third party's +// code. +// +// @@-COPYRIGHT-END-@@ +// +//============================================================================= + +#include "uORBCommunicatorMockLoopback.hpp" +#include "uORB/uORB.h" +#include "uORBGtestTopics.hpp" +#include "uORBManager.hpp" +#include +#include "px4_log.h" + +#define LOG_TAG "uORBCommunicatorMockLoopback.cpp" + + +uORB_test::uORBCommunicatorMockLoopback::uORBCommunicatorMockLoopback() +: _rx_handler( nullptr ) +{ + //_sub_topicA_clone_fd = orb_subscribe( ORB_ID( topicA_clone ), (void*)&_sub_semaphore ); + //_sub_topicB_clone_fd = orb_subscribe( ORB_ID( topicB_clone ), nullptr ); + + _topic_translation_map[ "topicA" ] = "topicA_clone"; + _topic_translation_map[ "topicB" ] = "topicB_clone"; + _topic_translation_map[ "topicA_clone" ] = "topicA"; + _topic_translation_map[ "topicB_clone" ] = "topicB"; +} + +int16_t uORB_test::uORBCommunicatorMockLoopback::add_subscription +( + const std::string& messageName, + int32_t msgRateInHz +) +{ + + int16_t rc = -1; + PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName.c_str(), msgRateInHz ); + + if( _rx_handler ) + { + if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) + { + rc = _rx_handler->process_add_subscription + ( + _topic_translation_map[messageName], + msgRateInHz + ); + } + } + return rc; +} + +int16_t uORB_test::uORBCommunicatorMockLoopback::remove_subscription +( + const std::string& messageName +) +{ + int16_t rc = -1; + PX4_INFO( "got remove_subscription for msg[%s]", messageName.c_str() ); + if( _rx_handler ) + { + if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) + { + rc = _rx_handler->process_remove_subscription + ( + _topic_translation_map[messageName] + ); + } + } + return rc; +} + +int16_t uORB_test::uORBCommunicatorMockLoopback::register_handler +( + uORBCommunicator::IChannelRxHandler* handler +) +{ + int16_t rc = 0; + _rx_handler = handler; + return rc; +} + + +int16_t uORB_test::uORBCommunicatorMockLoopback::send_message +( + const std::string& messageName, + int32_t length, + uint8_t* data +) +{ + int16_t rc = -1; + PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName.c_str(), length ); + if( _rx_handler ) + { + if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) + { + if( uORB::Manager::get_instance()->is_remote_subscriber_present( _topic_translation_map[messageName] ) ) + { + rc = _rx_handler->process_received_message + ( _topic_translation_map[messageName], length, data ); + PX4_INFO( "[uORBCommunicatorMockLoopback::send_message] return from[topic(%s)] _rx_handler->process_received_message[%d]", messageName.c_str(), rc ); + } + else + { + // this is eqvuilanet of not sending the message to the remote. + rc = 0; + } + } + } + return rc; +} diff --git a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp new file mode 100644 index 0000000000..2caef3977e --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp @@ -0,0 +1,127 @@ +//============================================================================= +// File: uORB_test.cpp +// +// @@-COPYRIGHT-START-@@ +// +// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. +// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") +// +// The party receiving this software directly from QTI (the "Recipient") +// may use this software as reasonably necessary solely for the purposes +// set forth in the agreement between the Recipient and QTI (the +// "Agreement"). The software may be used in source code form solely by +// the Recipient's employees (if any) authorized by the Agreement. Unless +// expressly authorized in the Agreement, the Recipient may not sublicense, +// assign, transfer or otherwise provide the source code to any third +// party. Qualcomm Technologies, Inc. retains all ownership rights in and +// to the software +// +// This notice supersedes any other QTI notices contained within the software +// except copyright notices indicating different years of publication for +// different portions of the software. This notice does not supersede the +// application of any third party copyright notice to that third party's +// code. +// +// @@-COPYRIGHT-END-@@ +// +//============================================================================= + +#ifndef _uORBCommunicatorMockLoopback_hpp_ +#define _uORBCommunicatorMockLoopback_hpp_ + +#include "uORB/uORBCommunicator.hpp" +#include "uORBGtestTopics.hpp" +#include +#include + +namespace uORB_test +{ + class uORBCommunicatorMockLoopback; +} + +class uORB_test::uORBCommunicatorMockLoopback : public uORBCommunicator::IChannel +{ + public: + + uORBCommunicatorMockLoopback(); + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription( const std::string& messageName, int32_t msgRateInHz ); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription( const std::string& messageName ); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler( uORBCommunicator::IChannelRxHandler* handler ); + + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message( const std::string& messageName, int32_t length, uint8_t* data); + + uORBCommunicator::IChannelRxHandler* get_rx_handler() + { + return _rx_handler; + } + +/* + bool get_remote_topicA_data( struct orb_topic_A* data ); + bool get_remote_topicB_data( struct orb_topic_B* data ); +*/ + + + private: + uORBCommunicator::IChannelRxHandler* _rx_handler; +/* + int _sub_topicA_clone_fd; + int _sub_topicB_clone_fd; + pal::Semaphore _sub_semaphore; +*/ + + std::map _topic_translation_map; + +/* + struct orb_topic_A _topicAData; + struct orb_topic_B _topicBData; +*/ +}; + +#endif /* _uORBCommunicatorMockLoopback_hpp_ */ diff --git a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp new file mode 100644 index 0000000000..86c5cdc90b --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp @@ -0,0 +1,481 @@ +//============================================================================= +// File: uORB_test.cpp +// +// @@-COPYRIGHT-START-@@ +// +// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. +// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") +// +// The party receiving this software directly from QTI (the "Recipient") +// may use this software as reasonably necessary solely for the purposes +// set forth in the agreement between the Recipient and QTI (the +// "Agreement"). The software may be used in source code form solely by +// the Recipient's employees (if any) authorized by the Agreement. Unless +// expressly authorized in the Agreement, the Recipient may not sublicense, +// assign, transfer or otherwise provide the source code to any third +// party. Qualcomm Technologies, Inc. retains all ownership rights in and +// to the software +// +// This notice supersedes any other QTI notices contained within the software +// except copyright notices indicating different years of publication for +// different portions of the software. This notice does not supersede the +// application of any third party copyright notice to that third party's +// code. +// +// @@-COPYRIGHT-END-@@ +// +//============================================================================= + +#include "uORBCommunicatorMock.hpp" +#include "uORBCommunicatorMockLoopback.hpp" +#include "gtest/gtest.h" +#include "uORB.h" +#include "uORBManager.hpp" +#include "uORBGtestTopics.hpp" +#include "uORBDevices.hpp" +#include "px4_log.h" +#include + +#define LOG_TAG "uORBCommunicator_gtests.cpp" + +namespace px4 +{ + void init_once(); +} + +namespace uORB_test +{ + + class uORBCommunicatorTest : public ::testing::Test + { + public: + uORBCommunicatorTest() : + _masterDevice( nullptr ) + { + px4::init_once(); + + // create the Master Device and initialize it + _masterDevice = new uORB::DeviceMaster(uORB::PUBSUB); + if( _masterDevice != nullptr ) + { + _masterDevice->init(); + } + + // get the uORB::Manager and set the mock instance + // explicitly. + uORB::Manager::get_instance()->set_uorb_communicator( &_comm_channel ); + } + protected: + uORB_test::uORBCommunicatorMock _comm_channel; + struct orb_topic_A _topicA; + struct orb_topic_B _topicB; + int _pub_fd; + int _sub_fd; + uORB::DeviceMaster* _masterDevice; + }; + + //================= Unit tests for add_subscription =================== + // this test will validate if the uORB calls the "add_subscription" + // method if there is atleast one subscriber in the local system. + //===================================================================== + TEST_F( uORBCommunicatorTest, add_subscription_case1 ) + { + // case 1: add subscription should not be called if there are no + // internal subscriber and only a publisher. + // Steps: + // 0. reset the interface counters. + // 1. advertize a topic + // 2. check to see if add_subscription is called. + // the count should be zero. + + //step 0. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._add_subscriptionCount, 0 ); + + //step 1. + // step 1. + _topicA.val = 1; + ASSERT_NE( ( _pub_fd = orb_advertise(ORB_ID(topicA), &_topicA) ), 0 ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + + //step 2. + c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._add_subscriptionCount, 0 ); + } + + TEST_F( uORBCommunicatorTest, add_subscription_case2 ) + { + // case 1: add subscription should not be called if there is atleast one + // internal subscriber and only a publisher. + // Steps: + // 0. reset the interface counters. + // 1. advertize a topic + // 2. Add in internal subscriber. + // 3. check to see if add_subscription is called. + // the count should be 1. + // 4. unsubscribe the topic + + //step 0. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._add_subscriptionCount, 0 ); + + //step 1. + // step 1. + _topicA.val = 1; + _pub_fd = orb_advertise(ORB_ID(topicA), &_topicA); + PX4_INFO( "[uORBCommunicatorTest.add_subscription_case2] orb_advertize(topicA) returncode:(%d)", _pub_fd ); + ASSERT_TRUE( ( _pub_fd != -1 && _pub_fd != 0 ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + + // step 2 + ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicA) ) ) , -1 ) << "Subscribe failed: %d" << errno; + PX4_INFO( "subscribe fd: %d", _sub_fd ); + + //step 3. + c = _comm_channel.get_interface_counters( "topicA" ); + PX4_INFO( "interface counter for topicA: %d", (int)c._add_subscriptionCount ); + ASSERT_EQ( c._add_subscriptionCount, 1 ); + + //step 4. + PX4_INFO( "Before unsubscribe for Topic A" ); + ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK ); + PX4_INFO( "After unsubscribe for Topic A" ); + } + + //============ unit tests for remove_subscribtion ======= + TEST_F( uORBCommunicatorTest, remove_subscribtion ) + { + // remove subscription should be called if there after a subscription is removed. + // Steps: + // 0. reset the interface counters. + // 1. advertize a topic + // 2. Add in internal subscriber. + // 3. unsubscribe the topic + // 4. check the remove_subsciption counter it should be 1. + + //step 0. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._remove_subscriptionCount, 0 ); + + //step 1. + // step 1. + _topicA.val = 1; + _pub_fd = orb_advertise(ORB_ID(topicA), &_topicA); + PX4_INFO( "[uORBCommunicatorTest.remove_subscribtion] orb_advertize(topicA) returncode:(%d)", _pub_fd ); + ASSERT_TRUE( ( _pub_fd != -1 && _pub_fd != 0 ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + + c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._remove_subscriptionCount, 0 ); + + + // step 2 + ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicA) )), -1 ); // << "Subscribe failed: " << _sub_fd << "errono: " << errno; + PX4_INFO( "subscribe fd: %d", _sub_fd ); + + c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._remove_subscriptionCount, 0 ); + + + //step 3. + ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK ); + + //step 4. + c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._remove_subscriptionCount, 1 ); + } + + //============ unit tests for send_message ======= + TEST_F(uORBCommunicatorTest, send_message_case1 ) + { + // Case1: send message should not be called when a topic is advertized + // and there are no remote remote subscribers. + // Steps: + // 0. reset the interface counters. + // 1. advertize a topic + // 2. check to see that the send message counter is 0. + //step 0. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA_sndmsg" ); + ASSERT_EQ( c._send_messageCount, 0 ); + + //step 1. + // step 1. + ORB_DEFINE( topicA_sndmsg, struct orb_topic_A ); + _topicA.val = 1; + _pub_fd = orb_advertise(ORB_ID(topicA_sndmsg ), &_topicA ); + ASSERT_TRUE( ( _pub_fd != -1 && _pub_fd != 0 ) ) << "Failed to advertize uORB Topic topicA_sndmsg: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + + c = _comm_channel.get_interface_counters( "topicA_sndmsg" ); + ASSERT_EQ( c._send_messageCount, 0 ); + } + + TEST_F(uORBCommunicatorTest, send_message_case2 ) + { + // Case2: send message should be called when a topic is advertized + // and there are remote remote subscribers. + // Steps: + // 0. reset the interface counters. + // 1. Add a remote subscriber by calling the "process_add_subscription" + // 2. advertize a topic + // 3. check to see that the send message counter is 1 and check the value. + // 4. publish new data. + // 5. check to see that send_msg is incremented by 1 and check the value. + // 6. remove remote subscriber by calling the "process_remove_subscription" + // 7. publish new data. + // 8. check to see the sndmessage counter, it should not increment. + + //step 0. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 0 ); + + //step 1. + uORBCommunicator::IChannelRxHandler* rx_handler = _comm_channel.get_rx_handler(); + // add a local subsciber to avoid the issue of creating a node for the first time + // remote. + ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB) ) ), -1 ) << "Subscribe failed for topicB: %d" << errno; + PX4_INFO( "subscribe fd: %d", _sub_fd ); + ASSERT_EQ( rx_handler->process_add_subscription( "topicB", 1 ), 0 ); + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 0 ); + + // step 2. + _topicB.val = 1; + _pub_fd = orb_advertise(ORB_ID(topicB), &_topicB); + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + ASSERT_TRUE( _pub_fd != -1 && _pub_fd != 0 ) << "Failed to advertize uORB Topic topicB: errno: " << errno; + + //step 3. + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 1 ); + + struct orb_topic_B test_val; + _comm_channel.get_remote_topicB_data( &test_val ); + ASSERT_EQ( test_val.val, 1 ); + + //step 4. publish new data. + _topicB.val = 2; + ASSERT_EQ( orb_publish( ORB_ID(topicB), _pub_fd, &_topicB), OK ); + + //step 5. + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 2 ); + + _comm_channel.get_remote_topicB_data( &test_val ); + ASSERT_EQ( test_val.val, 2 ); + + // step 6. + ASSERT_EQ( rx_handler->process_remove_subscription( "topicB" ), 0 ); + ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK ); + + //step 7. publish new data. + _topicB.val = 5; + ASSERT_EQ( orb_publish( ORB_ID(topicB), _pub_fd, &_topicB), OK ); + + //step 8. + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 2 ); + + _comm_channel.get_remote_topicB_data( &test_val ); + ASSERT_EQ( test_val.val, 2 ); + + } + + //========== UNIT tests to verify the process_receive_message interface + //========== of rx handler. + TEST_F( uORBCommunicatorTest, rx_handler_rcv_data_case1 ) + { + // this will mimic the process of calling the process_receive_message + // from remote and verify that the local subscribers got it + // are the steps. + // 1. clear the counters. + // 2. advertize a topic + // 3. add a local subscriber. + // 4. call process_receive_message. + // 5. check to see that the subscriber got the data and the message is not sent back + // by looking at the counter for snd message. + + //step 1. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 0 ); + + // step 2. + _topicB.val = 1; + _pub_fd = orb_advertise(ORB_ID(topicB), &_topicB); + ASSERT_TRUE( _pub_fd != -1 && _pub_fd != 0 ) << "Failed to advertize uORB Topic topicB: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + + // step 3. + ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB)) ), -1 ) << "Subscribe failed for topicB: %d" << errno; + PX4_INFO( "subscribe fd: %d", _sub_fd ); + + //step 4. + uORBCommunicator::IChannelRxHandler* rx_handler = _comm_channel.get_rx_handler(); + struct orb_topic_B testVal; + testVal.val = 10; + ASSERT_EQ( rx_handler->process_received_message( "topicB", sizeof( testVal ), (uint8_t*)(&testVal) ), 0 ); + + // step 5. check the values. + struct orb_topic_B receivedVal; + ASSERT_EQ( orb_copy(ORB_ID(topicB), _sub_fd, &receivedVal), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( testVal.val, receivedVal.val ) + << "copy(1) mismatch: " << testVal.val + << " expected " << receivedVal.val; + + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 0 ); + + // cleanup. + ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK ); + } + + TEST_F( uORBCommunicatorTest, rx_handler_rcv_data_case2 ) + { + // this will mimic the process of calling the process_receive_message + // from remote and verify that the local subscribers got it + // and also the message is send back to the remote. The following + // are the steps. + // 1. clear the counters. + // 2. advertize a topic + // 3. add a local subscriber. + // 4. add remote subscriber by calling the "process_add_subscription. + // 5. call process_receive_message. + // 6. check to see that the subscriber got the data and the message is not sent back + // by looking at the counter for snd message. + + //step 1. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 0 ); + + // step 2. + _topicB.val = 1; + _pub_fd = orb_advertise(ORB_ID(topicB), &_topicB); + ASSERT_TRUE( _pub_fd != -1 && _pub_fd != 0 ) << "Failed to advertize uORB Topic topicB: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + + // step 3. + ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB)) ), -1 ) << "Subscribe failed for topicB: %d" << errno; + PX4_INFO( "subscribe fd: %d", _sub_fd ); + + //step 4. + uORBCommunicator::IChannelRxHandler* rx_handler = _comm_channel.get_rx_handler(); + ASSERT_EQ( rx_handler->process_add_subscription( "topicB", 1 ), 0 ); + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 1 ); + + //step 5. + struct orb_topic_B testVal; + testVal.val = 15; + ASSERT_EQ( rx_handler->process_received_message( "topicB", sizeof( testVal ), (uint8_t*)(&testVal) ), 0 ); + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 1 ); + + + // step 6. check the values. + struct orb_topic_B receivedVal; + ASSERT_EQ( orb_copy(ORB_ID(topicB), _sub_fd, &receivedVal), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( testVal.val, receivedVal.val ) + << "copy(1) mismatch: " << testVal.val + << " expected " << receivedVal.val; + + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 1 ); + + // cleanup. + ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK ); + } + + TEST_F( uORBCommunicatorTest, Loopback ) + { + // create the loopback instance. + uORB_test::uORBCommunicatorMockLoopback _comm_channel_loopback; + + //intiailize the uorb to remove the node map entries; + //orb_initialize(); + + //set the communucation channel interface. + uORB::Manager::get_instance()->set_uorb_communicator( &_comm_channel_loopback ); + + // now for the actual test. + int pub_topicA_fd; + int sub_topicA_fd; + int sub_topicAClone_fd; + + struct orb_topic_A topicA; + struct orb_topic_A topicAClone; + struct orb_topic_A topicALocal; + + // step 1. + topicA.val = 10; + pub_topicA_fd = orb_advertise(ORB_ID(topicA), &topicA); + PX4_INFO( "[uORBCommunicatorTest.Loopback]orb_advertize(topicA) return code:(%d)", pub_topicA_fd ); + ASSERT_TRUE( pub_topicA_fd != -1 && pub_topicA_fd != 0 ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", pub_topicA_fd ); + + // step 2. + ASSERT_NE( ( sub_topicA_fd = orb_subscribe(ORB_ID(topicA)) ), -1 ) << "Subscribe failed: %d" << errno; + PX4_INFO( "subscribe fd: %d", sub_topicA_fd ); + + // step 3. check to see that the subscriber got a value of 10. + ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( topicA.val, topicALocal.val ) + << "copy(1) mismatch: " << topicA.val + << " expected " << topicALocal.val; + + // subscribe a remote subscriber. + ASSERT_NE( ( sub_topicAClone_fd = orb_subscribe(ORB_ID(topicA_clone)) ), -1 ) << "Subscribe failed: %d" << errno; + PX4_INFO( "subscribe fd: %d", sub_topicAClone_fd ); + + ASSERT_EQ( orb_copy(ORB_ID(topicA_clone), sub_topicAClone_fd, &topicAClone), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( topicA.val, topicAClone.val ) + << "copy(1) mismatch: " << topicA.val + << " expected " << topicAClone.val; + + // publish a new data and check to see that the data is received on the remote. + topicA.val = 15; + ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_fd, &topicA), OK ); + + // check to see that the subscriber got a new value. + ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( topicA.val, topicALocal.val ) + << "copy(1) mismatch: " << topicA.val + << " expected " << topicALocal.val; + + ASSERT_EQ( orb_copy(ORB_ID(topicA_clone), sub_topicAClone_fd, &topicAClone), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( topicA.val, topicAClone.val ) + << "copy(1) mismatch: " << topicA.val + << " expected " << topicAClone.val; + + // remove the remote subscriber and make sure that the data is not received, + ASSERT_EQ( orb_unsubscribe( sub_topicAClone_fd ), OK ); + + // publish a new data and check to see that the data is received on local this should not crash. + topicA.val = 20; + ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_fd, &topicA), OK ); + + // check to see that the subscriber got a new value. + ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( topicA.val, topicALocal.val ) + << "copy(1) mismatch: " << topicA.val + << " expected " << topicALocal.val; + + //remove the local subscriber as well and publish new data; system should not crash. + ASSERT_EQ( orb_unsubscribe( sub_topicA_fd ), OK ); + + // publish a new data; this should not crash. + topicA.val = 25; + ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_fd, &topicA), OK ); + + } +} + + + + diff --git a/unittests/uorb_unittests/uORBGtestTopics.hpp b/unittests/uorb_unittests/uORBGtestTopics.hpp new file mode 100644 index 0000000000..94822f54d5 --- /dev/null +++ b/unittests/uorb_unittests/uORBGtestTopics.hpp @@ -0,0 +1,54 @@ +//============================================================================= +// File: uORB_test.cpp +// +// @@-COPYRIGHT-START-@@ +// +// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. +// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") +// +// The party receiving this software directly from QTI (the "Recipient") +// may use this software as reasonably necessary solely for the purposes +// set forth in the agreement between the Recipient and QTI (the +// "Agreement"). The software may be used in source code form solely by +// the Recipient's employees (if any) authorized by the Agreement. Unless +// expressly authorized in the Agreement, the Recipient may not sublicense, +// assign, transfer or otherwise provide the source code to any third +// party. Qualcomm Technologies, Inc. retains all ownership rights in and +// to the software +// +// This notice supersedes any other QTI notices contained within the software +// except copyright notices indicating different years of publication for +// different portions of the software. This notice does not supersede the +// application of any third party copyright notice to that third party's +// code. +// +// @@-COPYRIGHT-END-@@ +// +//============================================================================= + +#ifndef _uORBGtestTopics_hpp_ +#define _uORBGtestTopics_hpp_ + +#include "uORB/uORB.h" + +namespace uORB_test +{ + struct orb_topic_A + { + int16_t val; + }; + + struct orb_topic_B + { + int16_t val; + }; + + + ORB_DEFINE( topicA, struct orb_topic_A ); + ORB_DEFINE( topicB, struct orb_topic_B ); + + ORB_DEFINE( topicA_clone, struct orb_topic_A ); + ORB_DEFINE( topicB_clone, struct orb_topic_B ); +} + +#endif // _UnitTest_uORBTopics_hpp_ diff --git a/unittests/uorb_unittests/uORB_test.cpp b/unittests/uorb_unittests/uORB_test.cpp new file mode 100644 index 0000000000..43ed366df0 --- /dev/null +++ b/unittests/uorb_unittests/uORB_test.cpp @@ -0,0 +1,206 @@ +//============================================================================= +// File: uORB_test.cpp +// +// @@-COPYRIGHT-START-@@ +// +// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. +// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") +// +// The party receiving this software directly from QTI (the "Recipient") +// may use this software as reasonably necessary solely for the purposes +// set forth in the agreement between the Recipient and QTI (the +// "Agreement"). The software may be used in source code form solely by +// the Recipient's employees (if any) authorized by the Agreement. Unless +// expressly authorized in the Agreement, the Recipient may not sublicense, +// assign, transfer or otherwise provide the source code to any third +// party. Qualcomm Technologies, Inc. retains all ownership rights in and +// to the software +// +// This notice supersedes any other QTI notices contained within the software +// except copyright notices indicating different years of publication for +// different portions of the software. This notice does not supersede the +// application of any third party copyright notice to that third party's +// code. +// +// @@-COPYRIGHT-END-@@ +// +//============================================================================= + +#include +#include +#include +#include +#include +#include +#include +#include "uORB/uORB.h" +#include "utils/general.h" +#include "utils/logging.h" + +#include + +#define LOG_TAG "uORB_test.cpp" + +//#ifdef INCLUDE_ANDROID_UNIT_TEST_UORB +namespace uORB_test +{ + + struct orb_test + { + int val; + }; + ORB_DEFINE(orb_test, struct orb_test); + + pal::Semaphore sub_semaphore; + static int pfd, sfd; + static struct orb_test t; + bool threadTestPassed = false; + + TEST( uORBTest, pub_sub_initialization ) + { + struct orb_test u; + bool updated; + + t.val = 0; + ASSERT_NE( ( pfd = orb_advertise(ORB_ID(orb_test), &t) ), 0 ) << "Failed to advertize uORB Topic orb_test: errno: " << errno; + IPRINTF( "publist handle: 0x%08x", pfd ); + + + ASSERT_NE( ( sfd = orb_subscribe(ORB_ID(orb_test), (void *)&sub_semaphore) ), 0 ) << "Subscribe failed: %d" << errno; + IPRINTF( "subscribe fd: %d", sfd ); + + u.val = 1; + + ASSERT_EQ( orb_copy(ORB_ID(orb_test), sfd, &u), OK ) << "copy(1) failed: " << errno; + + ASSERT_EQ( u.val, t.val ) << "copy(1) mismatch: " << u.val << " expected " << t.val; + + ASSERT_EQ(orb_check(sfd, &updated), OK ) << "check(1) failed"; + + ASSERT_FALSE( updated ) << "spurious updated flag"; + + ASSERT_EQ(orb_unsubscribe(sfd), OK ); + close(pfd); + } + + + void test_subscriber_thread(void *this_ptr) + { + struct orb_test u; + bool updated; + pal::Semaphore *sub_semaphore = (pal::Semaphore *)this_ptr; + assert(sub_semaphore != nullptr); + + IPRINTF( "waiting for new subscriber data"); + sub_semaphore->wait(); + + if (OK != orb_check(sfd, &updated)) + { + EPRINTF("check(2) failed"); + return; + } + + if (!updated) + { + EPRINTF("missing updated flag"); + return; + } + + if (OK != orb_copy(ORB_ID(orb_test), sfd, &u)) + { + EPRINTF("copy(2) failed: %d", errno); + return; + } + + if (u.val != t.val) + { + EPRINTF("copy(2) mismatch: %d expected %d", u.val, t.val); + return; + } + threadTestPassed = true; + } + + + TEST( uORB, pub_sub_withThread ) + { + pal::Thread sub_thread; + + threadTestPassed = false; + + // advertize the topic first if it is not created. + t.val = 1; + ASSERT_NE( (pfd = orb_advertise(ORB_ID(orb_test), &t) ), 0 ) << "Failed to advertize uORB Topic orb_test: errno: " << errno; + IPRINTF( "publist handle: 0x%08x", pfd ); + + ASSERT_NE( ( sfd = orb_subscribe(ORB_ID(orb_test), (void *)&sub_semaphore) ), 0 ) << "Subscribe failed: %d" << errno; + IPRINTF( "subscribe fd: %d", sfd ); + + sub_thread.create(test_subscriber_thread, (void *)&sub_semaphore); + + t.val = 2; + IPRINTF("try publish, creating new thread to await the results"); + + ASSERT_EQ(orb_publish(ORB_ID(orb_test), pfd, &t), OK) << "publish failed"; + + ASSERT_EQ( t.val, 2 ); + + IPRINTF("waiting for the subscriber thread to exit"); + sub_thread.wait(); + + ASSERT_TRUE( threadTestPassed ); + + ASSERT_EQ(orb_unsubscribe(sfd), OK ); + close(pfd); + } + + + +#if 0 + /* this is a hacky test that exploits the sensors app to test rate-limiting */ + + sfd = orb_subscribe(ORB_ID(sensor_combined)); + + hrt_abstime start, end; + unsigned count; + + start = hrt_absolute_time(); + count = 0; + + do + { + orb_check(sfd, &updated); + + if (updated) + { + orb_copy(ORB_ID(sensor_combined), sfd, nullptr); + count++; + } + }while (count < 100); + + end = hrt_absolute_time(); + test_note("full-speed, 100 updates in %llu", end - start); + + orb_set_interval(sfd, 10); + + start = hrt_absolute_time(); + count = 0; + + do + { + orb_check(sfd, &updated); + + if (updated) + { + orb_copy(ORB_ID(sensor_combined), sfd, nullptr); + count++; + } + }while (count < 100); + + end = hrt_absolute_time(); + test_note("100Hz, 100 updates in %llu", end - start); + + orb_unsubscribe(sfd); +#endif + +} + From 82b90281e97ac67a3d0590e26f3661555c1d008c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 4 Jun 2015 16:39:01 -0700 Subject: [PATCH 20/48] Cleanup of copyright headers Signed-off-by: Mark Charlebois --- src/platforms/px4_log.h | 35 +++++++++-------- src/platforms/qurt/dspal/dspal_stub.c | 32 ++++++++++++++++ src/platforms/qurt/px4_layer/qurt_stubs.c | 32 ++++++++++++++++ src/platforms/qurt/px4_layer/work_lock.c | 38 +++++++++++++++++-- .../qurt/tests/muorb/muorb_test_example.h | 6 --- .../qurt/tests/muorb/muorb_test_main.cpp | 5 +-- .../tests/muorb/muorb_test_start_qurt.cpp | 3 +- 7 files changed, 121 insertions(+), 30 deletions(-) diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 2adfd40f9f..0461689f2a 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -54,19 +54,22 @@ #include #include "HAP_farf.h" -//#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__); -//#define PX4_DEBUG(...) __px4_log("DEBUG", __VA_ARGS__); -//#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__); -//#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__); -//#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__); -#define PX4_DEBUG(...) FARF(HIGH, __VA_ARGS__); -#define PX4_INFO(...) FARF(HIGH, __VA_ARGS__); -#define PX4_WARN(...) FARF(HIGH, __VA_ARGS__); -#define PX4_ERR(...) FARF(HIGH, __VA_ARGS__); +#define __FARF_omit(level, ...) { } +#define __FARF_log(level, ...) { \ + FARF("%-5s ", level);\ + FARF(__VA_ARGS__);\ + FARF("\n");\ +} +#define __FARF_log_verbose(level, ...) { \ + FARF("%-5s ", level);\ + FARF(__VA_ARGS__);\ + FARF(" (file %s line %d)\n", __FILE__, __LINE__);\ +} -//#define PX4_INFO(...) __px4_log_omit("INFO", __VA_ARGS__); -//#define PX4_WARN(...) __px4_log_omit("WARN", __VA_ARGS__); -//#define PX4_ERR(...) __px4_log_omit("ERROR", __VA_ARGS__); +#define PX4_DEBUG(...) __FARF_omit(HIGH, __VA_ARGS__) +#define PX4_INFO(...) __FARF_log(HIGH, __VA_ARGS__) +#define PX4_WARN(...) __FARF_log_verbose(HIGH, __VA_ARGS__) +#define PX4_ERR(...) __FARF_log_verbose(HIGH, __VA_ARGS__) #elif defined(__PX4_LINUX) #include @@ -78,10 +81,10 @@ printf(" (file %s line %d)\n", __FILE__, __LINE__);\ } -#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__); -#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__); -#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__); -#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__); +#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__) +#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__) +#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__) +#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__) #elif defined(__PX4_ROS) diff --git a/src/platforms/qurt/dspal/dspal_stub.c b/src/platforms/qurt/dspal/dspal_stub.c index 311a352909..996a279d87 100644 --- a/src/platforms/qurt/dspal/dspal_stub.c +++ b/src/platforms/qurt/dspal/dspal_stub.c @@ -1,3 +1,35 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #include #include diff --git a/src/platforms/qurt/px4_layer/qurt_stubs.c b/src/platforms/qurt/px4_layer/qurt_stubs.c index 67955e5121..05e84995d3 100644 --- a/src/platforms/qurt/px4_layer/qurt_stubs.c +++ b/src/platforms/qurt/px4_layer/qurt_stubs.c @@ -1,3 +1,35 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ //extern "C" { diff --git a/src/platforms/qurt/px4_layer/work_lock.c b/src/platforms/qurt/px4_layer/work_lock.c index b2ad307d7c..99464a6a12 100644 --- a/src/platforms/qurt/px4_layer/work_lock.c +++ b/src/platforms/qurt/px4_layer/work_lock.c @@ -1,4 +1,36 @@ -//#pragma once +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include #include #include #include "work_lock.h" @@ -8,12 +40,12 @@ extern sem_t _work_lock[]; void work_lock(int id) { - //printf("work_lock %d\n", id); + //PX4_INFO("work_lock %d", id); sem_wait(&_work_lock[id]); } void work_unlock(int id) { - //printf("work_unlock %d\n", id); + //PX4_INFO("work_unlock %d", id); sem_post(&_work_lock[id]); } diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.h b/src/platforms/qurt/tests/muorb/muorb_test_example.h index d330233b37..304b8464e0 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_example.h +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.h @@ -31,12 +31,6 @@ * ****************************************************************************/ -/** - * @file hello_example.h - * Example app for Linux - * - * @author Mark Charlebois - */ #pragma once #include diff --git a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp index 5e3ad9bb22..2ded8976c6 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file hello_main.cpp - * Example for Linux + * @file muorb_test_main.cpp + * Test of Multi-uORB supoprt * * @author Mark Charlebois */ @@ -41,7 +41,6 @@ #include #include #include "muorb_test_example.h" -#include int PX4_MAIN(int argc, char **argv) { diff --git a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp index 943605f531..f063806766 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp @@ -32,9 +32,8 @@ ****************************************************************************/ /** - * @file hello_start_linux.cpp + * @file muorb_test_start_qurt.cpp * - * @author Thomas Gubler * @author Mark Charlebois */ #include "muorb_test_example.h" From aded2d3c0344cd656018667360002536b392aebe Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 5 Jun 2015 08:00:13 -0700 Subject: [PATCH 21/48] Enable passing udp port to mavlink module via start args. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Usage is: -d udp[:] If no port is specified, default port is set to 14556. If -d isn’t specified then default is serial. Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_main.cpp | 26 ++++++++++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 9 ++++++--- 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b56771d376..1baf6d3a9d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -176,6 +176,7 @@ Mavlink::Mavlink() : _rate_rx(0.0f), _socket_fd(-1), _protocol(SERIAL), + _udp_port(14556), _rstatus {}, _message_buffer {}, _message_buffer_mutex {}, @@ -923,10 +924,12 @@ Mavlink::resend_message(mavlink_message_t *msg) void Mavlink::init_udp() { + PX4_INFO("Setting up UDP w/port %d\n",_udp_port); + memset((char *)&_myaddr, 0, sizeof(_myaddr)); _myaddr.sin_family = AF_INET; _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); - _myaddr.sin_port = htons(14556); + _myaddr.sin_port = htons(_udp_port); if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { PX4_WARN("create socket failed\n"); @@ -1361,7 +1364,26 @@ Mavlink::task_main(int argc, char *argv[]) case 'd': _device_name = myoptarg; if (strncmp(_device_name, "udp",3) == 0) { + bool err = false; set_protocol(UDP); + const char* p = strchr(_device_name,':'); + if (p != nullptr ) { + p++; + if (strlen(p) > 0) { + char* eptr; + int port = strtol(p,&eptr,0); + if (*eptr == 0) { + _udp_port = port; + } + else { + err = true; + } + } + } + if (err) { + warnx("invalid device-name '%s'", myoptarg); + } + err_flag |= err; } break; @@ -1920,7 +1942,7 @@ Mavlink::stream_command(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); + warnx("usage: mavlink {start|stop-all|stream} [-d udp[:]] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 852859508c..07fe6b35eb 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -320,6 +320,8 @@ public: Protocol get_protocol() { return _protocol; }; + unsigned short get_udp_port() { return _udp_port; } + protected: Mavlink *next; @@ -335,8 +337,8 @@ private: bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ bool _is_usb_uart; /**< Port is USB */ - bool _wait_to_transmit; /**< Wait to transmit until received messages. */ - bool _received_messages; /**< Whether we've received valid mavlink messages. */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ @@ -397,7 +399,8 @@ private: struct sockaddr_in _myaddr; struct sockaddr_in _src_addr; - Protocol _protocol; + Protocol _protocol; + unsigned short _udp_port; struct telemetry_status_s _rstatus; ///< receive status From 59ad47003aab0583d20490dff8753b913deee90a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 5 Jun 2015 09:55:35 -0700 Subject: [PATCH 22/48] mavlink: simplified UDP suport by adding new -u option Use: mavlink start -u portnum to set the UDP port. Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_main.cpp | 37 +++++++++++----------------- 1 file changed, 15 insertions(+), 22 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1baf6d3a9d..05fbc4e24c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -176,7 +176,7 @@ Mavlink::Mavlink() : _rate_rx(0.0f), _socket_fd(-1), _protocol(SERIAL), - _udp_port(14556), + _udp_port(14556), _rstatus {}, _message_buffer {}, _message_buffer_mutex {}, @@ -1338,8 +1338,10 @@ Mavlink::task_main(int argc, char *argv[]) bool err_flag = false; int myoptind=1; const char *myoptarg = NULL; + char* eptr; + int temp_int_arg; - while ((ch = px4_getopt(argc, argv, "b:r:d:m:fpvwx", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:u:m:fpvwx", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(myoptarg, NULL, 10); @@ -1364,26 +1366,17 @@ Mavlink::task_main(int argc, char *argv[]) case 'd': _device_name = myoptarg; if (strncmp(_device_name, "udp",3) == 0) { - bool err = false; set_protocol(UDP); - const char* p = strchr(_device_name,':'); - if (p != nullptr ) { - p++; - if (strlen(p) > 0) { - char* eptr; - int port = strtol(p,&eptr,0); - if (*eptr == 0) { - _udp_port = port; - } - else { - err = true; - } - } - } - if (err) { - warnx("invalid device-name '%s'", myoptarg); - } - err_flag |= err; + } + break; + + case 'u': + temp_int_arg = strtoul(myoptarg, &eptr, 10); + if ( *eptr == '\0' ) { + _udp_port = temp_int_arg; + } else { + warnx("invalid data udp_port '%s'", myoptarg); + err_flag = true; } break; @@ -1942,7 +1935,7 @@ Mavlink::stream_command(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d udp[:]] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-u udp_port] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); } int mavlink_main(int argc, char *argv[]) From 58e263d53434627e69e55a1394a89d8ba05dd72a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sun, 7 Jun 2015 14:40:54 -0700 Subject: [PATCH 23/48] Added posix-arm target and refactored toolchain_* files The toolchain_* files are target OS specific so they were moved to the target OS subdir. The gcc_version.* files are only cleared once per make instantiation so a build that creates multiple HW targets will try to link with an incompatible .o file (i.e. x86 build linking ARM .o). I created posix-arm as a separate target to fix this problem. Signed-off-by: Mark Charlebois --- .gitignore | 1 + Makefile | 7 +- makefiles/firmware.mk | 3 + makefiles/nuttx/board_aerocore.mk | 2 +- makefiles/nuttx/board_px4-stm32f4discovery.mk | 2 +- makefiles/nuttx/board_px4fmu-v1.mk | 2 +- makefiles/nuttx/board_px4fmu-v2.mk | 2 +- makefiles/nuttx/board_px4io-v1.mk | 2 +- makefiles/nuttx/board_px4io-v2.mk | 2 +- .../{ => nuttx}/toolchain_gnu-arm-eabi.mk | 0 makefiles/posix-arm.mk | 40 +++ makefiles/posix-arm/board_eagle.mk | 11 + makefiles/posix-arm/config_eagle_default.mk | 85 +++++ .../toolchain_gnu-arm-linux-gnueabihf.mk | 329 ++++++++++++++++++ makefiles/posix/board_posix.mk | 2 +- makefiles/{ => posix}/toolchain_native.mk | 0 makefiles/qurt/board_qurt.mk | 2 +- makefiles/{ => qurt}/toolchain_hexagon.mk | 0 src/modules/uORB/module.mk | 3 + 19 files changed, 485 insertions(+), 10 deletions(-) rename makefiles/{ => nuttx}/toolchain_gnu-arm-eabi.mk (100%) create mode 100644 makefiles/posix-arm.mk create mode 100644 makefiles/posix-arm/board_eagle.mk create mode 100644 makefiles/posix-arm/config_eagle_default.mk create mode 100644 makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk rename makefiles/{ => posix}/toolchain_native.mk (100%) rename makefiles/{ => qurt}/toolchain_hexagon.mk (100%) diff --git a/.gitignore b/.gitignore index cd3f8f26c2..029a45346f 100644 --- a/.gitignore +++ b/.gitignore @@ -49,4 +49,5 @@ unittests/build *.pretty xcode src/platforms/posix/px4_messages/ +src/platforms/posix-arm/px4_messages/ src/platforms/qurt/px4_messages/ diff --git a/Makefile b/Makefile index 0075022892..1532f7ed35 100644 --- a/Makefile +++ b/Makefile @@ -33,7 +33,7 @@ # Top-level Makefile for building PX4 firmware images. # -TARGETS := nuttx posix qurt +TARGETS := nuttx posix posix-arm qurt EXPLICIT_TARGET := $(filter $(TARGETS),$(MAKECMDGOALS)) ifneq ($(EXPLICIT_TARGET),) export PX4_TARGET_OS=$(EXPLICIT_TARGET) @@ -241,6 +241,9 @@ endif ifeq ($(PX4_TARGET_OS),posix) include $(PX4_BASE)makefiles/firmware_posix.mk endif +ifeq ($(PX4_TARGET_OS),posix-arm) +include $(PX4_BASE)makefiles/firmware_posix.mk +endif ifeq ($(PX4_TARGET_OS),qurt) include $(PX4_BASE)makefiles/firmware_qurt.mk endif @@ -285,7 +288,7 @@ testbuild: $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) -nuttx posix qurt: +nuttx posix posix-arm qurt: ifeq ($(GOALS),) make PX4_TARGET_OS=$@ $(GOALS) else diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index f5f5d8188d..43cbfb59c0 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -548,6 +548,9 @@ endif ifeq ($(PX4_TARGET_OS),posix) include $(MK_DIR)/posix_elf.mk endif +ifeq ($(PX4_TARGET_OS),posix-arm) +include $(MK_DIR)/posix_elf.mk +endif ifeq ($(PX4_TARGET_OS),qurt) include $(MK_DIR)/qurt_elf.mk endif diff --git a/makefiles/nuttx/board_aerocore.mk b/makefiles/nuttx/board_aerocore.mk index 6f4b932666..71a0f264d0 100644 --- a/makefiles/nuttx/board_aerocore.mk +++ b/makefiles/nuttx/board_aerocore.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = CORTEXM4F CONFIG_BOARD = AEROCORE -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk +include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/nuttx/board_px4-stm32f4discovery.mk b/makefiles/nuttx/board_px4-stm32f4discovery.mk index fe761ba444..2477f55858 100644 --- a/makefiles/nuttx/board_px4-stm32f4discovery.mk +++ b/makefiles/nuttx/board_px4-stm32f4discovery.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = CORTEXM4F CONFIG_BOARD = PX4_STM32F4DISCOVERY -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk +include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/nuttx/board_px4fmu-v1.mk b/makefiles/nuttx/board_px4fmu-v1.mk index 4d692e31ab..45156efcdc 100644 --- a/makefiles/nuttx/board_px4fmu-v1.mk +++ b/makefiles/nuttx/board_px4fmu-v1.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = CORTEXM4F CONFIG_BOARD = PX4FMU_V1 -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk +include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/nuttx/board_px4fmu-v2.mk b/makefiles/nuttx/board_px4fmu-v2.mk index e9a2985b7f..7c3e00c5d0 100644 --- a/makefiles/nuttx/board_px4fmu-v2.mk +++ b/makefiles/nuttx/board_px4fmu-v2.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = CORTEXM4F CONFIG_BOARD = PX4FMU_V2 -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk +include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/nuttx/board_px4io-v1.mk b/makefiles/nuttx/board_px4io-v1.mk index 1872a4124f..66731a5694 100644 --- a/makefiles/nuttx/board_px4io-v1.mk +++ b/makefiles/nuttx/board_px4io-v1.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = CORTEXM3 CONFIG_BOARD = PX4IO_V1 -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk +include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/nuttx/board_px4io-v2.mk b/makefiles/nuttx/board_px4io-v2.mk index 50a4068fb2..94c3318199 100644 --- a/makefiles/nuttx/board_px4io-v2.mk +++ b/makefiles/nuttx/board_px4io-v2.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = CORTEXM3 CONFIG_BOARD = PX4IO_V2 -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk +include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk similarity index 100% rename from makefiles/toolchain_gnu-arm-eabi.mk rename to makefiles/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/posix-arm.mk b/makefiles/posix-arm.mk new file mode 100644 index 0000000000..13cb97f0db --- /dev/null +++ b/makefiles/posix-arm.mk @@ -0,0 +1,40 @@ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +# +# Rules and definitions related to handling the Linux specific impl when +# building firmware. +# + +MODULES += \ + platforms/common \ + platforms/posix/px4_layer + diff --git a/makefiles/posix-arm/board_eagle.mk b/makefiles/posix-arm/board_eagle.mk new file mode 100644 index 0000000000..94dd049367 --- /dev/null +++ b/makefiles/posix-arm/board_eagle.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the POSIX port of PX4 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXA8 +CONFIG_BOARD = EAGLE + +include $(PX4_MK_DIR)/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk diff --git a/makefiles/posix-arm/config_eagle_default.mk b/makefiles/posix-arm/config_eagle_default.mk new file mode 100644 index 0000000000..30dada7bbd --- /dev/null +++ b/makefiles/posix-arm/config_eagle_default.mk @@ -0,0 +1,85 @@ +# +# Makefile for the POSIXTEST *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/blinkm +MODULES += drivers/hil +MODULES += drivers/rgbled +MODULES += drivers/led +MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +MODULES += systemcmds/param +MODULES += systemcmds/mixer +MODULES += systemcmds/topic_listener + +# +# General system control +# +MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +MODULES += modules/attitude_estimator_ekf +MODULES += modules/ekf_att_pos_estimator + +# +# Vehicle Control +# +MODULES += modules/navigator +MODULES += modules/mc_pos_control +MODULES += modules/mc_att_control + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/uORB +MODULES += modules/dataman +MODULES += modules/sdlog2 +MODULES += modules/simulator +MODULES += modules/commander +MODULES += modules/controllib + +# +# Libraries +# +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/geo +MODULES += lib/geo_lookup +MODULES += lib/conversion + +# +# Linux port +# +MODULES += platforms/posix/px4_layer +MODULES += platforms/posix/drivers/accelsim +MODULES += platforms/posix/drivers/gyrosim +MODULES += platforms/posix/drivers/adcsim +MODULES += platforms/posix/drivers/barosim +MODULES += platforms/posix/drivers/tonealrmsim +MODULES += platforms/posix/drivers/airspeedsim +MODULES += platforms/posix/drivers/gpssim + +# +# Unit tests +# +#MODULES += platforms/posix/tests/hello +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + +# +# muorb fastrpc changes. +# +#MODULES += $(PX4_BASE)../muorb_krait diff --git a/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk new file mode 100644 index 0000000000..a5964e07d9 --- /dev/null +++ b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk @@ -0,0 +1,329 @@ +# +# Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +# +# Definitions for a generic GNU ARM-EABI toolchain +# + +#$(info TOOLCHAIN arm-linux-gnueabihf) + +# Toolchain commands. Normally only used inside this file. +# +CROSSDEV = arm-linux-gnueabihf- + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +# Check if the right version of the toolchain is available +# +CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.2 4.8.4 4.9.3 +CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) + +ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) +$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED)) +endif + + +# XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup + +MAXOPTIMIZATION ?= -O3 + +# Base CPU flags for each of the supported architectures. +# +ARCHCPUFLAGS_CORTEXA8 = -mtune=cortex-a8 \ + -mthumb-interwork \ + -march=armv7-a \ + -mfloat-abi=hard \ + -mfpu=neon + +ARCHCPUFLAGS_CORTEXM4F = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + +ARCHCPUFLAGS_CORTEXM4 = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfloat-abi=soft + +ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ + -mthumb \ + -march=armv7-m \ + -mfloat-abi=soft + +# Enabling stack checks if OS was build with them +# +TEST_FILE_STACKCHECK=$(WORK_DIR)nuttx-export/include/nuttx/config.h +TEST_VALUE_STACKCHECK=CONFIG_ARMV7M_STACKCHECK\ 1 +ENABLE_STACK_CHECKS=$(shell $(GREP) -q "$(TEST_VALUE_STACKCHECK)" $(TEST_FILE_STACKCHECK); echo $$?;) +ifeq ("$(ENABLE_STACK_CHECKS)","0") +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +else +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +endif + +# Pick the right set of flags for the architecture. +# +ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH)) +ifeq ($(ARCHCPUFLAGS),) +$(error Must set CONFIG_ARCH to one of CORTEXA8 CORTEXM4F, CORTEXM4 or CORTEXM3) +endif + +# Set the board flags +# +ifeq ($(CONFIG_BOARD),) +$(error Board config does not define CONFIG_BOARD) +endif +ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ + -D__PX4_LINUX -D__PX4_POSIX \ + -Dnoreturn_function= \ + -I$(PX4_BASE)/src/modules/systemlib \ + -I$(PX4_BASE)/src/lib/eigen \ + -I$(PX4_BASE)/src/platforms/posix/include \ + -I$(PX4_BASE)/mavlink/include/mavlink \ + -Wno-error=shadow + +# optimisation flags +# +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -g3 \ + -fno-strict-aliasing \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +# Language-specific flags +# +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=c++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ + +# Generic warnings +# +ARCHWARNINGS = -Wall \ + -Wextra \ + -Werror \ + -Wfloat-equal \ + -Wpointer-arith \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter \ + -Wno-packed \ + -Werror=format-security \ + -Werror=array-bounds \ + -Wfatal-errors \ + -Werror=unused-variable \ + -Werror=reorder \ + -Werror=uninitialized \ + -Werror=init-self \ + -Wno-error=logical-op \ + -Wdouble-promotion \ + -Wlogical-op \ + -Wformat=1 \ + -Werror=unused-but-set-variable \ + -Werror=double-promotion \ + -fno-strength-reduce \ + -Wno-error=unused-value + +ARCHOPTIMIZATION += -fno-strength-reduce + +# -Werror=float-conversion - works, just needs to be phased in with some effort and needs GCC 4.9+ +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +# C-specific warnings +# +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wmissing-prototypes \ + -Wnested-externs + +# C++-specific warnings +# +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-missing-field-initializers + +# pull in *just* libm from the toolchain ... this is grody +LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) +#EXTRA_LIBS += $(LIBM) +#EXTRA_LIBS += ${PX4_BASE}../muorb_krait/lib/libmuorb.so +EXTRA_LIBS += -pthread -lm -lrt + +# Flags we pass to the C compiler +# +CFLAGS = $(ARCHCFLAGS) \ + $(ARCHCWARNINGS) \ + $(ARCHOPTIMIZATION) \ + $(ARCHCPUFLAGS) \ + $(ARCHINCLUDES) \ + $(INSTRUMENTATIONDEFINES) \ + $(ARCHDEFINES) \ + $(EXTRADEFINES) \ + $(EXTRACFLAGS) \ + -fno-common \ + $(addprefix -I,$(INCLUDE_DIRS)) + +# Flags we pass to the C++ compiler +# +CXXFLAGS = $(ARCHCXXFLAGS) \ + $(ARCHWARNINGSXX) \ + $(ARCHOPTIMIZATION) \ + $(ARCHCPUFLAGS) \ + $(ARCHXXINCLUDES) \ + $(INSTRUMENTATIONDEFINES) \ + $(ARCHDEFINES) \ + -DCONFIG_WCHAR_BUILTIN \ + $(EXTRADEFINES) \ + $(EXTRACXXFLAGS) \ + -Wno-effc++ \ + $(addprefix -I,$(INCLUDE_DIRS)) + +# Flags we pass to the assembler +# +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ + $(EXTRADEFINES) \ + $(EXTRAAFLAGS) + +LDSCRIPT = $(PX4_BASE)/posix-configs/posixtest/scripts/ld.script +# Flags we pass to the linker +# +LDFLAGS += $(EXTRALDFLAGS) \ + $(addprefix -L,$(LIB_DIRS)) + +# Compiler support library +# +LIBGCC := $(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name) + +# Files that the final link depends on +# +#LINK_DEPS += $(LDSCRIPT) +LINK_DEPS += + +# Files to include to get automated dependencies +# +DEP_INCLUDES = $(subst .o,.d,$(OBJS)) + +# Compile C source $1 to object $2 +# as a side-effect, generate a dependency file +# +define COMPILE + @$(ECHO) "CC: $1" + @$(MKDIR) -p $(dir $2) + $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 +endef + +# Compile C++ source $1 to $2 +# as a side-effect, generate a dependency file +# +define COMPILEXX + @$(ECHO) "CXX: $1" + @$(MKDIR) -p $(dir $2) + @echo $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 + $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 +endef + +# Assemble $1 into $2 +# +define ASSEMBLE + @$(ECHO) "AS: $1" + @$(MKDIR) -p $(dir $2) + $(Q) $(CC) -c $(AFLAGS) $(abspath $1) -o $2 +endef + +# Produce partially-linked $1 from files in $2 +# +#$(Q) $(LD) -Ur -o $1 $2 # -Ur not supported in ld.gold +define PRELINK + @$(ECHO) "PRELINK: $1" + @$(MKDIR) -p $(dir $1) + $(Q) $(LD) -Ur -o $1 $2 + +endef +# Produce partially-linked $1 from files in $2 +# +#$(Q) $(LD) -Ur -o $1 $2 # -Ur not supported in ld.gold +define PRELINKF + @$(ECHO) "PRELINK: $1" + @$(MKDIR) -p $(dir $1) + $(Q) $(LD) -Ur -T$(LDSCRIPT) -o $1 $2 + +endef +# $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 + +# Update the archive $1 with the files in $2 +# +define ARCHIVE + @$(ECHO) "AR: $2" + @$(MKDIR) -p $(dir $1) + $(Q) $(AR) $1 $2 +endef + +# Link the objects in $2 into the shared library $1 +# +define LINK_A + @$(ECHO) "LINK_A: $1" + @$(MKDIR) -p $(dir $1) + echo "$(Q) $(AR) $1 $2" + $(Q) $(AR) $1 $2 +endef + +# Link the objects in $2 into the shared library $1 +# +define LINK_SO + @$(ECHO) "LINK_SO: $1" + @$(MKDIR) -p $(dir $1) + echo "$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) $(EXTRA_LIBS)" + $(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) -pthread -lc +endef + +# Link the objects in $2 into the application $1 +# +define LINK + @$(ECHO) "LINK: $1" + @$(MKDIR) -p $(dir $1) + $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) + +endef + diff --git a/makefiles/posix/board_posix.mk b/makefiles/posix/board_posix.mk index 93146b6a2e..9982680f92 100644 --- a/makefiles/posix/board_posix.mk +++ b/makefiles/posix/board_posix.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = NATIVE CONFIG_BOARD = POSIXTEST -include $(PX4_MK_DIR)/toolchain_native.mk +include $(PX4_MK_DIR)/posix/toolchain_native.mk diff --git a/makefiles/toolchain_native.mk b/makefiles/posix/toolchain_native.mk similarity index 100% rename from makefiles/toolchain_native.mk rename to makefiles/posix/toolchain_native.mk diff --git a/makefiles/qurt/board_qurt.mk b/makefiles/qurt/board_qurt.mk index 6d145f1c33..1038a90b76 100644 --- a/makefiles/qurt/board_qurt.mk +++ b/makefiles/qurt/board_qurt.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = HEXAGON CONFIG_BOARD = QURTTEST -include $(PX4_MK_DIR)/toolchain_hexagon.mk +include $(PX4_MK_DIR)/qurt/toolchain_hexagon.mk diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/qurt/toolchain_hexagon.mk similarity index 100% rename from makefiles/toolchain_hexagon.mk rename to makefiles/qurt/toolchain_hexagon.mk diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index bb6d4ea98b..30a997b317 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -52,6 +52,9 @@ endif ifeq ($(PX4_TARGET_OS),posix) SRCS += uORBTest_UnitTest.cpp endif +ifeq ($(PX4_TARGET_OS),posix-arm) +SRCS += uORBTest_UnitTest.cpp +endif SRCS += objects_common.cpp \ Publication.cpp \ From 8eee7ba32117d62288c938efca01ef061cde82fc Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 8 Jun 2015 09:15:50 +0200 Subject: [PATCH 24/48] compute atmospheric pressure from altitude --- src/modules/simulator/simulator_mavlink.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index d72d4d473c..65b73c2899 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -43,6 +43,10 @@ using namespace simulator; #define UDP_PORT 14550 #define PIXHAWK_DEVICE "/dev/ttyACM0" +#define PRESS_GROUND 101325.0f +#define DENSITY 1.2041f +#define GRAVITY 9.81f + static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; @@ -183,7 +187,8 @@ void Simulator::update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sen write_mag_data((void *)&mag); RawBaroData baro; - baro.pressure = imu->abs_pressure; + // calculate air pressure from altitude (valid for low altitude) + baro.pressure = PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt; baro.altitude = imu->pressure_alt; baro.temperature = imu->temperature; From 5694e378542bf6650656ee49cac4b148418ae7e7 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 8 Jun 2015 09:16:24 +0200 Subject: [PATCH 25/48] fix reading baro values from simulator --- src/platforms/posix/drivers/barosim/baro.cpp | 2 +- src/platforms/posix/drivers/barosim/baro_sim.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index f7ea4f6949..a70e394213 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -640,7 +640,7 @@ BAROSIM::collect() report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ - ret = _interface->dev_read(0, (void *)&baro_report, 0); + ret = _interface->dev_read(0, (void *)&baro_report, sizeof(baro_report)); if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 8b64962207..8e66dc3ac6 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -127,7 +127,7 @@ BARO_SIM::dev_read(unsigned offset, void *data, unsigned count) /* read the most recent measurement */ uint8_t cmd = 0; - int ret = transfer(&cmd, 1, static_cast(data), 3); + int ret = transfer(&cmd, 1, static_cast(data), count); /* if (ret == PX4_OK) { @@ -208,7 +208,7 @@ BARO_SIM::transfer(const uint8_t *send, unsigned send_len, if (recv_len == 0) { PX4_DEBUG("BARO_SIM measurement requested"); } - else if (send_len != 1 || send[0] != 0 || recv_len != 3) { + else if (send_len != 1 || send[0] != 0 ) { PX4_WARN("BARO_SIM::transfer invalid param %u %u %u", send_len, send[0], recv_len); return 1; } From 065ec5b2dc8b631f2adb89bb810b09a145bce600 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 8 Jun 2015 17:21:21 +0200 Subject: [PATCH 26/48] no need to send non-controls mavlink messages to jMAVSim because we can use mavlink app with udp --- src/modules/simulator/simulator.h | 8 +-- src/modules/simulator/simulator_mavlink.cpp | 67 ++------------------- 2 files changed, 8 insertions(+), 67 deletions(-) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 39491d8c0d..b2ebc880cd 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -266,16 +266,12 @@ private: int _fd; unsigned char _buf[200]; - hrt_abstime _time_last; - hrt_abstime _heartbeat_last; - hrt_abstime _attitude_last; - hrt_abstime _manual_last; struct sockaddr_in _srcaddr; socklen_t _addrlen = sizeof(_srcaddr); - void poll_topics(); + void poll_actuators(); void handle_message(mavlink_message_t *msg); - void send_data(); + void send_controls(); void pack_actuator_message(mavlink_hil_controls_t &actuator_msg); void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); void update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 65b73c2899..94ec3ced23 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -90,47 +90,10 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { actuator_msg.nav_mode = 0; } -void Simulator::send_data() { +void Simulator::send_controls() { mavlink_hil_controls_t msg; pack_actuator_message(msg); send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); - - // send heartbeat - if (hrt_elapsed_time(&_heartbeat_last) >= 1e6) { - _heartbeat_last = hrt_absolute_time(); - mavlink_heartbeat_t msg; - msg.base_mode = 0; - msg.custom_mode = 0; - msg.autopilot = MAV_AUTOPILOT_PX4; - msg.mavlink_version = 3; - send_mavlink_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 200); - } - - // send attitude message - if (hrt_elapsed_time(&_attitude_last) > 20000) { - _attitude_last = hrt_absolute_time(); - mavlink_attitude_t msg; - msg.time_boot_ms = _attitude.timestamp / 1000; - msg.roll = _attitude.roll; - msg.pitch = _attitude.pitch; - msg.yaw = _attitude.yaw; - msg.rollspeed = _attitude.rollspeed; - msg.pitchspeed = _attitude.pitchspeed; - msg.yawspeed = _attitude.yawspeed; - send_mavlink_message(MAVLINK_MSG_ID_ATTITUDE, &msg, 200); - } - - // send manual control setpoint - if (hrt_elapsed_time(&_manual_last) > 200000) { - _manual_last = hrt_absolute_time(); - mavlink_manual_control_t msg; - msg.x = _manual.x * 1000; - msg.y = _manual.y * 1000; - msg.z = _manual.z * 1000; - msg.r = _manual.r * 1000; - msg.buttons = 0; - send_mavlink_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg, 200); - } } static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels) { @@ -278,24 +241,13 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8 } } -void Simulator::poll_topics() { - // copy new data if available +void Simulator::poll_actuators() { + // copy new actuator data if available bool updated; orb_check(_actuator_outputs_sub, &updated); if(updated) { orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators); } - - orb_check(_vehicle_attitude_sub, &updated); - if(updated) { - orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude); - } - - orb_check(_manual_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); - } } void *Simulator::sending_trampoline(void *) { @@ -308,11 +260,6 @@ void Simulator::send() { fds[0].fd = _actuator_outputs_sub; fds[0].events = POLLIN; - _time_last = 0; - _heartbeat_last = 0; - _attitude_last = 0; - _manual_last = 0; - int pret; while(true) { @@ -333,8 +280,8 @@ void Simulator::send() { if (fds[0].revents & POLLIN) { // got new data to read, update all topics - poll_topics(); - send_data(); + poll_actuators(); + send_controls(); } } } @@ -365,8 +312,6 @@ void Simulator::updateSamples() // subscribe to topics _actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); - _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); // try to setup udp socket for communcation with simulator memset((char *)&_myaddr, 0, sizeof(_myaddr)); @@ -433,7 +378,7 @@ void Simulator::updateSamples() if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); - send_data(); + send_controls(); } // got data from simulator, now activate the sending thread From a5c214a7bbf3df582c2d747ff5a204e94565de4a Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 8 Jun 2015 19:41:54 +0200 Subject: [PATCH 27/48] use orb_advertise_multi: - subscribe to actuator controls after topic has been advertised --- src/drivers/hil/hil.cpp | 6 +++--- src/modules/simulator/simulator_mavlink.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index d6dbe9a53f..0efc490326 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -348,9 +348,9 @@ HIL::task_main() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); /* advertise the mixed control outputs */ - //int dummy; - _t_outputs = orb_advertise(ORB_ID(actuator_outputs), - &outputs); + int dummy; + _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &dummy, ORB_PRIO_LOW); px4_pollfd_struct_t fds[2]; fds[0].fd = _t_actuators; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 94ec3ced23..e4403fd221 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -310,9 +310,6 @@ void Simulator::updateSamples() struct mag_report mag; memset(&mag, 0 ,sizeof(mag)); - // subscribe to topics - _actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); - // try to setup udp socket for communcation with simulator memset((char *)&_myaddr, 0, sizeof(_myaddr)); _myaddr.sin_family = AF_INET; @@ -381,6 +378,9 @@ void Simulator::updateSamples() send_controls(); } + // subscribe to topics + _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); + // got data from simulator, now activate the sending thread pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); pthread_attr_destroy(&sender_thread_attr); From cb231e89f62a8b76253dc1547bfa8ab74c3a3293 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 8 Jun 2015 20:34:09 -0700 Subject: [PATCH 28/48] QuRT: Changes to enable qurt target to build QuRT doesn't support unlink and does not provide getpid(). The DSPAL layer provides access to usleep so an implementation is no longer needed. Signed-off-by: Mark Charlebois --- makefiles/qurt/toolchain_hexagon.mk | 4 +--- src/modules/mavlink/mavlink_main.h | 2 ++ src/modules/uORB/uORBDevices_posix.cpp | 4 ++-- src/platforms/px4_log.h | 10 +++++----- src/platforms/px4_posix.h | 7 +++++++ src/platforms/px4_time.h | 1 - src/platforms/qurt/px4_layer/px4_qurt_impl.cpp | 7 +------ src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 14 ++++++++++++++ src/systemcmds/param/param.c | 2 ++ 9 files changed, 34 insertions(+), 17 deletions(-) diff --git a/makefiles/qurt/toolchain_hexagon.mk b/makefiles/qurt/toolchain_hexagon.mk index 4574574999..8f44a84946 100644 --- a/makefiles/qurt/toolchain_hexagon.mk +++ b/makefiles/qurt/toolchain_hexagon.mk @@ -38,9 +38,7 @@ # Toolchain commands. Normally only used inside this file. # HEXAGON_TOOLS_ROOT = /opt/6.4.05 -#HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 -HEXAGON_SDK_ROOT = /prj/atlanticus/users/rkintada/Hexagon_SDK/2.0 -#V_ARCH = v4 +HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 V_ARCH = v5 CROSSDEV = hexagon- HEXAGON_BIN = $(addsuffix /gnu/bin,$(HEXAGON_TOOLS_ROOT)) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 07fe6b35eb..f90a0a6237 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -395,12 +395,14 @@ private: float _rate_txerr; float _rate_rx; +#ifdef __PX4_POSIX int _socket_fd; struct sockaddr_in _myaddr; struct sockaddr_in _src_addr; Protocol _protocol; unsigned short _udp_port; +#endif struct telemetry_status_s _rstatus; ///< receive status diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index 037ff395b3..ddbce3ff55 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -91,7 +91,7 @@ uORB::DeviceNode::open(device::file_t *filp) lock(); if (_publisher == 0) { - _publisher = getpid(); + _publisher = px4_getpid(); ret = PX4_OK; } else { @@ -153,7 +153,7 @@ uORB::DeviceNode::close(device::file_t *filp) { //warnx("uORB::DeviceNode::close fd = %d", filp->fd); /* is this the publisher closing? */ - if (getpid() == _publisher) { + if (px4_getpid() == _publisher) { _publisher = 0; } else { diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 0461689f2a..d655fa91d6 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -52,7 +52,7 @@ } #if defined(__PX4_QURT) #include -#include "HAP_farf.h" +#define FARF printf #define __FARF_omit(level, ...) { } #define __FARF_log(level, ...) { \ @@ -66,10 +66,10 @@ FARF(" (file %s line %d)\n", __FILE__, __LINE__);\ } -#define PX4_DEBUG(...) __FARF_omit(HIGH, __VA_ARGS__) -#define PX4_INFO(...) __FARF_log(HIGH, __VA_ARGS__) -#define PX4_WARN(...) __FARF_log_verbose(HIGH, __VA_ARGS__) -#define PX4_ERR(...) __FARF_log_verbose(HIGH, __VA_ARGS__) +#define PX4_DEBUG(...) __FARF_omit("DEBUG", __VA_ARGS__) +#define PX4_INFO(...) __FARF_log("INFO", __VA_ARGS__) +#define PX4_WARN(...) __FARF_log_verbose("WARN", __VA_ARGS__) +#define PX4_ERR(...) __FARF_log_verbose("ERROR", __VA_ARGS__) #elif defined(__PX4_LINUX) #include diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 9c392ac42a..da3e584b80 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -68,6 +68,7 @@ typedef struct pollfd px4_pollfd_struct_t; #define px4_poll _GLOBAL poll #define px4_fsync _GLOBAL fsync #define px4_access _GLOBAL access +#define px4_getpid _GLOBAL getpid #elif defined(__PX4_POSIX) @@ -98,6 +99,12 @@ __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); __EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout); __EXPORT int px4_fsync(int fd); __EXPORT int px4_access(const char *pathname, int mode); +#ifdef __PX4_QURT +typedef int pid_t; +__EXPORT int px4_getpid(void); +#else +#define px4_getpid getpid +#endif __END_DECLS #else diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h index 0c9b7f24c9..a54a18b9ab 100644 --- a/src/platforms/px4_time.h +++ b/src/platforms/px4_time.h @@ -28,7 +28,6 @@ struct timespec int px4_clock_gettime(clockid_t clk_id, struct timespec *tp); int px4_clock_settime(clockid_t clk_id, struct timespec *tp); -__EXPORT int usleep(useconds_t usec); __EXPORT unsigned int sleep(unsigned int sec); __END_DECLS diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 0987c6feb1..9087fd873e 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -48,7 +48,6 @@ #include #include #include -#include #include "systemlib/param/param.h" @@ -57,14 +56,10 @@ __BEGIN_DECLS // FIXME - sysconf(_SC_CLK_TCK) not supported long PX4_TICKS_PER_SEC = 1000; -void usleep(useconds_t usec) { - qurt_timer_sleep(usec); -} - unsigned int sleep(unsigned int sec) { for (unsigned int i=0; i< sec; i++) - qurt_timer_sleep(1000000); + usleep(1000000); return 0; } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index d688f1104a..75239b91d6 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -245,6 +245,20 @@ int px4_task_kill(px4_task_t id, int sig) return rv; } +pid_t px4_getpid() +{ + pthread_t pid = pthread_self(); + + // Get pthread ID from the opaque ID + for (int i=0; i Date: Mon, 8 Jun 2015 21:36:01 -0700 Subject: [PATCH 29/48] POSIX: use px4_getpid() The posix build only has one process so calling getpid() will not provide the expected result. Signed-off-by: Mark Charlebois --- .../posix/px4_layer/px4_posix_tasks.cpp | 15 +++++++++ src/platforms/px4_posix.h | 5 --- .../qurt/px4_layer/px4_qurt_tasks.cpp | 33 +++++++++++-------- 3 files changed, 34 insertions(+), 19 deletions(-) diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index c539fa7266..eb442e708b 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -273,6 +273,21 @@ void px4_show_tasks() } __BEGIN_DECLS + +int px4_getpid() +{ + pthread_t pid = pthread_self(); + + // Get pthread ID from the opaque ID + for (int i=0; i Date: Tue, 9 Jun 2015 20:14:42 +0200 Subject: [PATCH 30/48] clean up mavlink network capability --- src/modules/mavlink/mavlink_main.cpp | 84 ++++++++++++++++++++++------ src/modules/mavlink/mavlink_main.h | 8 ++- 2 files changed, 71 insertions(+), 21 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 05fbc4e24c..ef51f556ee 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -176,7 +176,7 @@ Mavlink::Mavlink() : _rate_rx(0.0f), _socket_fd(-1), _protocol(SERIAL), - _udp_port(14556), + _network_port(14556), _rstatus {}, _message_buffer {}, _message_buffer_mutex {}, @@ -329,6 +329,20 @@ Mavlink::get_instance_for_device(const char *device_name) return nullptr; } +Mavlink * +Mavlink::get_instance_for_network_port(unsigned long port) +{ + Mavlink *inst; + + LL_FOREACH(::_mavlink_instances, inst) { + if (inst->_network_port == port) { + return inst; + } + } + + return nullptr; +} + int Mavlink::destroy_all_instances() { @@ -847,14 +861,19 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); -//#ifndef __PX4_POSIX - /* send message to UART */ size_t ret = -1; +#ifndef __PX4_POSIX + /* send message to UART */ if (get_protocol() == SERIAL) { ret = ::write(_uart_fd, buf, packet_len); - } else if (get_protocol() == UDP) { - ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); } +#else + if (get_protocol() == UDP) { + ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); + } else if (get_protocol() == TCP) { + // not implemented, but possible to do so + } +#endif if (ret != (size_t) packet_len) { count_txerr(); @@ -864,7 +883,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } -//#endif pthread_mutex_unlock(&_send_mutex); } @@ -924,12 +942,12 @@ Mavlink::resend_message(mavlink_message_t *msg) void Mavlink::init_udp() { - PX4_INFO("Setting up UDP w/port %d\n",_udp_port); + PX4_INFO("Setting up UDP w/port %d\n",_network_port); memset((char *)&_myaddr, 0, sizeof(_myaddr)); _myaddr.sin_family = AF_INET; _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); - _myaddr.sin_port = htons(_udp_port); + _myaddr.sin_port = htons(_network_port); if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { PX4_WARN("create socket failed\n"); @@ -1365,15 +1383,14 @@ Mavlink::task_main(int argc, char *argv[]) case 'd': _device_name = myoptarg; - if (strncmp(_device_name, "udp",3) == 0) { - set_protocol(UDP); - } + set_protocol(SERIAL); break; case 'u': temp_int_arg = strtoul(myoptarg, &eptr, 10); if ( *eptr == '\0' ) { - _udp_port = temp_int_arg; + _network_port = temp_int_arg; + set_protocol(UDP); } else { warnx("invalid data udp_port '%s'", myoptarg); err_flag = true; @@ -1442,8 +1459,11 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } + if (get_protocol() == SERIAL) { warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); - + } else if (get_protocol() == UDP) { + warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port); + } /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1875,6 +1895,11 @@ Mavlink::stream_command(int argc, char *argv[]) const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; const char *stream_name = nullptr; + unsigned short network_port = 0; + char* eptr; + int temp_int_arg; + bool provided_device = false; + bool provided_network_port = false; argc -= 2; argv += 2; @@ -1897,13 +1922,22 @@ Mavlink::stream_command(int argc, char *argv[]) i++; } else if (0 == strcmp(argv[i], "-d") && i < argc - 1) { + provided_device = true; device_name = argv[i + 1]; i++; } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) { stream_name = argv[i + 1]; i++; - + } else if (0 == strcmp(argv[i], "-u") && i < argc - 1) { + provided_network_port = true; + temp_int_arg = strtoul(argv[i + 1], &eptr, 10); + if ( *eptr == '\0' ) { + network_port = temp_int_arg; + } else { + err_flag = true; + } + i++; } else { err_flag = true; } @@ -1912,7 +1946,16 @@ Mavlink::stream_command(int argc, char *argv[]) } if (!err_flag && rate >= 0.0f && stream_name != nullptr) { - Mavlink *inst = get_instance_for_device(device_name); + + Mavlink *inst = nullptr; + if (provided_device && !provided_network_port) { + inst = get_instance_for_device(device_name); + } else if (provided_network_port && !provided_device) { + inst = get_instance_for_network_port(network_port); + } else if (provided_device && provided_network_port) { + warnx("please provide either a device name or a network port"); + return 1; + } if (inst != nullptr) { inst->configure_stream_threadsafe(stream_name, rate); @@ -1921,12 +1964,17 @@ Mavlink::stream_command(int argc, char *argv[]) // If the link is not running we should complain, but not fall over // because this is so easy to get wrong and not fatal. Warning is sufficient. - warnx("mavlink for device %s is not running", device_name); - return 0; + if (provided_device) { + warnx("mavlink for device %s is not running", device_name); + } else { + warnx("mavlink for network on port %hu is not running", network_port); + } + + return 1; } } else { - warnx("usage: mavlink stream [-d device] -s stream -r rate"); + warnx("usage: mavlink stream [-d device] [-u network_port] -s stream -r rate"); return 1; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index f90a0a6237..72688af8f8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -111,7 +111,9 @@ public: static Mavlink *get_instance(unsigned instance); - static Mavlink *get_instance_for_device(const char *device_name); + static Mavlink *get_instance_for_device(const char *device_name); + + static Mavlink *get_instance_for_network_port(unsigned long port); static int destroy_all_instances(); @@ -320,7 +322,7 @@ public: Protocol get_protocol() { return _protocol; }; - unsigned short get_udp_port() { return _udp_port; } + unsigned short get_network_port() { return _network_port; } protected: Mavlink *next; @@ -401,7 +403,7 @@ private: struct sockaddr_in _src_addr; Protocol _protocol; - unsigned short _udp_port; + unsigned short _network_port; #endif struct telemetry_status_s _rstatus; ///< receive status From 13dd993e016e3642790802c21c9378710f0c00c6 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 9 Jun 2015 16:32:22 -0700 Subject: [PATCH 31/48] Nuttx: mavlink fixes Needed to ifdef SITL functionality not supoprted in NuttX build. Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_main.cpp | 4 ++++ src/modules/mavlink/mavlink_main.h | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ef51f556ee..619922eec8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -690,8 +690,10 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * _is_usb_uart = true; } +#ifdef __PX4_LINUX /* Put in raw mode */ cfmakeraw(&uart_config); +#endif if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { warnx("ERR SET CONF %s\n", uart_name); @@ -942,6 +944,7 @@ Mavlink::resend_message(mavlink_message_t *msg) void Mavlink::init_udp() { +#ifdef __PX4_LINUX PX4_INFO("Setting up UDP w/port %d\n",_network_port); memset((char *)&_myaddr, 0, sizeof(_myaddr)); @@ -964,6 +967,7 @@ Mavlink::init_udp() // wait for client to connect to socket recvfrom(_socket_fd,inbuf,sizeof(inbuf),0,(struct sockaddr *)&_src_addr,&addrlen); +#endif } void diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 72688af8f8..326182bbef 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -398,13 +398,13 @@ private: float _rate_rx; #ifdef __PX4_POSIX - int _socket_fd; struct sockaddr_in _myaddr; struct sockaddr_in _src_addr; +#endif + int _socket_fd; Protocol _protocol; unsigned short _network_port; -#endif struct telemetry_status_s _rstatus; ///< receive status From 4d28126e0af636f66cd347cef6f9a6cf1fdc38b0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 9 Jun 2015 18:56:28 -0700 Subject: [PATCH 32/48] Nuttx: remove use of std::string, std::map, std::set Nuttx complains about an unresolved _impure_ptr at link time. This is a known issue when using STL templates in NuttX on ARM. Created new ORBMap and ORBSet classes for NuttX. Signed-off-by: Mark Charlebois --- makefiles/nuttx/toolchain_gnu-arm-eabi.mk | 4 +- src/modules/uORB/ORBSet.h | 127 ++++++++++++++++++++++ src/modules/uORB/uORBCommunicator.hpp | 12 +- src/modules/uORB/uORBDevices_nuttx.cpp | 10 +- src/modules/uORB/uORBDevices_nuttx.hpp | 93 +++++++++++++++- src/modules/uORB/uORBDevices_posix.cpp | 7 +- src/modules/uORB/uORBDevices_posix.hpp | 2 +- src/modules/uORB/uORBManager.hpp | 18 +-- src/modules/uORB/uORBManager_nuttx.cpp | 22 ++-- src/modules/uORB/uORBManager_posix.cpp | 20 ++-- src/modules/uORB/uORBUtils.cpp | 4 +- src/modules/uORB/uORBUtils.hpp | 2 +- 12 files changed, 268 insertions(+), 53 deletions(-) create mode 100644 src/modules/uORB/ORBSet.h diff --git a/makefiles/nuttx/toolchain_gnu-arm-eabi.mk b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk index 143b15536c..3c2898cb0c 100644 --- a/makefiles/nuttx/toolchain_gnu-arm-eabi.mk +++ b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk @@ -125,7 +125,6 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ # note - requires corresponding support in NuttX INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) -LIBSTDCXX := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libstdc++.a) LIBC := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libc.a) # Language-specific flags @@ -282,8 +281,7 @@ endef define LINK @$(ECHO) "LINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) ${LIBSTDCXX} $(LIBGCC) --end-group - #$(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) ${LIBSTDCXX} $(LIBGCC) --end-group + $(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group endef # Convert $1 from a linked object to a raw binary in $2 diff --git a/src/modules/uORB/ORBSet.h b/src/modules/uORB/ORBSet.h new file mode 100644 index 0000000000..2d5c59e06d --- /dev/null +++ b/src/modules/uORB/ORBSet.h @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +class ORBSet +{ +public: + struct Node { + struct Node *next; + const char * node_name; + }; + + ORBSet() : + _top(nullptr), + _end(nullptr) + { } + ~ORBSet() { + while (_top != nullptr) { + unlinkNext(_top); + if (_top->next == nullptr) { + free((void *)_top->node_name); + free(_top); + _top = nullptr; + } + } + } + void insert(const char *node_name) + { + Node **p; + if (_top == nullptr) + p = &_top; + else + p = &_end->next; + + *p = (Node *)malloc(sizeof(Node)); + if (_end) + _end = _end->next; + else { + _end = _top; + } + _end->next = nullptr; + _end->node_name = strdup(node_name); + } + + bool find(const char *node_name) + { + Node *p = _top; + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return true; + } + p = p->next; + } + return false; + } + + bool erase(const char *node_name) + { + Node *p = _top; + if (_top && (strcmp(_top->node_name, node_name) == 0)) { + p = _top->next; + free((void *)_top->node_name); + free(_top); + _top = p; + if (_top == nullptr) { + _end = nullptr; + } + return true; + } + while (p->next) { + if (strcmp(p->next->node_name, node_name) == 0) { + unlinkNext(p); + } + } + return nullptr; + } + +private: + + void unlinkNext(Node *a) + { + Node *b = a->next; + if (b != nullptr) { + if (_end == b) { + _end = a; + } + a->next = b->next; + free((void *)b->node_name); + free(b); + } + } + + Node *_top; + Node *_end; +}; + diff --git a/src/modules/uORB/uORBCommunicator.hpp b/src/modules/uORB/uORBCommunicator.hpp index ad407bd7d8..de6984d1ac 100644 --- a/src/modules/uORB/uORBCommunicator.hpp +++ b/src/modules/uORB/uORBCommunicator.hpp @@ -70,7 +70,7 @@ public: * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t add_subscription( const std::string& messageName, int32_t msgRateInHz ) = 0; + virtual int16_t add_subscription( const char *messageName, int32_t msgRateInHz ) = 0; /** @@ -84,7 +84,7 @@ public: * Note: This does not necessarily mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t remove_subscription( const std::string& messageName ) = 0; + virtual int16_t remove_subscription( const char *messageName ) = 0; /** * Register Message Handler. This is internal for the IChannel implementer* @@ -110,7 +110,7 @@ public: * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t send_message( const std::string& messageName, int32_t length, uint8_t* data) = 0; + virtual int16_t send_message( const char *messageName, int32_t length, uint8_t* data) = 0; }; /** @@ -133,7 +133,7 @@ public: * handler. * otherwise = failure. */ - virtual int16_t process_add_subscription( const std::string& messageName, int32_t msgRateInHz ) = 0; + virtual int16_t process_add_subscription( const char *messageName, int32_t msgRateInHz ) = 0; /** * Interface to process a received control msg to remove subscription @@ -145,7 +145,7 @@ public: * handler. * otherwise = failure. */ - virtual int16_t process_remove_subscription( const std::string& messageName ) = 0; + virtual int16_t process_remove_subscription( const char *messageName ) = 0; /** * Interface to process the received data message. @@ -161,7 +161,7 @@ public: * handler. * otherwise = failure. */ - virtual int16_t process_received_message( const std::string& messageName, int32_t length, uint8_t* data ) = 0; + virtual int16_t process_received_message( const char *messageName, int32_t length, uint8_t* data ) = 0; }; #endif /* _uORBCommunicator_hpp_ */ diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index 319f724a67..bebb35f989 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -43,7 +43,7 @@ #include "uORBCommunicator.hpp" #include -std::map uORB::DeviceMaster::_node_map; +uORB::ORBMap uORB::DeviceMaster::_node_map; uORB::DeviceNode::DeviceNode ( @@ -608,7 +608,7 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) else { // add to the node map;. - _node_map[std::string(nodepath)] = node; + _node_map.insert(nodepath, node); } group_tries++; @@ -631,12 +631,12 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) } } -uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const std::string& nodepath ) +uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath ) { uORB::DeviceNode* rc = nullptr; - if( _node_map.find( nodepath ) != _node_map.end() ) + if( _node_map.find( nodepath ) ) { - rc = _node_map[nodepath]; + rc = _node_map.get(nodepath); } return rc; } diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp index e26598531a..ef4bb3b672 100644 --- a/src/modules/uORB/uORBDevices_nuttx.hpp +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -35,8 +35,8 @@ #define _uORBDevices_nuttx_hpp_ #include -#include -#include +#include +#include #include "uORBCommon.hpp" @@ -44,8 +44,93 @@ namespace uORB { class DeviceNode; class DeviceMaster; + class ORBMap; } +class uORB::ORBMap +{ +public: + struct Node { + struct Node *next; + const char * node_name; + uORB::DeviceNode *node; + }; + + ORBMap() : + _top(nullptr), + _end(nullptr) + { } + ~ORBMap() { + while (_top != nullptr) { + unlinkNext(_top); + if (_top->next == nullptr) { + free((void *)_top->node_name); + free(_top); + _top = nullptr; + _end = nullptr; + } + } + } + void insert(const char *node_name, uORB::DeviceNode*node) + { + Node **p; + if (_top == nullptr) + p = &_top; + else + p = &_end->next; + + *p = (Node *)malloc(sizeof(Node)); + if (_end) + _end = _end->next; + else { + _end = _top; + } + _end->next = nullptr; + _end->node_name = strdup(node_name); + _end->node = node; + } + + bool find(const char *node_name) + { + Node *p = _top; + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return true; + } + p = p->next; + } + return false; + } + + uORB::DeviceNode* get(const char *node_name) + { + Node *p = _top; + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return p->node; + } + } + return nullptr; + } + + void unlinkNext(Node *a) + { + Node *b = a->next; + if (b != nullptr) { + if (_end == b) { + _end = a; + } + a->next = b->next; + free((void *)b->node_name); + free(b); + } + } + +private: + Node *_top; + Node *_end; +}; + /** * Per-object device instance. */ @@ -226,11 +311,11 @@ public: DeviceMaster(Flavor f); virtual ~DeviceMaster(); - static uORB::DeviceNode* GetDeviceNode( const std::string& node_name ); + static uORB::DeviceNode* GetDeviceNode( const char * node_name ); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: Flavor _flavor; - static std::map _node_map; + static ORBMap _node_map; }; diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index ddbce3ff55..95a2374611 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -652,12 +652,13 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) } } -uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const std::string& nodepath ) +uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath ) { uORB::DeviceNode* rc = nullptr; - if( _node_map.find( nodepath ) != _node_map.end() ) + std::string np(nodepath); + if( _node_map.find( np ) != _node_map.end() ) { - rc = _node_map[nodepath]; + rc = _node_map[np]; } return rc; } diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index 11e333d83f..4fe4d499bd 100644 --- a/src/modules/uORB/uORBDevices_posix.hpp +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -159,7 +159,7 @@ public: DeviceMaster(Flavor f); ~DeviceMaster(); - static uORB::DeviceNode* GetDeviceNode( const std::string& node_name ); + static uORB::DeviceNode* GetDeviceNode( const char *node_name ); virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); private: diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index 89b9d7c13f..60a0cc5541 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -36,10 +36,14 @@ #include "uORBCommon.hpp" #include "uORBDevices.hpp" -#include -#include #include +#ifdef __PX4_NUTTX +#include "ORBSet.h" +#else +#include #include +#define ORBSet std::set +#endif #include "uORBCommunicator.hpp" @@ -306,7 +310,7 @@ class uORB::Manager : public uORBCommunicator::IChannelRxHandler * Utility method to check if there is a remote subscriber present * for a given topic */ - bool is_remote_subscriber_present( const std::string& messageName ); + bool is_remote_subscriber_present( const char * messageName ); private: // class methods /** @@ -345,7 +349,7 @@ class uORB::Manager : public uORBCommunicator::IChannelRxHandler static Manager _Instance; // the communicator channel instance. uORBCommunicator::IChannel* _comm_channel; - std::set _remote_subscriber_topics; + ORBSet _remote_subscriber_topics; private: //class methods Manager(); @@ -362,7 +366,7 @@ class uORB::Manager : public uORBCommunicator::IChannelRxHandler * handler. * otherwise = failure. */ - virtual int16_t process_add_subscription(const std::string& messageName, + virtual int16_t process_add_subscription(const char * messageName, int32_t msgRateInHz); /** @@ -375,7 +379,7 @@ class uORB::Manager : public uORBCommunicator::IChannelRxHandler * handler. * otherwise = failure. */ - virtual int16_t process_remove_subscription(const std::string& messageName); + virtual int16_t process_remove_subscription(const char * messageName); /** * Interface to process the received data message. @@ -391,7 +395,7 @@ class uORB::Manager : public uORBCommunicator::IChannelRxHandler * handler. * otherwise = failure. */ - virtual int16_t process_received_message(const std::string& messageName, + virtual int16_t process_received_message(const char * messageName, int32_t length, uint8_t* data); }; diff --git a/src/modules/uORB/uORBManager_nuttx.cpp b/src/modules/uORB/uORBManager_nuttx.cpp index 3a188c1646..7b1bcf33d1 100644 --- a/src/modules/uORB/uORBManager_nuttx.cpp +++ b/src/modules/uORB/uORBManager_nuttx.cpp @@ -298,11 +298,11 @@ uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void ) //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -int16_t uORB::Manager::process_add_subscription(const std::string& messageName, +int16_t uORB::Manager::process_add_subscription(const char * messageName, int32_t msgRateInHz) { warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", - __LINE__, messageName.c_str() ); + __LINE__, messageName ); int16_t rc = 0; _remote_subscriber_topics.insert( messageName ); char nodepath[orb_maxpath]; @@ -312,7 +312,7 @@ int16_t uORB::Manager::process_add_subscription(const std::string& messageName, uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); if ( node == nullptr) { warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet", - __LINE__, messageName.c_str() ); + __LINE__, messageName ); } else{ // node is present. @@ -327,10 +327,10 @@ int16_t uORB::Manager::process_add_subscription(const std::string& messageName, //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int16_t uORB::Manager::process_remove_subscription( - const std::string& messageName) + const char * messageName) { warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s", - __LINE__, messageName.c_str() ); + __LINE__, messageName ); int16_t rc = -1; _remote_subscriber_topics.erase( messageName ); char nodepath[orb_maxpath]; @@ -340,7 +340,7 @@ int16_t uORB::Manager::process_remove_subscription( // get the node name. if ( node == nullptr) { warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", - __LINE__, messageName.c_str()); + __LINE__, messageName); } else { // node is present. node->process_remove_subscription(); @@ -352,10 +352,10 @@ int16_t uORB::Manager::process_remove_subscription( //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -int16_t uORB::Manager::process_received_message(const std::string& messageName, +int16_t uORB::Manager::process_received_message(const char * messageName, int32_t length, uint8_t* data) { - //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName.c_str() ); + //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName ); int16_t rc = -1; char nodepath[orb_maxpath]; @@ -365,7 +365,7 @@ int16_t uORB::Manager::process_received_message(const std::string& messageName, // get the node name. if ( node == nullptr) { warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]", - __LINE__, messageName.c_str(), nodepath ); + __LINE__, messageName, nodepath ); } else { // node is present. @@ -376,7 +376,7 @@ int16_t uORB::Manager::process_received_message(const std::string& messageName, return rc; } -bool uORB::Manager::is_remote_subscriber_present( const std::string& messageName ) +bool uORB::Manager::is_remote_subscriber_present( const char * messageName ) { - return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() ); + return _remote_subscriber_topics.find( messageName ); } diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp index 1954de2784..7cd0d68e9f 100644 --- a/src/modules/uORB/uORBManager_posix.cpp +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -312,11 +312,11 @@ uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void ) //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -int16_t uORB::Manager::process_add_subscription(const std::string& messageName, +int16_t uORB::Manager::process_add_subscription(const char *messageName, int32_t msgRateInHz) { warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", - __LINE__, messageName.c_str() ); + __LINE__, messageName ); int16_t rc = 0; _remote_subscriber_topics.insert( messageName ); char nodepath[orb_maxpath]; @@ -326,7 +326,7 @@ int16_t uORB::Manager::process_add_subscription(const std::string& messageName, uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); if ( node == nullptr) { warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet", - __LINE__, messageName.c_str() ); + __LINE__, messageName ); } else{ // node is present. @@ -341,10 +341,10 @@ int16_t uORB::Manager::process_add_subscription(const std::string& messageName, //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int16_t uORB::Manager::process_remove_subscription( - const std::string& messageName) + const char * messageName) { warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s", - __LINE__, messageName.c_str() ); + __LINE__, messageName ); int16_t rc = -1; _remote_subscriber_topics.erase( messageName ); char nodepath[orb_maxpath]; @@ -354,7 +354,7 @@ int16_t uORB::Manager::process_remove_subscription( // get the node name. if ( node == nullptr) { warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", - __LINE__, messageName.c_str()); + __LINE__, messageName); } else { // node is present. node->process_remove_subscription(); @@ -366,10 +366,10 @@ int16_t uORB::Manager::process_remove_subscription( //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -int16_t uORB::Manager::process_received_message(const std::string& messageName, +int16_t uORB::Manager::process_received_message(const char * messageName, int32_t length, uint8_t* data) { - //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName.c_str() ); + //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName ); int16_t rc = -1; char nodepath[orb_maxpath]; @@ -379,7 +379,7 @@ int16_t uORB::Manager::process_received_message(const std::string& messageName, // get the node name. if ( node == nullptr) { warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]", - __LINE__, messageName.c_str(), nodepath ); + __LINE__, messageName, nodepath ); } else { // node is present. @@ -390,7 +390,7 @@ int16_t uORB::Manager::process_received_message(const std::string& messageName, return rc; } -bool uORB::Manager::is_remote_subscriber_present( const std::string& messageName ) +bool uORB::Manager::is_remote_subscriber_present( const char * messageName ) { return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() ); } diff --git a/src/modules/uORB/uORBUtils.cpp b/src/modules/uORB/uORBUtils.cpp index 3bedc5ecd9..a18fa1453a 100644 --- a/src/modules/uORB/uORBUtils.cpp +++ b/src/modules/uORB/uORBUtils.cpp @@ -65,14 +65,14 @@ int uORB::Utils::node_mkpath //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int uORB::Utils::node_mkpath(char *buf, Flavor f, - const std::string& orbMsgName ) + const char *orbMsgName ) { unsigned len; unsigned index = 0; len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param", - orbMsgName.c_str(), index ); + orbMsgName, index ); if (len >= orb_maxpath) return -ENAMETOOLONG; diff --git a/src/modules/uORB/uORBUtils.hpp b/src/modules/uORB/uORBUtils.hpp index f9df35eee8..522d255b26 100644 --- a/src/modules/uORB/uORBUtils.hpp +++ b/src/modules/uORB/uORBUtils.hpp @@ -55,7 +55,7 @@ public: /** * same as above except this generators the path based on the string. */ - static int node_mkpath(char *buf, Flavor f, const std::string& orbMsgName); + static int node_mkpath(char *buf, Flavor f, const char *orbMsgName); }; From 05b6bcd168fb5a1f304330ca52c06fca8671587e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 9 Jun 2015 19:16:06 -0700 Subject: [PATCH 33/48] Added missing return in ORBSet Signed-off-by: Mark Charlebois --- src/modules/uORB/ORBSet.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/ORBSet.h b/src/modules/uORB/ORBSet.h index 2d5c59e06d..8b1e0d00fb 100644 --- a/src/modules/uORB/ORBSet.h +++ b/src/modules/uORB/ORBSet.h @@ -101,6 +101,7 @@ public: while (p->next) { if (strcmp(p->next->node_name, node_name) == 0) { unlinkNext(p); + return true; } } return nullptr; From 4df833d25d03f6cdeb7eb12c5bd2ca54bf606b29 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 9 Jun 2015 19:31:56 -0700 Subject: [PATCH 34/48] uORB: factor out ORBMap.hpp into a separate file The new uORB::ORBMap class was put it its own file with proper copyright. Signed-off-by: Mark Charlebois --- src/modules/uORB/{ORBSet.h => ORBSet.hpp} | 0 src/modules/uORB/uORBDevices_nuttx.hpp | 86 +---------------------- src/modules/uORB/uORBManager.hpp | 2 +- 3 files changed, 2 insertions(+), 86 deletions(-) rename src/modules/uORB/{ORBSet.h => ORBSet.hpp} (100%) diff --git a/src/modules/uORB/ORBSet.h b/src/modules/uORB/ORBSet.hpp similarity index 100% rename from src/modules/uORB/ORBSet.h rename to src/modules/uORB/ORBSet.hpp diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp index ef4bb3b672..012e7893dd 100644 --- a/src/modules/uORB/uORBDevices_nuttx.hpp +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -37,6 +37,7 @@ #include #include #include +#include "ORBMap.hpp" #include "uORBCommon.hpp" @@ -44,93 +45,8 @@ namespace uORB { class DeviceNode; class DeviceMaster; - class ORBMap; } -class uORB::ORBMap -{ -public: - struct Node { - struct Node *next; - const char * node_name; - uORB::DeviceNode *node; - }; - - ORBMap() : - _top(nullptr), - _end(nullptr) - { } - ~ORBMap() { - while (_top != nullptr) { - unlinkNext(_top); - if (_top->next == nullptr) { - free((void *)_top->node_name); - free(_top); - _top = nullptr; - _end = nullptr; - } - } - } - void insert(const char *node_name, uORB::DeviceNode*node) - { - Node **p; - if (_top == nullptr) - p = &_top; - else - p = &_end->next; - - *p = (Node *)malloc(sizeof(Node)); - if (_end) - _end = _end->next; - else { - _end = _top; - } - _end->next = nullptr; - _end->node_name = strdup(node_name); - _end->node = node; - } - - bool find(const char *node_name) - { - Node *p = _top; - while (p) { - if (strcmp(p->node_name, node_name) == 0) { - return true; - } - p = p->next; - } - return false; - } - - uORB::DeviceNode* get(const char *node_name) - { - Node *p = _top; - while (p) { - if (strcmp(p->node_name, node_name) == 0) { - return p->node; - } - } - return nullptr; - } - - void unlinkNext(Node *a) - { - Node *b = a->next; - if (b != nullptr) { - if (_end == b) { - _end = a; - } - a->next = b->next; - free((void *)b->node_name); - free(b); - } - } - -private: - Node *_top; - Node *_end; -}; - /** * Per-object device instance. */ diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index 60a0cc5541..a4fb49c82c 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -38,7 +38,7 @@ #include "uORBDevices.hpp" #include #ifdef __PX4_NUTTX -#include "ORBSet.h" +#include "ORBSet.hpp" #else #include #include From 03d7d770a6f216603d17ade8af9a8c143b8080f7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 9 Jun 2015 19:34:34 -0700 Subject: [PATCH 35/48] Forgot to add ORBMap.hpp Signed-off-by: Mark Charlebois --- src/modules/uORB/ORBMap.hpp | 129 ++++++++++++++++++++++++++++++++++++ 1 file changed, 129 insertions(+) create mode 100644 src/modules/uORB/ORBMap.hpp diff --git a/src/modules/uORB/ORBMap.hpp b/src/modules/uORB/ORBMap.hpp new file mode 100644 index 0000000000..4b6c7e5a12 --- /dev/null +++ b/src/modules/uORB/ORBMap.hpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + + +namespace uORB +{ + class DeviceNode; + class ORBMap; +} + +class uORB::ORBMap +{ +public: + struct Node { + struct Node *next; + const char * node_name; + uORB::DeviceNode *node; + }; + + ORBMap() : + _top(nullptr), + _end(nullptr) + { } + ~ORBMap() { + while (_top != nullptr) { + unlinkNext(_top); + if (_top->next == nullptr) { + free((void *)_top->node_name); + free(_top); + _top = nullptr; + _end = nullptr; + } + } + } + void insert(const char *node_name, uORB::DeviceNode*node) + { + Node **p; + if (_top == nullptr) + p = &_top; + else + p = &_end->next; + + *p = (Node *)malloc(sizeof(Node)); + if (_end) + _end = _end->next; + else { + _end = _top; + } + _end->next = nullptr; + _end->node_name = strdup(node_name); + _end->node = node; + } + + bool find(const char *node_name) + { + Node *p = _top; + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return true; + } + p = p->next; + } + return false; + } + + uORB::DeviceNode* get(const char *node_name) + { + Node *p = _top; + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return p->node; + } + } + return nullptr; + } + + void unlinkNext(Node *a) + { + Node *b = a->next; + if (b != nullptr) { + if (_end == b) { + _end = a; + } + a->next = b->next; + free((void *)b->node_name); + free(b); + } + } + +private: + Node *_top; + Node *_end; +}; + From 9c90e474005eaddd15fbcaa418997e55088b357a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 9 Jun 2015 19:36:29 -0700 Subject: [PATCH 36/48] Fixed ORBMap.hpp copyright Signed-off-by: Mark Charlebois --- src/modules/uORB/ORBMap.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/ORBMap.hpp b/src/modules/uORB/ORBMap.hpp index 4b6c7e5a12..99247d951a 100644 --- a/src/modules/uORB/ORBMap.hpp +++ b/src/modules/uORB/ORBMap.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2015 Mark Charlebois. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions From 30969eb10c58979f7742758914747a37de17d1be Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 10 Jun 2015 13:20:13 +0200 Subject: [PATCH 37/48] Navigator: Use correct open call --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c104f8162c..28bd482cc2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -257,7 +257,7 @@ Navigator::task_main_trampoline(int argc, char *argv[]) void Navigator::task_main() { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); _geofence.setMavlinkFd(_mavlink_fd); /* Try to load the geofence: @@ -346,7 +346,7 @@ Navigator::task_main() if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { /* try to reopen the mavlink log device with specified interval */ mavlink_open_time = hrt_abstime() + mavlink_open_interval; - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); } static bool have_geofence_position_data = false; From 18304a2a0dff8f6b3499861ece27ae98615a367c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 11 Jun 2015 11:36:58 -0700 Subject: [PATCH 38/48] POSIX: Fixed px4_getpid() calls from shell context When px4_getpid() was called from the shell, there was no opaque thread ID to return. Added a special thread ID for the shell context. This ID only works for px4_getpid() and cannot be used for other px4_task_*() calls. Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/px4_posix_impl.cpp | 4 ++++ src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 13 ++++++++++--- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 0d926f0520..243716abe6 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -48,6 +48,8 @@ #include "systemlib/param/param.h" #include "hrt_work.h" +extern pthread_t _shell_task_id; + __BEGIN_DECLS long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK); @@ -63,6 +65,8 @@ void init_once(void); void init_once(void) { + _shell_task_id = pthread_self(); + PX4_INFO("Shell id is %lu", _shell_task_id); work_queues_init(); hrt_work_queue_init(); hrt_init(); diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index eb442e708b..ad24d32bc6 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -58,6 +58,10 @@ #define MAX_CMD_LEN 100 #define PX4_MAX_TASKS 100 +#define SHELL_TASK_ID (PX4_MAX_TASKS+1) + +pthread_t _shell_task_id = 0; + struct task_entry { pthread_t pid; @@ -243,7 +247,7 @@ int px4_task_kill(px4_task_t id, int sig) pthread_t pid; PX4_DEBUG("Called px4_task_kill %d", sig); - if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) + if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0) pid = taskmap[id].pid; else return -EINVAL; @@ -278,13 +282,16 @@ int px4_getpid() { pthread_t pid = pthread_self(); + if (pid == _shell_task_id) + return SHELL_TASK_ID; + // Get pthread ID from the opaque ID for (int i=0; i Date: Thu, 11 Jun 2015 17:28:46 -0700 Subject: [PATCH 39/48] POSIX: Added sleep command The baro was not fully initialized when the sensors module tried to open it. Added a sleep command and a sleep 2 to rc.S so the baro is initialized by the time the sensors module tried to read it. Fixed other noisy errors Signed-off-by: Mark Charlebois --- Tools/posix_apps.py | 12 ++++++++++++ makefiles/posix/config_posix_default.mk | 1 + posix-configs/posixtest/init/rc.S | 1 + src/drivers/device/i2c_posix.cpp | 2 +- src/modules/sensors/sensors.cpp | 2 +- src/platforms/posix/drivers/accelsim/accelsim.cpp | 9 ++++----- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 6 ++++++ 7 files changed, 26 insertions(+), 7 deletions(-) diff --git a/Tools/posix_apps.py b/Tools/posix_apps.py index 00e11fcbbb..1dfca71e27 100755 --- a/Tools/posix_apps.py +++ b/Tools/posix_apps.py @@ -48,6 +48,7 @@ print """ #include #include +#include using namespace std; @@ -64,6 +65,7 @@ static int list_tasks_main(int argc, char *argv[]); static int list_files_main(int argc, char *argv[]); static int list_devices_main(int argc, char *argv[]); static int list_topics_main(int argc, char *argv[]); +static int sleep_main(int argc, char *argv[]); } @@ -81,6 +83,7 @@ print '\tapps["list_tasks"] = list_tasks_main;' print '\tapps["list_files"] = list_files_main;' print '\tapps["list_devices"] = list_devices_main;' print '\tapps["list_topics"] = list_topics_main;' +print '\tapps["sleep"] = sleep_main;' print """ return apps; } @@ -122,5 +125,14 @@ static int list_files_main(int argc, char *argv[]) px4_show_files(); return 0; } +static int sleep_main(int argc, char *argv[]) +{ + if (argc != 2) { + cout << "Usage: sleep " << endl; + return 1; + } + sleep(atoi(argv[1])); + return 0; +} """ diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk index 30dada7bbd..5f1d80bdea 100644 --- a/makefiles/posix/config_posix_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -7,6 +7,7 @@ # MODULES += drivers/device MODULES += drivers/blinkm +MODULES += drivers/buzzer MODULES += drivers/hil MODULES += drivers/rgbled MODULES += drivers/led diff --git a/posix-configs/posixtest/init/rc.S b/posix-configs/posixtest/init/rc.S index 896062639e..93e6c69d01 100644 --- a/posix-configs/posixtest/init/rc.S +++ b/posix-configs/posixtest/init/rc.S @@ -1,5 +1,6 @@ uorb start simulator start -s +sleep 2 barosim start adcsim start accelsim start diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index ddf7a0db4c..aa031d9e42 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -69,7 +69,7 @@ I2C::I2C(const char *name, _address(address), _fd(-1) { - warnx("I2C::I2C name = %s devname = %s", name, devname); + //warnx("I2C::I2C name = %s devname = %s", name, devname); // fill in _device_id fields for a I2C device _device_id.devid_s.bus_type = DeviceBusType_I2C; _device_id.devid_s.bus = bus; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index bc1ae89906..0be31046b1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -868,7 +868,7 @@ Sensors::parameters_update() barofd = px4_open(BARO0_DEVICE_PATH, 0); if (barofd < 0) { - warnx("ERROR: no barometer foundon %s", BARO0_DEVICE_PATH); + warnx("ERROR: no barometer found on %s", BARO0_DEVICE_PATH); return ERROR; } else { diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 994b1bfb9b..174c522383 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -774,7 +774,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) unsigned period = 1000000 / arg; /* check against maximum sane rate (1ms) */ - if (period < 1000) + if (period < 10000) return -EINVAL; /* update interval for next measurement */ @@ -961,11 +961,10 @@ ACCELSIM::start() //PX4_INFO("ACCELSIM::start accel %u", _call_accel_interval); hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&ACCELSIM::measure_trampoline, this); - - // There is a race here where SENSORIOCSPOLLRATE on the accel starts polling of mag but mag period is set to 0 + // There is a race here where SENSORIOCSPOLLRATE on the accel starts polling of mag but _call_mag_interval is 0 if (_call_mag_interval == 0) { - PX4_ERR("_call_mag_interval uninitilized - would have set period delay of 0"); - _call_mag_interval = 1000; + //PX4_INFO("_call_mag_interval uninitilized - would have set period delay of 0"); + _call_mag_interval = 10000; // Max 100Hz } //PX4_INFO("ACCELSIM::start mag %u", _call_mag_interval); diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 54e448cabb..40d8f71c1b 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -466,6 +466,9 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro // disable debug() calls _debug_enabled = false; + // Don't publish until initialized + _pub_blocked = true; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; /* Prime _gyro with parents devid. */ @@ -588,6 +591,9 @@ GYROSIM::init() if (_accel_topic == nullptr) { PX4_WARN("ADVERT FAIL"); } + else { + _pub_blocked = false; + } /* advertise sensor topic, measure manually to initialize valid report */ From e4a8f32f1ba9cec74a71e06a1dd00000f8d2e310 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 11 Jun 2015 18:13:36 -0700 Subject: [PATCH 40/48] QuRT: Added HRT workqueues as per POSIX A high rate workqueue is required that acts like an interrupt handler for a HW timer. Signed-off-by: Mark Charlebois --- makefiles/posix/config_posix_default.mk | 1 - src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 2 +- src/platforms/qurt/px4_layer/module.mk | 3 +++ src/platforms/qurt/px4_layer/px4_qurt_impl.cpp | 6 ++++++ src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 13 ++++++++++--- 5 files changed, 20 insertions(+), 5 deletions(-) diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk index 5f1d80bdea..30dada7bbd 100644 --- a/makefiles/posix/config_posix_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -7,7 +7,6 @@ # MODULES += drivers/device MODULES += drivers/blinkm -MODULES += drivers/buzzer MODULES += drivers/hil MODULES += drivers/rgbled MODULES += drivers/led diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index ad24d32bc6..ac4c6b62bf 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -57,7 +57,7 @@ #define MAX_CMD_LEN 100 -#define PX4_MAX_TASKS 100 +#define PX4_MAX_TASKS 50 #define SHELL_TASK_ID (PX4_MAX_TASKS+1) pthread_t _shell_task_id = 0; diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index aa1a3b7d61..13b41db3ad 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -40,6 +40,9 @@ SRCDIR=$(dir $(MODULE_MK)) SRCS = \ px4_qurt_impl.cpp \ px4_qurt_tasks.cpp \ + hrt_thread.c \ + hrt_queue.c \ + hrt_work_cancel.c \ work_thread.c \ work_queue.c \ work_lock.c \ diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 9087fd873e..a5782a6f25 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -49,8 +49,11 @@ #include #include #include "systemlib/param/param.h" +#include "hrt_work.h" +extern pthread_t _shell_task_id; + __BEGIN_DECLS // FIXME - sysconf(_SC_CLK_TCK) not supported @@ -92,7 +95,10 @@ void init_once(void) // Required for QuRT //_posix_init(); + _shell_task_id = pthread_self(); + PX4_INFO("Shell id is %lu", _shell_task_id); work_queues_init(); + hrt_work_queue_init(); hrt_init(); } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index bea12adfbc..cf6d665724 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -57,7 +57,11 @@ #define MAX_CMD_LEN 100 -#define PX4_MAX_TASKS 100 +#define PX4_MAX_TASKS 50 +#define SHELL_TASK_ID (PX4_MAX_TASKS+1) + +pthread_t _shell_task_id = 0; + struct task_entry { pthread_t pid; @@ -269,13 +273,16 @@ int px4_getpid() { pthread_t pid = pthread_self(); + if (pid == _shell_task_id) + return SHELL_TASK_ID; + // Get pthread ID from the opaque ID for (int i=0; i Date: Thu, 11 Jun 2015 18:16:29 -0700 Subject: [PATCH 41/48] QuRT: Added missing hrt workqueue files Signed-off-by: Mark Charlebois --- src/platforms/qurt/px4_layer/hrt_queue.c | 129 +++++++++ src/platforms/qurt/px4_layer/hrt_thread.c | 256 ++++++++++++++++++ src/platforms/qurt/px4_layer/hrt_work.h | 65 +++++ .../qurt/px4_layer/hrt_work_cancel.c | 111 ++++++++ 4 files changed, 561 insertions(+) create mode 100644 src/platforms/qurt/px4_layer/hrt_queue.c create mode 100644 src/platforms/qurt/px4_layer/hrt_thread.c create mode 100644 src/platforms/qurt/px4_layer/hrt_work.h create mode 100644 src/platforms/qurt/px4_layer/hrt_work_cancel.c diff --git a/src/platforms/qurt/px4_layer/hrt_queue.c b/src/platforms/qurt/px4_layer/hrt_queue.c new file mode 100644 index 0000000000..04bf8bdd49 --- /dev/null +++ b/src/platforms/qurt/px4_layer/hrt_queue.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * libc/wqueue/work_queue.c + * + * Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include "hrt_work.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: hrt_work_queue + * + * Description: + * Queue work to be performed at a later time. All queued work will be + * performed on the worker thread of of execution (not the caller's). + * + * The work structure is allocated by caller, but completely managed by + * the work queue logic. The caller should never modify the contents of + * the work queue structure; the caller should not call work_queue() + * again until either (1) the previous work has been performed and removed + * from the queue, or (2) work_cancel() has been called to cancel the work + * and remove it from the work queue. + * + * Input parameters: + * work - The work structure to queue + * worker - The worker callback to be invoked. The callback will invoked + * on the worker thread of execution. + * arg - The argument that will be passed to the workder callback when + * int is invoked. + * delay - Delay (in microseconds) from the time queue until the worker + * is invoked. Zero means to perform the work immediately. + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay) +{ + struct wqueue_s *wqueue = &g_hrt_work; + + /* First, initialize the work structure */ + + work->worker = worker; /* Work callback */ + work->arg = arg; /* Callback argument */ + work->delay = delay; /* Delay until work performed */ + + /* Now, time-tag that entry and put it in the work queue. This must be + * done with interrupts disabled. This permits this function to be called + * from with task logic or interrupt handlers. + */ + + hrt_work_lock(); + work->qtime = hrt_absolute_time(); /* Time work queued */ + //PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime); + + dq_addlast((dq_entry_t *)work, &wqueue->q); + px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ + + hrt_work_unlock(); + return PX4_OK; +} + diff --git a/src/platforms/qurt/px4_layer/hrt_thread.c b/src/platforms/qurt/px4_layer/hrt_thread.c new file mode 100644 index 0000000000..dc0f3baa97 --- /dev/null +++ b/src/platforms/qurt/px4_layer/hrt_thread.c @@ -0,0 +1,256 @@ +/**************************************************************************** + * libc/wqueue/work_thread.c + * + * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * Modified by: Mark Charlebois to use hrt compatible times + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "hrt_work.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/* The state of each work queue. */ +struct wqueue_s g_hrt_work; + +/**************************************************************************** + * Private Variables + ****************************************************************************/ +sem_t _hrt_work_lock; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ +static void hrt_work_process(void); + +/**************************************************************************** + * Name: work_process + * + * Description: + * This is the logic that performs actions placed on any work list. + * + * Input parameters: + * wqueue - Describes the work queue to be processed + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void hrt_work_process() +{ + struct wqueue_s *wqueue = &g_hrt_work; + volatile FAR struct work_s *work; + worker_t worker; + FAR void *arg; + uint64_t elapsed; + uint32_t remaining; + uint32_t next; + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + /* Default to sleeping for 1 sec */ + next = 1000000; + + hrt_work_lock(); + + work = (FAR struct work_s *)wqueue->q.head; + while (work) + { + /* Is this work ready? It is ready if there is no delay or if + * the delay has elapsed. qtime is the time that the work was added + * to the work queue. It will always be greater than or equal to + * zero. Therefore a delay of zero will always execute immediately. + */ + + elapsed = hrt_absolute_time() - work->qtime; + //PX4_INFO("hrt work_process: in usec elapsed=%lu delay=%u work=%p", elapsed, work->delay, work); + if (elapsed >= work->delay) + { + /* Remove the ready-to-execute work from the list */ + + (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); + //PX4_INFO("Dequeued work=%p", work); + + /* Extract the work description from the entry (in case the work + * instance by the re-used after it has been de-queued). + */ + + worker = work->worker; + arg = work->arg; + + /* Mark the work as no longer being queued */ + + work->worker = NULL; + + /* Do the work. Re-enable interrupts while the work is being + * performed... we don't have any idea how long that will take! + */ + + hrt_work_unlock(); + if (!worker) { + PX4_ERR("MESSED UP: worker = 0"); + } + else { + worker(arg); + } + + /* Now, unfortunately, since we re-enabled interrupts we don't + * know the state of the work list and we will have to start + * back at the head of the list. + */ + + hrt_work_lock(); + work = (FAR struct work_s *)wqueue->q.head; + } + else + { + /* This one is not ready.. will it be ready before the next + * scheduled wakeup interval? + */ + + /* Here: elapsed < work->delay */ + remaining = work->delay - elapsed; + //PX4_INFO("remaining=%u delay=%u elapsed=%lu", remaining, work->delay, elapsed); + if (remaining < next) + { + /* Yes.. Then schedule to wake up when the work is ready */ + + next = remaining; + } + + /* Then try the next in the list. */ + + work = (FAR struct work_s *)work->dq.flink; + //PX4_INFO("next %u work %p", next, work); + } + } + + /* Wait awhile to check the work list. We will wait here until either + * the time elapses or until we are awakened by a signal. + */ + hrt_work_unlock(); + + /* might sleep less if a signal received and new item was queued */ + //PX4_INFO("Sleeping for %u usec", next); + usleep(next); +} + +/**************************************************************************** + * Name: work_hrtthread + * + * Description: + * This is the worker threads that performs actions placed on the ISR work + * list. + * + * work_hpthread and work_lpthread: These are the kernel mode work queues + * (also build in the flat build). One of these threads also performs + * periodic garbage collection (that is otherwise performed by the idle + * thread if CONFIG_SCHED_WORKQUEUE is not defined). + * + * These worker threads are started by the OS during normal bringup. + * + * All of these entrypoints are referenced by OS internally and should not + * not be accessed by application logic. + * + * Input parameters: + * argc, argv (not used) + * + * Returned Value: + * Does not return + * + ****************************************************************************/ + +static int work_hrtthread(int argc, char *argv[]) +{ + /* Loop forever */ + + for (;;) + { + /* First, perform garbage collection. This cleans-up memory de-allocations + * that were queued because they could not be freed in that execution + * context (for example, if the memory was freed from an interrupt handler). + * NOTE: If the work thread is disabled, this clean-up is performed by + * the IDLE thread (at a very, very low priority). + */ + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + hrt_work_process(); + } + + return PX4_OK; /* To keep some compilers happy */ +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void hrt_work_queue_init(void) +{ + sem_init(&_hrt_work_lock, 0, 1); + + // Create high priority worker thread + g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 2000, + work_hrtthread, + (char* const*)NULL); + +} + diff --git a/src/platforms/qurt/px4_layer/hrt_work.h b/src/platforms/qurt/px4_layer/hrt_work.h new file mode 100644 index 0000000000..566684eb86 --- /dev/null +++ b/src/platforms/qurt/px4_layer/hrt_work.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +#pragma once + +__BEGIN_DECLS + +extern sem_t _hrt_work_lock; +extern struct wqueue_s g_hrt_work; + +void hrt_work_queue_init(void); +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); +void hrt_work_cancel(struct work_s *work); + +inline void hrt_work_lock(void); +inline void hrt_work_unlock(void); + +inline void hrt_work_lock() +{ + //PX4_INFO("hrt_work_lock"); + sem_wait(&_hrt_work_lock); +} + +inline void hrt_work_unlock() +{ + //PX4_INFO("hrt_work_unlock"); + sem_post(&_hrt_work_lock); +} + +__END_DECLS + diff --git a/src/platforms/qurt/px4_layer/hrt_work_cancel.c b/src/platforms/qurt/px4_layer/hrt_work_cancel.c new file mode 100644 index 0000000000..c1c2c3bef7 --- /dev/null +++ b/src/platforms/qurt/px4_layer/hrt_work_cancel.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * libc/wqueue/work_cancel.c + * + * Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include "hrt_work.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: hrt_work_cancel + * + * Description: + * Cancel previously queued work. This removes work from the work queue. + * After work has been canceled, it may be re-queue by calling + * hrt_work_queue() again. + * + * Input parameters: + * work - The previously queue work structure to cancel + * + ****************************************************************************/ + +void hrt_work_cancel(struct work_s *work) +{ + struct wqueue_s *wqueue = &g_hrt_work; + + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + + /* Cancelling the work is simply a matter of removing the work structure + * from the work queue. This must be done with interrupts disabled because + * new work is typically added to the work queue from interrupt handlers. + */ + + hrt_work_lock(); + if (work->worker != NULL) + { + /* A little test of the integrity of the work queue */ + + //DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); + //DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); + + /* Remove the entry from the work queue and make sure that it is + * mark as availalbe (i.e., the worker field is nullified). + */ + + dq_rem((FAR dq_entry_t *)work, &wqueue->q); + work->worker = NULL; + } + + hrt_work_unlock(); +} From 527b97e8b4d08cae20b70c2bf7d2469b18ae1d04 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 11 Jun 2015 19:00:49 -0700 Subject: [PATCH 42/48] POSIX: added tone_alarm simulator The tone_alarm simulator was added to rc.S and the warning output for a hrt_timer with a 0 expiry times was disabled. Signed-off-by: Mark Charlebois --- posix-configs/posixtest/init/rc.S | 1 + src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp | 7 ++++--- src/platforms/posix/px4_layer/drv_hrt.c | 3 +++ 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/posix-configs/posixtest/init/rc.S b/posix-configs/posixtest/init/rc.S index 93e6c69d01..9a15db2f01 100644 --- a/posix-configs/posixtest/init/rc.S +++ b/posix-configs/posixtest/init/rc.S @@ -5,6 +5,7 @@ barosim start adcsim start accelsim start gyrosim start +tone_alarm start param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1310720 param set CAL_ACC1_ID 1376256 diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index 37f51ff607..b127128a66 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -350,7 +350,7 @@ ToneAlarm::start_note(unsigned note) // Silence warning of unused var do_something(period); - PX4_DEBUG("ToneAlarm::start_note %u", period); + PX4_INFO("ToneAlarm::start_note %u", period); } void @@ -361,6 +361,7 @@ ToneAlarm::stop_note() void ToneAlarm::start_tune(const char *tune) { + PX4_INFO("ToneAlarm::start_tune"); // kill any current playback hrt_cancel(&_note_call); @@ -533,7 +534,7 @@ ToneAlarm::next_note() // tune looks bad (unexpected EOF, bad character, etc.) tune_error: - printf("tune error\n"); + PX4_ERR("tune error\n"); _repeat = false; // don't loop on error // stop (and potentially restart) the tune @@ -605,7 +606,7 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* decide whether to increase the alarm level to cmd or leave it alone */ switch (cmd) { case TONE_SET_ALARM: - debug("TONE_SET_ALARM %u", arg); + PX4_INFO("TONE_SET_ALARM %lu", arg); if (arg < TONE_NUMBER_OF_TUNES) { if (arg == TONE_STOP_TUNE) { diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 3c1a2323fb..6ef217e218 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -301,9 +301,12 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte if (entry->deadline != 0) sq_rem(&entry->link, &callout_queue); +#if 0 + // Use this to debug busy CPU that keeps rescheduling with 0 period time if (interval < HRT_INTERVAL_MIN) { PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval); } +#endif entry->deadline = deadline; entry->period = interval; entry->callout = callout; From ea7d5070c54487420e5f2468c0bc9d65ad9215e2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 11 Jun 2015 19:42:54 -0700 Subject: [PATCH 43/48] POSIX: Fixed some of the failing gtests The orb_advert_t change from int to void * required some fixups for the gtests. Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/dq_addlast.c | 1 + src/platforms/posix/px4_layer/dq_rem.c | 1 + src/platforms/posix/px4_layer/dq_remfirst.c | 1 + src/platforms/posix/px4_layer/sq_addlast.c | 1 + src/platforms/posix/px4_layer/sq_remfirst.c | 1 + .../uorb_unittests/uORBCommunicatorMock.hpp | 60 ++++---- .../uORBCommunicator_gtests.cpp | 128 +++++++++--------- 7 files changed, 102 insertions(+), 91 deletions(-) diff --git a/src/platforms/posix/px4_layer/dq_addlast.c b/src/platforms/posix/px4_layer/dq_addlast.c index 3ef08abd05..3a2ec3dbea 100644 --- a/src/platforms/posix/px4_layer/dq_addlast.c +++ b/src/platforms/posix/px4_layer/dq_addlast.c @@ -41,6 +41,7 @@ * Included Files ************************************************************/ +#include #include /************************************************************ diff --git a/src/platforms/posix/px4_layer/dq_rem.c b/src/platforms/posix/px4_layer/dq_rem.c index db20762c75..21247add10 100644 --- a/src/platforms/posix/px4_layer/dq_rem.c +++ b/src/platforms/posix/px4_layer/dq_rem.c @@ -41,6 +41,7 @@ * Included Files ************************************************************/ +#include #include /************************************************************ diff --git a/src/platforms/posix/px4_layer/dq_remfirst.c b/src/platforms/posix/px4_layer/dq_remfirst.c index e87acc3382..fc80923cb2 100644 --- a/src/platforms/posix/px4_layer/dq_remfirst.c +++ b/src/platforms/posix/px4_layer/dq_remfirst.c @@ -41,6 +41,7 @@ * Included Files ************************************************************/ +#include #include /************************************************************ diff --git a/src/platforms/posix/px4_layer/sq_addlast.c b/src/platforms/posix/px4_layer/sq_addlast.c index faa07efb5c..25910bc9d7 100644 --- a/src/platforms/posix/px4_layer/sq_addlast.c +++ b/src/platforms/posix/px4_layer/sq_addlast.c @@ -41,6 +41,7 @@ * Included Files ************************************************************/ +#include #include /************************************************************ diff --git a/src/platforms/posix/px4_layer/sq_remfirst.c b/src/platforms/posix/px4_layer/sq_remfirst.c index f81c18dc2e..81751e8579 100644 --- a/src/platforms/posix/px4_layer/sq_remfirst.c +++ b/src/platforms/posix/px4_layer/sq_remfirst.c @@ -41,6 +41,7 @@ * Included Files ************************************************************/ +#include #include /************************************************************ diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.hpp b/unittests/uorb_unittests/uORBCommunicatorMock.hpp index 9279d6446d..89096c09ce 100644 --- a/unittests/uorb_unittests/uORBCommunicatorMock.hpp +++ b/unittests/uorb_unittests/uORBCommunicatorMock.hpp @@ -1,31 +1,35 @@ -//============================================================================= -// File: uORB_test.cpp -// -// @@-COPYRIGHT-START-@@ -// -// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. -// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") -// -// The party receiving this software directly from QTI (the "Recipient") -// may use this software as reasonably necessary solely for the purposes -// set forth in the agreement between the Recipient and QTI (the -// "Agreement"). The software may be used in source code form solely by -// the Recipient's employees (if any) authorized by the Agreement. Unless -// expressly authorized in the Agreement, the Recipient may not sublicense, -// assign, transfer or otherwise provide the source code to any third -// party. Qualcomm Technologies, Inc. retains all ownership rights in and -// to the software -// -// This notice supersedes any other QTI notices contained within the software -// except copyright notices indicating different years of publication for -// different portions of the software. This notice does not supersede the -// application of any third party copyright notice to that third party's -// code. -// -// @@-COPYRIGHT-END-@@ -// -//============================================================================= - +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #ifndef _uORBCommunicatorMock_hpp_ #define _uORBCommunicatorMock_hpp_ diff --git a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp index 86c5cdc90b..1ea12b7a96 100644 --- a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp +++ b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp @@ -1,30 +1,35 @@ -//============================================================================= -// File: uORB_test.cpp -// -// @@-COPYRIGHT-START-@@ -// -// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. -// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") -// -// The party receiving this software directly from QTI (the "Recipient") -// may use this software as reasonably necessary solely for the purposes -// set forth in the agreement between the Recipient and QTI (the -// "Agreement"). The software may be used in source code form solely by -// the Recipient's employees (if any) authorized by the Agreement. Unless -// expressly authorized in the Agreement, the Recipient may not sublicense, -// assign, transfer or otherwise provide the source code to any third -// party. Qualcomm Technologies, Inc. retains all ownership rights in and -// to the software -// -// This notice supersedes any other QTI notices contained within the software -// except copyright notices indicating different years of publication for -// different portions of the software. This notice does not supersede the -// application of any third party copyright notice to that third party's -// code. -// -// @@-COPYRIGHT-END-@@ -// -//============================================================================= +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #include "uORBCommunicatorMock.hpp" #include "uORBCommunicatorMockLoopback.hpp" @@ -69,7 +74,7 @@ namespace uORB_test uORB_test::uORBCommunicatorMock _comm_channel; struct orb_topic_A _topicA; struct orb_topic_B _topicB; - int _pub_fd; + orb_advert_t _pub_ptr; int _sub_fd; uORB::DeviceMaster* _masterDevice; }; @@ -94,10 +99,9 @@ namespace uORB_test ASSERT_EQ( c._add_subscriptionCount, 0 ); //step 1. - // step 1. _topicA.val = 1; - ASSERT_NE( ( _pub_fd = orb_advertise(ORB_ID(topicA), &_topicA) ), 0 ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; - PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + ASSERT_NE( ( _pub_ptr = orb_advertise(ORB_ID(topicA), &_topicA) ), nullptr ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); //step 2. c = _comm_channel.get_interface_counters( "topicA" ); @@ -122,12 +126,11 @@ namespace uORB_test ASSERT_EQ( c._add_subscriptionCount, 0 ); //step 1. - // step 1. _topicA.val = 1; - _pub_fd = orb_advertise(ORB_ID(topicA), &_topicA); - PX4_INFO( "[uORBCommunicatorTest.add_subscription_case2] orb_advertize(topicA) returncode:(%d)", _pub_fd ); - ASSERT_TRUE( ( _pub_fd != -1 && _pub_fd != 0 ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; - PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + _pub_ptr = orb_advertise(ORB_ID(topicA), &_topicA); + PX4_INFO( "[uORBCommunicatorTest.add_subscription_case2] orb_advertize(topicA) returncode:(%p)", _pub_ptr ); + ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); // step 2 ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicA) ) ) , -1 ) << "Subscribe failed: %d" << errno; @@ -163,10 +166,10 @@ namespace uORB_test //step 1. // step 1. _topicA.val = 1; - _pub_fd = orb_advertise(ORB_ID(topicA), &_topicA); - PX4_INFO( "[uORBCommunicatorTest.remove_subscribtion] orb_advertize(topicA) returncode:(%d)", _pub_fd ); - ASSERT_TRUE( ( _pub_fd != -1 && _pub_fd != 0 ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; - PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + _pub_ptr = orb_advertise(ORB_ID(topicA), &_topicA); + PX4_INFO( "[uORBCommunicatorTest.remove_subscribtion] orb_advertize(topicA) returncode:(%p)", _pub_ptr ); + ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); c = _comm_channel.get_interface_counters( "topicA" ); ASSERT_EQ( c._remove_subscriptionCount, 0 ); @@ -203,12 +206,11 @@ namespace uORB_test ASSERT_EQ( c._send_messageCount, 0 ); //step 1. - // step 1. ORB_DEFINE( topicA_sndmsg, struct orb_topic_A ); _topicA.val = 1; - _pub_fd = orb_advertise(ORB_ID(topicA_sndmsg ), &_topicA ); - ASSERT_TRUE( ( _pub_fd != -1 && _pub_fd != 0 ) ) << "Failed to advertize uORB Topic topicA_sndmsg: errno: " << errno; - PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + _pub_ptr = orb_advertise(ORB_ID(topicA_sndmsg ), &_topicA ); + ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic topicA_sndmsg: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); c = _comm_channel.get_interface_counters( "topicA_sndmsg" ); ASSERT_EQ( c._send_messageCount, 0 ); @@ -246,9 +248,9 @@ namespace uORB_test // step 2. _topicB.val = 1; - _pub_fd = orb_advertise(ORB_ID(topicB), &_topicB); - PX4_INFO( "publist handle: 0x%08x", _pub_fd ); - ASSERT_TRUE( _pub_fd != -1 && _pub_fd != 0 ) << "Failed to advertize uORB Topic topicB: errno: " << errno; + _pub_ptr = orb_advertise(ORB_ID(topicB), &_topicB); + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); + ASSERT_TRUE( _pub_ptr != nullptr ) << "Failed to advertize uORB Topic topicB: errno: " << errno; //step 3. c = _comm_channel.get_interface_counters( "topicB" ); @@ -260,7 +262,7 @@ namespace uORB_test //step 4. publish new data. _topicB.val = 2; - ASSERT_EQ( orb_publish( ORB_ID(topicB), _pub_fd, &_topicB), OK ); + ASSERT_EQ( orb_publish( ORB_ID(topicB), _pub_ptr, &_topicB), OK ); //step 5. c = _comm_channel.get_interface_counters( "topicB" ); @@ -275,7 +277,7 @@ namespace uORB_test //step 7. publish new data. _topicB.val = 5; - ASSERT_EQ( orb_publish( ORB_ID(topicB), _pub_fd, &_topicB), OK ); + ASSERT_EQ( orb_publish( ORB_ID(topicB), _pub_ptr, &_topicB), OK ); //step 8. c = _comm_channel.get_interface_counters( "topicB" ); @@ -307,9 +309,9 @@ namespace uORB_test // step 2. _topicB.val = 1; - _pub_fd = orb_advertise(ORB_ID(topicB), &_topicB); - ASSERT_TRUE( _pub_fd != -1 && _pub_fd != 0 ) << "Failed to advertize uORB Topic topicB: errno: " << errno; - PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + _pub_ptr = orb_advertise(ORB_ID(topicB), &_topicB); + ASSERT_TRUE( _pub_ptr != nullptr ) << "Failed to advertize uORB Topic topicB: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); // step 3. ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB)) ), -1 ) << "Subscribe failed for topicB: %d" << errno; @@ -356,9 +358,9 @@ namespace uORB_test // step 2. _topicB.val = 1; - _pub_fd = orb_advertise(ORB_ID(topicB), &_topicB); - ASSERT_TRUE( _pub_fd != -1 && _pub_fd != 0 ) << "Failed to advertize uORB Topic topicB: errno: " << errno; - PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + _pub_ptr = orb_advertise(ORB_ID(topicB), &_topicB); + ASSERT_TRUE( _pub_ptr != nullptr ) << "Failed to advertize uORB Topic topicB: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); // step 3. ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB)) ), -1 ) << "Subscribe failed for topicB: %d" << errno; @@ -404,7 +406,7 @@ namespace uORB_test uORB::Manager::get_instance()->set_uorb_communicator( &_comm_channel_loopback ); // now for the actual test. - int pub_topicA_fd; + orb_advert_t pub_topicA_ptr; int sub_topicA_fd; int sub_topicAClone_fd; @@ -414,10 +416,10 @@ namespace uORB_test // step 1. topicA.val = 10; - pub_topicA_fd = orb_advertise(ORB_ID(topicA), &topicA); - PX4_INFO( "[uORBCommunicatorTest.Loopback]orb_advertize(topicA) return code:(%d)", pub_topicA_fd ); - ASSERT_TRUE( pub_topicA_fd != -1 && pub_topicA_fd != 0 ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; - PX4_INFO( "publist handle: 0x%08x", pub_topicA_fd ); + pub_topicA_ptr = orb_advertise(ORB_ID(topicA), &topicA); + PX4_INFO( "[uORBCommunicatorTest.Loopback]orb_advertize(topicA) return code:(%p)", pub_topicA_ptr ); + ASSERT_TRUE( pub_topicA_ptr != nullptr ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)pub_topicA_ptr ); // step 2. ASSERT_NE( ( sub_topicA_fd = orb_subscribe(ORB_ID(topicA)) ), -1 ) << "Subscribe failed: %d" << errno; @@ -440,7 +442,7 @@ namespace uORB_test // publish a new data and check to see that the data is received on the remote. topicA.val = 15; - ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_fd, &topicA), OK ); + ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_ptr, &topicA), OK ); // check to see that the subscriber got a new value. ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno; @@ -458,7 +460,7 @@ namespace uORB_test // publish a new data and check to see that the data is received on local this should not crash. topicA.val = 20; - ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_fd, &topicA), OK ); + ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_ptr, &topicA), OK ); // check to see that the subscriber got a new value. ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno; @@ -471,7 +473,7 @@ namespace uORB_test // publish a new data; this should not crash. topicA.val = 25; - ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_fd, &topicA), OK ); + ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_ptr, &topicA), OK ); } } From fb402bc096fa2e20dc7a98c9922b03c332065858 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 11 Jun 2015 20:22:49 -0700 Subject: [PATCH 44/48] POSIX: Fixed remaining broke gtests The addition of the hrt workqueue required adding some additional files to unittests/CMakeLists.txt Signed-off-by: Mark Charlebois --- src/modules/uORB/uORBCommunicator.hpp | 1 - src/platforms/posix/px4_layer/hrt_work.h | 8 +++---- unittests/CMakeLists.txt | 3 +++ unittests/sbus2_test.cpp | 10 ++++----- .../uorb_unittests/uORBCommunicatorMock.cpp | 20 ++++++++--------- .../uorb_unittests/uORBCommunicatorMock.hpp | 9 ++++---- .../uORBCommunicatorMockLoopback.cpp | 22 +++++++++---------- .../uORBCommunicatorMockLoopback.hpp | 7 +++--- 8 files changed, 42 insertions(+), 38 deletions(-) diff --git a/src/modules/uORB/uORBCommunicator.hpp b/src/modules/uORB/uORBCommunicator.hpp index de6984d1ac..e7db2e464f 100644 --- a/src/modules/uORB/uORBCommunicator.hpp +++ b/src/modules/uORB/uORBCommunicator.hpp @@ -35,7 +35,6 @@ #define _uORBCommunicator_hpp_ #include -#include namespace uORBCommunicator { diff --git a/src/platforms/posix/px4_layer/hrt_work.h b/src/platforms/posix/px4_layer/hrt_work.h index 566684eb86..d926a6d250 100644 --- a/src/platforms/posix/px4_layer/hrt_work.h +++ b/src/platforms/posix/px4_layer/hrt_work.h @@ -46,16 +46,16 @@ void hrt_work_queue_init(void); int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); void hrt_work_cancel(struct work_s *work); -inline void hrt_work_lock(void); -inline void hrt_work_unlock(void); +//inline void hrt_work_lock(void); +//inline void hrt_work_unlock(void); -inline void hrt_work_lock() +static inline void hrt_work_lock() { //PX4_INFO("hrt_work_lock"); sem_wait(&_hrt_work_lock); } -inline void hrt_work_unlock() +static inline void hrt_work_unlock() { //PX4_INFO("hrt_work_unlock"); sem_post(&_hrt_work_lock); diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 810e0a2600..a006284df6 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -74,6 +74,7 @@ add_library( px4_platform ${PX_SRC}/platforms/posix/px4_layer/px4_posix_impl.cpp ${PX_SRC}/platforms/posix/px4_layer/px4_posix_tasks.cpp ${PX_SRC}/platforms/posix/px4_layer/work_lock.c + ${PX_SRC}/platforms/posix/px4_layer/hrt_queue.c ${PX_SRC}/platforms/posix/px4_layer/work_queue.c ${PX_SRC}/platforms/posix/px4_layer/dq_rem.c ${PX_SRC}/platforms/posix/px4_layer/sq_addlast.c @@ -81,7 +82,9 @@ add_library( px4_platform ${PX_SRC}/platforms/posix/px4_layer/sq_addafter.c ${PX_SRC}/platforms/posix/px4_layer/queue.c ${PX_SRC}/platforms/posix/px4_layer/work_cancel.c + ${PX_SRC}/platforms/posix/px4_layer/hrt_work_cancel.c ${PX_SRC}/platforms/posix/px4_layer/dq_remfirst.c + ${PX_SRC}/platforms/posix/px4_layer/hrt_thread.c ${PX_SRC}/platforms/posix/px4_layer/work_thread.c ${PX_SRC}/platforms/posix/px4_layer/drv_hrt.c ${PX_SRC}/platforms/posix/px4_layer/sq_remfirst.c diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 41f49627d4..1c32e3bf84 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -34,10 +34,10 @@ TEST(SBUS2Test, SBUS2) uint8_t frame[30]; unsigned partial_frame_count = 0; uint16_t rc_values[18]; - uint16_t num_values; - bool sbus_failsafe; - bool sbus_frame_drop; - uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); + //uint16_t num_values; + //bool sbus_failsafe; + //bool sbus_frame_drop; + //uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); float last_time = 0; @@ -59,7 +59,7 @@ TEST(SBUS2Test, SBUS2) last_time = f; // Pipe the data into the parser - hrt_abstime now = hrt_absolute_time(); + //hrt_abstime now = hrt_absolute_time(); //if (partial_frame_count % 25 == 0) //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.cpp b/unittests/uorb_unittests/uORBCommunicatorMock.cpp index 118bd95202..d824d9da07 100644 --- a/unittests/uorb_unittests/uORBCommunicatorMock.cpp +++ b/unittests/uorb_unittests/uORBCommunicatorMock.cpp @@ -50,13 +50,13 @@ uORB_test::uORBCommunicatorMock::uORBCommunicatorMock() int16_t uORB_test::uORBCommunicatorMock::add_subscription ( - const std::string& messageName, + const char * messageName, int32_t msgRateInHz ) { int16_t rc = 0; - PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName.c_str(), msgRateInHz ); + PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName, msgRateInHz ); _msgCounters[messageName]._add_subscriptionCount++; /* @@ -78,11 +78,11 @@ int16_t uORB_test::uORBCommunicatorMock::add_subscription int16_t uORB_test::uORBCommunicatorMock::remove_subscription ( - const std::string& messageName + const char * messageName ) { int16_t rc = 0; - PX4_INFO( "got remove_subscription for msg[%s]", messageName.c_str() ); + PX4_INFO( "got remove_subscription for msg[%s]", messageName ); _msgCounters[messageName]._remove_subscriptionCount++; /* int16_t rc = -1; @@ -113,27 +113,27 @@ int16_t uORB_test::uORBCommunicatorMock::register_handler int16_t uORB_test::uORBCommunicatorMock::send_message ( - const std::string& messageName, + const char * messageName, int32_t length, uint8_t* data ) { int16_t rc = 0; - PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName.c_str(), length ); + PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName, length ); if( uORB::Manager::get_instance()->is_remote_subscriber_present( messageName ) ) { _msgCounters[messageName]._send_messageCount++; - if( messageName == "topicA" ) + if( strcmp(messageName, "topicA") == 0 ) { memcpy( &_topicAData, (void*)data, length ); } - else if( messageName == "topicB" ) + else if( strcmp(messageName, "topicB") == 0 ) { memcpy( &_topicBData, (void*)data, length ); } else { - //EPRINTF( "error messageName[%s] is not supported", messageName.c_str() ); + //EPRINTF( "error messageName[%s] is not supported", messageName ); } } /* @@ -200,7 +200,7 @@ void uORB_test::uORBCommunicatorMock::reset_counters() } } -uORB_test::uORBCommunicatorMock::InterfaceCounters uORB_test::uORBCommunicatorMock::get_interface_counters(const std::string& messageName ) +uORB_test::uORBCommunicatorMock::InterfaceCounters uORB_test::uORBCommunicatorMock::get_interface_counters(const char * messageName ) { return _msgCounters[ messageName ]; } diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.hpp b/unittests/uorb_unittests/uORBCommunicatorMock.hpp index 89096c09ce..e0cb3da532 100644 --- a/unittests/uorb_unittests/uORBCommunicatorMock.hpp +++ b/unittests/uorb_unittests/uORBCommunicatorMock.hpp @@ -36,6 +36,7 @@ #include "uORB/uORBCommunicator.hpp" #include "uORBGtestTopics.hpp" #include +#include #include namespace uORB_test @@ -72,7 +73,7 @@ class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t add_subscription( const std::string& messageName, int32_t msgRateInHz ); + virtual int16_t add_subscription( const char *messageName, int32_t msgRateInHz ); /** @@ -86,7 +87,7 @@ class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel * Note: This does not necessarily mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t remove_subscription( const std::string& messageName ); + virtual int16_t remove_subscription( const char * messageName ); /** * Register Message Handler. This is internal for the IChannel implementer* @@ -108,7 +109,7 @@ class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t send_message( const std::string& messageName, int32_t length, uint8_t* data); + virtual int16_t send_message( const char * messageName, int32_t length, uint8_t* data); uORBCommunicator::IChannelRxHandler* get_rx_handler() { @@ -120,7 +121,7 @@ class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel void reset_counters(); - InterfaceCounters get_interface_counters( const std::string& messageName ); + InterfaceCounters get_interface_counters( const char * messageName ); private: diff --git a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp index e52d1d58f8..5e717bab98 100644 --- a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp +++ b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp @@ -50,13 +50,13 @@ uORB_test::uORBCommunicatorMockLoopback::uORBCommunicatorMockLoopback() int16_t uORB_test::uORBCommunicatorMockLoopback::add_subscription ( - const std::string& messageName, + const char * messageName, int32_t msgRateInHz ) { int16_t rc = -1; - PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName.c_str(), msgRateInHz ); + PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName, msgRateInHz ); if( _rx_handler ) { @@ -64,7 +64,7 @@ int16_t uORB_test::uORBCommunicatorMockLoopback::add_subscription { rc = _rx_handler->process_add_subscription ( - _topic_translation_map[messageName], + _topic_translation_map[messageName].c_str(), msgRateInHz ); } @@ -74,18 +74,18 @@ int16_t uORB_test::uORBCommunicatorMockLoopback::add_subscription int16_t uORB_test::uORBCommunicatorMockLoopback::remove_subscription ( - const std::string& messageName + const char * messageName ) { int16_t rc = -1; - PX4_INFO( "got remove_subscription for msg[%s]", messageName.c_str() ); + PX4_INFO( "got remove_subscription for msg[%s]", messageName ); if( _rx_handler ) { if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) { rc = _rx_handler->process_remove_subscription ( - _topic_translation_map[messageName] + _topic_translation_map[messageName].c_str() ); } } @@ -105,22 +105,22 @@ int16_t uORB_test::uORBCommunicatorMockLoopback::register_handler int16_t uORB_test::uORBCommunicatorMockLoopback::send_message ( - const std::string& messageName, + const char * messageName, int32_t length, uint8_t* data ) { int16_t rc = -1; - PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName.c_str(), length ); + PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName, length ); if( _rx_handler ) { if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) { - if( uORB::Manager::get_instance()->is_remote_subscriber_present( _topic_translation_map[messageName] ) ) + if( uORB::Manager::get_instance()->is_remote_subscriber_present( _topic_translation_map[messageName].c_str() ) ) { rc = _rx_handler->process_received_message - ( _topic_translation_map[messageName], length, data ); - PX4_INFO( "[uORBCommunicatorMockLoopback::send_message] return from[topic(%s)] _rx_handler->process_received_message[%d]", messageName.c_str(), rc ); + ( _topic_translation_map[messageName].c_str(), length, data ); + PX4_INFO( "[uORBCommunicatorMockLoopback::send_message] return from[topic(%s)] _rx_handler->process_received_message[%d]", messageName, rc ); } else { diff --git a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp index 2caef3977e..265d277d87 100644 --- a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp +++ b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp @@ -32,6 +32,7 @@ #include "uORB/uORBCommunicator.hpp" #include "uORBGtestTopics.hpp" #include +#include #include namespace uORB_test @@ -59,7 +60,7 @@ class uORB_test::uORBCommunicatorMockLoopback : public uORBCommunicator::IChanne * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t add_subscription( const std::string& messageName, int32_t msgRateInHz ); + virtual int16_t add_subscription( const char * messageName, int32_t msgRateInHz ); /** @@ -73,7 +74,7 @@ class uORB_test::uORBCommunicatorMockLoopback : public uORBCommunicator::IChanne * Note: This does not necessarily mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t remove_subscription( const std::string& messageName ); + virtual int16_t remove_subscription( const char * messageName ); /** * Register Message Handler. This is internal for the IChannel implementer* @@ -95,7 +96,7 @@ class uORB_test::uORBCommunicatorMockLoopback : public uORBCommunicator::IChanne * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t send_message( const std::string& messageName, int32_t length, uint8_t* data); + virtual int16_t send_message( const char * messageName, int32_t length, uint8_t* data); uORBCommunicator::IChannelRxHandler* get_rx_handler() { From 0c43803ec751185d371275ad2723c6b12728407e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 11 Jun 2015 21:03:45 -0700 Subject: [PATCH 45/48] POSIX: Fixed syntax error in posix_apps.py Signed-off-by: Mark Charlebois --- Tools/posix_apps.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/posix_apps.py b/Tools/posix_apps.py index 45837f1e8e..b7ddbc210c 100755 --- a/Tools/posix_apps.py +++ b/Tools/posix_apps.py @@ -82,7 +82,7 @@ print('\tapps["list_tasks"] = list_tasks_main;') print('\tapps["list_files"] = list_files_main;') print('\tapps["list_devices"] = list_devices_main;') print('\tapps["list_topics"] = list_topics_main;') -print('\tapps["sleep"] = sleep_main;' +print('\tapps["sleep"] = sleep_main;') print(""" return apps; } From 4d1ae6269bd9fac08097843524755214b5377c90 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 11 Jun 2015 21:36:13 -0700 Subject: [PATCH 46/48] POSIX: Added PX4_ROOTFSDIR to file paths Set a default path relative to current dir for the posix target. Running make posixrun will create the required directoroes and then run mainapp from its build location. PX4_ROOTFSDIR is set to nothing for nuttx. Signed-off-by: Mark Charlebois --- src/modules/dataman/dataman.c | 2 +- src/modules/mavlink/mavlink_ftp.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- src/modules/navigator/geofence.h | 3 ++- .../position_estimator_inav_main.c | 2 +- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/systemlib/param/param.c | 2 +- src/modules/uORB/uORBTest_UnitTest.cpp | 3 +-- src/modules/uavcan/uavcan_main.hpp | 6 +++--- src/platforms/px4_defines.h | 13 ++++++++++++- 10 files changed, 25 insertions(+), 14 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b9bf402afa..e48b0b5c89 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -132,7 +132,7 @@ static sem_t g_sys_state_mutex; /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; -static const char *default_device_path = "/fs/microsd/dataman"; +static const char *default_device_path = PX4_ROOTFSDIR"/fs/microsd/dataman"; static char *k_data_manager_device_path = NULL; /* The data manager work queues */ diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 36d2f2ff6a..44127f586e 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -571,7 +571,7 @@ MavlinkFTP::ErrorCode MavlinkFTP::_workTruncateFile(PayloadHeader* payload) { char file[kMaxDataLength]; - const char temp_file[] = "/fs/microsd/.trunc.tmp"; + const char temp_file[] = PX4_ROOTFSDIR"/fs/microsd/.trunc.tmp"; strncpy(file, _data_as_cstring(payload), kMaxDataLength); payload->size = 0; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1b2689e6bb..ffdc0e0730 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -389,7 +389,7 @@ protected: } else if (write_err_count < write_err_threshold) { /* string to hold the path to the log */ char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_path[70] = ""; timespec ts; px4_clock_gettime(CLOCK_REALTIME, &ts); @@ -400,7 +400,7 @@ protected: // XXX we do not want to interfere here with the SD log app strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt); - snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); + snprintf(log_file_path, sizeof(log_file_path), PX4_ROOTFSDIR"/fs/microsd/%s", log_file_name); fp = fopen(log_file_path, "ab"); /* write first message */ diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 96764cc976..c9518872f1 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -49,8 +49,9 @@ #include #include #include +#include -#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" +#define GEOFENCE_FILENAME PX4_ROOTFSDIR"/fs/microsd/etc/geofence.txt" class Geofence : public control::SuperBlock { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 8654a7cb11..53d6323e81 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -189,7 +189,7 @@ int position_estimator_inav_main(int argc, char *argv[]) static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { - FILE *f = fopen("/fs/microsd/inav.log", "a"); + FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index acc4771968..f35963d885 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -179,7 +179,7 @@ static const int MIN_BYTES_TO_WRITE = 512; static bool _extended_logging = false; static bool _gpstime_only = false; -#define MOUNTPOINT "/fs/microsd" +#define MOUNTPOINT PX4_ROOTFSDIR"/fs/microsd" static const char *mountpoint = MOUNTPOINT; static const char *log_root = MOUNTPOINT "/log"; static int mavlink_fd = -1; diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 5f2dc2df27..1bf986753d 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -695,7 +695,7 @@ param_reset_excludes(const char *excludes[], int num_excludes) param_notify_changes(); } -static const char *param_default_file = "/eeprom/parameters"; +static const char *param_default_file = PX4_ROOTFSDIR"/eeprom/parameters"; static char *param_user_file = NULL; int diff --git a/src/modules/uORB/uORBTest_UnitTest.cpp b/src/modules/uORB/uORBTest_UnitTest.cpp index 53a053dc92..e051e66efb 100644 --- a/src/modules/uORB/uORBTest_UnitTest.cpp +++ b/src/modules/uORB/uORBTest_UnitTest.cpp @@ -104,8 +104,7 @@ int uORBTest::UnitTest::pubsublatency_main(void) if (pubsubtest_print) { char fname[32]; - //sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup); - sprintf(fname, "/tmp/timings%u.txt", timingsgroup); + sprintf(fname, PX4_ROOTFSDIR"/fs/microsd/timings%u.txt", timingsgroup); FILE *f = fopen(fname, "w"); if (f == NULL) { warnx("Error opening file!\n"); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index aa67972b67..dc81fb57d6 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -65,9 +65,9 @@ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 #define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" -#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db" -#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw" -#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" +#define UAVCAN_NODE_DB_PATH PX4_ROOTFSDIR"/fs/microsd/uavcan.db" +#define UAVCAN_FIRMWARE_PATH PX4_ROOTFSDIR"/fs/microsd/fw" +#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" // we add two to allow for actuator_direct and busevent #define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index e123a008c5..ba5f716945 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -92,6 +92,8 @@ typedef param_t px4_param_t; */ #if defined(__PX4_NUTTX) +#define PX4_ROOTFSDIR + /* XXX this is a hack to resolve conflicts with NuttX headers */ #if !defined(__PX4_TESTS) #define isspace(c) \ @@ -136,6 +138,12 @@ __END_DECLS #define px4_statfs_buf_f_bavail_t unsigned long +#if defined(__PX4_QURT) +#define PX4_ROOTFSDIR +#else +#define PX4_ROOTFSDIR "rootfs" +#endif + #endif @@ -198,6 +206,10 @@ __END_DECLS #endif #if defined(__PX4_QURT) + +#define PX4_ROOTFSDIR +#define DEFAULT_PARAM_FILE "/fs/eeprom/parameters" + #define SIOCDEVPRIVATE 999999 // Missing math.h defines @@ -211,7 +223,6 @@ __END_DECLS #define fminf(x, y) ((x) > (y) ? y : x) #endif - #endif /* From c9be962f4cb4c1ce35ee87d501166927d8e938c9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 11 Jun 2015 21:44:21 -0700 Subject: [PATCH 47/48] POSIX: Modified posix_run.sh to create rootfs dirs The default rootfs is now in: Build/posix_default.build/rootfs/ The subdirs fs/microsd and eeprom are now created if they do not exist. Signed-off-by: Mark Charlebois --- Tools/posix_run.sh | 20 +++----------------- 1 file changed, 3 insertions(+), 17 deletions(-) diff --git a/Tools/posix_run.sh b/Tools/posix_run.sh index fdeefb4a29..3a20dc3766 100755 --- a/Tools/posix_run.sh +++ b/Tools/posix_run.sh @@ -1,19 +1,5 @@ #!/bin/bash -if [ ! -d /fs/msdcard ] && [ ! -w /fs/msdcard ] - then - echo "Need to create/mount writable /fs/microsd" - echo "sudo mkdir -p /fs/msdcard" - echo "sudo chmod 777 /fs/msdcard" - exit 1 -fi - -if [ ! -d /eeprom ] && [ ! -w /eeprom ] - then - echo "Need to create/mount writable /eeprom" - echo "sudo mkdir -p /eeprom" - echo "sudo chmod 777 /eeprom" - exit 1 -fi - -Build/posix_default.build/mainapp posix-configs/posixtest/init/rc.S +mkdir -p Build/posix_default.build/rootfs/fs/microsd +mkdir -p Build/posix_default.build/rootfs/eeprom +cd Build/posix_default.build && ./mainapp ../../posix-configs/posixtest/init/rc.S From 94313323aac474193879a05b8418ff92a2294f70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 13:53:20 +0200 Subject: [PATCH 48/48] MAVLink app: Fix sending of autopilot capabilities --- src/modules/mavlink/mavlink_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 51cab75538..1a117fb8a7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1620,7 +1620,10 @@ Mavlink::task_main(int argc, char *argv[]) /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); - //send_autopilot_capabilites(); + /* if the protocol is serial, we send the system version blindly */ + if (get_protocol() != SERIAL) { + send_autopilot_capabilites(); + } while (!_task_should_exit) { /* main loop */