From c04713f4a82ccb935ad7b8c5476fdf09916e21d6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 30 Nov 2019 12:39:22 -0500 Subject: [PATCH] simulator update orb usage --- src/modules/simulator/gpssim/gpssim.cpp | 81 ++----------- src/modules/simulator/simulator.h | 32 +++-- src/modules/simulator/simulator_mavlink.cpp | 123 +++++++------------- 3 files changed, 69 insertions(+), 167 deletions(-) diff --git a/src/modules/simulator/gpssim/gpssim.cpp b/src/modules/simulator/gpssim/gpssim.cpp index b02c131cdf..608bc80ee1 100644 --- a/src/modules/simulator/gpssim/gpssim.cpp +++ b/src/modules/simulator/gpssim/gpssim.cpp @@ -57,9 +57,8 @@ #include #include #include -#include +#include #include -#include #include @@ -74,19 +73,10 @@ using namespace DriverFramework; #define TIMEOUT_100MS 100000 #define RATE_MEASUREMENT_PERIOD 5000000 -/* class for dynamic allocation of satellite info data */ -class GPS_Sat_Info -{ -public: - struct satellite_info_s _data; -}; - - class GPSSIM : public VirtDevObj { public: - GPSSIM(bool fake_gps, bool enable_sat_info, - int fix_type, int num_sat, int noise_multiplier); + GPSSIM(bool fake_gps, bool enable_sat_info, int fix_type, int num_sat, int noise_multiplier); ~GPSSIM() override; int init() override; @@ -105,17 +95,14 @@ protected: private: - bool _task_should_exit; ///< flag to make the main worker task exit - volatile int _task; ///< worker task - 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 + bool _task_should_exit{false}; ///< flag to make the main worker task exit + volatile int _task{-1}; ///< worker task + vehicle_gps_position_s _report_gps_pos{}; ///< uORB topic for gps position + uORB::PublicationMulti _report_gps_pos_pub{ORB_ID(vehicle_gps_position)}; ///< uORB pub for gps position SyncObj _sync; - int _fix_type; - int _num_sat; - int _noise_multiplier; + int _fix_type{0}; + int _num_sat{0}; + int _noise_multiplier{0}; std::default_random_engine _gen; @@ -162,15 +149,8 @@ GPSSIM *g_dev = nullptr; } -GPSSIM::GPSSIM(bool fake_gps, bool enable_sat_info, - int fix_type, int num_sat, int noise_multiplier) : +GPSSIM::GPSSIM(bool fake_gps, bool enable_sat_info, int fix_type, int num_sat, int noise_multiplier) : VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 1e6 / 10), - _task_should_exit(false), - _Sat_Info(nullptr), - _report_gps_pos{}, - _report_gps_pos_pub(nullptr), - _p_report_sat_info(nullptr), - _report_sat_info_pub(nullptr), _fix_type(fix_type), _num_sat(num_sat), _noise_multiplier(noise_multiplier) @@ -179,19 +159,10 @@ GPSSIM::GPSSIM(bool fake_gps, bool enable_sat_info, g_dev = this; _report_gps_pos.heading = NAN; _report_gps_pos.heading_offset = NAN; - - /* 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)); - } } GPSSIM::~GPSSIM() { - delete _Sat_Info; - /* tell the task we want it to go away */ _task_should_exit = true; @@ -316,34 +287,11 @@ GPSSIM::task_main() int recv_ret = receive(TIMEOUT_100MS); if (recv_ret > 0) { - /* opportunistic publishing - else invalid data would end up on the bus */ - 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); - } - - 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); - } - } + _report_gps_pos_pub.publish(_report_gps_pos); } } - if (_report_gps_pos_pub) { - orb_unadvertise(_report_gps_pos_pub); - } - - if (_report_sat_info_pub) { - orb_unadvertise(_report_sat_info_pub); - } - PX4_INFO("exiting"); /* tell the dtor that we are exiting */ @@ -360,14 +308,9 @@ GPSSIM::cmd_reset() void GPSSIM::print_info() { - //GPS Mode + // GPS Mode PX4_INFO("protocol: SIM"); - PX4_INFO("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 != 0) { print_message(_report_gps_pos); } diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index daba595041..516f52f4b5 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -219,12 +220,12 @@ private: perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, "sim_network_interval")}; // uORB publisher handlers - orb_advert_t _battery_pub{nullptr}; - orb_advert_t _differential_pressure_pub{nullptr}; - orb_advert_t _dist_pub{nullptr}; - orb_advert_t _flow_pub{nullptr}; - orb_advert_t _irlock_report_pub{nullptr}; - orb_advert_t _visual_odometry_pub{nullptr}; + uORB::Publication _battery_pub{ORB_ID(battery_status)}; + uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::PublicationMulti _dist_pub{ORB_ID(battery_status)}; + uORB::PublicationMulti _flow_pub{ORB_ID(optical_flow)}; + uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; + uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; @@ -257,7 +258,7 @@ private: void handle_message_vision_position_estimate(const mavlink_message_t *msg); void parameters_update(bool force); - void poll_topics(); + void poll_for_MAVLink_messages(); void request_hil_state_quaternion(); void send(); @@ -270,15 +271,15 @@ private: static void *sending_trampoline(void *); // uORB publisher handlers - orb_advert_t _vehicle_angular_velocity_pub{nullptr}; - orb_advert_t _attitude_pub{nullptr}; - orb_advert_t _gpos_pub{nullptr}; - orb_advert_t _lpos_pub{nullptr}; - orb_advert_t _rc_channels_pub{nullptr}; + uORB::Publication _vehicle_angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; + uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; + uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; + uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; + uORB::Publication _input_rc_pub{ORB_ID(input_rc)}; // uORB subscription handlers int _actuator_outputs_sub{-1}; - int _vehicle_status_sub{-1}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // hil map_ref data struct map_projection_reference_s _hil_local_proj_ref {}; @@ -291,10 +292,7 @@ private: uint64_t _hil_ref_timestamp{0}; // uORB data containers - input_rc_s _rc_input {}; - manual_control_setpoint_s _manual {}; - vehicle_attitude_s _attitude {}; - vehicle_status_s _vehicle_status {}; + vehicle_status_s _vehicle_status{}; DEFINE_PARAMETERS( (ParamFloat) _param_sim_bat_drain, ///< battery drain interval diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index c64da6ac35..73defef146 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -192,33 +192,6 @@ void Simulator::send_controls() } } -static void fill_rc_input_msg(input_rc_s *rc, mavlink_rc_channels_t *rc_channels) -{ - rc->timestamp = hrt_absolute_time(); - rc->timestamp_last_signal = rc->timestamp; - 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; -} - void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &imu) { if ((imu.fields_updated & 0x1FFF) != 0x1FFF) { @@ -263,8 +236,7 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor report.differential_pressure_filtered_pa = imu.diff_pressure * 100.0f; // convert from millibar to bar; report.differential_pressure_raw_pa = imu.diff_pressure * 100.0f; // convert from millibar to bar; - int instance; - orb_publish_auto(ORB_ID(differential_pressure), &_differential_pressure_pub, &report, &instance, ORB_PRIO_DEFAULT); + _differential_pressure_pub.publish(report); } } @@ -400,10 +372,8 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, true, 0, throttle, armed, &_battery_status); - // publish the battery voltage - int batt_multi; - orb_publish_auto(ORB_ID(battery_status), &_battery_pub, &_battery_status, &batt_multi, ORB_PRIO_HIGH); + _battery_pub.publish(_battery_status); } } @@ -424,9 +394,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg hil_angular_velocity.xyz[2] = hil_state.yawspeed; // always publish ground truth attitude message - int hilstate_multi; - orb_publish_auto(ORB_ID(vehicle_angular_velocity_groundtruth), &_vehicle_angular_velocity_pub, &hil_angular_velocity, - &hilstate_multi, ORB_PRIO_HIGH); + _vehicle_angular_velocity_ground_truth_pub.publish(hil_angular_velocity); } /* attitude */ @@ -438,8 +406,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg q.copyTo(hil_attitude.q); // always publish ground truth attitude message - int hilstate_multi; - orb_publish_auto(ORB_ID(vehicle_attitude_groundtruth), &_attitude_pub, &hil_attitude, &hilstate_multi, ORB_PRIO_HIGH); + _attitude_ground_truth_pub.publish(hil_attitude); } /* global position */ @@ -456,8 +423,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg hil_gpos.vel_d = hil_state.vz / 100.0f; // always publish ground truth attitude message - int hil_gpos_multi; - orb_publish_auto(ORB_ID(vehicle_global_position_groundtruth), &_gpos_pub, &hil_gpos, &hil_gpos_multi, ORB_PRIO_HIGH); + _gpos_ground_truth_pub.publish(hil_gpos); } /* local position */ @@ -505,8 +471,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg hil_lpos.hagl_max = std::numeric_limits::infinity(); // always publish ground truth attitude message - int hil_lpos_multi; - orb_publish_auto(ORB_ID(vehicle_local_position_groundtruth), &_lpos_pub, &hil_lpos, &hil_lpos_multi, ORB_PRIO_HIGH); + _lpos_ground_truth_pub.publish(hil_lpos); } } @@ -515,8 +480,7 @@ void Simulator::handle_message_landing_target(const mavlink_message_t *msg) mavlink_landing_target_t landing_target_mavlink; mavlink_msg_landing_target_decode(msg, &landing_target_mavlink); - struct irlock_report_s report = {}; - + irlock_report_s report{}; report.timestamp = hrt_absolute_time(); report.signature = landing_target_mavlink.target_num; report.pos_x = landing_target_mavlink.angle_x; @@ -524,8 +488,7 @@ void Simulator::handle_message_landing_target(const mavlink_message_t *msg) report.size_x = landing_target_mavlink.size_x; report.size_y = landing_target_mavlink.size_y; - int irlock_multi; - orb_publish_auto(ORB_ID(irlock_report), &_irlock_report_pub, &report, &irlock_multi, ORB_PRIO_HIGH); + _irlock_report_pub.publish(report); } void Simulator::handle_message_odometry(const mavlink_message_t *msg) @@ -544,11 +507,34 @@ void Simulator::handle_message_rc_channels(const mavlink_message_t *msg) { mavlink_rc_channels_t rc_channels; mavlink_msg_rc_channels_decode(msg, &rc_channels); - fill_rc_input_msg(&_rc_input, &rc_channels); + + input_rc_s rc_input{}; + rc_input.timestamp_last_signal = hrt_absolute_time(); + rc_input.channel_count = rc_channels.chancount; + rc_input.rssi = rc_channels.rssi; + rc_input.values[0] = rc_channels.chan1_raw; + rc_input.values[1] = rc_channels.chan2_raw; + rc_input.values[2] = rc_channels.chan3_raw; + rc_input.values[3] = rc_channels.chan4_raw; + rc_input.values[4] = rc_channels.chan5_raw; + rc_input.values[5] = rc_channels.chan6_raw; + rc_input.values[6] = rc_channels.chan7_raw; + rc_input.values[7] = rc_channels.chan8_raw; + rc_input.values[8] = rc_channels.chan9_raw; + rc_input.values[9] = rc_channels.chan10_raw; + rc_input.values[10] = rc_channels.chan11_raw; + rc_input.values[11] = rc_channels.chan12_raw; + rc_input.values[12] = rc_channels.chan13_raw; + rc_input.values[13] = rc_channels.chan14_raw; + rc_input.values[14] = rc_channels.chan15_raw; + rc_input.values[15] = rc_channels.chan16_raw; + rc_input.values[16] = rc_channels.chan17_raw; + rc_input.values[17] = rc_channels.chan18_raw; + + rc_input.timestamp = hrt_absolute_time(); // publish message - int rc_multi; - orb_publish_auto(ORB_ID(input_rc), &_rc_channels_pub, &_rc_input, &rc_multi, ORB_PRIO_HIGH); + _input_rc_pub.publish(rc_input); } void Simulator::handle_message_vision_position_estimate(const mavlink_message_t *msg) @@ -577,18 +563,6 @@ void Simulator::send_mavlink_message(const mavlink_message_t &aMsg) } } -void Simulator::poll_topics() -{ - // copy new actuator data if available - bool updated = false; - - orb_check(_vehicle_status_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); - } -} - void *Simulator::sending_trampoline(void * /*unused*/) { _instance->send(); @@ -630,7 +604,7 @@ void Simulator::send() if (fds[0].revents & POLLIN) { // Got new data to read, update all topics. parameters_update(false); - poll_topics(); + _vehicle_status_sub.update(&_vehicle_status); send_controls(); } } @@ -775,7 +749,6 @@ void Simulator::poll_for_MAVLink_messages() // Subscribe to topics. // Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS. _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); - _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); // got data from simulator, now activate the sending thread pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, nullptr); @@ -839,10 +812,8 @@ void Simulator::poll_for_MAVLink_messages() } orb_unsubscribe(_actuator_outputs_sub); - orb_unsubscribe(_vehicle_status_sub); } - #ifdef ENABLE_UART_RC_INPUT int openUart(const char *uart_name, int baud) { @@ -939,12 +910,9 @@ int openUart(const char *uart_name, int baud) int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink) { - uint64_t timestamp = hrt_absolute_time(); - - struct optical_flow_s flow = {}; - + optical_flow_s flow = {}; flow.sensor_id = flow_mavlink->sensor_id; - flow.timestamp = timestamp; + flow.timestamp = hrt_absolute_time();; flow.time_since_last_sonar_update = 0; flow.frame_count_since_last_readout = 0; // ? flow.integration_timespan = flow_mavlink->integration_time_us; @@ -979,8 +947,7 @@ int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink rotate_3f(flow_rot, flow.pixel_flow_x_integral, flow.pixel_flow_y_integral, zeroval); rotate_3f(flow_rot, flow.gyro_x_rate_integral, flow.gyro_y_rate_integral, flow.gyro_z_rate_integral); - int flow_multi; - orb_publish_auto(ORB_ID(optical_flow), &_flow_pub, &flow, &flow_multi, ORB_PRIO_HIGH); + _flow_pub.publish(flow); return OK; } @@ -1084,21 +1051,16 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) } - int instance_id = 0; - /** @note: frame_id == MAV_FRAME_VISION_NED) */ - orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &odom, &instance_id, ORB_PRIO_HIGH); + _visual_odometry_pub.publish(odom); return OK; } int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavlink) { - uint64_t timestamp = hrt_absolute_time(); - - struct distance_sensor_s dist = {}; - - dist.timestamp = timestamp; + distance_sensor_s dist{}; + dist.timestamp = hrt_absolute_time(); dist.min_distance = dist_mavlink->min_distance / 100.0f; dist.max_distance = dist_mavlink->max_distance / 100.0f; dist.current_distance = dist_mavlink->current_distance / 100.0f; @@ -1115,8 +1077,7 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl dist.q[2] = dist_mavlink->quaternion[2]; dist.q[3] = dist_mavlink->quaternion[3]; - int dist_multi; - orb_publish_auto(ORB_ID(distance_sensor), &_dist_pub, &dist, &dist_multi, ORB_PRIO_HIGH); + _dist_pub.publish(dist); return OK; }