Reworked control interface, needs testing / validation

apps/commander/commander.c

@@ -742,6 +742,31 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 		}
 		break;
 
+		case PX4_CMD_CONTROLLER_SELECTION: {
+			bool changed = false;
+			if ((int)cmd->param1 != (int)current_vehicle_status->flag_control_rates_enabled) {
+				current_vehicle_status->flag_control_rates_enabled = cmd->param1;
+				changed = true;
+			}
+			if ((int)cmd->param2 != (int)current_vehicle_status->flag_control_attitude_enabled) {
+				current_vehicle_status->flag_control_attitude_enabled = cmd->param2;
+				changed = true;
+			}
+			if ((int)cmd->param3 != (int)current_vehicle_status->flag_control_velocity_enabled) {
+				current_vehicle_status->flag_control_velocity_enabled = cmd->param3;
+				changed = true;
+			}
+			if ((int)cmd->param4 != (int)current_vehicle_status->flag_control_position_enabled) {
+				current_vehicle_status->flag_control_position_enabled = cmd->param4;
+				changed = true;
+			}
+
+			if (changed) {
+				/* publish current state machine */
+				state_machine_publish(status_pub, current_vehicle_status, mavlink_fd);
+			}
+		}
+
 //		/* request to land */
 //		case MAV_CMD_NAV_LAND:
 //		 {

apps/mavlink/mavlink.c

@@ -65,7 +65,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/ardrone_motors_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
@@ -126,14 +126,13 @@ static struct vehicle_attitude_s hil_attitude;
 
 static struct vehicle_global_position_s hil_global_pos;
 
-static struct ardrone_motors_setpoint_s ardrone_motors;
+static struct vehicle_rates_setpoint_s vehicle_rates_sp;
 
 static struct vehicle_command_s vcmd;
 
 static struct actuator_armed_s armed;
 
 static orb_advert_t pub_hil_global_pos = -1;
-static orb_advert_t ardrone_motors_pub = -1;
 static orb_advert_t cmd_pub = -1;
 static orb_advert_t flow_pub = -1;
 static orb_advert_t global_position_setpoint_pub = -1;
@@ -188,6 +187,12 @@ static struct mavlink_subscriptions {
 	.initialized = false
 };
 
+static struct mavlink_publications {
+	orb_advert_t vehicle_rates_sp_pub;
+} mavlink_pubs = {
+	.vehicle_rates_sp_pub = -1
+};
+
 
 /* 3: Define waypoint helper functions */
 void mavlink_wpm_send_message(mavlink_message_t *msg);
@@ -1133,6 +1138,13 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 	}
 }
 
+#define MAVLINK_OFFBOARD_CONTROL_MODE_NONE 0
+#define MAVLINK_OFFBOARD_CONTROL_MODE_RATES 1
+#define MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE 2
+#define MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY 3
+#define MAVLINK_OFFBOARD_CONTROL_MODE_POSITION 4
+#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -1242,29 +1254,80 @@ void handleMessage(mavlink_message_t *msg)
 //		printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", quad_motors_setpoint.target_system, mavlink_system.sysid);
 
 		if (mavlink_system.sysid < 4) {
-			ardrone_motors.p1 = quad_motors_setpoint.roll[mavlink_system.sysid];
-			ardrone_motors.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid];
-			ardrone_motors.p3 = quad_motors_setpoint.yaw[mavlink_system.sysid];
-			ardrone_motors.p4 = quad_motors_setpoint.thrust[mavlink_system.sysid];
-
-			ardrone_motors.timestamp = hrt_absolute_time();
-
 			/* only send if RC is off */
 			if (v_status.rc_signal_lost) {
-				/* check if input has to be enabled */
-				if (!v_status.flag_control_offboard_enabled) {
-					/* XXX Enable offboard control */
-				}
 
-				/* XXX decode mode and set flags */
-				// if (mode == abc) xxx flag_control_rates_enabled;
+				/* rate control mode */
+				if (quad_motors_setpoint.mode == 1) {
+					vehicle_rates_sp.roll = quad_motors_setpoint.roll[mavlink_system.sysid]   / (float)INT16_MAX;
+					vehicle_rates_sp.pitch = quad_motors_setpoint.pitch[mavlink_system.sysid]  / (float)INT16_MAX;
+					vehicle_rates_sp.yaw = quad_motors_setpoint.yaw[mavlink_system.sysid]    / (float)INT16_MAX;
+					vehicle_rates_sp.thrust = quad_motors_setpoint.thrust[mavlink_system.sysid] / (float)UINT16_MAX;
+
+					vehicle_rates_sp.timestamp = hrt_absolute_time();
+				}
 
 				/* check if topic has to be advertised */
-				if (ardrone_motors_pub <= 0) {
-					ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors);
+				if (mavlink_pubs.vehicle_rates_sp_pub <= 0) {
+					mavlink_pubs.vehicle_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &vehicle_rates_sp);
 				}
 				/* Publish */
-				orb_publish(ORB_ID(ardrone_motors_setpoint), ardrone_motors_pub, &ardrone_motors);
+				orb_publish(ORB_ID(vehicle_rates_setpoint), mavlink_pubs.vehicle_rates_sp_pub, &vehicle_rates_sp);
+
+				/* change armed status if required */
+				bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED);
+
+				bool cmd_generated = false;
+
+				if (v_status.flag_control_offboard_enabled != cmd_armed) {
+					vcmd.param1 = cmd_armed;
+					vcmd.param2 = 0;
+					vcmd.param3 = 0;
+					vcmd.param4 = 0;
+					vcmd.param5 = 0;
+					vcmd.param6 = 0;
+					vcmd.param7 = 0;
+					vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
+					vcmd.target_system = mavlink_system.sysid;
+					vcmd.target_component = MAV_COMP_ID_ALL;
+					vcmd.source_system = msg->sysid;
+					vcmd.source_component = msg->compid;
+					vcmd.confirmation = 1;
+
+					cmd_generated = true;
+				}
+
+				/* check if input has to be enabled */
+				if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) ||
+					(v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) ||
+					(v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) ||
+					(v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) {
+					vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES);
+					vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE);
+					vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY);
+					vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION);
+					vcmd.param5 = 0;
+					vcmd.param6 = 0;
+					vcmd.param7 = 0;
+					vcmd.command = PX4_CMD_CONTROLLER_SELECTION;
+					vcmd.target_system = mavlink_system.sysid;
+					vcmd.target_component = MAV_COMP_ID_ALL;
+					vcmd.source_system = msg->sysid;
+					vcmd.source_component = msg->compid;
+					vcmd.confirmation = 1;
+
+					cmd_generated = true;
+				}
+
+				if (cmd_generated) {
+					/* check if topic is advertised */
+					if (cmd_pub <= 0) {
+						cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+					} else {
+						/* create command */
+						orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
+					}
+				}
 			}
 		}
 	}
@@ -1479,7 +1542,7 @@ int mavlink_thread_main(int argc, char *argv[])
 	memset(&rc, 0, sizeof(rc));
 	memset(&hil_attitude, 0, sizeof(hil_attitude));
 	memset(&hil_global_pos, 0, sizeof(hil_global_pos));
-	memset(&ardrone_motors, 0, sizeof(ardrone_motors));
+	memset(&vehicle_rates_sp, 0, sizeof(vehicle_rates_sp));
 	memset(&vcmd, 0, sizeof(vcmd));
 
 	/* print welcome text */

apps/multirotor_att_control/multirotor_att_control_main.c

@@ -58,13 +58,14 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/ardrone_motors_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/actuator_controls.h>
 
 #include <systemlib/perf_counter.h>
 
 #include "multirotor_attitude_control.h"
+#include "multirotor_rate_control.h"
 
 __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
 
@@ -93,18 +94,18 @@ mc_thread_main(int argc, char *argv[])
 	memset(&manual, 0, sizeof(manual));
 	struct sensor_combined_s raw;
 	memset(&raw, 0, sizeof(raw));
-	struct ardrone_motors_setpoint_s setpoint;
-	memset(&setpoint, 0, sizeof(setpoint));
+	struct vehicle_rates_setpoint_s rates_sp;
+	memset(&rates_sp, 0, sizeof(rates_sp));
 
 	struct actuator_controls_s actuators;
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	int setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
 
 	/* 
 	 * Do not rate-limit the loop to prevent aliasing
@@ -145,6 +146,12 @@ mc_thread_main(int argc, char *argv[])
 		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);
+		/* get a local copy of rates setpoint */
+		bool updated;
+		orb_check(setpoint_sub, &updated);
+		if (updated) {
+			orb_copy(ORB_ID(vehicle_rates_setpoint), setpoint_sub, &rates_sp);
+		}
 		/* get a local copy of the current sensor values */
 		orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
@@ -155,16 +162,15 @@ mc_thread_main(int argc, char *argv[])
 			/* manual inputs, from RC control or joystick */
 			att_sp.roll_body = manual.roll;
 			att_sp.pitch_body = manual.pitch;
-			att_sp.yaw_rate_body = manual.yaw;
+			att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
+			/* set yaw rate */
+			rates_sp.yaw = manual.yaw;
 			att_sp.timestamp = hrt_absolute_time();
 			
 			if (motor_test_mode) {
 				att_sp.roll_body = 0.0f;
 				att_sp.pitch_body = 0.0f;
 				att_sp.yaw_body = 0.0f;
-				att_sp.roll_rate_body = 0.0f;
-				att_sp.pitch_rate_body = 0.0f;
-				att_sp.yaw_rate_body = 0.0f;
 				att_sp.thrust = 0.1f;
 			} else {
 				att_sp.thrust = manual.throttle;
@@ -184,7 +190,14 @@ mc_thread_main(int argc, char *argv[])
 
 		/* XXX could be also run in an independent loop */
 		if (state.flag_control_rates_enabled) {
-			multirotor_control_rates(&att_sp, &raw.gyro_rad_s, &actuators);
+			/* lowpass gyros */
+			// XXX
+			static float gyro_lp[3] = {0, 0, 0};
+			gyro_lp[0] = raw.gyro_rad_s[0];
+			gyro_lp[1] = raw.gyro_rad_s[1];
+			gyro_lp[2] = raw.gyro_rad_s[2];
+
+			multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
 		}
 
 		/* STEP 3: publish the result to the vehicle actuators */

apps/multirotor_att_control/multirotor_attitude_control.c

@@ -243,7 +243,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
 					att->roll, att->rollspeed, deltaT);
 	/* control yaw rate */
-	float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_rate_body, att->yawspeed, 0.0f, deltaT);
+	float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT);
 
 	/*
 	 * compensate the vertical loss of thrust

apps/multirotor_att_control/multirotor_rate_control.c

@@ -132,7 +132,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 	return OK;
 }
 
-void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
+void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	const float rates[], struct actuator_controls_s *actuators)
 {
 	static uint64_t last_run = 0;
@@ -179,13 +179,13 @@ void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
 	/* calculate current control outputs */
 	
 	/* control pitch (forward) output */
-	float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch_rate_body,
+	float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch,
 					rates[1], 0.0f, deltaT);
 	/* control roll (left/right) output */
-	float roll_control = pid_calculate(&roll_controller, rate_sp->roll_rate_body,
+	float roll_control = pid_calculate(&roll_controller, rate_sp->roll,
 					rates[0], 0.0f, deltaT);
 	/* control yaw rate */
-	float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw_rate_body, rates[2], 0.0f, deltaT);
+	float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
 
 	/*
 	 * compensate the vertical loss of thrust

apps/multirotor_att_control/multirotor_rate_control.h

@@ -47,10 +47,10 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 
-void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
+void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	const float rates[], struct actuator_controls_s *actuators);
 
 #endif /* MULTIROTOR_RATE_CONTROL_H_ */

apps/px4/ground_estimator/ground_estimator.c

@@ -75,7 +75,7 @@ int ground_estimator_thread_main(int argc, char *argv[]) {
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
 
-	/* advertise attitude */
+	/* advertise debug value */
 	struct debug_key_value_s dbg = { .key = "posx", .value = 0.0f };
 	orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
 

apps/uORB/objects_common.cpp

@@ -78,8 +78,8 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
 #include "topics/vehicle_local_position.h"
 ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s);
 
-#include "topics/ardrone_motors_setpoint.h"
-ORB_DEFINE(ardrone_motors_setpoint, struct ardrone_motors_setpoint_s);
+#include "topics/vehicle_rates_setpoint.h"
+ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
 
 #include "topics/rc_channels.h"
 ORB_DEFINE(rc_channels, struct rc_channels_s);

apps/uORB/topics/vehicle_attitude_setpoint.h

@@ -56,21 +56,18 @@ struct vehicle_attitude_setpoint_s
 {
 	uint64_t timestamp;		/**< in microseconds since system start, is set whenever the writing thread stores new data */
 
-	float roll_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
-	float pitch_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
-	float yaw_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
-	float tait_bryan_valid;			/**< Set to true if Tait-Bryan angles are valid */
+	//float roll_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
+	//float pitch_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
+	//float yaw_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
+	//float tait_bryan_valid;			/**< Set to true if Tait-Bryan angles are valid */
 
 	float roll_body;				/**< body angle in NED frame		*/
 	float pitch_body;				/**< body angle in NED frame		*/
 	float yaw_body;					/**< body angle in NED frame		*/
-	float roll_rate_body;				/**< body angle in NED frame		*/
-	float pitch_rate_body;				/**< body angle in NED frame		*/
-	float yaw_rate_body;					/**< body angle in NED frame		*/
-	float body_valid;				/**< Set to true if Tait-Bryan angles are valid */
+	//float body_valid;				/**< Set to true if body angles are valid */
 
-	float R_body[9];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
-	bool R_valid;					/**< Set to true if rotation matrix is valid */
+	//float R_body[9];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
+	//bool R_valid;					/**< Set to true if rotation matrix is valid */
 
 	float thrust;					/**< Thrust in Newton the power system should generate */
 

apps/uORB/topics/vehicle_command.h

@@ -50,6 +50,10 @@
  * @{
  */
 
+enum PX4_CMD {
+	PX4_CMD_CONTROLLER_SELECTION = 1000,
+};
+
 struct vehicle_command_s
 {
 	float param1;			/**< Parameter 1, as defined by MAVLink MAV_CMD enum.   */

apps/uORB/topics/vehicle_rates_setpoint.h

@@ -33,12 +33,12 @@
  ****************************************************************************/
 
 /**
- * @file ardrone_motors_setpoint.h
- * Definition of the ardrone_motors_setpoint uORB topic.
+ * @file vehicle_rates_setpoint.h
+ * Definition of the vehicle rates setpoint topic
  */
 
-#ifndef TOPIC_ARDRONE_MOTORS_SETPOINT_H_
-#define TOPIC_ARDRONE_MOTORS_SETPOINT_H_
+#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_
+#define TOPIC_VEHICLE_RATES_SETPOINT_H_
 
 #include <stdint.h>
 #include "../uORB.h"
@@ -48,23 +48,22 @@
  * @{
  */
 
-struct ardrone_motors_setpoint_s
+struct vehicle_rates_setpoint_s
 {
-	uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data
+	uint64_t timestamp; /**< in microseconds since system start */
 
-	uint8_t group;	/**< quadrotor group */
-	uint8_t mode;	/**< requested flight mode XXX define */
-	float p1;		/**< parameter 1: rate control: roll rate, att control: roll angle (in radians, NED) */
-	float p2;		/**< parameter 2: rate control: pitch rate, att control: pitch angle (in radians, NED) */
-	float p3;		/**< parameter 3: rate control: yaw rate, att control: yaw angle (in radians, NED) */
-	float p4;		/**< parameter 4: thrust, [0..1] */
-}; /**< AR.Drone low level motors */
+	float roll;	/**< body angular rates in NED frame		*/
+	float pitch;	/**< body angular rates in NED frame		*/
+	float yaw;	/**< body angular rates in NED frame		*/
+	float thrust;	/**< thrust normalized to 0..1			*/
+
+}; /**< vehicle_rates_setpoint */
 
  /**
  * @}
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(ardrone_motors_setpoint);
+ORB_DECLARE(vehicle_rates_setpoint);
 
 #endif

apps/uORB/topics/vehicle_status.h

@@ -127,7 +127,7 @@ struct vehicle_status_s
 
 	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
 	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
-	bool flag_control_speed_enabled;		/**< true if speed (implies direction) is controlled */
+	bool flag_control_velocity_enabled;		/**< true if speed (implies direction) is controlled */
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 
 	bool flag_preflight_gyro_calibration;		/**< true if gyro calibration is requested */