From 3874bca2084bb88dcd739b309bd4a7929db3b417 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 13:48:33 +0100 Subject: mavlink: Only send messages when we have updates for them. --- src/modules/mavlink/mavlink_messages.cpp | 472 ++++++++++++----------- src/modules/mavlink/mavlink_orb_subscription.cpp | 21 +- src/modules/mavlink/mavlink_orb_subscription.h | 9 + src/modules/mavlink/mavlink_receiver.cpp | 9 +- 4 files changed, 281 insertions(+), 230 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4d83afe82..7d388f88d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -216,8 +216,8 @@ protected: void send(const hrt_abstime t) { - status_sub->update(t); - pos_sp_triplet_sub->update(t); + (void)status_sub->update(t); + (void)pos_sp_triplet_sub->update(t); uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; @@ -261,22 +261,23 @@ protected: void send(const hrt_abstime t) { - status_sub->update(t); - - mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); + if (status_sub->update(t)) { + + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); + } } }; @@ -284,7 +285,7 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: - MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0) + MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0) { } @@ -302,10 +303,10 @@ private: MavlinkOrbSubscription *sensor_sub; struct sensor_combined_s *sensor; - uint32_t accel_counter; - uint32_t gyro_counter; - uint32_t mag_counter; - uint32_t baro_counter; + uint64_t accel_timestamp; + uint64_t gyro_timestamp; + uint64_t mag_timestamp; + uint64_t baro_timestamp; protected: void subscribe(Mavlink *mavlink) @@ -316,42 +317,43 @@ protected: void send(const hrt_abstime t) { - sensor_sub->update(t); + if (sensor_sub->update(t)) { - uint16_t fields_updated = 0; + uint16_t fields_updated = 0; - if (accel_counter != sensor->accelerometer_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_counter = sensor->accelerometer_counter; - } + if (accel_timestamp != sensor->accelerometer_timestamp) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_timestamp = sensor->accelerometer_timestamp; + } - if (gyro_counter != sensor->gyro_counter) { - /* mark second group dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = sensor->gyro_counter; - } + if (gyro_timestamp != sensor->timestamp) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_timestamp = sensor->timestamp; + } - if (mag_counter != sensor->magnetometer_counter) { - /* mark third group dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = sensor->magnetometer_counter; - } + if (mag_timestamp != sensor->magnetometer_timestamp) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_timestamp = sensor->magnetometer_timestamp; + } - if (baro_counter != sensor->baro_counter) { - /* mark last group dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = sensor->baro_counter; - } + if (baro_timestamp != sensor->baro_timestamp) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_timestamp = sensor->baro_timestamp; + } - mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); + mavlink_msg_highres_imu_send(_channel, + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); + } } }; @@ -382,12 +384,13 @@ protected: void send(const hrt_abstime t) { - att_sub->update(t); + if (att_sub->update(t)) { - mavlink_msg_attitude_send(_channel, - att->timestamp / 1000, - att->roll, att->pitch, att->yaw, - att->rollspeed, att->pitchspeed, att->yawspeed); + mavlink_msg_attitude_send(_channel, + att->timestamp / 1000, + att->roll, att->pitch, att->yaw, + att->rollspeed, att->pitchspeed, att->yawspeed); + } } }; @@ -418,17 +421,18 @@ protected: void send(const hrt_abstime t) { - att_sub->update(t); - - mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + if (att_sub->update(t)) { + + mavlink_msg_attitude_quaternion_send(_channel, + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); + } } }; @@ -483,23 +487,26 @@ protected: void send(const hrt_abstime t) { - att_sub->update(t); - pos_sub->update(t); - armed_sub->update(t); - act_sub->update(t); - airspeed_sub->update(t); - - float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); - uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; - float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; - - mavlink_msg_vfr_hud_send(_channel, - airspeed->true_airspeed_m_s, - groundspeed, - heading, - throttle, - pos->alt, - -pos->vel_d); + bool updated = att_sub->update(t); + updated |= pos_sub->update(t); + updated |= armed_sub->update(t); + updated |= act_sub->update(t); + updated |= airspeed_sub->update(t); + + if (updated) { + + float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); + uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; + float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; + + mavlink_msg_vfr_hud_send(_channel, + airspeed->true_airspeed_m_s, + groundspeed, + heading, + throttle, + pos->alt, + -pos->vel_d); + } } }; @@ -530,19 +537,20 @@ protected: void send(const hrt_abstime t) { - gps_sub->update(t); - - mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + if (gps_sub->update(t)) { + + mavlink_msg_gps_raw_int_send(_channel, + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); + } } }; @@ -579,10 +587,11 @@ protected: void send(const hrt_abstime t) { - pos_sub->update(t); - home_sub->update(t); + bool updated = pos_sub->update(t); + updated |= home_sub->update(t); - mavlink_msg_global_position_int_send(_channel, + if (updated) { + mavlink_msg_global_position_int_send(_channel, pos->timestamp / 1000, pos->lat * 1e7, pos->lon * 1e7, @@ -592,6 +601,7 @@ protected: pos->vel_e * 100.0f, pos->vel_d * 100.0f, _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + } } }; @@ -622,16 +632,17 @@ protected: void send(const hrt_abstime t) { - pos_sub->update(t); + if (pos_sub->update(t)) { - mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + mavlink_msg_local_position_ned_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); + } } }; @@ -662,12 +673,17 @@ protected: void send(const hrt_abstime t) { - home_sub->update(t); - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home->lat * 1e7), - (int32_t)(home->lon * 1e7), - (int32_t)(home->alt) * 1000.0f); + /* we're sending the GPS home periodically to ensure the + * the GCS does pick it up at one point */ + if (home_sub->is_published()) { + home_sub->update(t); + + mavlink_msg_gps_global_origin_send(_channel, + (int32_t)(home->lat * 1e7), + (int32_t)(home->lon * 1e7), + (int32_t)(home->alt) * 1000.0f); + } } }; @@ -713,19 +729,20 @@ protected: void send(const hrt_abstime t) { - act_sub->update(t); - - mavlink_msg_servo_output_raw_send(_channel, - act->timestamp / 1000, - _n, - act->output[0], - act->output[1], - act->output[2], - act->output[3], - act->output[4], - act->output[5], - act->output[6], - act->output[7]); + if (act_sub->update(t)) { + + mavlink_msg_servo_output_raw_send(_channel, + act->timestamp / 1000, + _n, + act->output[0], + act->output[1], + act->output[2], + act->output[3], + act->output[4], + act->output[5], + act->output[6], + act->output[7]); + } } }; @@ -768,57 +785,60 @@ protected: void send(const hrt_abstime t) { - status_sub->update(t); - pos_sp_triplet_sub->update(t); - act_sub->update(t); + bool updated = status_sub->update(t); + updated |= pos_sp_triplet_sub->update(t); + updated |= act_sub->update(t); - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state; - uint8_t mavlink_base_mode; - uint32_t mavlink_custom_mode; - get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + if (updated) { - /* set number of valid outputs depending on vehicle type */ - unsigned n; + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state; + uint8_t mavlink_base_mode; + uint32_t mavlink_custom_mode; + get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; + /* set number of valid outputs depending on vehicle type */ + unsigned n; - case MAV_TYPE_HEXAROTOR: - n = 6; - break; + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; - default: - n = 8; - break; - } + case MAV_TYPE_HEXAROTOR: + n = 6; + break; + + default: + n = 8; + break; + } + + /* scale / assign outputs depending on system type */ + float out[8]; - /* scale / assign outputs depending on system type */ - float out[8]; + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..1200 us to 0..1*/ + out[i] = (act->output[i] - 900.0f) / 1200.0f; - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..1200 us to 0..1*/ - out[i] = (act->output[i] - 900.0f) / 1200.0f; + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } } else { - /* send 0 when disarmed */ - out[i] = 0.0f; + out[i] = -1.0f; } - - } else { - out[i] = -1.0f; } - } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); + } } }; @@ -849,14 +869,15 @@ protected: void send(const hrt_abstime t) { - pos_sp_triplet_sub->update(t); + if (pos_sp_triplet_sub->update(t)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet->current.lat * 1e7), - (int32_t)(pos_sp_triplet->current.lon * 1e7), - (int32_t)(pos_sp_triplet->current.alt * 1000), - (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); + mavlink_msg_global_position_setpoint_int_send(_channel, + MAV_FRAME_GLOBAL, + (int32_t)(pos_sp_triplet->current.lat * 1e7), + (int32_t)(pos_sp_triplet->current.lon * 1e7), + (int32_t)(pos_sp_triplet->current.alt * 1000), + (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); + } } }; @@ -925,14 +946,15 @@ protected: void send(const hrt_abstime t) { - att_sp_sub->update(t); + if (att_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, - att_sp->timestamp / 1000, - att_sp->roll_body, - att_sp->pitch_body, - att_sp->yaw_body, - att_sp->thrust); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, + att_sp->timestamp / 1000, + att_sp->roll_body, + att_sp->pitch_body, + att_sp->yaw_body, + att_sp->thrust); + } } }; @@ -963,14 +985,15 @@ protected: void send(const hrt_abstime t) { - att_rates_sp_sub->update(t); + if (att_rates_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, - att_rates_sp->timestamp / 1000, - att_rates_sp->roll, - att_rates_sp->pitch, - att_rates_sp->yaw, - att_rates_sp->thrust); + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, + att_rates_sp->timestamp / 1000, + att_rates_sp->roll, + att_rates_sp->pitch, + att_rates_sp->yaw, + att_rates_sp->thrust); + } } }; @@ -1001,24 +1024,25 @@ protected: void send(const hrt_abstime t) { - rc_sub->update(t); - - const unsigned port_width = 8; - - for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_channel, - rc->timestamp_publication / 1000, - i, - (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, - rc->rssi); + if (rc_sub->update(t)) { + + const unsigned port_width = 8; + + for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(_channel, + rc->timestamp_publication / 1000, + i, + (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, + rc->rssi); + } } } }; @@ -1050,15 +1074,16 @@ protected: void send(const hrt_abstime t) { - manual_sub->update(t); + if (manual_sub->update(t)) { - mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual->roll * 1000, - manual->pitch * 1000, - manual->yaw * 1000, - manual->throttle * 1000, - 0); + mavlink_msg_manual_control_send(_channel, + mavlink_system.sysid, + manual->roll * 1000, + manual->pitch * 1000, + manual->yaw * 1000, + manual->throttle * 1000, + 0); + } } }; @@ -1089,15 +1114,16 @@ protected: void send(const hrt_abstime t) { - flow_sub->update(t); + if (flow_sub->update(t)) { - mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + mavlink_msg_optical_flow_send(_channel, + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); + } } }; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 6279e5366..996318468 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -46,11 +46,15 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr) +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : + _fd(orb_subscribe(_topic)), + _published(false), + _topic(topic), + _last_check(0), + next(nullptr) { _data = malloc(topic->o_size); memset(_data, 0, topic->o_size); - _fd = orb_subscribe(_topic); } MavlinkOrbSubscription::~MavlinkOrbSubscription() @@ -87,3 +91,16 @@ MavlinkOrbSubscription::update(const hrt_abstime t) return false; } + +bool +MavlinkOrbSubscription::is_published() +{ + bool updated; + orb_check(_fd, &updated); + + if (updated) { + _published = true; + } + + return _published; +} diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index eacc27034..42d47e96e 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -54,12 +54,21 @@ public: ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); + + /** + * Check if the topic has been published. + * + * This call will return true if the topic was ever published. + * @param true if the topic has been published at least once. + */ + bool is_published(); void *get_data(); const orb_id_t get_topic(); private: const orb_id_t _topic; int _fd; + bool _published; void *_data; hrt_abstime _last_check; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c222a3ddf..2ce86396c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -586,7 +586,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_counter = _hil_counter; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; @@ -596,7 +595,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[2] = imu.zacc; hil_sensors.accelerometer_mode = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_counter = _hil_counter; + hil_sensors.accelerometer_timestamp = timestamp; hil_sensors.adc_voltage_v[0] = 0.0f; hil_sensors.adc_voltage_v[1] = 0.0f; @@ -611,15 +610,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.magnetometer_range_ga = 32.7f; // int16 hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_counter = _hil_counter; + hil_sensors.magnetometer_timestamp = timestamp; hil_sensors.baro_pres_mbar = imu.abs_pressure; hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_counter = _hil_counter; + hil_sensors.baro_timestamp = timestamp; hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = _hil_counter; + hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ if (_sensors_pub > 0) { -- cgit v1.2.3