From b705bf6b1c8f290f29173176fa38a6eb89ab9ee0 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 5 Mar 2019 10:23:46 -0700 Subject: [PATCH] Cut case MAVLINK_MSG_ID_HIL_SENSOR content and paste into handle_message_hil_sensor() method. --- src/modules/simulator/simulator.h | 1 + src/modules/simulator/simulator_mavlink.cpp | 128 ++++++++++---------- 2 files changed, 67 insertions(+), 62 deletions(-) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 594bc37379..6099339c44 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -332,6 +332,7 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_distance_sensor(const mavlink_message_t *msg); + void handle_message_hil_sensor(const mavlink_message_t *msg); void handle_message_hil_state_quaternion(const mavlink_message_t *msg); void handle_message_landing_target(const mavlink_message_t *msg); void handle_message_optical_flow(const mavlink_message_t *msg); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index c3fe6bbb74..aa3f3a853d 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -292,68 +292,8 @@ void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) void Simulator::handle_message(mavlink_message_t *msg) { switch (msg->msgid) { - case MAVLINK_MSG_ID_HIL_SENSOR: { - mavlink_hil_sensor_t imu; - mavlink_msg_hil_sensor_decode(msg, &imu); - - // set temperature to a decent value - imu.temperature = 32.0f; - - struct timespec ts; - abstime_to_ts(&ts, imu.time_usec); - px4_clock_settime(CLOCK_MONOTONIC, &ts); - - hrt_abstime now_us = hrt_absolute_time(); - -#if 0 - // This is just for to debug missing HIL_SENSOR messages. - static hrt_abstime last_time = 0; - hrt_abstime diff = now_us - last_time; - float step = diff / 4000.0f; - - if (step > 1.1f || step < 0.9f) { - PX4_INFO("HIL_SENSOR: imu time_usec: %lu, time_usec: %lu, diff: %lu, step: %.2f", imu.time_usec, now_us, diff, step); - } - - last_time = now_us; -#endif - - if (_publish) { - publish_sensor_topics(&imu); - } - - update_sensors(&imu); - - // battery simulation (limit update to 100Hz) - if (hrt_elapsed_time(&_battery_status.timestamp) >= 10_ms) { - - const float discharge_interval_us = _battery_drain_interval_s.get() * 1000 * 1000; - - bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - - if (!armed || batt_sim_start == 0 || batt_sim_start > now_us) { - batt_sim_start = now_us; - } - - float ibatt = -1.0f; // no current sensor in simulation - const float minimum_percentage = 0.499f; // change this value if you want to simulate low battery reaction - - /* Simulate the voltage of a linearly draining battery but stop at the minimum percentage */ - float battery_percentage = 1.0f - (now_us - batt_sim_start) / discharge_interval_us; - - battery_percentage = math::max(battery_percentage, minimum_percentage); - float vbatt = math::gradual(battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage()); - vbatt *= _battery.cell_count(); - - 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); - } - } + case MAVLINK_MSG_ID_HIL_SENSOR: + handle_message_hil_sensor(msg); break; case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: @@ -411,6 +351,70 @@ void Simulator::handle_message_distance_sensor(const mavlink_message_t *msg) publish_distance_topic(&dist); } +void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) +{ + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); + + // set temperature to a decent value + imu.temperature = 32.0f; + + struct timespec ts; + abstime_to_ts(&ts, imu.time_usec); + px4_clock_settime(CLOCK_MONOTONIC, &ts); + + hrt_abstime now_us = hrt_absolute_time(); + +#if 0 + // This is just for to debug missing HIL_SENSOR messages. + static hrt_abstime last_time = 0; + hrt_abstime diff = now_us - last_time; + float step = diff / 4000.0f; + + if (step > 1.1f || step < 0.9f) { + PX4_INFO("HIL_SENSOR: imu time_usec: %lu, time_usec: %lu, diff: %lu, step: %.2f", imu.time_usec, now_us, diff, step); + } + + last_time = now_us; +#endif + + if (_publish) { + publish_sensor_topics(&imu); + } + + update_sensors(&imu); + + // battery simulation (limit update to 100Hz) + if (hrt_elapsed_time(&_battery_status.timestamp) >= 10_ms) { + + const float discharge_interval_us = _battery_drain_interval_s.get() * 1000 * 1000; + + bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + if (!armed || batt_sim_start == 0 || batt_sim_start > now_us) { + batt_sim_start = now_us; + } + + float ibatt = -1.0f; // no current sensor in simulation + const float minimum_percentage = 0.499f; // change this value if you want to simulate low battery reaction + + /* Simulate the voltage of a linearly draining battery but stop at the minimum percentage */ + float battery_percentage = 1.0f - (now_us - batt_sim_start) / discharge_interval_us; + + battery_percentage = math::max(battery_percentage, minimum_percentage); + float vbatt = math::gradual(battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage()); + vbatt *= _battery.cell_count(); + + 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); + } +} + void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg) { mavlink_hil_state_quaternion_t hil_state;