Apply suggestions from code review

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
PX4 · Apr 25, 2024 · ca4192f · ca4192f
1 parent 150d69a
commit ca4192f
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 15 deletions.
diff --git a/src/modules/ackermann_drive/AckermannDrive.hpp b/src/modules/ackermann_drive/AckermannDrive.hpp
@@ -91,7 +91,7 @@ class AckermannDrive : public ModuleBase<AckermannDrive>, public ModuleParams,
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	vehicle_status_s _vehicle_status{};
 
-	// uORB sublications
+	// uORB publications
 	uORB::Publication<ackermann_drive_setpoint_s> _ackermann_drive_setpoint_pub{ORB_ID(ackermann_drive_setpoint)};
 
 	// Instances

diff --git a/src/modules/ackermann_drive/AckermannDriveGuidance/AckermannDriveGuidance.cpp b/src/modules/ackermann_drive/AckermannDriveGuidance/AckermannDriveGuidance.cpp
@@ -58,16 +58,12 @@ void AckermannDriveGuidance::purePursuit()
 		_vehicle_global_position_sub.copy(&_vehicle_global_position);
 	}
 
-	if (_vehicle_attitude_sub.updated()) {
-		if (_vehicle_attitude_sub.copy(&_vehicle_attitude)) {
-			matrix::Quatf _vehicle_attitude_quaternion = Quatf(_vehicle_attitude.q);
-			_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
-		}
+	if (_vehicle_attitude_sub.update(&_vehicle_attitude)) {
+		matrix::Quatf _vehicle_attitude_quaternion = Quatf(_vehicle_attitude.q);
+		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
 	}
 
-	if (_home_position_sub.updated()) {
-		_home_position_sub.copy(&_home_position);
-	}
+	_home_position_sub.update(&_home_position));
 
 	if (_local_position_sub.updated()) {
 		_local_position_sub.copy(&_local_position);
@@ -83,7 +79,7 @@ void AckermannDriveGuidance::purePursuit()
 	}
 
 	// Setup global frame variables
-	_curr_pos = {_vehicle_global_position.lat, _vehicle_global_position.lon};
+	_curr_pos = Vector2d(_vehicle_global_position.lat, _vehicle_global_position.lon);
 	_curr_wp = {_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon};
 	_next_wp = _curr_wp;
 	_prev_wp = {_home_position.lat, _home_position.lon};
@@ -194,7 +190,7 @@ void AckermannDriveGuidance::purePursuit()
 	_ackermann_drive_setpoint_pub.publish(_ackermann_drive_setpoint);
 }
 
-float AckermannDriveGuidance::calcDesiredHeading(Vector2f const &curr_wp_local, Vector2f const &prev_wp_local,
+float AckermannDriveGuidance::calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
 		Vector2f const &curr_pos_local,
 		float const &lookahead_distance)
 {
@@ -229,7 +225,7 @@ float AckermannDriveGuidance::calcDesiredHeading(Vector2f const &curr_wp_local,
 		Vector2f distance2 = (curr_wp_local - curr_pos_local) - point2;
 
 		// Return intersection point closer to current waypoint
-		if (distance1.norm() < distance2.norm()) {
+		if (distance1.norm_squared() < distance2.norm_squared()) {
 			return atan2f(ay, ax);
 
 		} else {

diff --git a/src/modules/ackermann_drive/AckermannDriveGuidance/AckermannDriveGuidance.hpp b/src/modules/ackermann_drive/AckermannDriveGuidance/AckermannDriveGuidance.hpp
@@ -140,8 +140,8 @@ class AckermannDriveGuidance : public ModuleParams
 	Vector2d _curr_pos{};
 	Vector2f _curr_pos_local{};
 	float _vehicle_yaw{0.f};
-	float _desired_speed;
-	float _desired_steering;
+	float _desired_speed{0.f};
+	float _desired_steering{0.f};
 
 	// Waypoint variables
 	Vector2d _curr_wp{};
@@ -150,7 +150,7 @@ class AckermannDriveGuidance : public ModuleParams
 	Vector2f _curr_wp_local{};
 	Vector2f _prev_wp_local{};
 	Vector2f _next_wp_local{};
-	float _prev_acc_rad;
+	float _prev_acc_rad{0.f};
 
 	// Parameters
 	DEFINE_PARAMETERS(