multirotor_pos_control fixes, systemlib/pid now accepts limit = 0.0, means no limit

2 files changed, 103 insertions, 58 deletions

diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 3e5857c26..1d2dd384a 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -61,6 +61,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/pid/pid.h>
 #include <mavlink/mavlink_log.h>
@@ -89,9 +90,11 @@ static float scale_control(float ctl, float end, float dz);
 
 static float norm(float x, float y);
 
-static void usage(const char *reason) {
+static void usage(const char *reason)
+{
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
+
 	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
 	exit(1);
 }
@@ -100,11 +103,12 @@ static void usage(const char *reason) {
  * The deamon app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
- * 
+ *
  * The actual stack size should be set in the call
  * to task_spawn().
  */
-int multirotor_pos_control_main(int argc, char *argv[]) {
+int multirotor_pos_control_main(int argc, char *argv[])
+{
 	if (argc < 1)
 		usage("missing command");
 
@@ -119,11 +123,11 @@ int multirotor_pos_control_main(int argc, char *argv[]) {
 		warnx("start");
 		thread_should_exit = false;
 		deamon_task = task_spawn_cmd("multirotor_pos_control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 60,
-					 4096,
-					 multirotor_pos_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MAX - 60,
+					     4096,
+					     multirotor_pos_control_thread_main,
+					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
@@ -136,9 +140,11 @@ int multirotor_pos_control_main(int argc, char *argv[]) {
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
 			warnx("app is running");
+
 		} else {
 			warnx("app not started");
 		}
+
 		exit(0);
 	}
 
@@ -146,26 +152,31 @@ int multirotor_pos_control_main(int argc, char *argv[]) {
 	exit(1);
 }
 
-static float scale_control(float ctl, float end, float dz) {
+static float scale_control(float ctl, float end, float dz)
+{
 	if (ctl > dz) {
 		return (ctl - dz) / (end - dz);
+
 	} else if (ctl < -dz) {
 		return (ctl + dz) / (end - dz);
+
 	} else {
 		return 0.0f;
 	}
 }
 
-static float norm(float x, float y) {
+static float norm(float x, float y)
+{
 	return sqrtf(x * x + y * y);
 }
 
-static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
+static int multirotor_pos_control_thread_main(int argc, char *argv[])
+{
 	/* welcome user */
 	warnx("started");
 	static int mavlink_fd;
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started");
+	mavlink_log_info(mavlink_fd, "[mpc] started");
 
 	/* structures */
 	struct vehicle_status_s status;
@@ -181,7 +192,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 	struct vehicle_local_position_setpoint_s local_pos_sp;
 	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
 	struct vehicle_global_position_setpoint_s global_pos_sp;
-	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
+	memset(&global_pos_sp, 0, sizeof(local_pos_sp));
+	struct vehicle_global_velocity_setpoint_s global_vel_sp;
+	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int param_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -195,6 +208,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 
 	/* publish setpoint */
 	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
+	orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
 	bool reset_sp_alt = true;
@@ -220,10 +234,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 	parameters_update(&params_h, &params);
 
 	for (int i = 0; i < 2; i++) {
-		pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+		pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
 		pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
 	}
-	pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+
+	pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
 	thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
 
 	int paramcheck_counter = 0;
@@ -234,18 +249,23 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 
 		/* check parameters at 1 Hz*/
 		paramcheck_counter++;
+
 		if (paramcheck_counter == 50) {
 			bool param_updated;
 			orb_check(param_sub, &param_updated);
+
 			if (param_updated) {
 				parameters_update(&params_h, &params);
+
 				for (int i = 0; i < 2; i++) {
-					pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max);
+					pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
 					pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max);
 				}
+
 				pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
 				thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
 			}
+
 			paramcheck_counter = 0;
 		}
 
@@ -256,31 +276,36 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 
 		/* Check if controller should act */
 		bool act = status.flag_system_armed && (
-		   		       /* SAS modes */
-				   	   (
-				   		   status.flag_control_manual_enabled &&
-						   status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && (
-								   status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE ||
-								   status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE
-						   )
-				   	   ) ||
-				   	   /* AUTO mode */
-				   	   status.state_machine == SYSTEM_STATE_AUTO
-				   );
+				   /* SAS modes */
+				   (
+					   status.flag_control_manual_enabled &&
+					   status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && (
+						   status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE ||
+						   status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE
+					   )
+				   ) ||
+				   /* AUTO mode */
+				   status.state_machine == SYSTEM_STATE_AUTO
+			   );
 
 		hrt_abstime t = hrt_absolute_time();
 		float dt;
+
 		if (t_prev != 0) {
 			dt = (t - t_prev) * 0.000001f;
+
 		} else {
 			dt = 0.0f;
 		}
+
 		t_prev = t;
+
 		if (act) {
 			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
 			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
 			orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
+
 			if (status.state_machine == SYSTEM_STATE_AUTO) {
 				//orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
 				if (local_pos.home_timestamp != local_home_timestamp) {
@@ -292,6 +317,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 					warnx("local pos home: lat = %.10f,  lon = %.10f", lat_home, lon_home);
 					mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home);
 				}
+
 				if (global_pos_sp_updated) {
 					global_pos_sp_updated = false;
 					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
@@ -299,11 +325,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 					double sp_lon = global_pos_sp.lon * 1e-7;
 					/* project global setpoint to local setpoint */
 					map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
+
 					if (global_pos_sp.altitude_is_relative) {
 						local_pos_sp.z = -global_pos_sp.altitude;
+
 					} else {
 						local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude;
 					}
+
 					warnx("new setpoint: lat = %.10f,  lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
 					mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
 					/* publish local position setpoint as projection of global position setpoint */
@@ -339,16 +368,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 						/* home alt changed, don't follow large ground level changes in manual flight */
 						local_pos_sp.z += local_pos.home_alt - home_alt;
 					}
+
 					home_alt_t = local_pos.home_timestamp;
 					home_alt = local_pos.home_alt;
 				}
+
 				/* move altitude setpoint with manual controls */
 				float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
+
 				if (z_sp_ctl != 0.0f) {
 					sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
 					local_pos_sp.z += sp_move_rate[2] * dt;
+
 					if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
 						local_pos_sp.z = local_pos.z + z_sp_offs_max;
+
 					} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
 						local_pos_sp.z = local_pos.z - z_sp_offs_max;
 					}
@@ -358,6 +392,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 					/* move position setpoint with manual controls */
 					float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
 					float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
+
 					if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
 						/* calculate direction and increment of control in NED frame */
 						float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
@@ -370,6 +405,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 						float pos_vec_x = local_pos_sp.x - local_pos.x;
 						float pos_vec_y = local_pos_sp.y - local_pos.y;
 						float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
+
 						if (pos_vec_norm > 1.0f) {
 							local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
 							local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
@@ -379,72 +415,79 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 			}
 
 			/* run position & altitude controllers, calculate velocity setpoint */
-			float vel_sp[3] = { 0.0f, 0.0f, 0.0f };
-			vel_sp[2] = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt);
+			global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt);
+
 			if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
 				/* calculate velocity set point in NED frame */
-				vel_sp[0] = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt);
-				vel_sp[1] = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt);
+				global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt);
+				global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt);
+
+				/* limit horizontal speed */
+				float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
+				if (xy_vel_sp_norm > 1.0f) {
+					global_vel_sp.vx /= xy_vel_sp_norm;
+					global_vel_sp.vy /= xy_vel_sp_norm;
+				}
+
 			} else {
 				reset_sp_pos = true;
+				global_vel_sp.vx = 0.0f;
+				global_vel_sp.vy = 0.0f;
 			}
-			/* calculate direction and norm of thrust in NED frame
-			 * limit 3D speed by ellipsoid:
-			 * (vx/xy_vel_max)^2 + (vy/xy_vel_max)^2 + (vz/z_vel_max)^2 = 1 */
-			float v;
-			float vel_sp_norm = 0.0f;
-			v = vel_sp[0] / params.xy_vel_max;
-			vel_sp_norm += v * v;
-			v = vel_sp[1] / params.xy_vel_max;
-			vel_sp_norm += v * v;
-			v = vel_sp[2] / params.z_vel_max;
-			vel_sp_norm += v * v;
-			vel_sp_norm = sqrtf(vel_sp_norm);
-			if (vel_sp_norm > 1.0f) {
-				vel_sp[0] /= vel_sp_norm;
-				vel_sp[1] /= vel_sp_norm;
-				vel_sp[2] /= vel_sp_norm;
-			}
+
+			/* publish new velocity setpoint */
+			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
 
 			/* run velocity controllers, calculate thrust vector */
 			float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
-			thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, vel_sp[2], local_pos.vz, dt);
+			thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt);
+
 			if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
 				/* calculate velocity set point in NED frame */
-				thrust_sp[0] = pid_calculate(&xy_vel_pids[0], vel_sp[0], local_pos.vx, 0.0f, dt);
-				thrust_sp[1] = pid_calculate(&xy_vel_pids[1], vel_sp[1], local_pos.vy, 0.0f, dt);
+				thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
+				thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
 			}
+
 			/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
 			/* limit horizontal part of thrust */
 			float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
 			float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]);
+
 			if (thrust_xy_norm > params.slope_max) {
 				thrust_xy_norm = params.slope_max;
 			}
+
 			/* use approximation: slope ~ sin(slope) = force */
 			/* convert direction to body frame */
 			thrust_xy_dir -= att.yaw;
+
 			if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
 				/* calculate roll and pitch */
 				att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm;
 				att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body);	// reverse pitch
 			}
+
 			/* attitude-thrust compensation */
 			float att_comp;
+
 			if (att.R[2][2] > 0.8f)
 				att_comp = 1.0f / att.R[2][2];
 			else if (att.R[2][2] > 0.0f)
 				att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f;
 			else
 				att_comp = 1.0f;
+
 			att_sp.thrust = -thrust_sp[2] * att_comp;
 			att_sp.timestamp = hrt_absolute_time();
+
 			if (status.flag_control_manual_enabled) {
 				/* publish local position setpoint in manual mode */
 				orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
 			}
+
 			/* publish new attitude setpoint */
 			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+
 		} else {
 			reset_sp_alt = true;
 			reset_sp_pos = true;
@@ -456,7 +499,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 	}
 
 	warnx("stopped");
-	mavlink_log_info(mavlink_fd, "[multirotor_pos_control] stopped");
+	mavlink_log_info(mavlink_fd, "[mpc] stopped");
 
 	thread_running = false;
 
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 4996a8f66..562f3ac04 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -168,8 +168,8 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	// Calculate the error integral and check for saturation
 	i = pid->integral + (error * dt);
 
-	if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit ||
-	    fabsf(i) > pid->intmax) {
+	if ((pid->limit != 0.0f && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) ||
+			fabsf(i) > pid->intmax) {
 		i = pid->integral;		// If saturated then do not update integral value
 		pid->saturated = 1;
 
@@ -186,11 +186,13 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
 
 	if (isfinite(output)) {
-		if (output > pid->limit) {
-			output = pid->limit;
+		if (pid->limit != 0.0f) {
+			if (output > pid->limit) {
+				output = pid->limit;
 
-		} else if (output < -pid->limit) {
-			output = -pid->limit;
+			} else if (output < -pid->limit) {
+				output = -pid->limit;
+			}
 		}
 
 		pid->last_output = output;