Merge branch 'master' of https://github.com/PX4/Firmware

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;

apps/mavlink/mavlink.c

@@ -1416,15 +1416,15 @@ void handleMessage(mavlink_message_t *msg)
 			memset(&rc_hil, 0, sizeof(rc_hil));
 			static orb_advert_t rc_pub = 0;
 
-			rc_hil.chan[0].raw = 1510 + man.x / 2;
-			rc_hil.chan[1].raw = 1520 + man.y / 2;
-			rc_hil.chan[2].raw = 1590 + man.r / 2;
-			rc_hil.chan[3].raw = 1420 + man.z / 2;
+			rc_hil.chan[0].raw = 1500 + man.x / 2;
+			rc_hil.chan[1].raw = 1500 + man.y / 2;
+			rc_hil.chan[2].raw = 1500 + man.r / 2;
+			rc_hil.chan[3].raw = 1500 + man.z / 2;
 
-			rc_hil.chan[0].scaled = man.x;
-			rc_hil.chan[1].scaled = man.y;
-			rc_hil.chan[2].scaled = man.r;
-			rc_hil.chan[3].scaled = man.z;
+			rc_hil.chan[0].scaled = man.x / 1000.0f;
+			rc_hil.chan[1].scaled = man.y / 1000.0f;
+			rc_hil.chan[2].scaled = man.r / 1000.0f;
+			rc_hil.chan[3].scaled = man.z / 1000.0f;
 
 			struct manual_control_setpoint_s mc;
 			static orb_advert_t mc_pub = 0;
@@ -1432,7 +1432,7 @@ void handleMessage(mavlink_message_t *msg)
 			mc.roll = man.x / 1000.0f;
 			mc.pitch = man.y / 1000.0f;
 			mc.yaw = man.r / 1000.0f;
-			mc.roll = man.z / 1000.0f;
+			mc.throttle = man.z / 1000.0f;
 
 			/* fake RC channels with manual control input from simulator */
 

apps/px4/fmu/fmu.cpp

@@ -66,7 +66,6 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 
-
 class FMUServo : public device::CDev
 {
 public:
@@ -75,7 +74,7 @@ public:
 		MODE_4PWM,
 		MODE_NONE
 	};
-	FMUServo(Mode mode);
+	FMUServo(Mode mode, int update_rate);
 	~FMUServo();
 
 	virtual int	ioctl(struct file *filp, int cmd, unsigned long arg);
@@ -86,6 +85,7 @@ private:
 	static const unsigned _max_actuators = 4;
 
 	Mode		_mode;
+	int 		_update_rate;
 	int		_task;
 	int		_t_actuators;
 	int		_t_armed;
@@ -115,9 +115,10 @@ FMUServo	*g_servo;
 
 } // namespace
 
-FMUServo::FMUServo(Mode mode) :
+FMUServo::FMUServo(Mode mode, int update_rate) :
 	CDev("fmuservo", PWM_OUTPUT_DEVICE_PATH),
 	_mode(mode),
+	_update_rate(update_rate),
 	_task(-1),
 	_t_actuators(-1),
 	_t_armed(-1),
@@ -199,6 +200,8 @@ FMUServo::task_main()
 		/* multi-port as 4 PWM outs */
 		/* XXX magic numbers */
 		up_pwm_servo_init(0xf);
+		/* set the update rate for 4 PWM mode only for now */
+		up_pwm_servo_set_rate(_update_rate);
 		break;
 
 	case MODE_NONE:
@@ -208,7 +211,9 @@ FMUServo::task_main()
 
 	/* subscribe to objects that we are interested in watching */
 	_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	orb_set_interval(_t_actuators, 20);		/* 50Hz update rate */
+	/* convert the update rate in hz to milliseconds, rounding down if necessary */
+	int update_rate_in_ms = int(1000 / _update_rate);
+	orb_set_interval(_t_actuators, update_rate_in_ms);
 
 	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
 	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
@@ -444,7 +449,7 @@ enum PortMode {
 PortMode g_port_mode;
 
 int
-fmu_new_mode(PortMode new_mode)
+fmu_new_mode(PortMode new_mode, int update_rate)
 {
 	int fd;
 	int ret = OK;
@@ -511,7 +516,7 @@ fmu_new_mode(PortMode new_mode)
 
 	/* create new PWM driver if required */
 	if (servo_mode != FMUServo::MODE_NONE) {
-		g_servo = new FMUServo(servo_mode);
+		g_servo = new FMUServo(servo_mode, update_rate);
 
 		if (g_servo == nullptr) {
 			ret = -ENOMEM;
@@ -585,6 +590,7 @@ int
 fmu_main(int argc, char *argv[])
 {
 	PortMode new_mode = PORT_MODE_UNSET;
+	int pwm_update_rate_in_hz = 50;
 
 	if (!strcmp(argv[1], "test"))
 		test();
@@ -597,24 +603,40 @@ fmu_main(int argc, char *argv[])
 	 *
 	 * XXX use getopt?
 	 */
-	if (!strcmp(argv[1], "mode_gpio")) {
-		new_mode = PORT_FULL_GPIO;
+	for (int i = 1; i < argc; i++) {   /* argv[0] is "fmu" */
+		if (!strcmp(argv[i], "mode_gpio")) {
+			new_mode = PORT_FULL_GPIO;
 
-	} else if (!strcmp(argv[1], "mode_serial")) {
-		new_mode = PORT_FULL_SERIAL;
+		} else if (!strcmp(argv[i], "mode_serial")) {
+			new_mode = PORT_FULL_SERIAL;
 
-	} else if (!strcmp(argv[1], "mode_pwm")) {
-		new_mode = PORT_FULL_PWM;
+		} else if (!strcmp(argv[i], "mode_pwm")) {
+			new_mode = PORT_FULL_PWM;
 
-	} else if (!strcmp(argv[1], "mode_gpio_serial")) {
-		new_mode = PORT_GPIO_AND_SERIAL;
+		} else if (!strcmp(argv[i], "mode_gpio_serial")) {
+			new_mode = PORT_GPIO_AND_SERIAL;
 
-	} else if (!strcmp(argv[1], "mode_pwm_serial")) {
-		new_mode = PORT_PWM_AND_SERIAL;
+		} else if (!strcmp(argv[i], "mode_pwm_serial")) {
+			new_mode = PORT_PWM_AND_SERIAL;
 
-	} else if (!strcmp(argv[1], "mode_pwm_gpio")) {
-		new_mode = PORT_PWM_AND_GPIO;
-	}
+		} else if (!strcmp(argv[i], "mode_pwm_gpio")) {
+			new_mode = PORT_PWM_AND_GPIO;
+		}
+
+		/* look for the optional pwm update rate for the supported modes */
+		if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) {
+			if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) {
+				if (argc > i + 1) {
+					pwm_update_rate_in_hz = atoi(argv[i + 1]);
+				} else {
+					fprintf(stderr, "missing argument for pwm update rate (-u)\n");
+					return 1;
+				}
+			} else {
+				fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n");
+			}
+		}
+        }
 
 	/* was a new mode set? */
 	if (new_mode != PORT_MODE_UNSET) {
@@ -624,12 +646,12 @@ fmu_main(int argc, char *argv[])
 			return OK;
 
 		/* switch modes */
-		return fmu_new_mode(new_mode);
+		return fmu_new_mode(new_mode, pwm_update_rate_in_hz);
 	}
 
 	/* test, etc. here */
 
 	fprintf(stderr, "FMU: unrecognised command, try:\n");
-	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
+	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm [-r pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
 	return -EINVAL;
 }

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;

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;

nuttx/configs/px4fmu/src/up_pwm_servo.c

@@ -179,7 +179,7 @@ static void
 pwm_timer_set_rate(unsigned timer, unsigned rate)
 {
 	/* configure the timer to update at the desired rate */
-	rARR(timer) = 1000000 / pwm_update_rate;
+	rARR(timer) = 1000000 / rate;
 
 	/* generate an update event; reloads the counter and all registers */
 	rEGR(timer) = GTIM_EGR_UG;