From 22c2878cf8dd3a0f183b2a6db573aef643619359 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Wed, 11 Sep 2024 11:16:07 +0200 Subject: [PATCH 01/18] send acknowledgement after receiving vehicle wind cmd --- src/modules/ekf2/EKF2.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 38e5660e4ff4..46a28e469f99 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -571,7 +571,12 @@ void EKF2::Run() const float wind_direction_accuracy_rad = math::radians(vehicle_command.param4); _ekf.resetWindToExternalObservation(vehicle_command.param1, wind_direction_rad, vehicle_command.param2, wind_direction_accuracy_rad); + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; +#else + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; #endif // CONFIG_EKF2_WIND + command_ack.timestamp = hrt_absolute_time(); + _vehicle_command_ack_pub.publish(command_ack); } } } From 4ba4b340cc146d65b3f97abb70a22a826137d099 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Tue, 10 Sep 2024 11:03:18 +0200 Subject: [PATCH 02/18] Reduce the orbit jerk by using a slew rate --- .../tasks/Orbit/FlightTaskOrbit.cpp | 11 +++++++++-- .../tasks/Orbit/FlightTaskOrbit.hpp | 2 ++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 144c6c7ba158..3136c9805176 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -181,6 +181,7 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint) _initial_heading = _yaw; _slew_rate_yaw.setForcedValue(_yaw); _slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get())); + _slew_rate_velocity.setSlewRate(_param_mpc_acc_hor.get()); // need a valid position and velocity ret = ret && _position.isAllFinite() && _velocity.isAllFinite(); @@ -216,6 +217,7 @@ bool FlightTaskOrbit::update() if (_is_position_on_circle()) { if (_in_circle_approach) { _in_circle_approach = false; + _slew_rate_velocity.setForcedValue(0.f); // reset the slew rate when moving between orbits. FlightTaskManualAltitudeSmoothVel::_smoothing.reset( PX4_ISFINITE(_acceleration_setpoint(2)) ? _acceleration_setpoint(2) : 0.f, PX4_ISFINITE(_velocity_setpoint(2)) ? _velocity_setpoint(2) : _velocity(2), @@ -329,15 +331,20 @@ void FlightTaskOrbit::_generate_circle_setpoints() Vector3f center_to_position = _position - _center; // xy velocity to go around in a circle Vector2f velocity_xy(-center_to_position(1), center_to_position(0)); + + // slew rate is used to reduce the jerk when starting an orbit. + _slew_rate_velocity.update(_orbit_velocity, _deltatime); + velocity_xy = velocity_xy.unit_or_zero(); - velocity_xy *= _orbit_velocity; + velocity_xy *= _slew_rate_velocity.getState(); // xy velocity adjustment to stay on the radius distance velocity_xy += (_orbit_radius - center_to_position.xy().norm()) * Vector2f(center_to_position).unit_or_zero(); _position_setpoint(0) = _position_setpoint(1) = NAN; _velocity_setpoint.xy() = velocity_xy; - _acceleration_setpoint.xy() = -Vector2f(center_to_position.unit_or_zero()) * _orbit_velocity * _orbit_velocity / + _acceleration_setpoint.xy() = -Vector2f(center_to_position.unit_or_zero()) * _slew_rate_velocity.getState() * + _slew_rate_velocity.getState() / _orbit_radius; } diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index 2c55bbc15ba7..13e53bf83fe9 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -47,6 +47,7 @@ #include #include #include +#include class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel @@ -127,6 +128,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel bool _currently_orbiting{false}; float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */ SlewRateYaw _slew_rate_yaw; + SlewRate _slew_rate_velocity; orb_advert_t _mavlink_log_pub{nullptr}; uORB::PublicationMulti _orbit_status_pub{ORB_ID(orbit_status)}; From 585c92796d30058845b8e62ea13adb25f0b2181c Mon Sep 17 00:00:00 2001 From: Konrad Date: Fri, 13 Sep 2024 12:35:50 +0200 Subject: [PATCH 03/18] mission_base: do not make terrain avoidance check when mission is not run anymore --- src/modules/navigator/mission_block.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 84d95ae5a310..c2b17bbedbab 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -1030,6 +1030,7 @@ void MissionBlock::updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_i if (_navigator->get_nav_min_gnd_dist_param() > FLT_EPSILON && _mission_item.nav_cmd != NAV_CMD_LAND && _mission_item.nav_cmd != NAV_CMD_VTOL_LAND && _mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION + && _mission_item.nav_cmd != NAV_CMD_IDLE && _navigator->get_local_position()->dist_bottom_valid && _navigator->get_local_position()->dist_bottom < _navigator->get_nav_min_gnd_dist_param() && _navigator->get_local_position()->vz > FLT_EPSILON From 4713a6737ee660af113bd412671a62a2fb8a755a Mon Sep 17 00:00:00 2001 From: Konrad Date: Thu, 5 Sep 2024 17:08:53 +0200 Subject: [PATCH 04/18] TECS: ramp up fast descend over 2_s to ramp down the throttle command --- msg/TecsStatus.msg | 1 + src/lib/tecs/TECS.cpp | 36 +++++++++++++------ src/lib/tecs/TECS.hpp | 27 +++++++++++--- .../FixedwingPositionControl.cpp | 3 +- 4 files changed, 50 insertions(+), 17 deletions(-) diff --git a/msg/TecsStatus.msg b/msg/TecsStatus.msg index ae6835dc5249..af3826b1555d 100644 --- a/msg/TecsStatus.msg +++ b/msg/TecsStatus.msg @@ -27,3 +27,4 @@ float32 pitch_sp_rad # Current pitch setpoint [rad] float32 throttle_trim # estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions float32 underspeed_ratio # 0: no underspeed, 1: maximal underspeed. Controller takes measures to avoid stall proportional to ratio if >0. +float32 fast_descend_ratio # value indicating if fast descend mode is enabled with ramp up and ramp down [0-1] diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 4a6d3593e894..6a09b8775913 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -44,11 +44,11 @@ #include "matrix/Matrix.hpp" #include "matrix/Vector2.hpp" +#include using math::constrain; using math::max; using math::min; -using namespace time_literals; static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);} @@ -525,16 +525,12 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec if (1.f - param.fast_descend < FLT_EPSILON) { // During fast descend, we control airspeed over the pitch control loop. Give minimal thrust as soon as we are descending - if (specific_energy_rates.spe_rate.estimate > 0) { // We have a positive altitude rate and are stil climbing - throttle_setpoint = param.throttle_trim; // Do not cut off throttle yet - - } else { - throttle_setpoint = param.throttle_min; - } + throttle_setpoint = param.throttle_min; } else { _calcThrottleControlUpdate(dt, limit, ste_rate, param, flag); - throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag); + throttle_setpoint = (1.f - param.fast_descend) * _calcThrottleControlOutput(limit, ste_rate, param, + flag) + param.fast_descend * param.throttle_min; } // Rate limit the throttle demand @@ -677,6 +673,11 @@ void TECS::initControlParams(float target_climbrate, float target_sinkrate, floa _control_param.throttle_min = throttle_min; } +float TECS::calcTrueAirspeedSetpoint(float eas_to_tas, float eas_setpoint) +{ + return lerp(eas_to_tas * eas_setpoint, _control_param.tas_max, _fast_descend); +} + void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed, float eas_to_tas) { @@ -699,6 +700,9 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo .tas_rate = 0.0f}; _control.initialize(control_setpoint, control_input, _control_param, _control_flag); + + _fast_descend = 0.f; + _enabled_fast_descend_timestamp = 0U; } void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, @@ -753,8 +757,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set TECSControl::Setpoint control_setpoint; control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference(); control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect(); - control_setpoint.tas_setpoint = _control_param.tas_max * _fast_descend + (1 - _fast_descend) * eas_to_tas * - EAS_setpoint; + control_setpoint.tas_setpoint = calcTrueAirspeedSetpoint(eas_to_tas, EAS_setpoint); const TECSControl::Input control_input{ .altitude = altitude, .altitude_rate = hgt_rate, @@ -765,11 +768,13 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set } _debug_status.control = _control.getDebugOutput(); + _debug_status.true_airspeed_sp = calcTrueAirspeedSetpoint(eas_to_tas, EAS_setpoint); _debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed; _debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate; _debug_status.altitude_reference = _altitude_reference_model.getAltitudeReference().alt; _debug_status.height_rate_reference = _altitude_reference_model.getAltitudeReference().alt_rate; _debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect(); + _debug_status.fast_descend = _fast_descend; _update_timestamp = now; } @@ -778,13 +783,22 @@ void TECS::_setFastDescend(const float alt_setpoint, const float alt) { if (_control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON) && ((alt_setpoint + _fast_descend_alt_err) < alt)) { - _fast_descend = 1.f; + auto now = hrt_absolute_time(); + + if (_enabled_fast_descend_timestamp == 0U) { + _enabled_fast_descend_timestamp = now; + } + + _fast_descend = constrain(max(_fast_descend, static_cast(now - _enabled_fast_descend_timestamp) / + static_cast(FAST_DESCEND_RAMP_UP_TIME)), 0.f, 1.f); } else if ((_fast_descend > FLT_EPSILON) && (_fast_descend_alt_err > FLT_EPSILON)) { // Were in fast descend, scale it down. up until 5m above target altitude _fast_descend = constrain((alt - alt_setpoint - 5.f) / _fast_descend_alt_err, 0.f, 1.f); + _enabled_fast_descend_timestamp = 0U; } else { _fast_descend = 0.f; + _enabled_fast_descend_timestamp = 0U; } } diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index f033bf72dabf..57cc12690b14 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -50,6 +50,8 @@ #include #include +using namespace time_literals; + class TECSAirspeedFilter { public: @@ -203,8 +205,8 @@ class TECSControl float equivalent_airspeed_trim; ///< Equivalent cruise airspeed for airspeed less mode [m/s]. float tas_min; ///< True airspeed demand lower limit [m/s]. float tas_max; ///< True airspeed demand upper limit [m/s]. - float pitch_max; ///< Maximum pitch angle allowed in [rad]. - float pitch_min; ///< Minimal pitch angle allowed in [rad]. + float pitch_max; ///< Maximum pitch angle above trim allowed in [rad]. + float pitch_min; ///< Minimal pitch angle below trim allowed in [rad]. float throttle_trim; ///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1] float throttle_max; ///< Normalized throttle upper limit. float throttle_min; ///< Normalized throttle lower limit. @@ -320,7 +322,7 @@ class TECSControl /** * @brief Get the pitch setpoint. * - * @return THe commanded pitch angle in [rad]. + * @return The commanded pitch angle above trim in [rad]. */ float getPitchSetpoint() const {return _pitch_setpoint;}; /** @@ -478,7 +480,7 @@ class TECSControl * @param seb_rate is the specific energy balance rate in [m²/s³]. * @param param is the control parameters. * @param flag is the control flags. - * @return pitch setpoint angle [rad]. + * @return pitch setpoint angle above trim [rad]. */ float _calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param ¶m, const Flag &flag) const; @@ -537,7 +539,7 @@ class TECSControl // Output DebugOutput _debug_output; ///< Debug output. - float _pitch_setpoint{0.0f}; ///< Controlled pitch setpoint [rad]. + float _pitch_setpoint{0.0f}; ///< Controlled pitch setpoint above trim [rad]. float _throttle_setpoint{0.0f}; ///< Controlled throttle setpoint [0,1]. float _ratio_undersped{0.0f}; ///< A continuous representation of how "undersped" the TAS is [0,1] }; @@ -547,11 +549,13 @@ class TECS public: struct DebugOutput { TECSControl::DebugOutput control; + float true_airspeed_sp; float true_airspeed_filtered; float true_airspeed_derivative; float altitude_reference; float height_rate_reference; float height_rate_direct; + float fast_descend; }; public: TECS() = default; @@ -658,6 +662,17 @@ class TECS void initControlParams(float target_climbrate, float target_sinkrate, float eas_to_tas, float pitch_limit_max, float pitch_limit_min, float throttle_min, float throttle_setpoint_max, float throttle_trim); + /** + * @brief calculate true airspeed setpoint + * + * Calculate true airspeed setpoint based on input and fast descend ratio + * + * @param eas_to_tas is the eas to tas conversion factor + * @param eas_setpoint is the desired equivalent airspeed setpoint [m/s] + * @return true airspeed setpoint[m/s] + */ + float calcTrueAirspeedSetpoint(float eas_to_tas, float eas_setpoint); + /** * @brief Initialize the control loop * @@ -675,9 +690,11 @@ class TECS float _equivalent_airspeed_max{20.0f}; ///< equivalent airspeed demand upper limit (m/sec) float _fast_descend_alt_err{-1.f}; ///< Altitude difference between current altitude to altitude setpoint needed to descend with higher airspeed [m]. float _fast_descend{0.f}; ///< Value for fast descend in [0,1]. continuous value used to flatten the high speed value out when close to target altitude. + hrt_abstime _enabled_fast_descend_timestamp{0U}; ///< timestamp at activation of fast descend mode static constexpr float DT_MIN = 0.001f; ///< minimum allowed value of _dt (sec) static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a filter state reset is performed (sec) + static constexpr hrt_abstime FAST_DESCEND_RAMP_UP_TIME = 2_s; ///< Ramp up time until fast descend is fully engaged DebugOutput _debug_status{}; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 00e7869c6949..fcd5ce553903 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -424,7 +424,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control; tecs_status.height_rate = -_local_pos.vz; tecs_status.equivalent_airspeed_sp = equivalent_airspeed_sp; - tecs_status.true_airspeed_sp = _eas2tas * equivalent_airspeed_sp; + tecs_status.true_airspeed_sp = debug_output.true_airspeed_sp; tecs_status.true_airspeed_filtered = debug_output.true_airspeed_filtered; tecs_status.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control; tecs_status.true_airspeed_derivative = debug_output.true_airspeed_derivative; @@ -439,6 +439,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint(); tecs_status.throttle_trim = throttle_trim; tecs_status.underspeed_ratio = _tecs.get_underspeed_ratio(); + tecs_status.fast_descend_ratio = debug_output.fast_descend; tecs_status.timestamp = hrt_absolute_time(); From 878c8bfcce19d4d3e8c5f4cbe1ec44da41b0dd34 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 10 Sep 2024 17:50:03 +0300 Subject: [PATCH 05/18] SIH: fix airspeed for tailsitter Signed-off-by: RomanBapst --- src/modules/simulation/simulator_sih/sih.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 399a4ecc938d..2ca4987338e8 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -450,7 +450,8 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) // TODO: send differential pressure instead? airspeed_s airspeed{}; airspeed.timestamp_sample = time_now_us; - airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B(0) + generate_wgn() * 0.2f); + // airspeed sensor is mounted along the negative Z axis since the vehicle is a tailsitter + airspeed.true_airspeed_m_s = fmaxf(0.1f, -_v_B(2) + generate_wgn() * 0.2f); airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO); airspeed.air_temperature_celsius = NAN; airspeed.confidence = 0.7f; From 11440cfb454871db8276d4c3a1a3fcd33b7534df Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 13 Sep 2024 12:13:11 +0300 Subject: [PATCH 06/18] added some default parameteter that allow the transition to complete Signed-off-by: RomanBapst --- .../init.d/airframes/1102_tailsitter_duo_sih.hil | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil index a9b2f7f03b1b..6ac2671d0730 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil @@ -43,6 +43,10 @@ param set-default CA_SV_CS1_TRQ_P 0.3 param set-default CA_SV_CS1_TRQ_Y -0.3 param set-default CA_SV_CS1_TYPE 6 +param set-default FW_AIRSPD_MAX 12 +param set-default FW_AIRSPD_MIN 7 +param set-default FW_AIRSPD_TRIM 10 + param set-default HIL_ACT_FUNC1 101 param set-default HIL_ACT_FUNC2 102 param set-default HIL_ACT_FUNC5 202 @@ -75,3 +79,5 @@ param set SIH_L_ROLL 0.145 # sih as tailsitter param set SIH_VEHICLE_TYPE 2 + +param set-default VT_ARSP_TRANS 6 From 9ca0630376fca98396943c065034d3cacae34746 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 13 Sep 2024 13:51:18 +0200 Subject: [PATCH 07/18] airframes: SIH_tailsitter: add SENS_DPRES_OFF to bypass airspeed cal Signed-off-by: Silvan Fuhrer --- .../px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil index 6ac2671d0730..d9e8611cdb0d 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil @@ -66,6 +66,8 @@ param set-default CBRK_SUPPLY_CHK 894281 # - without safety switch param set-default CBRK_IO_SAFETY 22027 +param set-default SENS_DPRES_OFF 0.001 + param set SIH_T_MAX 2.0 param set SIH_Q_MAX 0.0165 param set SIH_MASS 0.2 From e6f07bde2aa1daef45ea6336d892efb4ef89b1fa Mon Sep 17 00:00:00 2001 From: Konrad Date: Mon, 9 Sep 2024 10:35:35 +0200 Subject: [PATCH 08/18] lightware_laser: add option to listen to system to enable/disable distance sensor --- msg/CMakeLists.txt | 1 + msg/DistanceSensorModeChangeRequest.msg | 5 +++++ .../lightware_laser_i2c/lightware_laser_i2c.cpp | 16 +++++++++++++++- .../lightware_laser_i2c/parameters.c | 2 +- 4 files changed, 22 insertions(+), 2 deletions(-) create mode 100644 msg/DistanceSensorModeChangeRequest.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 91c38488f5aa..596ca30fcdf7 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -73,6 +73,7 @@ set(msg_files DebugVect.msg DifferentialPressure.msg DistanceSensor.msg + DistanceSensorModeChangeRequest.msg Ekf2Timestamps.msg EscReport.msg EscStatus.msg diff --git a/msg/DistanceSensorModeChangeRequest.msg b/msg/DistanceSensorModeChangeRequest.msg new file mode 100644 index 000000000000..294e225e354f --- /dev/null +++ b/msg/DistanceSensorModeChangeRequest.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 req_mode # requested mode of operation +uint8 MODE_DISABLED = 0 +uint8 MODE_ENABLED = 1 diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 6c1dfbcea068..152073f2ac5c 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -55,6 +55,7 @@ #include #include #include +#include using namespace time_literals; @@ -143,6 +144,8 @@ class LightwareLaser : public device::I2C, public I2CSPIDriver, uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN}; + uORB::Subscription _dist_sense_mode_change_sub{ORB_ID(distance_sensor_mode_change_request)}; + typeof(px4::msg::DistanceSensorModeChangeRequest::req_mode) _req_mode{px4::msg::DistanceSensorModeChangeRequest::MODE_DISABLED}; bool _restriction{false}; bool _auto_restriction{false}; bool _prev_restriction{false}; @@ -412,6 +415,17 @@ void LightwareLaser::start() int LightwareLaser::updateRestriction() { + if (_dist_sense_mode_change_sub.updated()) { + distance_sensor_mode_change_request_s dist_sense_mode_change; + + if (_dist_sense_mode_change_sub.copy(&dist_sense_mode_change)) { + _req_mode = dist_sense_mode_change.req_mode; + + } else { + _req_mode = distance_sensor_mode_change_request_s::MODE_DISABLED; + } + } + px4::msg::VehicleStatus vehicle_status; if (_vehicle_status_sub.update(&vehicle_status)) { @@ -452,7 +466,7 @@ int LightwareLaser::updateRestriction() break; case 2: - _restriction = _auto_restriction; + _restriction = _auto_restriction && _req_mode != distance_sensor_mode_change_request_s::MODE_ENABLED; break; } diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c b/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c index defe335751f3..161e9d3cabe0 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c +++ b/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c @@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0); * * @value 0 Disabled * @value 1 Enabled - * @value 2 Disabled during VTOL fast forward flight + * @value 2 Enabled in VTOL MC mode, listen to request from system in FW mode * * @min 0 * @max 2 From 1755b8304e689fc5ef744f03742c7e626975e01e Mon Sep 17 00:00:00 2001 From: Konrad Date: Mon, 9 Sep 2024 16:48:04 +0200 Subject: [PATCH 09/18] RTL direct: Make sure the _rtl_state captures the current status and not the next one --- src/modules/navigator/rtl_direct.cpp | 79 ++++++++++++++++++++-------- src/modules/navigator/rtl_direct.h | 6 +++ 2 files changed, 63 insertions(+), 22 deletions(-) diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 145b99d0d1db..088ad1baebdd 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -101,10 +101,11 @@ void RtlDirect::on_active() parameters_update(); if (_rtl_state != RTLState::IDLE && is_mission_item_reached_or_completed()) { + _updateRtlState(); set_rtl_item(); } - if (_rtl_state != RTLState::IDLE) { //TODO: rename _rtl_state to _rtl_state_next (when in IDLE we're actually in LAND) + if (_rtl_state != RTLState::IDLE && _rtl_state != RTLState::LAND) { //check for terrain collision and update altitude if needed // note: it may trigger multiple times during a RTL, as every time the altitude set is reset updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item); @@ -154,6 +155,61 @@ void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s } } +void RtlDirect::_updateRtlState() +{ + RTLState new_state{RTLState::IDLE}; + + switch (_rtl_state) { + case RTLState::CLIMBING: + new_state = RTLState::MOVE_TO_LOITER; + break; + + case RTLState::MOVE_TO_LOITER: + new_state = RTLState::LOITER_DOWN; + break; + + case RTLState::LOITER_DOWN: + new_state = RTLState::LOITER_HOLD; + break; + + case RTLState::LOITER_HOLD: + if (_vehicle_status_sub.get().is_vtol + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + new_state = RTLState::MOVE_TO_LAND; + + } else { + new_state = RTLState::MOVE_TO_LAND_HOVER; + } + + break; + + case RTLState::MOVE_TO_LAND: + new_state = RTLState::TRANSITION_TO_MC; + break; + + case RTLState::TRANSITION_TO_MC: + new_state = RTLState::MOVE_TO_LAND_HOVER; + break; + + case RTLState::MOVE_TO_LAND_HOVER: + new_state = RTLState::LAND; + break; + + case RTLState::LAND: + new_state = RTLState::IDLE; + break; + + case RTLState::IDLE: // Fallthrough + default: + new_state = RTLState::IDLE; + break; + } + + _rtl_state = new_state; + +} + + void RtlDirect::set_rtl_item() { position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -174,7 +230,6 @@ void RtlDirect::set_rtl_item() }; setLoiterToAltMissionItem(_mission_item, pos_yaw_sp, _navigator->get_loiter_radius()); - _rtl_state = RTLState::MOVE_TO_LOITER; break; } @@ -197,8 +252,6 @@ void RtlDirect::set_rtl_item() setMoveToPositionMissionItem(_mission_item, pos_yaw_sp); } - _rtl_state = RTLState::LOITER_DOWN; - break; } @@ -224,8 +277,6 @@ void RtlDirect::set_rtl_item() // Disable previous setpoint to prevent drift. pos_sp_triplet->previous.valid = false; - _rtl_state = RTLState::LOITER_HOLD; - break; } @@ -244,14 +295,6 @@ void RtlDirect::set_rtl_item() events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering"); } - if (_vehicle_status_sub.get().is_vtol - && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - _rtl_state = RTLState::MOVE_TO_LAND; - - } else { - _rtl_state = RTLState::MOVE_TO_LAND_HOVER; - } - break; } @@ -274,16 +317,12 @@ void RtlDirect::set_rtl_item() pos_sp_triplet->previous.alt = get_absolute_altitude_for_item(_mission_item); pos_sp_triplet->previous.valid = true; - _rtl_state = RTLState::TRANSITION_TO_MC; - break; } case RTLState::TRANSITION_TO_MC: { set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); - _rtl_state = RTLState::MOVE_TO_LAND_HOVER; - break; } @@ -295,8 +334,6 @@ void RtlDirect::set_rtl_item() setMoveToPositionMissionItem(_mission_item, pos_yaw_sp); _navigator->reset_position_setpoint(pos_sp_triplet->previous); - _rtl_state = RTLState::LAND; - break; } @@ -309,8 +346,6 @@ void RtlDirect::set_rtl_item() startPrecLand(_mission_item.land_precision); - _rtl_state = RTLState::IDLE; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); break; diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index b1f6a75b81ae..91e53dbe6b79 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -121,6 +121,12 @@ class RtlDirect : public MissionBlock, public ModuleParams } _rtl_state{RTLState::IDLE}; /*< Current state in the state machine.*/ private: + /** + * @brief Update the RTL state machine. + * + */ + void _updateRtlState(); + /** * @brief Set the return to launch control setpoint. * From aab2390e5145357c01706c29b842321364087c6b Mon Sep 17 00:00:00 2001 From: Konrad Date: Mon, 9 Sep 2024 11:06:03 +0200 Subject: [PATCH 10/18] navigator: publish distance sensor mode change request when in RTL landing phase or during mission landing --- src/modules/logger/logged_topics.cpp | 1 + src/modules/navigator/mission_base.h | 2 +- src/modules/navigator/navigator.h | 5 +++ src/modules/navigator/navigator_main.cpp | 31 +++++++++++++++++++ src/modules/navigator/rtl.cpp | 22 +++++++++++++ src/modules/navigator/rtl.h | 2 ++ src/modules/navigator/rtl_direct.h | 2 ++ .../navigator/rtl_mission_fast_reverse.cpp | 9 ++++++ .../navigator/rtl_mission_fast_reverse.h | 5 +++ 9 files changed, 78 insertions(+), 1 deletion(-) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index f5ce636c23c9..d468563597b7 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -58,6 +58,7 @@ void LoggedTopics::add_default_topics() add_topic("commander_state"); add_topic("config_overrides"); add_topic("cpuload"); + add_topic("distance_sensor_mode_change_request"); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position"); diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 1a6cbf977534..b8093ca01cf9 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -73,7 +73,7 @@ class MissionBase : public MissionBlock, public ModuleParams virtual void on_activation() override; virtual void on_active() override; - bool isLanding(); + virtual bool isLanding(); protected: diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ec72881fc527..7fece40a4ecc 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -316,6 +317,7 @@ class Navigator : public ModuleBase, public ModuleParams uORB::Publication _vehicle_cmd_pub{ORB_ID(vehicle_command)}; uORB::Publication _vehicle_roi_pub{ORB_ID(vehicle_roi)}; uORB::Publication _mode_completed_pub{ORB_ID(mode_completed)}; + uORB::PublicationData _distance_sensor_mode_change_request_pub{ORB_ID(distance_sensor_mode_change_request)}; orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */ @@ -336,6 +338,7 @@ class Navigator : public ModuleBase, public ModuleParams position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */ vehicle_roi_s _vroi{}; /**< vehicle ROI */ + perf_counter_t _loop_perf; /**< loop performance counter */ Geofence _geofence; /**< class that handles the geofence */ @@ -400,6 +403,8 @@ class Navigator : public ModuleBase, public ModuleParams void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result); + void publish_distance_sensor_mode_request(); + bool geofence_allows_position(const vehicle_global_position_s &pos); DEFINE_PARAMETERS( diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9a9f46e96282..e46decdfb19a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -110,6 +110,11 @@ Navigator::Navigator() : _mission_sub = orb_subscribe(ORB_ID(mission)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + _distance_sensor_mode_change_request_pub.advertise(); + _distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time(); + _distance_sensor_mode_change_request_pub.get().req_mode = distance_sensor_mode_change_request_s::MODE_DISABLED; + _distance_sensor_mode_change_request_pub.update(); + // Update the timeout used in mission_block (which can't hold it's own parameters) _mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get()); @@ -898,6 +903,8 @@ void Navigator::run() publish_navigator_status(); + publish_distance_sensor_mode_request(); + _geofence.run(); perf_end(_loop_perf); @@ -1447,6 +1454,30 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) _vehicle_cmd_pub.publish(*vcmd); } +void Navigator::publish_distance_sensor_mode_request() +{ + // Send request to enable distance sensor when in the landing phase of a mission or RTL + if (((_navigation_mode == &_rtl) && _rtl.isLanding()) || ((_navigation_mode == &_mission) && _mission.isLanding())) { + + if (_distance_sensor_mode_change_request_pub.get().req_mode != + distance_sensor_mode_change_request_s::MODE_ENABLED) { + + _distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time(); + _distance_sensor_mode_change_request_pub.get().req_mode = + distance_sensor_mode_change_request_s::MODE_ENABLED; + _distance_sensor_mode_change_request_pub.update(); + } + + } else if (_distance_sensor_mode_change_request_pub.get().req_mode != + distance_sensor_mode_change_request_s::MODE_DISABLED) { + + _distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time(); + _distance_sensor_mode_change_request_pub.get().req_mode = + distance_sensor_mode_change_request_s::MODE_DISABLED; + _distance_sensor_mode_change_request_pub.update(); + } +} + void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result) { vehicle_command_ack_s command_ack = {}; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 63e2ab185ff1..cb33f350b7b1 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -304,6 +304,28 @@ void RTL::on_active() } } +bool RTL::isLanding() +{ + bool is_landing{false}; + + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + case RtlType::RTL_MISSION_FAST_REVERSE: + case RtlType::RTL_DIRECT_MISSION_LAND: + is_landing = _rtl_mission_type_handle->isLanding(); + break; + + case RtlType::RTL_DIRECT: + is_landing = _rtl_direct.isLanding(); + break; + + default: + break; + } + + return is_landing; +} + void RTL::setRtlTypeAndDestination() { diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index e7b720bd6152..4ef13cb7874d 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -89,6 +89,8 @@ class RTL : public NavigatorMode, public ModuleParams void updateSafePoints(uint32_t new_safe_point_id) { _initiate_safe_points_updated = true; _safe_points_id = new_safe_point_id; } + bool isLanding(); + private: enum class DestinationType { DESTINATION_TYPE_HOME, diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 91e53dbe6b79..aed7cf3e4c6d 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -103,6 +103,8 @@ class RtlDirect : public MissionBlock, public ModuleParams void setRtlPosition(PositionYawSetpoint position, loiter_point_s loiter_pos); + bool isLanding() { return (_rtl_state != RTLState::IDLE) && (_rtl_state >= RTLState::LOITER_DOWN);}; + private: /** * @brief Return to launch state machine. diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index d7c42516e41d..d78f6c7fa627 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -60,6 +60,12 @@ void RtlMissionFastReverse::on_inactive() _mission.current_seq : -1; } +void RtlMissionFastReverse::on_inactivation() +{ + MissionBase::on_inactivation(); + _in_landing_phase = false; +} + void RtlMissionFastReverse::on_activation() { _home_pos_sub.update(); @@ -120,6 +126,7 @@ void RtlMissionFastReverse::setActiveMissionItems() _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || num_found_items == 0) { handleLanding(new_work_item_type); + _in_landing_phase = true; } else { // convert mission item to a simple waypoint, keep loiter to alt @@ -131,6 +138,8 @@ void RtlMissionFastReverse::setActiveMissionItems() _mission_item.time_inside = 0.0f; pos_sp_triplet->previous = pos_sp_triplet->current; + + _in_landing_phase = false; } if (num_found_items > 0) { diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h index f1301a5ccafd..168d20c2e341 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.h +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -58,6 +58,9 @@ class RtlMissionFastReverse : public RtlBase void on_activation() override; void on_active() override; void on_inactive() override; + void on_inactivation() override; + + bool isLanding() override {return _in_landing_phase;}; rtl_time_estimate_s calc_rtl_time_estimate() override; @@ -68,5 +71,7 @@ class RtlMissionFastReverse : public RtlBase int _mission_index_prior_rtl{-1}; + bool _in_landing_phase{false}; + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ }; From 82a7d0410cdca5be4c324c57be8c9edaf50fcffd Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 11 Sep 2024 18:10:52 +0200 Subject: [PATCH 11/18] DistanceSensorModeChangeRequest: renaming of variable --- msg/DistanceSensorModeChangeRequest.msg | 6 +++--- .../lightware_laser_i2c.cpp | 8 ++++---- src/modules/navigator/navigator_main.cpp | 18 +++++++++--------- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/msg/DistanceSensorModeChangeRequest.msg b/msg/DistanceSensorModeChangeRequest.msg index 294e225e354f..01d5a49cd330 100644 --- a/msg/DistanceSensorModeChangeRequest.msg +++ b/msg/DistanceSensorModeChangeRequest.msg @@ -1,5 +1,5 @@ uint64 timestamp # time since system start (microseconds) -uint8 req_mode # requested mode of operation -uint8 MODE_DISABLED = 0 -uint8 MODE_ENABLED = 1 +uint8 request_on_off # request to disable/enable the distance sensor +uint8 REQUEST_OFF = 0 +uint8 REQUEST_ON = 1 diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 152073f2ac5c..614d408a5e30 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -145,7 +145,7 @@ class LightwareLaser : public device::I2C, public I2CSPIDriver, uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN}; uORB::Subscription _dist_sense_mode_change_sub{ORB_ID(distance_sensor_mode_change_request)}; - typeof(px4::msg::DistanceSensorModeChangeRequest::req_mode) _req_mode{px4::msg::DistanceSensorModeChangeRequest::MODE_DISABLED}; + typeof(px4::msg::DistanceSensorModeChangeRequest::request_on_off) _req_mode{px4::msg::DistanceSensorModeChangeRequest::REQUEST_OFF}; bool _restriction{false}; bool _auto_restriction{false}; bool _prev_restriction{false}; @@ -419,10 +419,10 @@ int LightwareLaser::updateRestriction() distance_sensor_mode_change_request_s dist_sense_mode_change; if (_dist_sense_mode_change_sub.copy(&dist_sense_mode_change)) { - _req_mode = dist_sense_mode_change.req_mode; + _req_mode = dist_sense_mode_change.request_on_off; } else { - _req_mode = distance_sensor_mode_change_request_s::MODE_DISABLED; + _req_mode = distance_sensor_mode_change_request_s::REQUEST_OFF; } } @@ -466,7 +466,7 @@ int LightwareLaser::updateRestriction() break; case 2: - _restriction = _auto_restriction && _req_mode != distance_sensor_mode_change_request_s::MODE_ENABLED; + _restriction = _auto_restriction && _req_mode != distance_sensor_mode_change_request_s::REQUEST_ON; break; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e46decdfb19a..cd73e7e7d37c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -112,7 +112,7 @@ Navigator::Navigator() : _distance_sensor_mode_change_request_pub.advertise(); _distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time(); - _distance_sensor_mode_change_request_pub.get().req_mode = distance_sensor_mode_change_request_s::MODE_DISABLED; + _distance_sensor_mode_change_request_pub.get().request_on_off = distance_sensor_mode_change_request_s::REQUEST_OFF; _distance_sensor_mode_change_request_pub.update(); // Update the timeout used in mission_block (which can't hold it's own parameters) @@ -1459,21 +1459,21 @@ void Navigator::publish_distance_sensor_mode_request() // Send request to enable distance sensor when in the landing phase of a mission or RTL if (((_navigation_mode == &_rtl) && _rtl.isLanding()) || ((_navigation_mode == &_mission) && _mission.isLanding())) { - if (_distance_sensor_mode_change_request_pub.get().req_mode != - distance_sensor_mode_change_request_s::MODE_ENABLED) { + if (_distance_sensor_mode_change_request_pub.get().request_on_off != + distance_sensor_mode_change_request_s::REQUEST_ON) { _distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time(); - _distance_sensor_mode_change_request_pub.get().req_mode = - distance_sensor_mode_change_request_s::MODE_ENABLED; + _distance_sensor_mode_change_request_pub.get().request_on_off = + distance_sensor_mode_change_request_s::REQUEST_ON; _distance_sensor_mode_change_request_pub.update(); } - } else if (_distance_sensor_mode_change_request_pub.get().req_mode != - distance_sensor_mode_change_request_s::MODE_DISABLED) { + } else if (_distance_sensor_mode_change_request_pub.get().request_on_off != + distance_sensor_mode_change_request_s::REQUEST_OFF) { _distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time(); - _distance_sensor_mode_change_request_pub.get().req_mode = - distance_sensor_mode_change_request_s::MODE_DISABLED; + _distance_sensor_mode_change_request_pub.get().request_on_off = + distance_sensor_mode_change_request_s::REQUEST_OFF; _distance_sensor_mode_change_request_pub.update(); } } From 1c9c5e51c2b34f8cad4e19b9bed63191d2fe3686 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 13 Sep 2024 14:13:36 +0200 Subject: [PATCH 12/18] boards: cuav x7pro: remove build of ROVER and Q_ATTITUDE_ESTIMATOR to save flash Signed-off-by: Silvan Fuhrer --- boards/cuav/x7pro/default.px4board | 2 -- 1 file changed, 2 deletions(-) diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board index 4ba4d4a17754..c7c30b1a1ed2 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -39,7 +39,6 @@ CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 CONFIG_MODULES_AIRSPEED_SELECTOR=y -CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y @@ -70,7 +69,6 @@ CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y -CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y From 81747f35bb153cf3bae553d147e8d22c06ff2970 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Fri, 13 Sep 2024 15:11:56 +0200 Subject: [PATCH 13/18] rover: add descend navigation state to land detection --- src/modules/land_detector/RoverLandDetector.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/RoverLandDetector.cpp b/src/modules/land_detector/RoverLandDetector.cpp index 2011419db8c3..8e59c0c5d296 100644 --- a/src/modules/land_detector/RoverLandDetector.cpp +++ b/src/modules/land_detector/RoverLandDetector.cpp @@ -55,12 +55,13 @@ bool RoverLandDetector::_get_landed_state() const float distance_to_home = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), _home_position(0), _home_position(1)); - if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) { return true; // If Landing has been requested then say we have landed. } else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL && distance_to_home < _param_nav_acc_rad.get() && _param_rtl_land_delay.get() > -FLT_EPSILON) { - return true; // If the rover reaches the home position during RTL we say we have landed + return true; // If the rover reaches the home position during RTL we say we have landed. } else { return !_armed; // If we are armed we are not landed. From 2fd4150b389b0bac3812d577be0db70b3360587f Mon Sep 17 00:00:00 2001 From: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> Date: Mon, 16 Sep 2024 12:09:51 +0200 Subject: [PATCH 14/18] differential: Add stabilized and position mode (#23669) * differential: add position and stabilized mode * differential: add hardcoded stick input deadzones --- .../init.d-posix/airframes/4009_gz_r1_rover | 1 + .../airframes/50001_aion_robotics_r1_rover | 1 + src/lib/pure_pursuit/PurePursuit.cpp | 16 ++-- src/lib/pure_pursuit/PurePursuit.hpp | 6 +- .../rover_differential/RoverDifferential.cpp | 92 ++++++++++++++++++- .../rover_differential/RoverDifferential.hpp | 15 ++- .../RoverDifferentialGuidance.cpp | 2 +- .../RoverDifferentialGuidance.hpp | 1 + src/modules/rover_differential/module.yaml | 16 +++- 9 files changed, 136 insertions(+), 14 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index 4bf8815520af..b41d61355c35 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -21,6 +21,7 @@ param set-default RD_YAW_RATE_P 0.1 param set-default RD_YAW_RATE_I 0 param set-default RD_YAW_P 5 param set-default RD_YAW_I 0 +param set-default RD_MAX_SPEED 5 param set-default RD_MAX_THR_SPD 7 param set-default RD_SPEED_P 1 param set-default RD_SPEED_I 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index d00063c573c3..901127f09bd2 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -31,6 +31,7 @@ param set-default RD_YAW_RATE_P 0.1 param set-default RD_YAW_RATE_I 0 param set-default RD_YAW_P 5 param set-default RD_YAW_I 0 +param set-default RD_MAX_SPEED 1.8 param set-default RD_MAX_THR_SPD 2 param set-default RD_SPEED_P 0.5 param set-default RD_SPEED_I 0.1 diff --git a/src/lib/pure_pursuit/PurePursuit.cpp b/src/lib/pure_pursuit/PurePursuit.cpp index 324a504959ba..e28e7b0924a8 100644 --- a/src/lib/pure_pursuit/PurePursuit.cpp +++ b/src/lib/pure_pursuit/PurePursuit.cpp @@ -78,18 +78,16 @@ float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2 const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned; const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero(); - const Vector2f distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * - prev_wp_to_curr_wp_u; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp - const Vector2f curr_pos_to_path = distance_on_line_segment - - prev_wp_to_curr_pos; // Shortest vector from the current position to the path + _distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * prev_wp_to_curr_wp_u; + _curr_pos_to_path = _distance_on_line_segment - prev_wp_to_curr_pos; - if (curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point - return atan2f(curr_pos_to_path(1), curr_pos_to_path(0)); + if (_curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point + return atan2f(_curr_pos_to_path(1), _curr_pos_to_path(0)); } - const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(), - 2.f)); // Length of the vector from the endpoint of distance_on_line_segment to the intersection point - const Vector2f prev_wp_to_intersection_point = distance_on_line_segment + line_extension * + const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(_curr_pos_to_path.norm(), + 2.f)); // Length of the vector from the endpoint of _distance_on_line_segment to the intersection point + const Vector2f prev_wp_to_intersection_point = _distance_on_line_segment + line_extension * prev_wp_to_curr_wp_u; const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos; return atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0)); diff --git a/src/lib/pure_pursuit/PurePursuit.hpp b/src/lib/pure_pursuit/PurePursuit.hpp index 9de498c2e1b1..5740d0184c7a 100644 --- a/src/lib/pure_pursuit/PurePursuit.hpp +++ b/src/lib/pure_pursuit/PurePursuit.hpp @@ -103,6 +103,8 @@ class PurePursuit : public ModuleParams float vehicle_speed); float getLookaheadDistance() {return _lookahead_distance;}; + float getCrosstrackError() {return _curr_pos_to_path.norm();}; + float getDistanceOnLineSegment() {return _distance_on_line_segment.norm();}; protected: /** @@ -122,5 +124,7 @@ class PurePursuit : public ModuleParams float lookahead_min{1.f}; } _params{}; private: - float _lookahead_distance{0.f}; + float _lookahead_distance{0.f}; // Radius of the circle around the vehicle + Vector2f _distance_on_line_segment{}; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp + Vector2f _curr_pos_to_path{}; // Shortest vector from the current position to the path }; diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index 411c5c4b5559..a7a9e82d0217 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -99,12 +99,94 @@ void RoverDifferential::Run() } break; + case vehicle_status_s::NAVIGATION_STATE_STAB: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.timestamp = timestamp; + rover_differential_setpoint.forward_speed_setpoint = NAN; + rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; + rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_setpoint = NAN; + + if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control + _yaw_ctl = false; + + + } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + if (!_yaw_ctl) { + _stab_desired_yaw = _vehicle_yaw; + _yaw_ctl = true; + } + + rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw; + rover_differential_setpoint.yaw_rate_setpoint = NAN; + + } + + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); + } + + } break; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.timestamp = timestamp; + rover_differential_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); + rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_setpoint = NAN; + + if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + _yaw_ctl = false; + + + } else { // Course control if the yaw rate input is zero (keep driving on a straight line) + if (!_yaw_ctl) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + _yaw_ctl = true; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( + rover_differential_setpoint.forward_speed_setpoint) * + vector_scaling * _pos_ctl_course_direction; + // Calculate yaw setpoint + const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, + _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed)); + rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ? + yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards + rover_differential_setpoint.yaw_rate_setpoint = NAN; + + } + + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); + } + + } break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state); break; default: // Unimplemented nav states will stop the rover + _rover_differential_control.resetControllers(); + _yaw_ctl = false; rover_differential_setpoint_s rover_differential_setpoint{}; rover_differential_setpoint.forward_speed_setpoint = NAN; rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f; @@ -115,8 +197,9 @@ void RoverDifferential::Run() break; } - if (!_armed) { // Reset on disarm + if (!_armed) { // Reset when disarmed _rover_differential_control.resetControllers(); + _yaw_ctl = false; } _rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed); @@ -135,6 +218,12 @@ void RoverDifferential::updateSubscriptions() if (_vehicle_status_sub.updated()) { vehicle_status_s vehicle_status{}; _vehicle_status_sub.copy(&vehicle_status); + + if (vehicle_status.nav_state != _nav_state) { // Reset on mode change + _rover_differential_control.resetControllers(); + _yaw_ctl = false; + } + _nav_state = vehicle_status.nav_state; _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; } @@ -155,6 +244,7 @@ void RoverDifferential::updateSubscriptions() if (_vehicle_local_position_sub.updated()) { vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); _vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f; diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index eee464ede127..ad238aa428dc 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -39,6 +39,7 @@ #include #include #include +#include // uORB includes #include @@ -103,8 +104,10 @@ class RoverDifferential : public ModuleBase, public ModulePar // Instances RoverDifferentialGuidance _rover_differential_guidance{this}; RoverDifferentialControl _rover_differential_control{this}; + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library // Variables + Vector2f _curr_pos_ned{}; matrix::Quatf _vehicle_attitude_quaternion{}; float _vehicle_yaw_rate{0.f}; float _vehicle_forward_speed{0.f}; @@ -112,6 +115,10 @@ class RoverDifferential : public ModuleBase, public ModulePar float _max_yaw_rate{0.f}; int _nav_state{0}; bool _armed{false}; + bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode + float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode + Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode + Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode // Thresholds to avoid moving at rest due to measurement noise static constexpr float YAW_RATE_THRESHOLD = @@ -119,8 +126,14 @@ class RoverDifferential : public ModuleBase, public ModulePar static constexpr float SPEED_THRESHOLD = 0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero + // Stick input deadzone + static constexpr float STICK_DEADZONE = + 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value + DEFINE_PARAMETERS( (ParamFloat) _param_rd_man_yaw_scale, - (ParamFloat) _param_rd_max_yaw_rate + (ParamFloat) _param_rd_max_yaw_rate, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_pp_lookahd_max ) }; diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp index c56830ad08a2..848a7f513acf 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -216,7 +216,7 @@ void RoverDifferentialGuidance::updateWaypoints() // Waypoint cruising speed if (position_setpoint_triplet.current.cruising_speed > 0.f) { - _max_forward_speed = position_setpoint_triplet.current.cruising_speed; + _max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get()); } else { _max_forward_speed = _param_rd_miss_spd_def.get(); diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp index d2b9862b2cc0..4b3815247e59 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -143,6 +143,7 @@ class RoverDifferentialGuidance : public ModuleParams (ParamFloat) _param_nav_acc_rad, (ParamFloat) _param_rd_max_jerk, (ParamFloat) _param_rd_max_accel, + (ParamFloat) _param_rd_max_speed, (ParamFloat) _param_rd_miss_spd_def, (ParamFloat) _param_rd_trans_trn_drv, (ParamFloat) _param_rd_trans_drv_trn diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index 8ef9f62f5e30..3bad343f2e8b 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -113,6 +113,19 @@ parameters: decimal: 2 default: 0.5 + RD_MAX_SPEED: + description: + short: Maximum speed setpoint + long: | + This parameter is used to cap desired forward speed and map controller inputs to desired speeds in Position mode. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + RD_MAX_THR_SPD: description: short: Speed the rover drives at maximum throttle @@ -133,7 +146,8 @@ parameters: description: short: Maximum allowed yaw rate for the rover long: | - This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode. + This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates + in Acro,Stabilized and Position mode. type: float unit: deg/s min: 0.01 From 8aece9bff21c0d830c7dfeae6921095a619d5997 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 17 Sep 2024 16:58:46 +0200 Subject: [PATCH 15/18] differential: fix CI issue --- .../rover_differential/RoverDifferential.hpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index ad238aa428dc..8d83519410ad 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -61,6 +61,14 @@ using namespace time_literals; +// Constants +static constexpr float STICK_DEADZONE = + 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value +static constexpr float YAW_RATE_THRESHOLD = + 0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero +static constexpr float SPEED_THRESHOLD = + 0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero + class RoverDifferential : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { @@ -120,16 +128,6 @@ class RoverDifferential : public ModuleBase, public ModulePar Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode - // Thresholds to avoid moving at rest due to measurement noise - static constexpr float YAW_RATE_THRESHOLD = - 0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero - static constexpr float SPEED_THRESHOLD = - 0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero - - // Stick input deadzone - static constexpr float STICK_DEADZONE = - 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value - DEFINE_PARAMETERS( (ParamFloat) _param_rd_man_yaw_scale, (ParamFloat) _param_rd_max_yaw_rate, From 4a99a51fb19de668a9d56bc0cb9fd506bc0ff227 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Fri, 13 Sep 2024 13:55:38 +0200 Subject: [PATCH 16/18] update upload-artifact v2->v4 --- .github/workflows/mavros_mission_tests.yml | 8 ++++---- .github/workflows/mavros_offboard_tests.yml | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/.github/workflows/mavros_mission_tests.yml b/.github/workflows/mavros_mission_tests.yml index a90d51804ef5..7cd95e207744 100644 --- a/.github/workflows/mavros_mission_tests.yml +++ b/.github/workflows/mavros_mission_tests.yml @@ -88,7 +88,7 @@ jobs: run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit" - name: Upload px4 coredump if: failure() - uses: actions/upload-artifact@v2-preview + uses: actions/upload-artifact@v4 with: name: coredump path: px4.core @@ -103,21 +103,21 @@ jobs: - name: Upload px4 binary if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: binary path: build/px4_sitl_default/bin/px4 - name: Store PX4 log if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: px4_log path: ~/.ros/log/*/*.ulg - name: Store ROS log if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: ros_log path: ~/.ros/**/rostest-*.log diff --git a/.github/workflows/mavros_offboard_tests.yml b/.github/workflows/mavros_offboard_tests.yml index 020caaa2d048..2305b8da13ab 100644 --- a/.github/workflows/mavros_offboard_tests.yml +++ b/.github/workflows/mavros_offboard_tests.yml @@ -83,7 +83,7 @@ jobs: run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit" - name: Upload px4 coredump if: failure() - uses: actions/upload-artifact@v2-preview + uses: actions/upload-artifact@v4 with: name: coredump path: px4.core @@ -98,21 +98,21 @@ jobs: - name: Upload px4 binary if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: binary path: build/px4_sitl_default/bin/px4 - name: Store PX4 log if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: px4_log path: ~/.ros/log/*/*.ulg - name: Store ROS log if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: ros_log path: ~/.ros/**/rostest-*.log From 5d2e7c8748c306b81bebf1092dfee41f4b6c6efc Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Wed, 18 Sep 2024 11:17:37 +0200 Subject: [PATCH 17/18] Misc: Matrix: Added Addition and Subtraction to Slices (#23679) * Added Addition and Subtraction to Slices * MatrixSliceTest: refactor Addition/Substraction checks * Slice: replace operations returning a Matrix with calling the existing Matrix function --------- Co-authored-by: Matthias Grob --- src/lib/matrix/matrix/Slice.hpp | 55 ++++++++++++----- src/lib/matrix/matrix/SquareMatrix.hpp | 1 + src/lib/matrix/matrix/Vector2.hpp | 1 - src/lib/matrix/test/MatrixSliceTest.cpp | 73 +++++++++++++++++++++++ src/lib/matrix/test/MatrixVector3Test.cpp | 9 +++ 5 files changed, 122 insertions(+), 17 deletions(-) diff --git a/src/lib/matrix/matrix/Slice.hpp b/src/lib/matrix/matrix/Slice.hpp index 57f7cfcb7002..53d24769e957 100644 --- a/src/lib/matrix/matrix/Slice.hpp +++ b/src/lib/matrix/matrix/Slice.hpp @@ -116,6 +116,39 @@ class SliceT return self; } + template + Matrix operator-(const SliceT, Type, P, Q, MM, NN> &other) + { + return Matrix {*this} - other; + } + + + Matrix operator-(const Matrix &other) + { + return Matrix {*this} - other; + } + + Matrix operator-(const Type &other) + { + return Matrix {*this} - other; + } + + template + Matrix operator+(const SliceT, Type, P, Q, MM, NN> &other) + { + return Matrix {*this} + other; + } + + Matrix operator+(const Matrix &other) + { + return Matrix {*this} + other; + } + + Matrix operator+(const Type &other) + { + return Matrix {*this} + other; + } + // allow assigning vectors to a slice that are in the axis template // make this a template function since it only exists for some instantiations SliceT &operator=(const Vector &other) @@ -222,29 +255,19 @@ class SliceT return self; } - SliceT &operator/=(const Type &other) + SliceT &operator/=(const Type &scalar) { - return operator*=(Type(1) / other); + return operator*=(Type(1) / scalar); } - Matrix operator*(const Type &other) const + Matrix operator*(const Type &scalar) const { - const SliceT &self = *this; - Matrix res; - - for (size_t i = 0; i < P; i++) { - for (size_t j = 0; j < Q; j++) { - res(i, j) = self(i, j) * other; - } - } - - return res; + return Matrix {*this} * scalar; } - Matrix operator/(const Type &other) const + Matrix operator/(const Type &scalar) const { - const SliceT &self = *this; - return self * (Type(1) / other); + return (*this) * (1 / scalar); } template diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp index 0cd20bc5bc4e..610e313d386d 100644 --- a/src/lib/matrix/matrix/SquareMatrix.hpp +++ b/src/lib/matrix/matrix/SquareMatrix.hpp @@ -317,6 +317,7 @@ class SquareMatrix : public Matrix } }; +using SquareMatrix2f = SquareMatrix; using SquareMatrix3f = SquareMatrix; using SquareMatrix3d = SquareMatrix; diff --git a/src/lib/matrix/matrix/Vector2.hpp b/src/lib/matrix/matrix/Vector2.hpp index 3376445fdaec..88ef40ba5505 100644 --- a/src/lib/matrix/matrix/Vector2.hpp +++ b/src/lib/matrix/matrix/Vector2.hpp @@ -102,7 +102,6 @@ class Vector2 : public Vector }; - using Vector2f = Vector2; using Vector2d = Vector2; diff --git a/src/lib/matrix/test/MatrixSliceTest.cpp b/src/lib/matrix/test/MatrixSliceTest.cpp index 9240e6b5a8c2..ec53f8eaaf6c 100644 --- a/src/lib/matrix/test/MatrixSliceTest.cpp +++ b/src/lib/matrix/test/MatrixSliceTest.cpp @@ -262,6 +262,79 @@ TEST(MatrixSliceTest, Slice) float O_check_data_12 [4] = {2.5, 3, 4, 5}; EXPECT_EQ(res_12, (SquareMatrix(O_check_data_12))); } +TEST(MatrixSliceTest, SliceAdditions) +{ + float data[9] = {0, 2, 3, + 4, 5, 6, + 7, 8, 10 + }; + SquareMatrix3f A{data}; + + float operand_data [4] = {2, 1, + -3, -1 + }; + const SquareMatrix2f operand(operand_data); + + // 2x2 Slice + 2x2 Matrix + SquareMatrix2f res_1 = A.slice<2, 2>(1, 0) + operand; + float res_1_check_data[4] = {6, 6, + 4, 7 + }; + EXPECT_EQ(res_1, (SquareMatrix2f(res_1_check_data))); + + // 2x1 Slice + 2x1 Slice + Vector2f res_2 = A.slice<2, 1>(1, 1) + operand.slice<2, 1>(0, 0); + EXPECT_EQ(res_2, Vector2f(7, 5)); + + // 3x3 Slice + Scalar + SquareMatrix3f res_3 = A.slice<3, 3>(0, 0) + (-1); + float res_3_check_data[9] = {-1, 1, 2, + 3, 4, 5, + 6, 7, 9 + }; + EXPECT_EQ(res_3, (SquareMatrix3f(res_3_check_data))); + + // 3x1 Slice + 3 Vector + Vector3f res_4 = A.col(1) + Vector3f(1, -2, 3); + EXPECT_EQ(res_4, Vector3f(3, 3, 11)); + +} +TEST(MatrixSliceTest, SliceSubtractions) +{ + float data[9] = {0, 2, 3, + 4, 5, 6, + 7, 8, 10 + }; + SquareMatrix3f A{data}; + + float operand_data[4] = {2, 1, + -3, -1 + }; + const SquareMatrix2f operand(operand_data); + + // 2x2 Slice - 2x2 Matrix + SquareMatrix2f res_1 = A.slice<2, 2>(1, 0) - operand; + float res_1_check_data[4] = {2, 4, + 10, 9 + }; + EXPECT_EQ(res_1, (SquareMatrix2f(res_1_check_data))); + + // 2x1 Slice - 2x1 Slice + Vector2f res_2 = A.slice<2, 1>(1, 1) - operand.slice<2, 1>(0, 0); + EXPECT_EQ(res_2, Vector2f(3, 11)); + + // 3x3 Slice - Scalar + SquareMatrix3f res_3 = A.slice<3, 3>(0, 0) - (-1); + float res_3_check_data[9] = {1, 3, 4, + 5, 6, 7, + 8, 9, 11 + }; + EXPECT_EQ(res_3, (SquareMatrix3f(res_3_check_data))); + + // 3x1 Slice - 3 Vector + Vector3f res_4 = A.col(1) - Vector3f(1, -2, 3); + EXPECT_EQ(res_4, Vector3f(1, 7, 5)); +} TEST(MatrixSliceTest, XYAssignmentTest) { diff --git a/src/lib/matrix/test/MatrixVector3Test.cpp b/src/lib/matrix/test/MatrixVector3Test.cpp index f125ad92dff6..ce4f998ac27a 100644 --- a/src/lib/matrix/test/MatrixVector3Test.cpp +++ b/src/lib/matrix/test/MatrixVector3Test.cpp @@ -80,4 +80,13 @@ TEST(MatrixVector3Test, Vector3) Vector3f m2(3.1f, 4.1f, 5.1f); EXPECT_EQ(m2, m1 + 2.1f); EXPECT_EQ(m2 - 2.1f, m1); + + // Test Addition and Subtraction of Slices + Vector3f v1(3, 13, 0); + Vector3f v2(42, 6, 256); + + EXPECT_EQ(v1.xy() - v2.xy(), Vector2f(-39, 7)); + EXPECT_EQ(v1.xy() + v2.xy(), Vector2f(45, 19)); + EXPECT_EQ(v1.xy() + 2.f, Vector2f(5, 15)); + EXPECT_EQ(v1.xy() - 2.f, Vector2f(1, 11)); } From ab41927bbd729e6b6e481bf8088496447d94f1f6 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Wed, 18 Sep 2024 15:16:27 +0200 Subject: [PATCH 18/18] SIM: GZ: Added mono_cam_down and aruco world (#23687) --- src/modules/simulation/gz_bridge/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 2af0a069b34c..afb6b899c6c4 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -88,6 +88,7 @@ if(gz-transport_FOUND) windy baylands lawn + aruco rover aruco )