Merge branch 'master' into inav_fix

src/modules/commander/commander.cpp

@@ -470,8 +470,26 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM:
-		// XXX implement later
-		result = VEHICLE_CMD_RESULT_DENIED;
+	{
+		transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+		if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
+			if (safety->safety_switch_available && !safety->safety_off) {
+				print_reject_arm("NOT ARMING: Press safety switch first.");
+				arming_res = TRANSITION_DENIED;
+
+			} else {
+				arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+			}
+
+			if (arming_res == TRANSITION_CHANGED) {
+				mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+			} else {
+				mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+		}
+	}
 		break;
 
 	default:
@@ -806,16 +824,6 @@ int commander_thread_main(int argc, char *argv[])
 
 		check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
 
-		orb_check(cmd_sub, &updated);
-
-		if (updated) {
-			/* got command */
-			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
-
-			/* handle it */
-			handle_command(&status, &safety, &control_mode, &cmd, &armed);
-		}
-
 		/* update safety topic */
 		orb_check(safety_sub, &updated);
 
@@ -1147,6 +1155,18 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+
+		/* handle commands last, as the system needs to be updated to handle them */
+		orb_check(cmd_sub, &updated);
+
+		if (updated) {
+			/* got command */
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+			/* handle it */
+			handle_command(&status, &safety, &control_mode, &cmd, &armed);
+		}
+
 		/* evaluate the navigation state machine */
 		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
 

src/modules/mavlink/mavlink.c

@@ -531,7 +531,7 @@ int mavlink_thread_main(int argc, char *argv[])
 		case 'b':
 			baudrate = strtoul(optarg, NULL, 10);
 
-			if (baudrate == 0)
+			if (baudrate < 9600 || baudrate > 921600)
 				errx(1, "invalid baud rate '%s'", optarg);
 
 			break;

src/modules/mavlink/mavlink_receiver.cpp

@@ -102,6 +102,7 @@ struct vehicle_global_position_s hil_global_pos;
 struct vehicle_attitude_s hil_attitude;
 struct vehicle_gps_position_s hil_gps;
 struct sensor_combined_s hil_sensors;
+struct battery_status_s	hil_battery_status;
 static orb_advert_t pub_hil_global_pos = -1;
 static orb_advert_t pub_hil_attitude = -1;
 static orb_advert_t pub_hil_gps = -1;
@@ -111,6 +112,12 @@ static orb_advert_t pub_hil_accel = -1;
 static orb_advert_t pub_hil_mag = -1;
 static orb_advert_t pub_hil_baro = -1;
 static orb_advert_t pub_hil_airspeed = -1;
+static orb_advert_t pub_hil_battery = -1;
+
+/* packet counter */
+static int hil_counter = 0;
+static int hil_frames = 0;
+static uint64_t old_timestamp = 0;
 
 static orb_advert_t cmd_pub = -1;
 static orb_advert_t flow_pub = -1;
@@ -149,7 +156,8 @@ handle_message(mavlink_message_t *msg)
 				vcmd.param5 = cmd_mavlink.param5;
 				vcmd.param6 = cmd_mavlink.param6;
 				vcmd.param7 = cmd_mavlink.param7;
-				vcmd.command = cmd_mavlink.command;
+				// XXX do proper translation
+				vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
 				vcmd.target_system = cmd_mavlink.target_system;
 				vcmd.target_component = cmd_mavlink.target_component;
 				vcmd.source_system = msg->sysid;
@@ -207,7 +215,7 @@ handle_message(mavlink_message_t *msg)
 		vcmd.param5 = 0;
 		vcmd.param6 = 0;
 		vcmd.param7 = 0;
-		vcmd.command = MAV_CMD_DO_SET_MODE;
+		vcmd.command = VEHICLE_CMD_DO_SET_MODE;
 		vcmd.target_system = new_mode.target_system;
 		vcmd.target_component = MAV_COMP_ID_ALL;
 		vcmd.source_system = msg->sysid;
@@ -360,11 +368,6 @@ handle_message(mavlink_message_t *msg)
 			mavlink_hil_sensor_t imu;
 			mavlink_msg_hil_sensor_decode(msg, &imu);
 
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
-
 			/* sensors general */
 			hil_sensors.timestamp = hrt_absolute_time();
 
@@ -391,9 +394,9 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
 
 			/* adc */
-			hil_sensors.adc_voltage_v[0] = 0;
-			hil_sensors.adc_voltage_v[1] = 0;
-			hil_sensors.adc_voltage_v[2] = 0;
+			hil_sensors.adc_voltage_v[0] = 0.0f;
+			hil_sensors.adc_voltage_v[1] = 0.0f;
+			hil_sensors.adc_voltage_v[2] = 0.0f;
 
 			/* magnetometer */
 			float mga2ga = 1.0e-3f;
@@ -506,6 +509,18 @@ handle_message(mavlink_message_t *msg)
 				pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
 			}
 
+			/* fill in HIL battery status */
+			hil_battery_status.timestamp = hrt_absolute_time();
+			hil_battery_status.voltage_v = 11.1f;
+			hil_battery_status.current_a = 10.0f;
+
+			/* lazily publish the battery voltage */
+			if (pub_hil_battery > 0) {
+				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+			} else {
+				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			}
+
 			// increment counters
 			hil_counter++;
 			hil_frames++;
@@ -640,6 +655,18 @@ handle_message(mavlink_message_t *msg)
 			} else {
 				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
 			}
+
+			/* fill in HIL battery status */
+			hil_battery_status.timestamp = hrt_absolute_time();
+			hil_battery_status.voltage_v = 11.1f;
+			hil_battery_status.current_a = 10.0f;
+
+			/* lazily publish the battery voltage */
+			if (pub_hil_battery > 0) {
+				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+			} else {
+				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			}
 		}
 
 		if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {

src/modules/mavlink/orb_topics.h

@@ -65,6 +65,7 @@
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
 #include <drivers/drv_rc_input.h>
 
 struct mavlink_subscriptions {

src/modules/multirotor_pos_control/multirotor_pos_control.c

@@ -199,7 +199,7 @@ 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(&global_pos_sp, 0, sizeof(local_pos_sp));
+	memset(&global_pos_sp, 0, sizeof(global_pos_sp));
 	struct vehicle_global_velocity_setpoint_s global_vel_sp;
 	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
 
@@ -414,6 +414,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					}
 				}
 
+				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
 				local_pos_sp.yaw = att_sp.yaw_body;
 
 				/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
@@ -436,7 +437,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						local_pos_sp.x = local_pos.x;
 						local_pos_sp.y = local_pos.y;
 						local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
-						local_pos_sp.yaw = att.yaw;
 						att_sp.yaw_body = att.yaw;
 						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
 					}
@@ -477,8 +477,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 							} else {
 								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
 							}
-
-							local_pos_sp.yaw = global_pos_sp.yaw;
 							att_sp.yaw_body = global_pos_sp.yaw;
 
 							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
@@ -491,7 +489,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 								local_pos_sp.x = local_pos.x;
 								local_pos_sp.y = local_pos.y;
 								local_pos_sp.yaw = att.yaw;
-								att_sp.yaw_body = global_pos_sp.yaw;
 							}
 
 							if (reset_auto_sp_z) {
@@ -515,6 +512,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					reset_mission_sp = true;
 				}
 
+				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
+				local_pos_sp.yaw = att_sp.yaw_body;
+
 				/* reset setpoints after AUTO mode */
 				reset_man_sp_xy = true;
 				reset_man_sp_z = true;

src/modules/px4iofirmware/controls.c

@@ -286,7 +286,7 @@ controls_tick() {
 		 * requested override.
 		 *
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH))
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
 			override = true;
 
 		if (override) {

src/modules/px4iofirmware/mixer.cpp

@@ -221,7 +221,7 @@ mixer_tick(void)
 		}
 
 		/* do the calculations during the ramp for all at once */
-		if(esc_state == ESC_RAMP) {
+		if (esc_state == ESC_RAMP) {
 			ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
 		}
 

src/modules/systemlib/mixer/mixer.cpp

@@ -142,6 +142,22 @@ NullMixer *
 NullMixer::from_text(const char *buf, unsigned &buflen)
 {
 	NullMixer *nm = nullptr;
+	const char *end = buf + buflen;
+
+	/* enforce that the mixer ends with space or a new line */
+	for (int i = buflen - 1; i >= 0; i--) {
+		if (buf[i] == '\0')
+			continue;
+
+		/* require a space or newline at the end of the buffer, fail on printable chars */
+		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+			/* found a line ending or space, so no split symbols / numbers. good. */
+			break;
+		} else {
+			return nm;
+		}
+
+	}
 
 	if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
 		nm = new NullMixer;

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -181,6 +181,23 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	char geomname[8];
 	int s[4];
 	int used;
+	const char *end = buf + buflen;
+
+	/* enforce that the mixer ends with space or a new line */
+	for (int i = buflen - 1; i >= 0; i--) {
+		if (buf[i] == '\0')
+			continue;
+
+		/* require a space or newline at the end of the buffer, fail on printable chars */
+		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+			/* found a line ending or space, so no split symbols / numbers. good. */
+			break;
+		} else {
+			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
+			return nullptr;
+		}
+
+	}
 
 	if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
 		debug("multirotor parse failed on '%s'", buf);

src/modules/systemlib/mixer/mixer_simple.cpp

@@ -55,7 +55,7 @@
 #include "mixer.h"
 
 #define debug(fmt, args...)	do { } while(0)
-//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+// #define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
 
 SimpleMixer::SimpleMixer(ControlCallback control_cb,
 			 uintptr_t cb_handle,
@@ -98,14 +98,17 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 {
 	int ret;
 	int s[5];
+	int n = -1;
 	
 	buf = findtag(buf, buflen, 'O');
-	if ((buf == nullptr) || (buflen < 12))
+	if ((buf == nullptr) || (buflen < 12)) {
+		debug("output parser failed finding tag, ret: '%s'", buf);
 		return -1;
+	}
 
-	if ((ret = sscanf(buf, "O: %d %d %d %d %d",
-			  &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) {
-		debug("scaler parse failed on '%s' (got %d)", buf, ret);
+	if ((ret = sscanf(buf, "O: %d %d %d %d %d %n",
+			  &s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) {
+		debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
 		return -1;
 	}
 	skipline(buf, buflen);
@@ -126,8 +129,10 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 	int s[5];
 
 	buf = findtag(buf, buflen, 'S');
-	if ((buf == nullptr) || (buflen < 16))
+	if ((buf == nullptr) || (buflen < 16)) {
+		debug("contorl parser failed finding tag, ret: '%s'", buf);
 		return -1;
+	}
 
 	if (sscanf(buf, "S: %u %u %d %d %d %d %d",
 		   &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
@@ -156,6 +161,22 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 	int used;
 	const char *end = buf + buflen;
 
+	/* enforce that the mixer ends with space or a new line */
+	for (int i = buflen - 1; i >= 0; i--) {
+		if (buf[i] == '\0')
+			continue;
+
+		/* require a space or newline at the end of the buffer, fail on printable chars */
+		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+			/* found a line ending or space, so no split symbols / numbers. good. */
+			break;
+		} else {
+			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
+			goto out;
+		}
+
+	}
+
 	/* get the base info for the mixer */
 	if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
 		debug("simple parse failed on '%s'", buf);
@@ -173,22 +194,27 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 
 	mixinfo->control_count = inputs;
 
-	if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler))
+	if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) {
+		debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf);
 		goto out;
+	}
 
 	for (unsigned i = 0; i < inputs; i++) {
 		if (parse_control_scaler(end - buflen, buflen,
 					 mixinfo->controls[i].scaler,
 					 mixinfo->controls[i].control_group,
-					 mixinfo->controls[i].control_index))
+					 mixinfo->controls[i].control_index)) {
+			debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf);
 			goto out;
+		}
+
 	}
 
 	sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
 
 	if (sm != nullptr) {
 		mixinfo = nullptr;
-		debug("loaded mixer with %d inputs", inputs);
+		debug("loaded mixer with %d input(s)", inputs);
 
 	} else {
 		debug("could not allocate memory for mixer");

src/modules/uORB/topics/vehicle_command.h

@@ -120,7 +120,7 @@ struct vehicle_command_s
 	float param5;			/**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param6;			/**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param7;			/**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum.   */
-	uint16_t command;		/**< Command ID, as defined MAVLink by VEHICLE_CMD enum.   */
+	enum VEHICLE_CMD command;		/**< Command ID, as defined MAVLink by VEHICLE_CMD enum.   */
 	uint8_t target_system;		/**< System which should execute the command  */
 	uint8_t target_component;	/**< Component which should execute the command, 0 for all components  */
 	uint8_t source_system;		/**< System sending the command  */

src/modules/uORB/topics/vehicle_gps_position.h

@@ -56,9 +56,9 @@
 struct vehicle_gps_position_s
 {
 	uint64_t timestamp_position;			/**< Timestamp for position information */
-	int32_t lat;					/**< Latitude in 1E7 degrees */
-	int32_t lon;					/**< Longitude in 1E7 degrees */
-	int32_t alt;					/**< Altitude in 1E3 meters (millimeters) above MSL  */
+	int32_t lat;					/**< Latitude in 1E-7 degrees */
+	int32_t lon;					/**< Longitude in 1E-7 degrees */
+	int32_t alt;					/**< Altitude in 1E-3 meters (millimeters) above MSL  */
 	
 	uint64_t timestamp_variance;
 	float s_variance_m_s;				/**< speed accuracy estimate m/s */