Replace old pid library with new one

src/modules/rover_pos_control/CMakeLists.txt

@@ -38,5 +38,5 @@ px4_add_module(
 		RoverPositionControl.hpp
 	DEPENDS
 		l1
-		pid
+		PID
 	)

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -92,13 +92,9 @@ void RoverPositionControl::parameters_update(bool force)
 		_gnd_control.set_l1_damping(_param_l1_damping.get());
 		_gnd_control.set_l1_period(_param_l1_period.get());
 
-		pid_init(&_speed_ctrl, PID_MODE_DERIVATIV_CALC, 0.01f);
-		pid_set_parameters(&_speed_ctrl,
-				   _param_speed_p.get(),
-				   _param_speed_i.get(),
-				   _param_speed_d.get(),
-				   _param_speed_imax.get(),
-				   _param_gndspeed_max.get());
+		_speed_ctrl.setGains(_param_speed_p.get(), _param_speed_i.get(), _param_speed_d.get());
+		_speed_ctrl.setIntegralLimit(_param_speed_imax.get());
+		_speed_ctrl.setOutputLimit(_param_gndspeed_max.get());
 	}
 }
 
@@ -239,12 +235,9 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 			const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
 			const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2));
 
-			const float x_vel = vel(0);
-			const float x_acc = _vehicle_acceleration_sub.get().xyz[0];
-
 			// Compute airspeed control out and just scale it as a constant
-			mission_throttle = _param_throttle_speed_scaler.get()
-					   * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);
+			_speed_ctrl.setSetpoint(mission_target_speed);
+			mission_throttle = _param_throttle_speed_scaler.get() * _speed_ctrl.update(vel(0), dt);
 
 			// Constrain throttle between min and max
 			mission_throttle = math::constrain(mission_throttle, _param_throttle_min.get(), _param_throttle_max.get());
@@ -327,10 +320,8 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
 		const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
 		const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2));
 
-		const float x_vel = vel(0);
-		const float x_acc = _vehicle_acceleration_sub.get().xyz[0];
-
-		const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt);
+		_speed_ctrl.setSetpoint(desired_speed);
+		const float control_throttle = _speed_ctrl.update(vel(0), dt);
 
 		//Constrain maximum throttle to mission throttle
 		_throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle);
@@ -392,8 +383,6 @@ RoverPositionControl::Run()
 		vehicle_attitude_poll();
 		manual_control_setpoint_poll();
 
-		_vehicle_acceleration_sub.update();
-
 		/* update parameters from storage */
 		parameters_update();
 

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -48,7 +48,7 @@
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/pid/pid.h>
+#include <lib/pid/PID.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
@@ -128,8 +128,6 @@ private:
 	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
 
-	uORB::SubscriptionData<vehicle_acceleration_s>		_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
-
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	hrt_abstime _control_position_last_called{0}; 	/**<last call of control_position  */
@@ -139,7 +137,7 @@ private:
 
 	/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
 	 the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/
-	PID_t _speed_ctrl{};
+	PID _speed_ctrl{};
 
 	// estimator reset counters
 	uint8_t _pos_reset_counter{0};		// captures the number of times the estimator has reset the horizontal position