diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index d9a1f2a5c3..3b0dd50913 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -259,39 +259,39 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) update_sensors(&imu); - /* battery */ - { - hrt_abstime now = hrt_absolute_time(); + // battery simulation + hrt_abstime now = hrt_absolute_time(); - const float discharge_interval_us = 60 * 1000 * 1000; + const double discharge_interval_us = 60 * 1000 * 1000; - static hrt_abstime batt_sim_start = now; + static hrt_abstime batt_sim_start = 0; - float cellcount = 3.0f; + if (batt_sim_start == 0 || batt_sim_start > now) { + batt_sim_start = now; + } - float vbatt = 4.2f * cellcount; - float ibatt = 20.0f; + unsigned cellcount = _battery.cell_count(); - vbatt -= (0.5f * cellcount) * ((now - batt_sim_start) / discharge_interval_us); + float vbatt = _battery.full_cell_voltage() ; + float ibatt = -1.0f; - if (vbatt < (cellcount * 3.7f)) { - vbatt = cellcount * 3.7f; - } + float discharge_v = _battery.full_cell_voltage() - _battery.empty_cell_voltage(); - battery_status_s battery_status; + vbatt = (_battery.full_cell_voltage() - (discharge_v * ((now - batt_sim_start) / discharge_interval_us))) * cellcount; - // TODO: don't hard-code throttle. - const float throttle = 0.5f; - _battery.updateBatteryStatus(now, vbatt, ibatt, throttle, &battery_status); + if (vbatt < (cellcount * _battery.empty_cell_voltage())) { + vbatt = cellcount * _battery.empty_cell_voltage(); + } - /* lazily publish the battery voltage */ - if (_battery_pub != nullptr) { - orb_publish(ORB_ID(battery_status), _battery_pub, &battery_status); + battery_status_s battery_status = {}; - } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &battery_status); - } - } + // TODO: don't hard-code throttle. + const float throttle = 0.5f; + _battery.updateBatteryStatus(now, vbatt, ibatt, throttle, &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); } break; @@ -319,12 +319,8 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) // publish message if (publish) { - if (_rc_channels_pub == nullptr) { - _rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input); - - } else { - orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input); - } + int rc_multi; + orb_publish_auto(ORB_ID(input_rc), &_rc_channels_pub, &_rc_input, &rc_multi, ORB_PRIO_HIGH); } break; @@ -560,7 +556,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) // have a message, handle it handle_message(&msg, publish); - if (msg.msgid != 0 && (hrt_system_time() - pstart_time > 5000000)) { + if (msg.msgid != 0 && (hrt_system_time() - pstart_time > 1000000)) { PX4_INFO("Got initial simuation data, running sim.."); no_sim_data = false; } @@ -798,12 +794,8 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) gyro.temperature = imu->temperature; - if (_gyro_pub == nullptr) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - - } else { - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); - } + int gyro_multi; + orb_publish_auto(ORB_ID(sensor_gyro), &_gyro_pub, &gyro, &gyro_multi, ORB_PRIO_HIGH); } /* accelerometer */ @@ -819,13 +811,9 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) accel.z = imu->zacc; accel.temperature = imu->temperature; - - if (_accel_pub == nullptr) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - - } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); - } + + int accel_multi; + orb_publish_auto(ORB_ID(sensor_accel), &_accel_pub, &accel, &accel_multi, ORB_PRIO_HIGH); } /* magnetometer */ @@ -842,13 +830,8 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) mag.temperature = imu->temperature; - if (_mag_pub == nullptr) { - /* publish to the first mag topic */ - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - - } else { - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - } + int mag_multi; + orb_publish_auto(ORB_ID(sensor_mag), &_mag_pub, &mag, &mag_multi, ORB_PRIO_HIGH); } /* baro */ @@ -860,12 +843,8 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) baro.altitude = imu->pressure_alt; baro.temperature = imu->temperature; - if (_baro_pub == nullptr) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - - } else { - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - } + int baro_multi; + orb_publish_auto(ORB_ID(sensor_baro), &_baro_pub, &baro, &baro_multi, ORB_PRIO_HIGH); } return OK; @@ -875,33 +854,26 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink) { uint64_t timestamp = hrt_absolute_time(); - /* flow */ - { - struct optical_flow_s flow; - memset(&flow, 0, sizeof(flow)); - - flow.sensor_id = flow_mavlink->sensor_id; - flow.timestamp = timestamp; - flow.time_since_last_sonar_update = 0; - flow.frame_count_since_last_readout = 0; // ? - flow.integration_timespan = flow_mavlink->integration_time_us; - - flow.ground_distance_m = flow_mavlink->distance; - flow.gyro_temperature = flow_mavlink->temperature; - flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro; - flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro; - flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro; - flow.pixel_flow_x_integral = flow_mavlink->integrated_x; - flow.pixel_flow_x_integral = flow_mavlink->integrated_y; - flow.quality = flow_mavlink->quality; - - if (_flow_pub == nullptr) { - _flow_pub = orb_advertise(ORB_ID(optical_flow), &flow); - - } else { - orb_publish(ORB_ID(optical_flow), _flow_pub, &flow); - } - } + struct optical_flow_s flow; + memset(&flow, 0, sizeof(flow)); + + flow.sensor_id = flow_mavlink->sensor_id; + flow.timestamp = timestamp; + flow.time_since_last_sonar_update = 0; + flow.frame_count_since_last_readout = 0; // ? + flow.integration_timespan = flow_mavlink->integration_time_us; + + flow.ground_distance_m = flow_mavlink->distance; + flow.gyro_temperature = flow_mavlink->temperature; + flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro; + flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro; + flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro; + flow.pixel_flow_x_integral = flow_mavlink->integrated_x; + flow.pixel_flow_x_integral = flow_mavlink->integrated_y; + flow.quality = flow_mavlink->quality; + + int flow_multi; + orb_publish_auto(ORB_ID(optical_flow), &_flow_pub, &flow, &flow_multi, ORB_PRIO_HIGH); return OK; }