Fixed heading calculation, fixed heading controller

3 files changed, 47 insertions, 15 deletions

diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c
index bdf797673..beabc1142 100644
--- a/apps/fixedwing_control/fixedwing_control.c
+++ b/apps/fixedwing_control/fixedwing_control.c
@@ -60,6 +60,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/pid/pid.h>
 #include <systemlib/geo/geo.h>
+#include <uORB/topics/debug_key_value.h>
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
@@ -158,14 +159,14 @@ static int pos_parameters_update(const struct fw_pos_control_param_handles *h, s
  */
 int fixedwing_control_thread_main(int argc, char *argv[])
 {
-	// /* read arguments */
-	// bool verbose = false;
+	/* read arguments */
+	bool verbose = false;
 
-	// for (int i = 1; i < argc; i++) {
-	// 	if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
-	// 		verbose = true;
-	// 	}
-	// }
+	for (int i = 1; i < argc; i++) {
+		if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
+			verbose = true;
+		}
+	}
 
 	/* welcome user */
 	printf("[fixedwing control] started\n");
@@ -229,6 +230,11 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f,
 			PID_MODE_DERIVATIV_SET);
 
+	// XXX remove in production
+	/* advertise debug value */
+	struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
+	orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
+
 	while(!thread_should_exit) {
 
 		struct pollfd fds[1] = {
@@ -268,7 +274,11 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 			/* get a local copy of attitude */
 			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 			/* get a local copy of attitude setpoint */
-			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
+			//orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
+			// XXX update to switch between external attitude reference and the
+			// attitude calculated here
+
+			char name[10];
 
 			if (pos_updated) {
 
@@ -288,7 +298,14 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 						global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d);
 
 					/* shift error to prevent wrapping issues */
-					float bearing_error = att.yaw - target_bearing;
+					float bearing_error = target_bearing - att.yaw;
+
+					if (loopcounter % 2 == 0) {
+						sprintf(name, "hdng err1");
+						memcpy(dbg.key, name, sizeof(name));
+						dbg.value = bearing_error;
+						orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
+					}
 
 					if (bearing_error < M_PI_F) {
 						bearing_error += 2.0f * M_PI_F;
@@ -298,6 +315,13 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 						bearing_error -= 2.0f * M_PI_F;
 					}
 
+					if (loopcounter % 2 != 0) {
+						sprintf(name, "hdng err2");
+						memcpy(dbg.key, name, sizeof(name));
+						dbg.value = bearing_error;
+						orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
+					}
+
 					/* calculate roll setpoint, do this artificially around zero */
 					att_sp.roll_body = pid_calculate(&heading_controller, bearing_error,
 					0.0f, att.yawspeed, deltaT);
@@ -313,7 +337,6 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 						heading_controller.saturated = 1;
 					}
 
-					att_sp.roll_body = att_sp.roll_body;
 					att_sp.pitch_body = 0.0f;
 					att_sp.yaw_body = 0.0f;
 
@@ -331,7 +354,13 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 			const float deltaTpos = (hrt_absolute_time() - last_run_pos) / 1000000.0f;
 			last_run_pos = hrt_absolute_time();
 
-			actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body,
+			if (verbose && (loopcounter % 20 == 0)) {
+				printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body);
+			}
+
+			// actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body,
+			// 		att.roll, att.rollspeed, deltaTpos);
+						actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body,
 					att.roll, att.rollspeed, deltaTpos);
 			actuators.control[1] = 0;
 			actuators.control[2] = 0;
diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c
index 42d8d9c15..ce46d01cc 100644
--- a/apps/systemlib/geo/geo.c
+++ b/apps/systemlib/geo/geo.c
@@ -79,9 +79,12 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
 	float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
 
-	// XXX wrapping check is incomplete
-	if (theta < 0.0f) {
-		theta = theta + 2.0f * M_PI_F;
+	if (theta < M_PI_F) {
+		theta += 2.0f * M_PI_F;
+	}
+
+	if (theta > M_PI_F) {
+		theta -= 2.0f * M_PI_F;
 	}
 
 	return theta;
diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c
index 61dd5757f..cff5e6bbe 100644
--- a/apps/systemlib/pid/pid.c
+++ b/apps/systemlib/pid/pid.c
@@ -179,7 +179,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		pid->error_previous = error;
 	}
 
-	float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
+	float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
 
 	if (isfinite(output)) {
 		pid->last_output = output;