From 6259324030ec5332c3c3fc32dd8a815301c63577 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Thu, 20 Jun 2024 12:43:56 -0600 Subject: [PATCH 1/7] Modified to make everything compile with colcon build. --- rosflight_sim/CMakeLists.txt | 1 - .../include/rosflight_sim/rosflight_firmware | 2 +- .../include/rosflight_sim/sil_board.hpp | 75 +++----- .../include/rosflight_sim/udp_board.hpp | 2 +- rosflight_sim/src/sil_board.cpp | 172 ++++++++---------- rosflight_sim/src/udp_board.cpp | 2 +- 6 files changed, 104 insertions(+), 150 deletions(-) diff --git a/rosflight_sim/CMakeLists.txt b/rosflight_sim/CMakeLists.txt index 35d4319..70dc412 100644 --- a/rosflight_sim/CMakeLists.txt +++ b/rosflight_sim/CMakeLists.txt @@ -82,7 +82,6 @@ endif() # rosflight_firmware add_library(rosflight_firmware include/rosflight_sim/rosflight_firmware/src/rosflight.cpp - include/rosflight_sim/rosflight_firmware/src/nanoprintf.cpp include/rosflight_sim/rosflight_firmware/src/estimator.cpp include/rosflight_sim/rosflight_firmware/src/mixer.cpp include/rosflight_sim/rosflight_firmware/src/controller.cpp diff --git a/rosflight_sim/include/rosflight_sim/rosflight_firmware b/rosflight_sim/include/rosflight_sim/rosflight_firmware index 1d20caa..258f003 160000 --- a/rosflight_sim/include/rosflight_sim/rosflight_firmware +++ b/rosflight_sim/include/rosflight_sim/rosflight_firmware @@ -1 +1 @@ -Subproject commit 1d20caaf27744e4238e107111b4b11f475483235 +Subproject commit 258f003427bb59e53677be3e6c63d0b3db616d0c diff --git a/rosflight_sim/include/rosflight_sim/sil_board.hpp b/rosflight_sim/include/rosflight_sim/sil_board.hpp index c719259..ba929ba 100644 --- a/rosflight_sim/include/rosflight_sim/sil_board.hpp +++ b/rosflight_sim/include/rosflight_sim/sil_board.hpp @@ -35,6 +35,7 @@ #ifndef ROSFLIGHT_SIM_SIL_BOARD_H #define ROSFLIGHT_SIM_SIL_BOARD_H +#include "sensors.h" #include #include #include @@ -47,6 +48,7 @@ #include #include +#include #include #include @@ -231,7 +233,7 @@ class SILBoard : public UDPBoard * * @return true if unprocessed IMU data exists. */ - bool new_imu_data() override; + bool imu_has_new_data() override; /** * @brief Generates simulated IMU data from truth data from Gazebo. Utilizes noise, bias, and walk * parameters provided. All data is returned through the values given as the function arguments. @@ -259,11 +261,11 @@ class SILBoard : public UDPBoard * * @param mag Magnetometer values to populate. */ - void mag_read(float mag[3]) override; + bool mag_read(float mag[3]) override; /** * @brief Function required to be overridden, but not used by sim. */ - void mag_update() override{}; + bool mag_has_new_data() override{return false;}; /** * @brief Function used to check if a barometer is present. Currently returns true. @@ -278,11 +280,11 @@ class SILBoard : public UDPBoard * @param pressure Pressure value to populate. * @param temperature Temperature value to populate. */ - void baro_read(float * pressure, float * temperature) override; + bool baro_read(float * pressure, float * temperature) override; /** * @brief Function required to be overridden, but not used by sim. */ - void baro_update() override{}; + bool baro_has_new_data() override{return false;}; /** * @brief Checks if a pitot tube sensor is present. Returns true if sim is a fixedwing sim. @@ -297,11 +299,11 @@ class SILBoard : public UDPBoard * @param diff_pressure Differential pressure value to populate. * @param temperature Temperature value to populate. */ - void diff_pressure_read(float * diff_pressure, float * temperature) override; + bool diff_pressure_read(float * diff_pressure, float * temperature) override; /** * @brief Function required to be overridden, but not used by sim. */ - void diff_pressure_update() override{}; + bool diff_pressure_has_new_data() override{return false;}; /** * @brief Function used to see if a sonar altitude sensor is present. Currently returns true. @@ -317,11 +319,11 @@ class SILBoard : public UDPBoard * * @return Sonar reading. */ - float sonar_read() override; + bool sonar_read(float * range) override; /** * @brief Function required to be overridden, but not used by sim. */ - void sonar_update() override{}; + bool sonar_has_new_data() override{return false;}; // PWM // TODO make these deal in normalized (-1 to 1 or 0 to 1) values (not pwm-specific) @@ -356,7 +358,7 @@ class SILBoard : public UDPBoard * @param channel Channel to read value from. * @return 0.5 */ - float rc_read(uint8_t channel) override; + float rc_read(uint8_t chan) override; /** * @brief Function required to be overridden, but not used by sim. */ @@ -368,6 +370,8 @@ class SILBoard : public UDPBoard * @return False if /RC has had a message published, true otherwise. */ bool rc_lost() override; + + bool rc_has_new_data() override{return false;}; // non-volatile memory /** @@ -453,70 +457,35 @@ class SILBoard : public UDPBoard * @return true */ bool gnss_present() override; - /** - * @brief Function required to be overridden, but not used by sim. - */ - void gnss_update() override{}; - /** * @brief Generates GNSS data based on Gazebo truth and noise/bias parameters. * * @return Populated GNSSData object */ - rosflight_firmware::GNSSData gnss_read() override; + bool gnss_read(rosflight_firmware::GNSSData * gnss, rosflight_firmware::GNSSFull * gnss_full) override; /** * @brief Function used to check if gnss has new data to read. Currently returns true. * * @return true */ bool gnss_has_new_data() override; - /** - * @brief Generates GNSSFull data based on Gazebo truth and noise/bias parameters. - * - * @return Populated GNSSFull object - */ - rosflight_firmware::GNSSFull gnss_full_read() override; - /** - * @brief Function used to check if battery volt meter is present. Currently returns true. - * - * @return true - */ - bool battery_voltage_present() const override; - /** - * @brief Checks battery voltage. Currently just returns constant value. - * - * @note No battery simulation exists, function returns constant value. - * - * @return Battery voltage - */ - float battery_voltage_read() const override; + bool battery_present() override; + + bool battery_has_new_data() override; + + bool battery_read(float * voltage, float * current) override; /** * @brief Sets battery voltage calibration constant. * * @param multiplier Voltage calibration constant */ void battery_voltage_set_multiplier(double multiplier) override; - - /** - * @brief Function used to check if current meter is present. Currently returns true. - * - * @return true - */ - bool battery_current_present() const override; /** - * @brief Checks battery current draw. Currently just returns constant value. - * - * @note No battery simulation exists, function returns constant value. + * @brief Sets battery current calibration constant. * - * @return Battery current draw + * @param multiplier Current calibration constant */ - float battery_current_read() const override; - /** - * @brief Sets battery current calibration constant. - * - * @param multiplier Current calibration constant - */ void battery_current_set_multiplier(double multiplier) override; // Gazebo stuff diff --git a/rosflight_sim/include/rosflight_sim/udp_board.hpp b/rosflight_sim/include/rosflight_sim/udp_board.hpp index 6471864..09c70af 100644 --- a/rosflight_sim/include/rosflight_sim/udp_board.hpp +++ b/rosflight_sim/include/rosflight_sim/udp_board.hpp @@ -56,7 +56,7 @@ class UDPBoard : public rosflight_firmware::Board ~UDPBoard(); void serial_init(uint32_t baud_rate, uint32_t dev) override; - void serial_write(const uint8_t * src, size_t len) override; + void serial_write(const uint8_t * src, size_t len, uint8_t qos) override; uint16_t serial_bytes_available() override; uint8_t serial_read() override; void serial_flush() override; diff --git a/rosflight_sim/src/sil_board.cpp b/rosflight_sim/src/sil_board.cpp index 968d12c..30aaf56 100644 --- a/rosflight_sim/src/sil_board.cpp +++ b/rosflight_sim/src/sil_board.cpp @@ -223,7 +223,7 @@ void SILBoard::sensors_init() uint16_t SILBoard::num_sensor_errors() { return 0; } -bool SILBoard::new_imu_data() +bool SILBoard::imu_has_new_data() { uint64_t now_us = clock_micros(); if (now_us >= next_imu_update_time_us_) { @@ -324,7 +324,7 @@ void SILBoard::imu_not_responding_error() RCLCPP_ERROR(node_->get_logger(), "[gazebo_rosflight_sil] imu not responding"); } -void SILBoard::mag_read(float mag[3]) +bool SILBoard::mag_read(float mag[3]) { GazeboPose I_to_B = GZ_COMPAT_GET_WORLD_POSE(link_); GazeboVector noise; @@ -351,13 +351,15 @@ void SILBoard::mag_read(float mag[3]) mag[0] = GZ_COMPAT_GET_X(y_mag); mag[1] = (float) -GZ_COMPAT_GET_Y(y_mag); mag[2] = (float) -GZ_COMPAT_GET_Z(y_mag); + + return true; } bool SILBoard::mag_present() { return true; } bool SILBoard::baro_present() { return true; } -void SILBoard::baro_read(float * pressure, float * temperature) +bool SILBoard::baro_read(float * pressure, float * temperature) { // pull z measurement out of Gazebo GazeboPose current_state_NWU = GZ_COMPAT_GET_WORLD_POSE(link_); @@ -379,6 +381,8 @@ void SILBoard::baro_read(float * pressure, float * temperature) (*pressure) = (float) y_baro; (*temperature) = 27.0f + 273.15f; + + return true; } bool SILBoard::diff_pressure_present() @@ -390,7 +394,7 @@ bool SILBoard::diff_pressure_present() } } -void SILBoard::diff_pressure_read(float * diff_pressure, float * temperature) +bool SILBoard::diff_pressure_read(float * diff_pressure, float * temperature) { // Calculate Airspeed GazeboVector vel = GZ_COMPAT_GET_RELATIVE_LINEAR_VEL(link_); @@ -407,37 +411,42 @@ void SILBoard::diff_pressure_read(float * diff_pressure, float * temperature) *diff_pressure = (float) y_as; *temperature = 27.0 + 273.15; + + return true; } bool SILBoard::sonar_present() { return true; } -float SILBoard::sonar_read() +bool SILBoard::sonar_read(float * range) { GazeboPose current_state_NWU = GZ_COMPAT_GET_WORLD_POSE(link_); double alt = GZ_COMPAT_GET_Z(GZ_COMPAT_GET_POS(current_state_NWU)); if (alt < sonar_min_range_) { - return (float) sonar_min_range_; + *range = (float) sonar_min_range_; } else if (alt > sonar_max_range_) { - return (float) sonar_max_range_; + *range = (float) sonar_max_range_; } else { - return (float) (alt + sonar_stdev_ * normal_distribution_(noise_generator_)); + *range = (float) (alt + sonar_stdev_ * normal_distribution_(noise_generator_)); } + + return true; } -bool SILBoard::battery_voltage_present() const { return true; } +bool SILBoard::battery_present() { return true; } -float SILBoard::battery_voltage_read() const { return 15 * battery_voltage_multiplier; } +bool SILBoard::battery_read(float * voltage, float * current) +{ + *voltage = 15 * battery_voltage_multiplier; + *current = 1 * battery_current_multiplier; + return true; +} void SILBoard::battery_voltage_set_multiplier(double multiplier) { battery_voltage_multiplier = (float) multiplier; } -bool SILBoard::battery_current_present() const { return true; } - -float SILBoard::battery_current_read() const { return 1 * battery_current_multiplier; } - void SILBoard::battery_current_set_multiplier(double multiplier) { battery_current_multiplier = (float) multiplier; @@ -569,9 +578,8 @@ void SILBoard::RC_callback(const rosflight_msgs::msg::RCRaw & msg) bool SILBoard::gnss_present() { return true; } -rosflight_firmware::GNSSData SILBoard::gnss_read() +bool SILBoard::gnss_read(rosflight_firmware::GNSSData * gnss, rosflight_firmware::GNSSFull * gnss_full) { - rosflight_firmware::GNSSData out; using Vec3 = ignition::math::Vector3d; using Coord = gazebo::common::SphericalCoordinates::CoordinateType; @@ -591,106 +599,84 @@ rosflight_firmware::GNSSData SILBoard::gnss_read() Vec3 ecef_vel = sph_coord_.VelocityTransform(local_vel, Coord::LOCAL, Coord::ECEF); Vec3 lla = sph_coord_.PositionTransform(local_pos, Coord::LOCAL, Coord::SPHERICAL); - out.lat = (int) std::round(rad2Deg(lla.X()) * 1e7); - out.lon = (int) std::round(rad2Deg(lla.Y()) * 1e7); - out.height = (int) std::round(lla.Z() * 1e3); + gnss->lat = (int) std::round(rad2Deg(lla.X()) * 1e7); + gnss->lon = (int) std::round(rad2Deg(lla.Y()) * 1e7); + gnss->height = (int) std::round(lla.Z() * 1e3); // For now, we have defined the Gazebo Local Frame as NWU. This should be fixed in a future // commit - out.vel_n = (int) std::round(local_vel.X() * 1e3); - out.vel_e = (int) std::round(-local_vel.Y() * 1e3); - out.vel_d = (int) std::round(-local_vel.Z() * 1e3); - - out.fix_type = rosflight_firmware::GNSSFixType::GNSS_FIX_TYPE_FIX; - out.time_of_week = GZ_COMPAT_GET_SIM_TIME(world_).Double() * 1000; - out.time = GZ_COMPAT_GET_SIM_TIME(world_).Double(); - out.nanos = - (uint64_t) std::round((GZ_COMPAT_GET_SIM_TIME(world_).Double() - (double) out.time) * 1e9); - - out.h_acc = (int) std::round(horizontal_gps_stdev_ * 1000.0); - out.v_acc = (int) std::round(vertical_gps_stdev_ * 1000.0); - - out.ecef.x = (int) std::round(ecef_pos.X() * 100); - out.ecef.y = (int) std::round(ecef_pos.Y() * 100); - out.ecef.z = (int) std::round(ecef_pos.Z() * 100); - out.ecef.p_acc = (int) std::round(out.h_acc / 10.0); - out.ecef.vx = (int) std::round(ecef_vel.X() * 100); - out.ecef.vy = (int) std::round(ecef_vel.Y() * 100); - out.ecef.vz = (int) std::round(ecef_vel.Z() * 100); - out.ecef.s_acc = (int) std::round(gps_velocity_stdev_ * 100); - - out.rosflight_timestamp = clock_micros(); - - return out; -} + gnss->vel_n = (int) std::round(local_vel.X() * 1e3); + gnss->vel_e = (int) std::round(-local_vel.Y() * 1e3); + gnss->vel_d = (int) std::round(-local_vel.Z() * 1e3); + + gnss->fix_type = rosflight_firmware::GNSSFixType::GNSS_FIX_TYPE_FIX; + gnss->time_of_week = GZ_COMPAT_GET_SIM_TIME(world_).Double() * 1000; + gnss->time = GZ_COMPAT_GET_SIM_TIME(world_).Double(); + gnss->nanos = + (uint64_t) std::round((GZ_COMPAT_GET_SIM_TIME(world_).Double() - (double) gnss->time) * 1e9); + + gnss->h_acc = (int) std::round(horizontal_gps_stdev_ * 1000.0); + gnss->v_acc = (int) std::round(vertical_gps_stdev_ * 1000.0); + + gnss->ecef.x = (int) std::round(ecef_pos.X() * 100); + gnss->ecef.y = (int) std::round(ecef_pos.Y() * 100); + gnss->ecef.z = (int) std::round(ecef_pos.Z() * 100); + gnss->ecef.p_acc = (int) std::round(gnss->h_acc / 10.0); + gnss->ecef.vx = (int) std::round(ecef_vel.X() * 100); + gnss->ecef.vy = (int) std::round(ecef_vel.Y() * 100); + gnss->ecef.vz = (int) std::round(ecef_vel.Z() * 100); + gnss->ecef.s_acc = (int) std::round(gps_velocity_stdev_ * 100); + + gnss->rosflight_timestamp = clock_micros(); -bool SILBoard::gnss_has_new_data() { return true; } - -rosflight_firmware::GNSSFull SILBoard::gnss_full_read() -{ - rosflight_firmware::GNSSFull out; using Vec3 = ignition::math::Vector3d; using Vec3 = ignition::math::Vector3d; using Coord = gazebo::common::SphericalCoordinates::CoordinateType; - GazeboPose local_pose = GZ_COMPAT_GET_WORLD_POSE(link_); - Vec3 pos_noise(horizontal_gps_stdev_ * normal_distribution_(noise_generator_), - horizontal_gps_stdev_ * normal_distribution_(noise_generator_), - vertical_gps_stdev_ * normal_distribution_(noise_generator_)); - Vec3 local_pos = GZ_COMPAT_GET_POS(local_pose) + pos_noise; - - Vec3 local_vel = GZ_COMPAT_GET_WORLD_LINEAR_VEL(link_); - Vec3 vel_noise(gps_velocity_stdev_ * normal_distribution_(noise_generator_), - gps_velocity_stdev_ * normal_distribution_(noise_generator_), - gps_velocity_stdev_ * normal_distribution_(noise_generator_)); - local_vel += vel_noise; - // TODO: Do a better job of simulating the wander of GPS - Vec3 ecef_pos = sph_coord_.PositionTransform(local_pos, Coord::LOCAL, Coord::ECEF); - Vec3 ecef_vel = sph_coord_.VelocityTransform(local_vel, Coord::LOCAL, Coord::ECEF); - Vec3 lla = sph_coord_.PositionTransform(local_pos, Coord::LOCAL, Coord::SPHERICAL); - - out.lat = (int) std::round(rad2Deg(lla.X()) * 1e7); - out.lon = (int) std::round(rad2Deg(lla.Y()) * 1e7); - out.height = (int) std::round(rad2Deg(lla.Z()) * 1e3); - out.height_msl = out.height; // TODO + gnss_full->lat = (int) std::round(rad2Deg(lla.X()) * 1e7); + gnss_full->lon = (int) std::round(rad2Deg(lla.Y()) * 1e7); + gnss_full->height = (int) std::round(rad2Deg(lla.Z()) * 1e3); + gnss_full->height_msl = gnss_full->height; // TODO // For now, we have defined the Gazebo Local Frame as NWU. This should be // fixed in a future commit - out.vel_n = (int) std::round(local_vel.X() * 1e3); - out.vel_e = (int) std::round(-local_vel.Y() * 1e3); - out.vel_d = (int) std::round(-local_vel.Z() * 1e3); + gnss_full->vel_n = (int) std::round(local_vel.X() * 1e3); + gnss_full->vel_e = (int) std::round(-local_vel.Y() * 1e3); + gnss_full->vel_d = (int) std::round(-local_vel.Z() * 1e3); - out.fix_type = rosflight_firmware::GNSSFixType::GNSS_FIX_TYPE_FIX; - out.time_of_week = GZ_COMPAT_GET_SIM_TIME(world_).Double() * 1000; - out.num_sat = 15; + gnss_full->fix_type = rosflight_firmware::GNSSFixType::GNSS_FIX_TYPE_FIX; + gnss_full->time_of_week = GZ_COMPAT_GET_SIM_TIME(world_).Double() * 1000; + gnss_full->num_sat = 15; // TODO - out.year = 0; - out.month = 0; - out.day = 0; - out.hour = 0; - out.min = 0; - out.sec = 0; - out.valid = 0; - out.t_acc = 0; - out.nano = 0; - - out.h_acc = (int) std::round(horizontal_gps_stdev_ * 1000.0); - out.v_acc = (int) std::round(vertical_gps_stdev_ * 1000.0); + gnss_full->year = 0; + gnss_full->month = 0; + gnss_full->day = 0; + gnss_full->hour = 0; + gnss_full->min = 0; + gnss_full->sec = 0; + gnss_full->valid = 0; + gnss_full->t_acc = 0; + gnss_full->nano = 0; + + gnss_full->h_acc = (int) std::round(horizontal_gps_stdev_ * 1000.0); + gnss_full->v_acc = (int) std::round(vertical_gps_stdev_ * 1000.0); // Again, TODO switch to using ENU convention per REP double vn = local_vel.X(); double ve = -local_vel.Y(); double ground_speed = std::sqrt(vn * vn + ve * ve); - out.g_speed = (int) std::round(ground_speed * 1000); + gnss_full->g_speed = (int) std::round(ground_speed * 1000); double head_mot = std::atan2(ve, vn); - out.head_mot = (int) std::round(rad2Deg(head_mot) * 1e5); - out.p_dop = 0.0; // TODO - out.rosflight_timestamp = clock_micros(); + gnss_full->head_mot = (int) std::round(rad2Deg(head_mot) * 1e5); + gnss_full->p_dop = 0.0; // TODO + gnss_full->rosflight_timestamp = clock_micros(); - return out; + return true; } +bool SILBoard::gnss_has_new_data() { return true; } + } // namespace rosflight_sim diff --git a/rosflight_sim/src/udp_board.cpp b/rosflight_sim/src/udp_board.cpp index 6a7fa55..92026ad 100644 --- a/rosflight_sim/src/udp_board.cpp +++ b/rosflight_sim/src/udp_board.cpp @@ -98,7 +98,7 @@ void UDPBoard::serial_init(uint32_t baud_rate, uint32_t dev) void UDPBoard::serial_flush() {} -void UDPBoard::serial_write(const uint8_t * src, size_t len) +void UDPBoard::serial_write(const uint8_t * src, size_t len, uint8_t qos) { auto buffer = new Buffer(src, len); From 8a45bf18f947531488af68034813b1567d79470f Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Thu, 20 Jun 2024 13:47:51 -0600 Subject: [PATCH 2/7] Added defenition for battery read. Now the aircraft actually loads all parameters in sim, but it won't arm. I have not tested if all RC inputs fail, though it is likely. --- rosflight_sim/include/rosflight_sim/sil_board.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosflight_sim/include/rosflight_sim/sil_board.hpp b/rosflight_sim/include/rosflight_sim/sil_board.hpp index ba929ba..1a2cbc2 100644 --- a/rosflight_sim/include/rosflight_sim/sil_board.hpp +++ b/rosflight_sim/include/rosflight_sim/sil_board.hpp @@ -472,7 +472,7 @@ class SILBoard : public UDPBoard bool battery_present() override; - bool battery_has_new_data() override; + bool battery_has_new_data() override{return true;}; bool battery_read(float * voltage, float * current) override; /** From 8fd08fd9db02dc437fdeaedc9fd0fcf711d5105e Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Fri, 21 Jun 2024 10:29:57 -0600 Subject: [PATCH 3/7] Fixed sensor return errors. The firmware appears to be functional. --- rosflight_sim/include/rosflight_sim/sil_board.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rosflight_sim/include/rosflight_sim/sil_board.hpp b/rosflight_sim/include/rosflight_sim/sil_board.hpp index 1a2cbc2..e568f71 100644 --- a/rosflight_sim/include/rosflight_sim/sil_board.hpp +++ b/rosflight_sim/include/rosflight_sim/sil_board.hpp @@ -265,7 +265,7 @@ class SILBoard : public UDPBoard /** * @brief Function required to be overridden, but not used by sim. */ - bool mag_has_new_data() override{return false;}; + bool mag_has_new_data() override{return true;}; /** * @brief Function used to check if a barometer is present. Currently returns true. @@ -284,7 +284,7 @@ class SILBoard : public UDPBoard /** * @brief Function required to be overridden, but not used by sim. */ - bool baro_has_new_data() override{return false;}; + bool baro_has_new_data() override{return true;}; /** * @brief Checks if a pitot tube sensor is present. Returns true if sim is a fixedwing sim. @@ -303,7 +303,7 @@ class SILBoard : public UDPBoard /** * @brief Function required to be overridden, but not used by sim. */ - bool diff_pressure_has_new_data() override{return false;}; + bool diff_pressure_has_new_data() override{return true;}; /** * @brief Function used to see if a sonar altitude sensor is present. Currently returns true. @@ -371,7 +371,7 @@ class SILBoard : public UDPBoard */ bool rc_lost() override; - bool rc_has_new_data() override{return false;}; + bool rc_has_new_data() override{return true;}; // non-volatile memory /** From a419d8197953e24c56a2ffd825ef6f9278de852d Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Fri, 21 Jun 2024 10:57:28 -0600 Subject: [PATCH 4/7] Added doxygen to the sil_board changes. The sim works for waypoint missions. --- .../include/rosflight_sim/sil_board.hpp | 54 +++++++++++++++---- 1 file changed, 43 insertions(+), 11 deletions(-) diff --git a/rosflight_sim/include/rosflight_sim/sil_board.hpp b/rosflight_sim/include/rosflight_sim/sil_board.hpp index e568f71..f9b1d0a 100644 --- a/rosflight_sim/include/rosflight_sim/sil_board.hpp +++ b/rosflight_sim/include/rosflight_sim/sil_board.hpp @@ -257,13 +257,14 @@ class SILBoard : public UDPBoard bool mag_present() override; /** * @brief Generates magnetometer data based on Gazebo orientation and given noise/bias parameters. - * Returns through function arguments. * * @param mag Magnetometer values to populate. + * @return true if successful. */ bool mag_read(float mag[3]) override; /** * @brief Function required to be overridden, but not used by sim. + * @return true if mag has new data. */ bool mag_has_new_data() override{return true;}; @@ -275,14 +276,16 @@ class SILBoard : public UDPBoard bool baro_present() override; /** * @brief Generates barometer measurement based on Gazebo altitude and noise/bias parameters. - * Returns output through function arguments. * * @param pressure Pressure value to populate. * @param temperature Temperature value to populate. + * + * @return true if successful. */ bool baro_read(float * pressure, float * temperature) override; /** * @brief Function required to be overridden, but not used by sim. + * @return true if baro has new data. */ bool baro_has_new_data() override{return true;}; @@ -294,14 +297,18 @@ class SILBoard : public UDPBoard bool diff_pressure_present() override; /** * @brief Generates a differential pressure measurement based on Gazebo speed and noise/bias - * parameters. Return output through function arguments. + * parameters. * * @param diff_pressure Differential pressure value to populate. * @param temperature Temperature value to populate. + * + * @return true if successful. */ bool diff_pressure_read(float * diff_pressure, float * temperature) override; /** * @brief Function required to be overridden, but not used by sim. + * + * @return true if diff_pressure sensor has new data. */ bool diff_pressure_has_new_data() override{return true;}; @@ -315,15 +322,19 @@ class SILBoard : public UDPBoard * @brief Generates sonar reading based on min/max range and noise parameters. Based on Gazebo * altitude value. * + * @param range measurement to update. + * * @note Currently does not take UAV attitude into account, only absolute altitude. * - * @return Sonar reading. + * @return true if successful. */ bool sonar_read(float * range) override; /** * @brief Function required to be overridden, but not used by sim. + * + * @return true if sonar sensor has new data. */ - bool sonar_has_new_data() override{return false;}; + bool sonar_has_new_data() override{return true;}; // PWM // TODO make these deal in normalized (-1 to 1 or 0 to 1) values (not pwm-specific) @@ -370,7 +381,11 @@ class SILBoard : public UDPBoard * @return False if /RC has had a message published, true otherwise. */ bool rc_lost() override; - + /** + * @brief Function used to determine if the rc has new data. Unused by sim. + * + * @return true if rc has new data. + */ bool rc_has_new_data() override{return true;}; // non-volatile memory @@ -450,7 +465,6 @@ class SILBoard : public UDPBoard * @param len Length of data to clear. */ void backup_memory_clear(size_t len) override; - /** * @brief Function used to check if GNSS is present. Currenlty returns true. * @@ -460,7 +474,10 @@ class SILBoard : public UDPBoard /** * @brief Generates GNSS data based on Gazebo truth and noise/bias parameters. * - * @return Populated GNSSData object + * @param gnss GNSSData object to update. + * @param gnss_full GNSSFull object to update. + * + * @return true if successful. */ bool gnss_read(rosflight_firmware::GNSSData * gnss, rosflight_firmware::GNSSFull * gnss_full) override; /** @@ -469,11 +486,26 @@ class SILBoard : public UDPBoard * @return true */ bool gnss_has_new_data() override; - + /** + * @brief Function used to check if a battery is present. Currenlty returns true. + * + * @return true + */ bool battery_present() override; - + /** + * @brief Function used to determine if the battery sensor has new data. Unused by sim. + * + * @return true if battery has new data. + */ bool battery_has_new_data() override{return true;}; - + /** + * @brief Creates battery data based on sim model. + * + * @param voltage The voltage float to update + * @param current The current float to update + * + * @return true if successful. + */ bool battery_read(float * voltage, float * current) override; /** * @brief Sets battery voltage calibration constant. From 1048d7a6dbbe14bdbc6c44db6abe86138c313a37 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Fri, 21 Jun 2024 14:20:28 -0600 Subject: [PATCH 5/7] Fixed the sensor frequency issue. --- .../include/rosflight_sim/sil_board.hpp | 33 +++++- rosflight_sim/src/sil_board.cpp | 111 +++++++++++++++++- 2 files changed, 136 insertions(+), 8 deletions(-) diff --git a/rosflight_sim/include/rosflight_sim/sil_board.hpp b/rosflight_sim/include/rosflight_sim/sil_board.hpp index f9b1d0a..bac8ecb 100644 --- a/rosflight_sim/include/rosflight_sim/sil_board.hpp +++ b/rosflight_sim/include/rosflight_sim/sil_board.hpp @@ -67,6 +67,13 @@ class SILBoard : public UDPBoard GazeboVector inertial_magnetic_field_; double imu_update_rate_ = 0; + double mag_update_rate_ = 0; + double gnss_update_rate_ = 0; + double baro_update_rate_ = 0; + double diff_pressure_update_rate_ = 0; + double sonar_update_rate_ = 0; + double rc_update_rate_ = 0; + double battery_update_rate_ = 0; long serial_delay_ns_ = 0; std::queue> serial_delay_queue_; @@ -139,7 +146,21 @@ class SILBoard : public UDPBoard // Time variables gazebo::common::Time boot_time_; uint64_t next_imu_update_time_us_ = 0; + uint64_t next_mag_update_time_us_ = 0; + uint64_t next_gnss_update_time_us_ = 0; + uint64_t next_baro_update_time_us_ = 0; + uint64_t next_diff_pressure_update_time_us_ = 0; + uint64_t next_sonar_update_time_us_ = 0; + uint64_t next_rc_update_time_us_ = 0; + uint64_t next_battery_update_time_us_ = 0; uint64_t imu_update_period_us_ = 0; + uint64_t mag_update_period_us_ = 0; + uint64_t gnss_update_period_us_ = 0; + uint64_t baro_update_period_us_ = 0; + uint64_t diff_pressure_update_period_us_ = 0; + uint64_t sonar_update_period_us_ = 0; + uint64_t rc_update_period_us_ = 0; + uint64_t battery_update_period_us_ = 0; /** * @brief Callback function to update RC values when new values are received. @@ -266,7 +287,7 @@ class SILBoard : public UDPBoard * @brief Function required to be overridden, but not used by sim. * @return true if mag has new data. */ - bool mag_has_new_data() override{return true;}; + bool mag_has_new_data() override; /** * @brief Function used to check if a barometer is present. Currently returns true. @@ -287,7 +308,7 @@ class SILBoard : public UDPBoard * @brief Function required to be overridden, but not used by sim. * @return true if baro has new data. */ - bool baro_has_new_data() override{return true;}; + bool baro_has_new_data() override; /** * @brief Checks if a pitot tube sensor is present. Returns true if sim is a fixedwing sim. @@ -310,7 +331,7 @@ class SILBoard : public UDPBoard * * @return true if diff_pressure sensor has new data. */ - bool diff_pressure_has_new_data() override{return true;}; + bool diff_pressure_has_new_data() override; /** * @brief Function used to see if a sonar altitude sensor is present. Currently returns true. @@ -334,7 +355,7 @@ class SILBoard : public UDPBoard * * @return true if sonar sensor has new data. */ - bool sonar_has_new_data() override{return true;}; + bool sonar_has_new_data() override; // PWM // TODO make these deal in normalized (-1 to 1 or 0 to 1) values (not pwm-specific) @@ -386,7 +407,7 @@ class SILBoard : public UDPBoard * * @return true if rc has new data. */ - bool rc_has_new_data() override{return true;}; + bool rc_has_new_data() override; // non-volatile memory /** @@ -497,7 +518,7 @@ class SILBoard : public UDPBoard * * @return true if battery has new data. */ - bool battery_has_new_data() override{return true;}; + bool battery_has_new_data() override; /** * @brief Creates battery data based on sim model. * diff --git a/rosflight_sim/src/sil_board.cpp b/rosflight_sim/src/sil_board.cpp index 30aaf56..67c7863 100644 --- a/rosflight_sim/src/sil_board.cpp +++ b/rosflight_sim/src/sil_board.cpp @@ -70,6 +70,16 @@ void SILBoard::gazebo_setup(gazebo::physics::LinkPtr link, gazebo::physics::Worl auto remote_host = node_->get_parameter_or("ROS_host", "localhost"); int remote_port = node_->get_parameter_or("ROS_port", 14520); + // IMU timer + // Sonar timer + // RC timer + // Baro timer + // Diff Pressure timer + // Mag timer + // Battery timer + // GNSS timer + + set_ports(bind_host, bind_port, remote_host, remote_port); gzmsg << "ROSflight SIL Conneced to " << remote_host << ":" << remote_port << " from " << bind_host << ":" << bind_port << "\n"; @@ -107,6 +117,27 @@ void SILBoard::gazebo_setup(gazebo::physics::LinkPtr link, gazebo::physics::Worl imu_update_rate_ = node_->get_parameter_or("imu_update_rate", 1000.0); imu_update_period_us_ = (uint64_t) (1e6 / imu_update_rate_); + + mag_update_rate_ = node_->get_parameter_or("mag_update_rate", 50.0); + mag_update_period_us_ = (uint64_t) (1e6 / mag_update_rate_); + + gnss_update_rate_ = node_->get_parameter_or("gnss_update_rate", 10.0); + gnss_update_period_us_ = (uint64_t) (1e6 / gnss_update_rate_); + + baro_update_rate_ = node_->get_parameter_or("baro_update_rate", 50.0); + baro_update_period_us_ = (uint64_t) (1e6 / baro_update_rate_); + + diff_pressure_update_rate_ = node_->get_parameter_or("diff_pressure_update_rate", 50.0); + diff_pressure_update_period_us_ = (uint64_t) (1e6 / diff_pressure_update_rate_); + + sonar_update_rate_ = node_->get_parameter_or("sonar_update_rate", 50.0); + sonar_update_period_us_ = (uint64_t) (1e6 / sonar_update_rate_); + + rc_update_rate_ = node_->get_parameter_or("rc_update_rate", 50.0); + rc_update_period_us_ = (uint64_t) (1e6 / rc_update_rate_); + + battery_update_rate_ = node_->get_parameter_or("battery_update_rate", 5.0); + battery_update_period_us_ = (uint64_t) (1e6 / battery_update_rate_); mass_ = node_->get_parameter_or("mass", 2.28); rho_ = node_->get_parameter_or("rho", 1.225); @@ -152,6 +183,7 @@ void SILBoard::gazebo_setup(gazebo::physics::LinkPtr link, gazebo::physics::Worl prev_vel_3_ = GZ_COMPAT_GET_RELATIVE_LINEAR_VEL(link_); last_time_ = GZ_COMPAT_GET_SIM_TIME(world_); next_imu_update_time_us_ = 0; + next_mag_update_time_us_ = 0; } // clock @@ -234,6 +266,83 @@ bool SILBoard::imu_has_new_data() } } +bool SILBoard::mag_has_new_data() +{ + uint64_t now_us = clock_micros(); + if (now_us >= next_mag_update_time_us_) { + next_mag_update_time_us_ = now_us + mag_update_period_us_; + return true; + } else { + return false; + } +} + +bool SILBoard::gnss_has_new_data() +{ + uint64_t now_us = clock_micros(); + if (now_us >= next_gnss_update_time_us_) { + next_gnss_update_time_us_ = now_us + gnss_update_period_us_; + return true; + } else { + return false; + } +} + +bool SILBoard::baro_has_new_data() +{ + uint64_t now_us = clock_micros(); + if (now_us >= next_baro_update_time_us_) { + next_baro_update_time_us_ = now_us + baro_update_period_us_; + return true; + } else { + return false; + } +} + +bool SILBoard::diff_pressure_has_new_data() +{ + uint64_t now_us = clock_micros(); + if (now_us >= next_diff_pressure_update_time_us_) { + next_diff_pressure_update_time_us_ = now_us + diff_pressure_update_period_us_; + return true; + } else { + return false; + } +} + +bool SILBoard::sonar_has_new_data() +{ + uint64_t now_us = clock_micros(); + if (now_us >= next_sonar_update_time_us_) { + next_sonar_update_time_us_ = now_us + sonar_update_period_us_; + return true; + } else { + return false; + } +} + +bool SILBoard::rc_has_new_data() +{ + uint64_t now_us = clock_micros(); + if (now_us >= next_rc_update_time_us_) { + next_rc_update_time_us_ = now_us + rc_update_period_us_; + return true; + } else { + return false; + } +} + +bool SILBoard::battery_has_new_data() +{ + uint64_t now_us = clock_micros(); + if (now_us >= next_battery_update_time_us_) { + next_battery_update_time_us_ = now_us + battery_update_period_us_; + return true; + } else { + return false; + } +} + bool SILBoard::imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time_us) { GazeboQuaternion q_I_NWU = GZ_COMPAT_GET_ROT(GZ_COMPAT_GET_WORLD_POSE(link_)); @@ -677,6 +786,4 @@ bool SILBoard::gnss_read(rosflight_firmware::GNSSData * gnss, rosflight_firmware return true; } -bool SILBoard::gnss_has_new_data() { return true; } - } // namespace rosflight_sim From 4c4d09994975073a2de68dae0b182122b4b437b1 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Fri, 21 Jun 2024 14:23:45 -0600 Subject: [PATCH 6/7] Updated doxygen. --- rosflight_sim/include/rosflight_sim/sil_board.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/rosflight_sim/include/rosflight_sim/sil_board.hpp b/rosflight_sim/include/rosflight_sim/sil_board.hpp index bac8ecb..6f7d203 100644 --- a/rosflight_sim/include/rosflight_sim/sil_board.hpp +++ b/rosflight_sim/include/rosflight_sim/sil_board.hpp @@ -284,7 +284,7 @@ class SILBoard : public UDPBoard */ bool mag_read(float mag[3]) override; /** - * @brief Function required to be overridden, but not used by sim. + * @brief Checks to see if it has been enough time to warrant new data. * @return true if mag has new data. */ bool mag_has_new_data() override; @@ -305,7 +305,7 @@ class SILBoard : public UDPBoard */ bool baro_read(float * pressure, float * temperature) override; /** - * @brief Function required to be overridden, but not used by sim. + * @brief Checks to see if it has been enough time to warrant new data. * @return true if baro has new data. */ bool baro_has_new_data() override; @@ -327,7 +327,7 @@ class SILBoard : public UDPBoard */ bool diff_pressure_read(float * diff_pressure, float * temperature) override; /** - * @brief Function required to be overridden, but not used by sim. + * @brief Checks to see if it has been enough time to warrant new data. * * @return true if diff_pressure sensor has new data. */ @@ -351,7 +351,7 @@ class SILBoard : public UDPBoard */ bool sonar_read(float * range) override; /** - * @brief Function required to be overridden, but not used by sim. + * @brief Checks to see if it has been enough time to warrant new data. * * @return true if sonar sensor has new data. */ @@ -403,7 +403,7 @@ class SILBoard : public UDPBoard */ bool rc_lost() override; /** - * @brief Function used to determine if the rc has new data. Unused by sim. + * @brief Checks to see if it has been enough time to warrant new data. * * @return true if rc has new data. */ @@ -502,7 +502,7 @@ class SILBoard : public UDPBoard */ bool gnss_read(rosflight_firmware::GNSSData * gnss, rosflight_firmware::GNSSFull * gnss_full) override; /** - * @brief Function used to check if gnss has new data to read. Currently returns true. + * @brief Checks to see if it has been enough time to warrant new data. * * @return true */ @@ -514,7 +514,7 @@ class SILBoard : public UDPBoard */ bool battery_present() override; /** - * @brief Function used to determine if the battery sensor has new data. Unused by sim. + * @brief Checks to see if it has been enough time to warrant new data. * * @return true if battery has new data. */ From aaebb4a1e6e263a3decccb8a5c58fbb162b0756a Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Fri, 21 Jun 2024 15:09:29 -0600 Subject: [PATCH 7/7] Removed comments. --- rosflight_sim/src/sil_board.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/rosflight_sim/src/sil_board.cpp b/rosflight_sim/src/sil_board.cpp index 67c7863..784c608 100644 --- a/rosflight_sim/src/sil_board.cpp +++ b/rosflight_sim/src/sil_board.cpp @@ -70,16 +70,6 @@ void SILBoard::gazebo_setup(gazebo::physics::LinkPtr link, gazebo::physics::Worl auto remote_host = node_->get_parameter_or("ROS_host", "localhost"); int remote_port = node_->get_parameter_or("ROS_port", 14520); - // IMU timer - // Sonar timer - // RC timer - // Baro timer - // Diff Pressure timer - // Mag timer - // Battery timer - // GNSS timer - - set_ports(bind_host, bind_port, remote_host, remote_port); gzmsg << "ROSflight SIL Conneced to " << remote_host << ":" << remote_port << " from " << bind_host << ":" << bind_port << "\n";