Merge branch 'offboard2' into offboard3

src/modules/att_pos_estimator_ekf/KalmanNav.cpp

@@ -314,14 +314,13 @@ void KalmanNav::updatePublications()
 	// global position publication
 	_pos.timestamp = _pubTimeStamp;
 	_pos.time_gps_usec = _gps.timestamp_position;
-	_pos.valid = true;
-	_pos.lat = getLatDegE7();
-	_pos.lon = getLonDegE7();
+	_pos.global_valid = true;
+	_pos.lat = lat * M_RAD_TO_DEG;
+	_pos.lon = lon * M_RAD_TO_DEG;
 	_pos.alt = float(alt);
-	_pos.relative_alt = float(alt); // TODO, make relative
-	_pos.vx = vN;
-	_pos.vy = vE;
-	_pos.vz = vD;
+	_pos.vel_n = vN;
+	_pos.vel_e = vE;
+	_pos.vel_d = vD;
 	_pos.yaw = psi;
 
 	// local position publication

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -410,13 +410,13 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 							vel(2) = gps.vel_d_m_s;
 						}
 
-					} else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
+					} else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
 						vel_valid = true;
 						if (global_pos_updated) {
 							vel_t = global_pos.timestamp;
-							vel(0) = global_pos.vx;
-							vel(1) = global_pos.vy;
-							vel(2) = global_pos.vz;
+							vel(0) = global_pos.vel_n;
+							vel(1) = global_pos.vel_e;
+							vel(2) = global_pos.vel_d;
 						}
 					}
 

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
 /* magnetic declination, in degrees */
 PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
 
-PARAM_DEFINE_INT32(ATT_ACC_COMP, 0);
+PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
 
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {

src/modules/commander/commander.cpp

@@ -153,6 +153,12 @@ static uint64_t last_print_mode_reject_time = 0;
 static bool on_usb_power = false;
 
 static float takeoff_alt = 5.0f;
+static int parachute_enabled = 0;
+
+static struct vehicle_status_s status;
+static struct actuator_armed_s armed;
+static struct safety_s safety;
+static struct vehicle_control_mode_s control_mode;
 
 /* tasks waiting for low prio thread */
 typedef enum {
@@ -200,16 +206,23 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
-void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
+void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
 
-transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
+transition_result_t set_main_state_rc(struct vehicle_status_s *status);
 
-void print_reject_mode(const char *msg);
+void set_control_mode();
+
+void print_reject_mode(struct vehicle_status_s *current_status, const char *msg);
 
 void print_reject_arm(const char *msg);
 
 void print_status();
 
+int arm();
+int disarm();
+
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
+
 /**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
  */
@@ -275,6 +288,16 @@ int commander_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "arm")) {
+		arm();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "disarm")) {
+		disarm();
+		exit(0);
+	}
+
 	usage("unrecognized command");
 	exit(1);
 }
@@ -341,6 +364,32 @@ void print_status()
 
 static orb_advert_t status_pub;
 
+int arm()
+{
+	int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+
+	if (arming_res == TRANSITION_CHANGED) {
+		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
+		return 0;
+
+	} else {
+		return 1;
+	}
+}
+
+int disarm()
+{
+	int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+
+	if (arming_res == TRANSITION_CHANGED) {
+		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
+		return 0;
+
+	} else {
+		return 1;
+	}
+}
+
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
 {
 	/* result of the command */
@@ -414,51 +463,45 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			/* set main state */
 			transition_result_t main_res = TRANSITION_DENIED;
 
-			if (status->rc_signal_lost) {
-				/* allow mode switching by command only if no RC signal */
-				if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
-					/* use autopilot-specific mode */
-					if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
-						/* MANUAL */
-						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+				/* use autopilot-specific mode */
+				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
+					/* MANUAL */
+					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
 
-					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
-						/* SEATBELT */
-						main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
+					/* SEATBELT */
+					main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
-					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
-						/* EASY */
-						main_res = main_state_transition(status, MAIN_STATE_EASY);
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
+					/* EASY */
+					main_res = main_state_transition(status, MAIN_STATE_EASY);
 
-					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
-						/* AUTO */
-						main_res = main_state_transition(status, MAIN_STATE_AUTO);
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
 
-					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
-						/* OFFBOARD */
-						main_res = main_state_transition(status, MAIN_STATE_OFFBOARD);
-					}
-
-				} else {
-					/* use base mode */
-					if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
-						/* AUTO */
-						main_res = main_state_transition(status, MAIN_STATE_AUTO);
-
-					} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-						if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
-							/* EASY */
-							main_res = main_state_transition(status, MAIN_STATE_EASY);
-
-						} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
-							/* MANUAL */
-							main_res = main_state_transition(status, MAIN_STATE_MANUAL);
-						}
-					}
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
+					/* OFFBOARD */
+					main_res = main_state_transition(status, MAIN_STATE_OFFBOARD);
 				}
 
 			} else {
-				mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd");
+				/* use base mode */
+				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
+						/* EASY */
+						main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
+						/* MANUAL */
+						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+					}
+				}
 			}
 
 			if (main_res == TRANSITION_CHANGED)
@@ -526,8 +569,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		/* Flight termination */
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
-			if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
-				transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
+			//XXX: to enable the parachute, a param needs to be set
+			//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
+			if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
+				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
 				ret = true;
 
@@ -565,18 +610,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 }
 
-static struct vehicle_status_s status;
-
-/* armed topic */
-static struct actuator_armed_s armed;
-
-static struct safety_s safety;
-
 int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
 	commander_initialized = false;
-	bool home_position_set = false;
 
 	bool battery_tune_played = false;
 	bool arm_tune_played = false;
@@ -586,10 +623,33 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_system_id = param_find("MAV_SYS_ID");
 	param_t _param_component_id = param_find("MAV_COMP_ID");
 	param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
+	param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
 
 	/* welcome user */
 	warnx("starting");
 
+	char *main_states_str[MAIN_STATE_MAX];
+	main_states_str[0] = "MANUAL";
+	main_states_str[1] = "SEATBELT";
+	main_states_str[2] = "EASY";
+	main_states_str[3] = "AUTO";
+	main_states_str[4] = "OFFBOARD";
+
+	char *arming_states_str[ARMING_STATE_MAX];
+	arming_states_str[0] = "INIT";
+	arming_states_str[1] = "STANDBY";
+	arming_states_str[2] = "ARMED";
+	arming_states_str[3] = "ARMED_ERROR";
+	arming_states_str[4] = "STANDBY_ERROR";
+	arming_states_str[5] = "REBOOT";
+	arming_states_str[6] = "IN_AIR_RESTORE";
+
+	char *failsafe_states_str[FAILSAFE_STATE_MAX];
+	failsafe_states_str[0] = "NORMAL";
+	failsafe_states_str[1] = "RTL";
+	failsafe_states_str[2] = "LAND";
+	failsafe_states_str[3] = "TERMINATION";
+
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
 
@@ -604,21 +664,17 @@ int commander_thread_main(int argc, char *argv[])
 
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	/* Main state machine */
-	/* make sure we are in preflight state */
+	/* vehicle status topic */
 	memset(&status, 0, sizeof(status));
 	status.condition_landed = true;	// initialize to safe value
-
-	/* armed topic */
-	orb_advert_t armed_pub;
-	/* Initialize armed with all false */
-	memset(&armed, 0, sizeof(armed));
-
+	// We want to accept RC inputs as default
+	status.rc_input_blocked = false;
 	status.main_state = MAIN_STATE_MANUAL;
 	status.set_nav_state = NAV_STATE_NONE;
 	status.set_nav_state_timestamp = 0;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
+	status.failsafe_state = FAILSAFE_STATE_NORMAL;
 
 	/* neither manual nor offboard control commands have been received */
 	status.offboard_control_signal_found_once = false;
@@ -635,14 +691,20 @@ int commander_thread_main(int argc, char *argv[])
 	// XXX for now just set sensors as initialized
 	status.condition_system_sensors_initialized = true;
 
-	/* advertise to ORB */
-	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
-	/* publish current state machine */
-
-	/* publish initial state */
 	status.counter++;
 	status.timestamp = hrt_absolute_time();
-	orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
+	/* publish initial state */
+	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+
+	/* armed topic */
+	orb_advert_t armed_pub;
+	/* Initialize armed with all false */
+	memset(&armed, 0, sizeof(armed));
+
+	/* vehicle control mode topic */
+	memset(&control_mode, 0, sizeof(control_mode));
+	orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
 
 	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
 
@@ -704,9 +766,9 @@ int commander_thread_main(int argc, char *argv[])
 	memset(&sp_man, 0, sizeof(sp_man));
 
 	/* Subscribe to offboard control data */
-	int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	struct offboard_control_setpoint_s offboard_sp;
-	memset(&offboard_sp, 0, sizeof(offboard_sp));
+	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
+	struct offboard_control_setpoint_s sp_offboard;
+	memset(&sp_offboard, 0, sizeof(sp_offboard));
 
 	/* Subscribe to global position */
 	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -808,10 +870,10 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* re-check RC calibration */
 				rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
-
-				/* navigation parameters */
-				param_get(_param_takeoff_alt, &takeoff_alt);
 			}
+			/* navigation parameters */
+			param_get(_param_takeoff_alt, &takeoff_alt);
+			param_get(_param_enable_parachute, &parachute_enabled);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -820,10 +882,10 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
 		}
 
-		orb_check(offboard_sp_sub, &updated);
+		orb_check(sp_offboard_sub, &updated);
 
 		if (updated) {
-			orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp);
+			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
 		}
 
 		orb_check(sensor_sub, &updated);
@@ -862,7 +924,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* update condition_global_position_valid */
-		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
+		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed);
 
 		/* update local position estimate */
 		orb_check(local_position_sub, &updated);
@@ -876,10 +938,12 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
+		static bool published_condition_landed_fw = false;
 		if (status.is_rotary_wing && status.condition_local_altitude_valid) {
 			if (status.condition_landed != local_position.landed) {
 				status.condition_landed = local_position.landed;
 				status_changed = true;
+				published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes
 
 				if (status.condition_landed) {
 					mavlink_log_critical(mavlink_fd, "#audio: LANDED");
@@ -888,6 +952,12 @@ int commander_thread_main(int argc, char *argv[])
 					mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
 				}
 			}
+		} else {
+			if (!published_condition_landed_fw) {
+				status.condition_landed = false; // Fixedwing does not have a landing detector currently
+				published_condition_landed_fw = true;
+				status_changed = true;
+			}
 		}
 
 		/* update battery status */
@@ -1018,19 +1088,18 @@ int commander_thread_main(int argc, char *argv[])
 			 * position to the current position.
 			 */
 
-			if (!home_position_set && gps_position.fix_type >= 3 &&
-			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&	// XXX note that vdop is 0 for mtk
+			if (!status.condition_home_position_valid && gps_position.fix_type >= 3 &&
+			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&
 			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
-			    && global_position.valid) {
+			    && global_position.global_valid) {
+
 				/* copy position data to uORB home message, store it locally as well */
+				home.lat = global_position.lat;
+				home.lon = global_position.lon;
+				home.alt = global_position.alt;
 
-
-				home.lat = (double)global_position.lat / 1e7d;
-				home.lon = (double)global_position.lon / 1e7d;
-				home.altitude = (float)global_position.alt;
-
-				warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude);
-				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude);
+				warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt);
+				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt);
 
 				/* announce new home position */
 				if (home_pub > 0) {
@@ -1041,13 +1110,13 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* mark home position as set */
-				home_position_set = true;
+				status.condition_home_position_valid = true;
 				tune_positive();
 			}
 		}
 
 		/* start RC input check */
-		if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
+		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
@@ -1122,24 +1191,27 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 			} else if (res == TRANSITION_DENIED) {
-				warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-				mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+				/* DENIED here indicates bug in the commander */
+				mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
 			}
 
-			/* fill current_status according to mode switches */
+			if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+				/* recover from failsafe */
+				transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+			}
+
+			/* fill status according to mode switches */
 			check_mode_switches(&sp_man, &status);
 
-			/* evaluate the main state machine */
-			res = check_main_state_machine(&status);
+			/* evaluate the main state machine according to mode switches */
+			res = set_main_state_rc(&status);
 
 			if (res == TRANSITION_CHANGED) {
-				//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
 				tune_positive();
 
 			} else if (res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
-				warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-				mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
 			}
 
 		} else {
@@ -1149,26 +1221,66 @@ int commander_thread_main(int argc, char *argv[])
 				status_changed = true;
 			}
 
-			if (status.main_state != MAIN_STATE_AUTO && armed.armed && status.main_state != MAIN_STATE_OFFBOARD) {
-				transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+			/* switch to OFFBOARD mode if offboard signal available */
+			transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD);
 
-				if (res == TRANSITION_CHANGED) {
-					mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode");
-					status.set_nav_state = NAV_STATE_RTL;
-					status.set_nav_state_timestamp = hrt_absolute_time();
+			if (res == TRANSITION_DENIED) {
+				/* can't switch to OFFBOARD, do normal failsafe if needed */
+				if (armed.armed) {
+					if (status.main_state == MAIN_STATE_AUTO) {
+						/* check if AUTO mode still allowed */
+						transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
 
-				} else if (status.main_state != MAIN_STATE_SEATBELT) {
-					res = main_state_transition(&status, MAIN_STATE_SEATBELT);
+						if (res == TRANSITION_DENIED) {
+							/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
+							res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
 
-					if (res == TRANSITION_CHANGED) {
-						mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode");
+							if (res == TRANSITION_DENIED) {
+								/* LAND not allowed, set TERMINATION state */
+								transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+							}
+						}
+
+					} else if (status.main_state == MAIN_STATE_OFFBOARD) {
+						/* check if OFFBOARD mode still allowed */
+						transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD);
+
+						if (res == TRANSITION_DENIED) {
+							/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
+							res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+
+							if (res == TRANSITION_DENIED) {
+								/* LAND not allowed, set TERMINATION state */
+								transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+							}
+						}
+
+					} else {
+						/* failsafe for manual modes */
+						transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+
+						if (res == TRANSITION_DENIED) {
+							/* RTL not allowed (no global position estimate), try LAND */
+							res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+
+							if (res == TRANSITION_DENIED) {
+								/* LAND not allowed, set TERMINATION state */
+								res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+							}
+						}
+					}
+
+				} else {
+					if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+						/* reset failsafe when disarmed */
+						transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
 					}
 				}
 			}
 		}
 
 		/* check offboard signal */
-		if (hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) {
+		if (sp_offboard.timestamp != 0 && hrt_absolute_time() < sp_offboard.timestamp + OFFBOARD_TIMEOUT) {
 			if (!status.offboard_control_signal_found_once) {
 				status.offboard_control_signal_found_once = true;
 				mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time");
@@ -1184,16 +1296,16 @@ int commander_thread_main(int argc, char *argv[])
 			status.offboard_control_signal_lost = false;
 
 			if (status.main_state == MAIN_STATE_OFFBOARD) {
-				if (offboard_sp.armed && !armed.armed) {
+				if (sp_offboard.armed && !armed.armed) {
 					if (!safety.safety_switch_available || safety.safety_off) {
 						transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
 
 						if (res == TRANSITION_CHANGED) {
-							mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal");
+								mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal");
 						}
 					}
 
-				} else if (!offboard_sp.armed && armed.armed) {
+				} else if (!sp_offboard.armed && armed.armed) {
 					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
 					transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed);
 
@@ -1211,19 +1323,14 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/*  Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
-		if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
-			transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
-
-			if (flighttermination_res == TRANSITION_CHANGED) {
+		// TODO remove this hack
+		/* flight termination in manual mode if assisted switch is on easy position */
+		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
 				tune_positive();
 			}
-
-		} else {
-			flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
 		}
 
-
 		/* handle commands last, as the system needs to be updated to handle them */
 		orb_check(cmd_sub, &updated);
 
@@ -1239,19 +1346,35 @@ int commander_thread_main(int argc, char *argv[])
 		/* check which state machines for changes, clear "changed" flag */
 		bool arming_state_changed = check_arming_state_changed();
 		bool main_state_changed = check_main_state_changed();
-		bool flighttermination_state_changed = check_flighttermination_state_changed();
+		bool failsafe_state_changed = check_failsafe_state_changed();
 
 		hrt_abstime t1 = hrt_absolute_time();
 
-		if (arming_state_changed || main_state_changed) {
-			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state);
+		/* print new state */
+		if (arming_state_changed) {
 			status_changed = true;
+			mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]);
+		}
+
+		if (main_state_changed) {
+			status_changed = true;
+			mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
+		}
+
+		if (failsafe_state_changed) {
+			status_changed = true;
+			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
 		}
 
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
 		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
+			set_control_mode();
+			control_mode.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+
 			status.timestamp = t1;
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
 			armed.timestamp = t1;
 			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
@@ -1317,7 +1440,7 @@ int commander_thread_main(int argc, char *argv[])
 	led_deinit();
 	buzzer_deinit();
 	close(sp_man_sub);
-	close(offboard_sp_sub);
+	close(sp_offboard_sub);
 	close(local_position_sub);
 	close(global_position_sub);
 	close(gps_sub);
@@ -1424,145 +1547,258 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 }
 
 void
-check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status)
+check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status)
 {
 	/* main mode switch */
 	if (!isfinite(sp_man->mode_switch)) {
-		warnx("mode sw not finite");
-		current_status->mode_switch = MODE_SWITCH_MANUAL;
+		/* default to manual if signal is invalid */
+		status->mode_switch = MODE_SWITCH_MANUAL;
 
 	} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
-		current_status->mode_switch = MODE_SWITCH_AUTO;
+		status->mode_switch = MODE_SWITCH_AUTO;
 
 	} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
-		current_status->mode_switch = MODE_SWITCH_MANUAL;
+		status->mode_switch = MODE_SWITCH_MANUAL;
 
 	} else {
-		current_status->mode_switch = MODE_SWITCH_ASSISTED;
+		status->mode_switch = MODE_SWITCH_ASSISTED;
 	}
 
 	/* return switch */
 	if (!isfinite(sp_man->return_switch)) {
-		current_status->return_switch = RETURN_SWITCH_NONE;
+		status->return_switch = RETURN_SWITCH_NONE;
 
 	} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
-		current_status->return_switch = RETURN_SWITCH_RETURN;
+		status->return_switch = RETURN_SWITCH_RETURN;
 
 	} else {
-		current_status->return_switch = RETURN_SWITCH_NORMAL;
+		status->return_switch = RETURN_SWITCH_NORMAL;
 	}
 
 	/* assisted switch */
 	if (!isfinite(sp_man->assisted_switch)) {
-		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+		status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
 
 	} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
-		current_status->assisted_switch = ASSISTED_SWITCH_EASY;
+		status->assisted_switch = ASSISTED_SWITCH_EASY;
 
 	} else {
-		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+		status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
 	}
 
 	/* mission switch  */
 	if (!isfinite(sp_man->mission_switch)) {
-		current_status->mission_switch = MISSION_SWITCH_NONE;
+		status->mission_switch = MISSION_SWITCH_NONE;
 
 	} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
-		current_status->mission_switch = MISSION_SWITCH_LOITER;
+		status->mission_switch = MISSION_SWITCH_LOITER;
 
 	} else {
-		current_status->mission_switch = MISSION_SWITCH_MISSION;
+		status->mission_switch = MISSION_SWITCH_MISSION;
 	}
 
-	/* offboard switch  */
+	 /* offboard switch  */
 	if (!isfinite(sp_man->offboard_switch)) {
-		current_status->offboard_switch = OFFBOARD_SWITCH_NONE;
+		status->offboard_switch = OFFBOARD_SWITCH_NONE;
 
-	} else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT  && !status.offboard_control_signal_lost) {		
-		current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD;		
+	} else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) {
+		status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD;
 
 	} else {
-		current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD;
+		status->offboard_switch = OFFBOARD_SWITCH_ONBOARD;
 	}
 }
 
 transition_result_t
-check_main_state_machine(struct vehicle_status_s *current_status)
+set_main_state_rc(struct vehicle_status_s *status)
 {
-	/* evaluate the main state machine */
+	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;
-	if (current_status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) {
-		/* offboard switch overrides main switch */
-		res = main_state_transition(current_status, MAIN_STATE_OFFBOARD);
 
-	} else {
-		switch (current_status->mode_switch) {
-		case MODE_SWITCH_MANUAL:
-			res = main_state_transition(current_status, MAIN_STATE_MANUAL);
-			// TRANSITION_DENIED is not possible here
-			break;
+	/* offboard switch overrides main switch */
+	if (status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) {
+		res = main_state_transition(status, MAIN_STATE_OFFBOARD);
+		if (res == TRANSITION_DENIED) {
+			print_reject_mode(status, "OFFBOARD");
 
-		case MODE_SWITCH_ASSISTED:
-			if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
-				res = main_state_transition(current_status, MAIN_STATE_EASY);
-
-				if (res != TRANSITION_DENIED)
-					break;	// changed successfully or already in this state
-
-				// else fallback to SEATBELT
-				print_reject_mode("EASY");
-			}
-
-			res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
-
-			if (res != TRANSITION_DENIED)
-				break;	// changed successfully or already in this mode
-
-			if (current_status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
-				print_reject_mode("SEATBELT");
-
-			// else fallback to MANUAL
-			res = main_state_transition(current_status, MAIN_STATE_MANUAL);
-			// TRANSITION_DENIED is not possible here
-			break;
-
-		case MODE_SWITCH_AUTO:
-			res = main_state_transition(current_status, MAIN_STATE_AUTO);
-
-			if (res != TRANSITION_DENIED)
-				break;	// changed successfully or already in this state
-
-			// else fallback to SEATBELT (EASY likely will not work too)
-			print_reject_mode("AUTO");
-			res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
-
-			if (res != TRANSITION_DENIED)
-				break;	// changed successfully or already in this state
-
-			// else fallback to MANUAL
-			res = main_state_transition(current_status, MAIN_STATE_MANUAL);
-			// TRANSITION_DENIED is not possible here
-			break;
-
-		default:
-			break;
+		} else {
+			return res;
 		}
 	}
 
+	/* offboard switched off or denied, check mode switch */
+	switch (status->mode_switch) {
+	case MODE_SWITCH_MANUAL:
+		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	case MODE_SWITCH_ASSISTED:
+		if (status->assisted_switch == ASSISTED_SWITCH_EASY) {
+			res = main_state_transition(status, MAIN_STATE_EASY);
+
+			if (res != TRANSITION_DENIED)
+				break;	// changed successfully or already in this state
+
+			// else fallback to SEATBELT
+			print_reject_mode(status, "EASY");
+		}
+
+		res = main_state_transition(status, MAIN_STATE_SEATBELT);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this mode
+
+		if (status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
+			print_reject_mode(status, "SEATBELT");
+
+		// else fallback to MANUAL
+		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	case MODE_SWITCH_AUTO:
+		res = main_state_transition(status, MAIN_STATE_AUTO);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this state
+
+		// else fallback to SEATBELT (EASY likely will not work too)
+		print_reject_mode(status, "AUTO");
+		res = main_state_transition(status, MAIN_STATE_SEATBELT);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this state
+
+		// else fallback to MANUAL
+		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	default:
+		break;
+	}
+
 	return res;
 }
 
 void
-print_reject_mode(const char *msg)
+
+set_control_mode()
+{
+	/* set vehicle_control_mode according to main state and failsafe state */
+	control_mode.flag_armed = armed.armed;
+	control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
+	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
+
+	control_mode.flag_control_termination_enabled = false;
+
+	/* set this flag when navigator should act */
+	bool navigator_enabled = false;
+
+	switch (status.failsafe_state) {
+	case FAILSAFE_STATE_NORMAL:
+		switch (status.main_state) {
+		case MAIN_STATE_MANUAL:
+			control_mode.flag_control_manual_enabled = true;
+			control_mode.flag_control_auto_enabled = false;
+			control_mode.flag_control_rates_enabled = status.is_rotary_wing;
+			control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case MAIN_STATE_SEATBELT:
+			control_mode.flag_control_manual_enabled = true;
+			control_mode.flag_control_auto_enabled = false;
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = true;
+			control_mode.flag_control_climb_rate_enabled = true;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case MAIN_STATE_EASY:
+			control_mode.flag_control_manual_enabled = true;
+			control_mode.flag_control_auto_enabled = false;
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = true;
+			control_mode.flag_control_climb_rate_enabled = true;
+			control_mode.flag_control_position_enabled = true;
+			control_mode.flag_control_velocity_enabled = true;
+			break;
+
+		case MAIN_STATE_AUTO:
+			navigator_enabled = true;
+
+		default:
+			break;
+		}
+
+		break;
+
+	case FAILSAFE_STATE_RTL:
+		navigator_enabled = true;
+		break;
+
+	case FAILSAFE_STATE_LAND:
+		navigator_enabled = true;
+		break;
+
+	case FAILSAFE_STATE_TERMINATION:
+		/* disable all controllers on termination */
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = false;
+		control_mode.flag_control_attitude_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_termination_enabled = true;
+		break;
+
+	default:
+		break;
+	}
+
+	/* navigator has control, set control mode flags according to nav state*/
+	if (navigator_enabled) {
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+	}
+}
+
+void
+print_reject_mode(struct vehicle_status_s *status, const char *msg)
 {
 	hrt_abstime t = hrt_absolute_time();
 
 	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
 		last_print_mode_reject_time = t;
 		char s[80];
-		sprintf(s, "#audio: warning: reject %s", msg);
+		sprintf(s, "#audio: REJECT %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
-		tune_negative();
+
+		// only buzz if armed, because else we're driving people nuts indoors
+		// they really need to look at the leds as well.
+		if (status->arming_state == ARMING_STATE_ARMED) {
+			tune_negative();
+		} else {
+
+			// Always show the led indication
+			led_negative();
+		}
 	}
 }
 
@@ -1710,7 +1946,15 @@ void *commander_low_prio_loop(void *arg)
 				} else if ((int)(cmd.param4) == 1) {
 					/* RC calibration */
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					calib_ret = do_rc_calibration(mavlink_fd);
+					/* disable RC control input completely */
+					status.rc_input_blocked = true;
+					calib_ret = OK;
+					mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN");
+
+				} else if ((int)(cmd.param4) == 2) {
+					/* RC trim calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_trim_calibration(mavlink_fd);
 
 				} else if ((int)(cmd.param5) == 1) {
 					/* accelerometer calibration */
@@ -1721,6 +1965,18 @@ void *commander_low_prio_loop(void *arg)
 					/* airspeed calibration */
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 					calib_ret = do_airspeed_calibration(mavlink_fd);
+				} else if ((int)(cmd.param4) == 0) {
+					/* RC calibration ended - have we been in one worth confirming? */
+					if (status.rc_input_blocked) {
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+						/* enable RC control input */
+						status.rc_input_blocked = false;
+						mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
+					}
+
+					/* this always succeeds */
+					calib_ret = OK;
+
 				}
 
 				if (calib_ret == OK)
@@ -1779,6 +2035,10 @@ void *commander_low_prio_loop(void *arg)
 				break;
 			}
 
+		case VEHICLE_CMD_START_RX_PAIR:
+		/* handled in the IO driver */
+		break;
+
 		default:
 			/* don't answer on unsupported commands, it will be done in main loop */
 			break;

src/modules/commander/commander_helper.cpp

@@ -123,11 +123,16 @@ void tune_neutral()
 }
 
 void tune_negative()
+{
+	led_negative();
+	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
+}
+
+void led_negative()
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_RED);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
 }
 
 int tune_arm()

src/modules/commander/commander_helper.h

@@ -62,6 +62,9 @@ int tune_arm(void);
 int tune_low_bat(void);
 int tune_critical_bat(void);
 void tune_stop(void);
+
+void led_negative();
+
 int blink_msg_state();
 
 int led_init(void);

src/modules/commander/rc_calibration.cpp

@@ -53,17 +53,16 @@
 #endif
 static const int ERROR = -1;
 
-int do_rc_calibration(int mavlink_fd)
+int do_trim_calibration(int mavlink_fd)
 {
-	mavlink_log_info(mavlink_fd, "trim calibration starting");
-
 	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
+	usleep(200000);
 	struct manual_control_setpoint_s sp;
 	bool changed;
 	orb_check(sub_man, &changed);
 
 	if (!changed) {
-		mavlink_log_critical(mavlink_fd, "no manual control, aborting");
+		mavlink_log_critical(mavlink_fd, "no inputs, aborting");
 		return ERROR;
 	}
 
@@ -82,12 +81,12 @@ int do_rc_calibration(int mavlink_fd)
 	int save_ret = param_save_default();
 
 	if (save_ret != 0) {
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
+		mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
 		close(sub_man);
 		return ERROR;
 	}
 
-	mavlink_log_info(mavlink_fd, "trim calibration done");
+	mavlink_log_info(mavlink_fd, "trim cal done");
 	close(sub_man);
 	return OK;
 }

src/modules/commander/rc_calibration.h

@@ -41,6 +41,6 @@
 
 #include <stdint.h>
 
-int do_rc_calibration(int mavlink_fd);
+int do_trim_calibration(int mavlink_fd);
 
 #endif /* RC_CALIBRATION_H_ */

src/modules/commander/state_machine_helper.cpp

@@ -42,6 +42,8 @@
 #include <unistd.h>
 #include <stdint.h>
 #include <stdbool.h>
+#include <dirent.h>
+#include <fcntl.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
@@ -50,6 +52,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_device.h>
 #include <mavlink/mavlink_log.h>
 
 #include "state_machine_helper.h"
@@ -63,7 +66,7 @@ static const int ERROR = -1;
 
 static bool arming_state_changed = true;
 static bool main_state_changed = true;
-static bool flighttermination_state_changed = true;
+static bool failsafe_state_changed = true;
 
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
@@ -220,61 +223,66 @@ check_arming_state_changed()
 }
 
 transition_result_t
-main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state)
+main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
-	/* only check transition if the new state is actually different from the current one */
-	if (new_main_state == current_state->main_state) {
-		ret = TRANSITION_NOT_CHANGED;
+	/* transition may be denied even if requested the same state because conditions may be changed */
+	switch (new_main_state) {
+	case MAIN_STATE_MANUAL:
+		ret = TRANSITION_CHANGED;
+		break;
 
-	} else {
+	case MAIN_STATE_SEATBELT:
 
-		switch (new_main_state) {
-		case MAIN_STATE_MANUAL:
+		/* need at minimum altitude estimate */
+		if (!status->is_rotary_wing ||
+		    (status->condition_local_altitude_valid ||
+		     status->condition_global_position_valid)) {
 			ret = TRANSITION_CHANGED;
-			break;
-
-		case MAIN_STATE_SEATBELT:
-
-			/* need at minimum altitude estimate */
-			if (!current_state->is_rotary_wing ||
-			    (current_state->condition_local_altitude_valid ||
-			     current_state->condition_global_position_valid)) {
-				ret = TRANSITION_CHANGED;
-			}
-
-			break;
-
-		case MAIN_STATE_EASY:
-
-			/* need at minimum local position estimate */
-			if (current_state->condition_local_position_valid ||
-			    current_state->condition_global_position_valid) {
-				ret = TRANSITION_CHANGED;
-			}
-
-			break;
-
-		case MAIN_STATE_AUTO:
-
-			/* need global position estimate */
-			if (current_state->condition_global_position_valid) {
-				ret = TRANSITION_CHANGED;
-			}
-
-			break;
-
-		case MAIN_STATE_OFFBOARD:
-
-			ret = TRANSITION_CHANGED;
-
-			break;
 		}
 
-		if (ret == TRANSITION_CHANGED) {
-			current_state->main_state = new_main_state;
+		break;
+
+	case MAIN_STATE_EASY:
+
+		/* need at minimum local position estimate */
+		if (status->condition_local_position_valid ||
+		    status->condition_global_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
+
+		break;
+
+	case MAIN_STATE_AUTO:
+
+		/* need global position estimate */
+		if (status->condition_global_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
+
+		break;
+
+	case MAIN_STATE_OFFBOARD:
+
+		/* need global position estimate */
+		if (!status->offboard_control_signal_lost) {
+			ret = TRANSITION_CHANGED;
+		}
+
+		break;
+
+	default:
+		break;
+	}
+
+	if (ret == TRANSITION_CHANGED) {
+		if (status->main_state != new_main_state) {
+			status->main_state = new_main_state;
 			main_state_changed = true;
+
+		} else {
+			ret = TRANSITION_NOT_CHANGED;
 		}
 	}
 
@@ -294,10 +302,10 @@ check_main_state_changed()
 }
 
 bool
-check_flighttermination_state_changed()
+check_failsafe_state_changed()
 {
-	if (flighttermination_state_changed) {
-		flighttermination_state_changed = false;
+	if (failsafe_state_changed) {
+		failsafe_state_changed = false;
 		return true;
 
 	} else {
@@ -339,6 +347,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
 				valid_transition = true;
+
+				// Disable publication of all attached sensors
+
+				/* list directory */
+				DIR		*d;
+				struct dirent	*direntry;
+				d = opendir("/dev");
+				if (d) {
+
+					while ((direntry = readdir(d)) != NULL) {
+
+						int sensfd = ::open(direntry->d_name, 0);
+						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
+						close(sensfd);
+
+						printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
+					}
+
+					closedir(d);
+
+					warnx("directory listing ok (FS mounted and readable)");
+
+				} else {
+					/* failed opening dir */
+					warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
+					return 1;
+				}
 			}
 
 			break;
@@ -368,28 +403,51 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 
 /**
-* Transition from one flightermination state to another
+* Transition from one failsafe state to another
 */
-transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state)
+transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
-	/* only check transition if the new state is actually different from the current one */
-	if (new_flighttermination_state == status->flighttermination_state) {
-		ret = TRANSITION_NOT_CHANGED;
+	/* transition may be denied even if requested the same state because conditions may be changed */
+	if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) {
+		/* transitions from TERMINATION to other states not allowed */
+		if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) {
+			ret = TRANSITION_NOT_CHANGED;
+		}
 
 	} else {
-
-		switch (new_flighttermination_state) {
-		case FLIGHTTERMINATION_STATE_ON:
+		switch (new_failsafe_state) {
+		case FAILSAFE_STATE_NORMAL:
+			/* always allowed (except from TERMINATION state) */
 			ret = TRANSITION_CHANGED;
-			status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
-			warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
 			break;
 
-		case FLIGHTTERMINATION_STATE_OFF:
+		case FAILSAFE_STATE_RTL:
+
+			/* global position and home position required for RTL */
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->set_nav_state = NAV_STATE_RTL;
+				status->set_nav_state_timestamp = hrt_absolute_time();
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+
+		case FAILSAFE_STATE_LAND:
+
+			/* at least relative altitude estimate required for landing */
+			if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
+				status->set_nav_state = NAV_STATE_LAND;
+				status->set_nav_state_timestamp = hrt_absolute_time();
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+
+		case FAILSAFE_STATE_TERMINATION:
+			/* always allowed */
 			ret = TRANSITION_CHANGED;
-			status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
 			break;
 
 		default:
@@ -397,9 +455,13 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s *
 		}
 
 		if (ret == TRANSITION_CHANGED) {
-			flighttermination_state_changed = true;
-			// TODO
-			//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
+			if (status->failsafe_state != new_failsafe_state) {
+				status->failsafe_state = new_failsafe_state;
+				failsafe_state_changed = true;
+
+			} else {
+				ret = TRANSITION_NOT_CHANGED;
+			}
 		}
 	}
 

src/modules/commander/state_machine_helper.h

@@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
 
 bool check_main_state_changed();
 
-transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state);
+transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
 
 bool check_navigation_state_changed();
 
-bool check_flighttermination_state_changed();
+bool check_failsafe_state_changed();
 
 void set_navigation_state_changed();
 

src/modules/controllib/uorb/blocks.cpp

@@ -54,8 +54,8 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {};
 
 void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
 				   vehicle_attitude_s &att,
-				   mission_item_s &missionCmd,
-				   mission_item_s &lastMissionCmd)
+				   position_setpoint_s &missionCmd,
+				   position_setpoint_s &lastMissionCmd)
 {
 
 	// heading to waypoint
@@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
 	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
 	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
 	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
-	_missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20),
+	_missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20),
 	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
 	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz

src/modules/controllib/uorb/blocks.hpp

@@ -43,7 +43,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
@@ -82,8 +82,8 @@ public:
 	virtual ~BlockWaypointGuidance();
 	void update(vehicle_global_position_s &pos,
 		    vehicle_attitude_s &att,
-		    mission_item_s &missionCmd,
-		    mission_item_s &lastMissionCmd);
+		    position_setpoint_s &missionCmd,
+		    position_setpoint_s &lastMissionCmd);
 	float getPsiCmd() { return _psiCmd; }
 };
 
@@ -98,7 +98,7 @@ protected:
 	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
 	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
 	UOrbSubscription<vehicle_global_position_s> _pos;
-	UOrbSubscription<mission_item_triplet_s> _missionCmd;
+	UOrbSubscription<position_setpoint_triplet_s> _missionCmd;
 	UOrbSubscription<manual_control_setpoint_s> _manual;
 	UOrbSubscription<vehicle_status_s> _status;
 	UOrbSubscription<parameter_update_s> _param_update;

src/modules/dataman/dataman.c

@@ -126,45 +126,46 @@ static const char *k_data_manager_device_path = "/fs/microsd/dataman";
 /* The data manager work queues */
 
 typedef struct {
-	sq_queue_t q;
-	sem_t mutex;	/* Mutual exclusion on work queue adds and deletes */
-	unsigned size;
-	unsigned max_size;
+	sq_queue_t q;		/* Nuttx queue */
+	sem_t mutex;		/* Mutual exclusion on work queue adds and deletes */
+	unsigned size;		/* Current size of queue */
+	unsigned max_size;	/* Maximum queue size reached */
 } work_q_t;
 
-static work_q_t g_free_q;
-static work_q_t g_work_q;
+static work_q_t g_free_q;	/* queue of free work items. So that we don't always need to call malloc and free*/
+static work_q_t g_work_q;	/* pending work items. To be consumed by worker thread */
 
-sem_t g_work_queued_sema;
+sem_t g_work_queued_sema;	/* To notify worker thread a work item has been queued */
 sem_t g_init_sema;
 
 static bool g_task_should_exit;		/**< if true, dataman task should exit */
 
-#define DM_SECTOR_HDR_SIZE 4
-static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE;
+#define DM_SECTOR_HDR_SIZE 4	/* data manager per item header overhead */
+static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */
 
 static void init_q(work_q_t *q)
 {
-	sq_init(&(q->q));
-	sem_init(&(q->mutex), 1, 1);
-	q->size = q->max_size = 0;
+	sq_init(&(q->q));				/* Initialize the NuttX queue structure */
+	sem_init(&(q->mutex), 1, 1);	/* Queue is initially unlocked */
+	q->size = q->max_size = 0;		/* Queue is initially empty */
 }
 
-static void destroy_q(work_q_t *q)
+static inline void
+destroy_q(work_q_t *q)
 {
-	sem_destroy(&(q->mutex));
+	sem_destroy(&(q->mutex));		/* Destroy the queue lock */
 }
 
 static inline void
 lock_queue(work_q_t *q)
 {
-	sem_wait(&(q->mutex));
+	sem_wait(&(q->mutex));	/* Acquire the queue lock */
 }
 
 static inline void
 unlock_queue(work_q_t *q)
 {
-	sem_post(&(q->mutex));
+	sem_post(&(q->mutex));	/* Release the queue lock */
 }
 
 static work_q_item_t *
@@ -172,54 +173,47 @@ create_work_item(void)
 {
 	work_q_item_t *item;
 
+	/* Try to reuse item from free item queue */
 	lock_queue(&g_free_q);
 	if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q))))
 		g_free_q.size--;
 	unlock_queue(&g_free_q);
 
+	/* If we there weren't any free items then obtain memory for a new one */
 	if (item == NULL)
 		item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
 
+	/* If we got one then lock the item*/
 	if (item)
 		sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */
 
+	/* return the item pointer, or NULL if all failed */
 	return item;
 }
 
 /* Work queue management functions */
-static void
-enqueue_work_item(work_q_item_t *item)
-{
-	/* put the work item on the work queue */
-	lock_queue(&g_work_q);
-	sq_addlast(&item->link, &(g_work_q.q));
 
-	if (++g_work_q.size > g_work_q.max_size)
-		g_work_q.max_size = g_work_q.size;
-
-	unlock_queue(&g_work_q);
-
-	/* tell the work thread that work is available */
-	sem_post(&g_work_queued_sema);
-}
-
-static void
+static inline void
 destroy_work_item(work_q_item_t *item)
 {
-	sem_destroy(&item->wait_sem);
+	sem_destroy(&item->wait_sem); /* Destroy the item lock */
+	/* Return the item to the free item queue for later reuse */
 	lock_queue(&g_free_q);
 	sq_addfirst(&item->link, &(g_free_q.q));
 
+	/* Update the queue size and potentially the maximum queue size */
 	if (++g_free_q.size > g_free_q.max_size)
 		g_free_q.max_size = g_free_q.size;
 
 	unlock_queue(&g_free_q);
 }
 
-static work_q_item_t *
+static inline work_q_item_t *
 dequeue_work_item(void)
 {
 	work_q_item_t *work;
+
+	/* retrieve the 1st item on the work queue */
 	lock_queue(&g_work_q);
 
 	if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q)))
@@ -229,6 +223,32 @@ dequeue_work_item(void)
 	return work;
 }
 
+static int
+enqueue_work_item_and_wait_for_result(work_q_item_t *item)
+{
+	/* put the work item at the end of the work queue */
+	lock_queue(&g_work_q);
+	sq_addlast(&item->link, &(g_work_q.q));
+
+	/* Adjust the queue size and potentially the maximum queue size */
+	if (++g_work_q.size > g_work_q.max_size)
+		g_work_q.max_size = g_work_q.size;
+
+	unlock_queue(&g_work_q);
+
+	/* tell the work thread that work is available */
+	sem_post(&g_work_queued_sema);
+
+	/* wait for the result */
+	sem_wait(&item->wait_sem);
+
+	int result = item->result;
+
+	destroy_work_item(item);
+
+	return result;
+}
+
 /* Calculate the offset in file of specific item */
 static int
 calculate_offset(dm_item_t item, unsigned char index)
@@ -250,6 +270,8 @@ calculate_offset(dm_item_t item, unsigned char index)
  *
  * byte 0: Length of user data item
  * byte 1: Persistence of this data item
+ * byte 2: Unused (for future use)
+ * byte 3: Unused (for future use)
  * byte DM_SECTOR_HDR_SIZE... : data item value
  *
  * The total size must not exceed k_sector_size
@@ -266,6 +288,7 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
 	/* Get the offset for this item */
 	offset = calculate_offset(item, index);
 
+	/* If item type or index out of range, return error */
 	if (offset < 0) 
 		return -1;
 
@@ -283,10 +306,12 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
 
 	len = -1;
 
+	/* Seek to the right spot in the data manager file and write the data item */
 	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
 		if ((len = write(g_task_fd, buffer, count)) == count)
-			fsync(g_task_fd);
+			fsync(g_task_fd); /* Make sure data is written to physical media */
 
+	/* Make sure the write succeeded */
 	if (len != count)
 		return -1;
 
@@ -304,6 +329,7 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count)
 	/* Get the offset for this item */
 	offset = calculate_offset(item, index);
 
+	/* If item type or index out of range, return error */
 	if (offset < 0)
 		return -1;
 
@@ -316,14 +342,17 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count)
 	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
 		len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE);
 
-	/* Check for length issues */
+	/* Check for read error */
 	if (len < 0)
 		return -1;
 
+	/* A zero length entry is a empty entry */
 	if (len == 0)
 		buffer[0] = 0;
 
+	/* See if we got data */
 	if (buffer[0] > 0) {
+		/* We got more than requested!!! */
 		if (buffer[0] > count)
 			return -1;
 
@@ -340,11 +369,14 @@ _clear(dm_item_t item)
 {
 	int i, result = 0;
 
+	/* Get the offset of 1st item of this type */
 	int offset = calculate_offset(item, 0);
 
+	/* Check for item type out of range */
 	if (offset < 0)
 		return -1;
 
+	/* Clear all items of this type */
 	for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) {
 		char buf[1];
 
@@ -353,9 +385,11 @@ _clear(dm_item_t item)
 			break;
 		}
 
+		/* Avoid SD flash wear by only doing writes where necessary */
 		if (read(g_task_fd, buf, 1) < 1)
 			break;
 
+		/* If item has length greater than 0 it needs to be overwritten */
 		if (buf[0]) {
 			if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
 				result = -1;
@@ -373,6 +407,7 @@ _clear(dm_item_t item)
 		offset += k_sector_size;
 	}
 
+	/* Make sure data is actually written to physical media */
 	fsync(g_task_fd);
 	return result;
 }
@@ -452,12 +487,13 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
 {
 	work_q_item_t *work;
 
+	/* Make sure data manager has been started and is not shutting down */
 	if ((g_fd < 0) || g_task_should_exit)
 		return -1;
 
-	/* Will return with queues locked */
+	/* get a work item and queue up a write request */
 	if ((work = create_work_item()) == NULL) 
-		return -1; /* queues unlocked on failure */
+		return -1;
 
 	work->func = dm_write_func;
 	work->write_params.item = item;
@@ -465,12 +501,9 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
 	work->write_params.persistence = persistence;
 	work->write_params.buf = buf;
 	work->write_params.count = count;
-	enqueue_work_item(work);
 
-	sem_wait(&work->wait_sem);
-	ssize_t result = work->result;
-	destroy_work_item(work);
-	return result;
+	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
+	return (ssize_t)enqueue_work_item_and_wait_for_result(work);
 }
 
 /* Retrieve from the data manager file */
@@ -479,24 +512,22 @@ dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
 {
 	work_q_item_t *work;
 
+	/* Make sure data manager has been started and is not shutting down */
 	if ((g_fd < 0) || g_task_should_exit)
 		return -1;
 
-	/* Will return with queues locked */
+	/* get a work item and queue up a read request */
 	if ((work = create_work_item()) == NULL)
-		return -1; /* queues unlocked on failure */
+		return -1;
 
 	work->func = dm_read_func;
 	work->read_params.item = item;
 	work->read_params.index = index;
 	work->read_params.buf = buf;
 	work->read_params.count = count;
-	enqueue_work_item(work);
 
-	sem_wait(&work->wait_sem);
-	ssize_t result = work->result;
-	destroy_work_item(work);
-	return result;
+	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
+	return (ssize_t)enqueue_work_item_and_wait_for_result(work);
 }
 
 __EXPORT int
@@ -504,21 +535,19 @@ dm_clear(dm_item_t item)
 {
 	work_q_item_t *work;
 
+	/* Make sure data manager has been started and is not shutting down */
 	if ((g_fd < 0) || g_task_should_exit)
 		return -1;
 
-	/* Will return with queues locked */
+	/* get a work item and queue up a clear request */
 	if ((work = create_work_item()) == NULL)
-		return -1; /* queues unlocked on failure */
+		return -1;
 
 	work->func = dm_clear_func;
 	work->clear_params.item = item;
-	enqueue_work_item(work);
 
-	sem_wait(&work->wait_sem);
-	int result = work->result;
-	destroy_work_item(work);
-	return result;
+	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
+	return enqueue_work_item_and_wait_for_result(work);
 }
 
 /* Tell the data manager about the type of the last reset */
@@ -527,21 +556,19 @@ dm_restart(dm_reset_reason reason)
 {
 	work_q_item_t *work;
 
+	/* Make sure data manager has been started and is not shutting down */
 	if ((g_fd < 0) || g_task_should_exit)
 		return -1;
 
-	/* Will return with queues locked */
+	/* get a work item and queue up a restart request */
 	if ((work = create_work_item()) == NULL)
-		return -1; /* queues unlocked on failure */
+		return -1;
 
 	work->func = dm_restart_func;
 	work->restart_params.reason = reason;
-	enqueue_work_item(work);
 
-	sem_wait(&work->wait_sem);
-	int result = work->result;
-	destroy_work_item(work);
-	return result;
+	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
+	return enqueue_work_item_and_wait_for_result(work);
 }
 
 static int
@@ -570,24 +597,29 @@ task_main(int argc, char *argv[])
 
 	sem_init(&g_work_queued_sema, 1, 0);
 
+	/* Open or create the data manager file */
 	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
 	if (g_task_fd < 0) {
 		warnx("Could not open data manager file %s", k_data_manager_device_path);
-		sem_post(&g_init_sema);
+		sem_post(&g_init_sema); /* Don't want to hang startup */
 		return -1;
 	}
 	if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
 		close(g_task_fd);
 		warnx("Could not seek data manager file %s", k_data_manager_device_path);
-		sem_post(&g_init_sema);
+		sem_post(&g_init_sema); /* Don't want to hang startup */
 		return -1;
 	}
 	fsync(g_task_fd);
 
+	/* We use two file descriptors, one for the caller context and one for the worker thread */
+	/* They are actually the same but we need to some way to reject caller request while the */
+	/* worker thread is shutting down but still processing requests */
 	g_fd = g_task_fd;
 
 	warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset);
 
+	/* Tell startup that the worker thread has completed its initialization */
 	sem_post(&g_init_sema);
 
 	/* Start the endless loop, waiting for then processing work requests */
@@ -595,7 +627,7 @@ task_main(int argc, char *argv[])
 
 		/* do we need to exit ??? */
 		if ((g_task_should_exit) && (g_fd >= 0)) {
-			/* Close the file handle to stop further queueing */
+			/* Close the file handle to stop further queuing */
 			g_fd = -1;
 		}
 
@@ -607,6 +639,7 @@ task_main(int argc, char *argv[])
 		/* Empty the work queue */
 		while ((work = dequeue_work_item())) {
 
+			/* handle each work item with the appropriate handler */
 			switch (work->func) {
 			case dm_write_func:
 				g_func_counts[dm_write_func]++;
@@ -647,7 +680,7 @@ task_main(int argc, char *argv[])
 	close(g_task_fd);
 	g_task_fd = -1;
 
-	/* Empty the work queue */
+	/* The work queue is now empty, empty the free queue */
 	for (;;) {
 		if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
 			break;
@@ -669,7 +702,7 @@ start(void)
 
 	sem_init(&g_init_sema, 1, 0);
 
-	/* start the task */
+	/* start the worker thread */
 	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) {
 		warn("task start failed");
 		return -1;
@@ -704,7 +737,7 @@ stop(void)
 static void
 usage(void)
 {
-	errx(1, "usage: dataman {start|stop|status}");
+	errx(1, "usage: dataman {start|stop|status|poweronrestart|inflightrestart}");
 }
 
 int
@@ -726,6 +759,7 @@ dataman_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	/* Worker thread should be running for all other commands */
 	if (g_fd < 0)
 		errx(1, "not running");
 
@@ -733,6 +767,10 @@ dataman_main(int argc, char *argv[])
 		stop();
 	else if (!strcmp(argv[1], "status"))
 		status();
+    else if (!strcmp(argv[1], "poweronrestart"))
+        dm_restart(DM_INIT_REASON_POWER_ON);
+    else if (!strcmp(argv[1], "inflightrestart"))
+        dm_restart(DM_INIT_REASON_IN_FLIGHT);
 	else
 		usage();
 

src/modules/fixedwing_backside/fixedwing.cpp

@@ -174,9 +174,9 @@ void BlockMultiModeBacksideAutopilot::update()
 		// of control we will limit the velocity feedback between
 		// the min/max velocity
 		float v = _vLimit.update(sqrtf(
-					_pos.vx * _pos.vx +
+					_pos.vel_n * _pos.vel_n +
 					_pos.vy * _pos.vy +
-					_pos.vz * _pos.vz));
+					_pos.vel_d * _pos.vel_d));
 
 		// limit velocity command between min/max velocity
 		float vCmd = _vLimit.update(_vCmd.get());
@@ -236,9 +236,9 @@ void BlockMultiModeBacksideAutopilot::update()
 		// for the purpose of control we will limit the velocity feedback between
 		// the min/max velocity
 		float v = _vLimit.update(sqrtf(
-					_pos.vx * _pos.vx +
+					_pos.vel_n * _pos.vel_n +
 					_pos.vy * _pos.vy +
-					_pos.vz * _pos.vz));
+					_pos.vel_d * _pos.vel_d));
 
 		// pitch channel -> rate of climb
 		// TODO, might want to put a gain on this, otherwise commanding

src/modules/fixedwing_backside/fixedwing.hpp

@@ -264,7 +264,7 @@ private:
 	BlockParamFloat _crMax;
 
 	struct pollfd _attPoll;
-	mission_item_triplet_s _lastMissionCmd;
+	position_setpoint_triplet_s _lastMissionCmd;
 	enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
 	uint64_t _timeStamp;
 public:

src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c

@@ -299,7 +299,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
 					start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
 					global_sp_updated_set_once = true;
-					psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
+					psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon,
 							(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
 
 					printf("next wp direction: %0.4f\n", (double)psi_track);

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main()
 			}
 
 			/* Simple handling of failsafe: deploy parachute if failsafe is on */
-			if (_vcontrol_mode.flag_control_flighttermination_enabled) {
+			if (_vcontrol_mode.flag_control_termination_enabled) {
 				_actuators_airframe.control[1] = 1.0f;
 //				warnx("_actuators_airframe.control[1] = 1.0f;");
 			} else {
@@ -704,9 +704,9 @@ FixedwingAttitudeControl::task_main()
 				float speed_body_v = 0.0f;
 				float speed_body_w = 0.0f;
 				if(_att.R_valid) 	{
-					speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz;
-					speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz;
-					speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz;
+					speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
+					speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
+					speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
 				} else	{
 					warnx("Did not get a valid R\n");
 				}

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -68,7 +68,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
@@ -126,7 +126,7 @@ private:
 	int		_control_task;			/**< task handle for sensor task */
 
 	int		_global_pos_sub;
-	int		_mission_item_triplet_sub;
+	int		_pos_sp_triplet_sub;
 	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
@@ -145,7 +145,7 @@ private:
 	struct airspeed_s				_airspeed;		/**< airspeed */
 	struct vehicle_control_mode_s				_control_mode;		/**< vehicle status */
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct mission_item_triplet_s			_mission_item_triplet;		/**< triplet of mission items */
+	struct position_setpoint_triplet_s			_pos_sp_triplet;		/**< triplet of mission items */
 	struct sensor_combined_s				_sensor_combined;			/**< for body frame accelerations */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
@@ -236,7 +236,7 @@ private:
 		float land_slope_length;
 		float land_H1_virt;
 		float land_flare_alt_relative;
-		float land_thrust_lim_horizontal_distance;
+		float land_thrust_lim_alt_relative;
 		float land_heading_hold_horizontal_distance;
 
 	}		_parameters;			/**< local copies of interesting parameters */
@@ -281,7 +281,7 @@ private:
 		param_t land_slope_length;
 		param_t land_H1_virt;
 		param_t land_flare_alt_relative;
-		param_t land_thrust_lim_horizontal_distance;
+		param_t land_thrust_lim_alt_relative;
 		param_t land_heading_hold_horizontal_distance;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
@@ -332,10 +332,10 @@ private:
 	 * Control position.
 	 */
 	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
-					 const struct mission_item_triplet_s &_mission_item_triplet);
+					 const struct position_setpoint_triplet_s &_pos_sp_triplet);
 
 	float calculate_target_airspeed(float airspeed_demand);
-	void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet);
+	void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet);
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -367,7 +367,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 /* subscriptions */
 	_global_pos_sub(-1),
-	_mission_item_triplet_sub(-1),
+	_pos_sp_triplet_sub(-1),
 	_att_sub(-1),
 	_airspeed_sub(-1),
 	_control_mode_sub(-1),
@@ -406,7 +406,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	airspeed_s				_airspeed = {0};
 	vehicle_control_mode_s			_control_mode = {0};
 	vehicle_global_position_s		_global_pos = {0};
-	mission_item_triplet_s			_mission_item_triplet = {0};
+	position_setpoint_triplet_s			_pos_sp_triplet = {0};
 	sensor_combined_s				_sensor_combined = {0};
 
 
@@ -434,7 +434,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_slope_length = param_find("FW_LND_SLLR");
 	_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
 	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
-	_parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST");
+	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
 	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
@@ -523,7 +523,7 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
 	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
 	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
-	param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance));
+	param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
 	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
 
 	_l1_control.set_l1_damping(_parameters.l1_damping);
@@ -558,7 +558,7 @@ FixedwingPositionControl::parameters_update()
 	}
 
 	/* Update the landing slope */
-	landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt);
+	landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
 
 	/* Update and publish the navigation capabilities */
 	_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
@@ -653,11 +653,11 @@ void
 FixedwingPositionControl::vehicle_setpoint_poll()
 {
 	/* check if there is a new setpoint */
-	bool mission_item_triplet_updated;
-	orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated);
+	bool pos_sp_triplet_updated;
+	orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
 
-	if (mission_item_triplet_updated) {
-		orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet);
+	if (pos_sp_triplet_updated) {
+		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
 		_setpoint_valid = true;
 	}
 }
@@ -700,7 +700,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
 }
 
 void
-FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet)
+FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
 	if (_global_pos_valid) {
@@ -713,12 +713,12 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
 		/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
 		float distance = 0.0f;
 		float delta_altitude = 0.0f;
-		if (mission_item_triplet.previous_valid) {
-			distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon);
-			delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude;
+		if (pos_sp_triplet.previous.valid) {
+			distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+			delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt;
 		} else {
-			distance = get_distance_to_next_waypoint(current_position(0), current_position(1), mission_item_triplet.current.lat, mission_item_triplet.current.lon);
-			delta_altitude = mission_item_triplet.current.altitude -  _global_pos.alt;
+			distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+			delta_altitude = pos_sp_triplet.current.alt -  _global_pos.alt;
 		}
 
 		float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
@@ -751,11 +751,11 @@ void FixedwingPositionControl::navigation_capabilities_publish()
 
 bool
 FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
-		const struct mission_item_triplet_s &mission_item_triplet)
+		const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 	bool setpoint = true;
 
-	calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet);
+	calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet);
 
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
@@ -767,7 +767,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
 	_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
-	float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt;
+	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
 
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
@@ -785,58 +785,58 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_tecs.set_speed_weight(_parameters.speed_weight);
 
 		/* current waypoint (the one currently heading for) */
-		math::Vector<2> next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
+		math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
 		/* current waypoint (the one currently heading for) */
-		math::Vector<2> curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
+		math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
 
 		/* previous waypoint */
 		math::Vector<2> prev_wp;
 
-		if (mission_item_triplet.previous_valid) {
-			prev_wp(0) = mission_item_triplet.previous.lat;
-			prev_wp(1) = mission_item_triplet.previous.lon;
+		if (pos_sp_triplet.previous.valid) {
+			prev_wp(0) = (float)pos_sp_triplet.previous.lat;
+			prev_wp(1) = (float)pos_sp_triplet.previous.lon;
 
 		} else {
 			/*
 			 * No valid previous waypoint, go for the current wp.
 			 * This is automatically handled by the L1 library.
 			 */
-			prev_wp(0) = mission_item_triplet.current.lat;
-			prev_wp(1) = mission_item_triplet.current.lon;
+			prev_wp(0) = (float)pos_sp_triplet.current.lat;
+			prev_wp(1) = (float)pos_sp_triplet.current.lon;
 
 		}
 
-		if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+		if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
 			/* waypoint is a plain navigation waypoint */
 			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-			_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+			_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
 						    _airspeed.indicated_airspeed_m_s, eas2tas,
 						    false, math::radians(_parameters.pitch_limit_min),
 						    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 						    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
-		} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-			   mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-			   mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
 
 			/* waypoint is a loiter waypoint */
-			_l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius,
-						  mission_item_triplet.current.loiter_direction, ground_speed);
+			_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
+						  pos_sp_triplet.current.loiter_direction, ground_speed);
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-			_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+			_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
 						    _airspeed.indicated_airspeed_m_s, eas2tas,
 						    false, math::radians(_parameters.pitch_limit_min),
 						    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 						    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
-		} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
+		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+
+			float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
 
 			/* Horizontal landing control */
 			/* switch to heading hold for the last meters, continue heading hold after */
@@ -847,8 +847,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* heading hold, along the line connecting this and the last waypoint */
 
 				if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
-					if (mission_item_triplet.previous_valid) {
-						target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
+					if (pos_sp_triplet.previous.valid) {
+						target_bearing = bearing_lastwp_currwp;
 					} else {
 						target_bearing = _att.yaw;
 					}
@@ -879,23 +879,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 //				/* do not go down too early */
 //				if (wp_distance > 50.0f) {
-//					altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
+//					altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
 //				}
 			/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
 			// XXX this could make a great param
 
-			float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1)
+			float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
 			float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
 			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
-			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
-			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
+			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
+			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
 
-
-
-			if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
+			if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 
 				/* land with minimal speed */
 
@@ -905,7 +904,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* kill the throttle if param requests it */
 				throttle_max = _parameters.throttle_max;
 
-				 if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) {
+				 if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 					if (!land_motor_lim) {
 						land_motor_lim  = true;
@@ -914,12 +913,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 				 }
 
-				float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude);
+				float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
 
 				/* avoid climbout */
 				if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
 				{
-					flare_curve_alt = mission_item_triplet.current.altitude;
+					flare_curve_alt = pos_sp_triplet.current.alt;
 					land_stayonground = true;
 				}
 
@@ -977,7 +976,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 			}
 
-		} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
 
 			/* Perform launch detection */
 //			warnx("Launch detection running");
@@ -1011,9 +1010,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				if (altitude_error > 15.0f) {
 
 					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
 									_airspeed.indicated_airspeed_m_s, eas2tas,
-									true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)),
+									true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)),
 									_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 									math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
@@ -1022,7 +1021,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 				} else {
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
 									_airspeed.indicated_airspeed_m_s, eas2tas,
 									false, math::radians(_parameters.pitch_limit_min),
 									_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -1037,14 +1036,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
 		//       (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
 		// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
-		//       (double)next_wp(0), (double)next_wp(1), (mission_item_triplet.previous_valid) ? "valid" : "invalid");
+		//       (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid");
 
 		// XXX at this point we always want no loiter hold if a
 		// mission is active
 		_loiter_hold = false;
 
 		/* reset land state */
-		if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) {
+		if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
 			land_noreturn_horizontal = false;
 			land_noreturn_vertical = false;
 			land_stayonground = false;
@@ -1053,7 +1052,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		}
 
 		/* reset takeoff/launch state */
-		if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
+		if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
 			launch_detected = false;
 			usePreTakeoffThrust = false;
 		}
@@ -1176,7 +1175,7 @@ FixedwingPositionControl::task_main()
 	 * do subscriptions
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
+	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -1264,14 +1263,14 @@ FixedwingPositionControl::task_main()
 			vehicle_airspeed_poll();
 			// vehicle_baro_poll();
 
-			math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy);
-			math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
+			math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
+			math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
 
 			/*
 			 * Attempt to control position, on success (= sensors present and not in manual mode),
 			 * publish setpoint.
 			 */
-			if (control_position(current_position, ground_speed, _mission_item_triplet)) {
+			if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
 				_att_sp.timestamp = hrt_absolute_time();
 
 				/* lazily publish the setpoint only once available */
@@ -1285,7 +1284,7 @@ FixedwingPositionControl::task_main()
 				}
 
 				/* XXX check if radius makes sense here */
-				float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius);
+				float turn_distance = _l1_control.switch_distance(100.0f);
 
 				/* lazily publish navigation capabilities */
 				if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -168,9 +168,9 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
 PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
 PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
 
-PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
+PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
 PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
 PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
 PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
-PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
+PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
 PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);

src/modules/fw_pos_control_l1/landingslope.cpp

@@ -48,13 +48,13 @@
 
 void Landingslope::update(float landing_slope_angle_rad,
 		float flare_relative_alt,
-		float motor_lim_horizontal_distance,
+		float motor_lim_relative_alt,
 		float H1_virt)
 {
 
 	_landing_slope_angle_rad = landing_slope_angle_rad;
 	_flare_relative_alt = flare_relative_alt;
-	_motor_lim_horizontal_distance = motor_lim_horizontal_distance;
+	_motor_lim_relative_alt = motor_lim_relative_alt;
 	_H1_virt = H1_virt;
 
 	calculateSlopeValues();
@@ -74,9 +74,21 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_
 	return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
 }
 
-float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude)
+float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
 {
-	return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
-
+	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+		return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude);
+	else
+		return wp_altitude;
+}
+
+float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
+{
+	/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+		return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
+	else
+		return wp_landing_altitude;
 }
 

src/modules/fw_pos_control_l1/landingslope.h

@@ -49,7 +49,7 @@ private:
 	/* see Documentation/fw_landing.png for a plot of the landing slope */
 	float _landing_slope_angle_rad;			/**< phi in the plot */
 	float _flare_relative_alt;				/**< h_flare,rel in the plot */
-	float _motor_lim_horizontal_distance;
+	float _motor_lim_relative_alt;
 	float _H1_virt;							/**< H1 in the plot */
 	float _H0;								/**< h_flare,rel + H1 in the plot */
 	float _d1;								/**< d1 in the plot */
@@ -63,7 +63,18 @@ public:
 	Landingslope() {}
 	~Landingslope() {}
 
-	float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude);
+	/**
+	 *
+	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
+
+	/**
+	 *
+	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 * Performs check if aircraft is in front of waypoint to avoid climbout
+	 */
+	float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude);
 
 	/**
 	 *
@@ -85,17 +96,17 @@ public:
 	}
 
 
-	float getFlareCurveAltitude(float wp_distance, float wp_altitude);
+	float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
 
 	void update(float landing_slope_angle_rad,
 			float flare_relative_alt,
-			float motor_lim_horizontal_distance,
+			float motor_lim_relative_alt,
 			float H1_virt);
 
 
 	inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}
 	inline float flare_relative_alt() {return _flare_relative_alt;}
-	inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;}
+	inline float motor_lim_relative_alt() {return _motor_lim_relative_alt;}
 	inline float H1_virt() {return _H1_virt;}
 	inline float H0() {return _H0;}
 	inline float d1() {return _d1;}

src/modules/mavlink/mavlink.c

@@ -199,8 +199,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 	}
 
 	/* arming state */
-	if (v_status.arming_state == ARMING_STATE_ARMED
-			|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+	if (armed.armed) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
@@ -208,31 +207,40 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
 	union px4_custom_mode custom_mode;
 	custom_mode.data = 0;
-	if (v_status.main_state == MAIN_STATE_MANUAL) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
-	} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
-	} else if (v_status.main_state == MAIN_STATE_EASY) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
-	} else if (v_status.main_state == MAIN_STATE_AUTO) {
+	if (pos_sp_triplet.nav_state == NAV_STATE_NONE) {
+		/* use main state when navigator is not active */
+		if (v_status.main_state == MAIN_STATE_MANUAL) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+		} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+		} else if (v_status.main_state == MAIN_STATE_EASY) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+		} else if (v_status.main_state == MAIN_STATE_AUTO) {
+			/* this must not happen */
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		} else if (v_status.main_state == MAIN_STATE_OFFBOARD) {
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
+		}
+	} else {
+		/* use navigation state when navigator is active */
 		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-		if (control_mode.nav_state == NAV_STATE_NONE) {	// failsafe, shouldn't happen
+		if (pos_sp_triplet.nav_state == NAV_STATE_READY) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		} else if (control_mode.nav_state == NAV_STATE_READY) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		} else if (control_mode.nav_state == NAV_STATE_LOITER) {
+		} else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-		} else if (control_mode.nav_state == NAV_STATE_MISSION) {
+		} else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-		} else if (control_mode.nav_state == NAV_STATE_RTL) {
+		} else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+		} else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 		}
-	} else if (v_status.main_state == MAIN_STATE_OFFBOARD) {
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
 	}
 	*mavlink_custom_mode = custom_mode.data;
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -379,7 +379,7 @@ handle_message(mavlink_message_t *msg)
 		tstatus.rxerrors = rstatus.rxerrors;
 		tstatus.fixed = rstatus.fixed;
 
-		if (telemetry_status_pub == 0) {
+		if (telemetry_status_pub <= 0) {
 			telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
 
 		} else {
@@ -662,13 +662,13 @@ handle_message(mavlink_message_t *msg)
 				orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
 				// global position packet
 				hil_global_pos.timestamp = timestamp;
-				hil_global_pos.valid = true;
+				hil_global_pos.global_valid = true;
 				hil_global_pos.lat = hil_state.lat;
 				hil_global_pos.lon = hil_state.lon;
 				hil_global_pos.alt = hil_state.alt / 1000.0f;
-				hil_global_pos.vx = hil_state.vx / 100.0f;
-				hil_global_pos.vy = hil_state.vy / 100.0f;
-				hil_global_pos.vz = hil_state.vz / 100.0f;
+				hil_global_pos.vel_n = hil_state.vx / 100.0f;
+				hil_global_pos.vel_e = hil_state.vy / 100.0f;
+				hil_global_pos.vel_d = hil_state.vz / 100.0f;
 
 			} else {
 				pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);

src/modules/mavlink/orb_listener.c

@@ -67,9 +67,10 @@ extern bool gcs_link;
 
 struct vehicle_global_position_s global_pos;
 struct vehicle_local_position_s local_pos;
+struct home_position_s home;
 struct navigation_capabilities_s nav_cap;
 struct vehicle_status_s v_status;
-struct vehicle_control_mode_s control_mode;
+struct position_setpoint_triplet_s pos_sp_triplet;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
@@ -126,7 +127,6 @@ static void	l_vehicle_rates_setpoint(const struct listener *l);
 static void	l_home(const struct listener *l);
 static void	l_airspeed(const struct listener *l);
 static void	l_nav_cap(const struct listener *l);
-static void	l_control_mode(const struct listener *l);
 
 static const struct listener listeners[] = {
 	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
@@ -153,7 +153,6 @@ static const struct listener listeners[] = {
 	{l_home,			&mavlink_subs.home_sub,		0},
 	{l_airspeed,			&mavlink_subs.airspeed_sub,		0},
 	{l_nav_cap,			&mavlink_subs.navigation_capabilities_sub,		0},
-	{l_control_mode,			&mavlink_subs.control_mode_sub,		0},
 };
 
 static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -247,10 +246,10 @@ l_vehicle_attitude(const struct listener *l)
 		hrt_abstime t = hrt_absolute_time();
 		if (t >= last_sent_vfr + 100000) {
 			last_sent_vfr = t;
-			float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
+			float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
 			uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
 			float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
-			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
+			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d);
 		}
 		
 		/* send quaternion values if it exists */
@@ -314,6 +313,7 @@ l_vehicle_status(const struct listener *l)
 	/* immediately communicate state changes back to user */
 	orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
 	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
+	orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.position_setpoint_triplet_sub, &pos_sp_triplet);
 
 	/* enable or disable HIL */
 	if (v_status.hil_state == HIL_STATE_ON)
@@ -354,10 +354,10 @@ l_input_rc(const struct listener *l)
 
 		const unsigned port_width = 8;
 
-		for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) {
+		for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) {
 			/* Channels are sent in MAVLink main loop at a fixed interval */
 			mavlink_msg_rc_channels_raw_send(chan,
-							 rc_raw.timestamp / 1000,
+							 rc_raw.timestamp_publication / 1000,
 							 i,
 							 (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
 							 (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
@@ -380,13 +380,13 @@ l_global_position(const struct listener *l)
 
 	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
 						 global_pos.timestamp / 1000,
-					     global_pos.lat,
-					     global_pos.lon,
+					     global_pos.lat * 1e7,
+					     global_pos.lon * 1e7,
 					     global_pos.alt * 1000.0f,
-					     global_pos.relative_alt * 1000.0f,
-					     global_pos.vx * 100.0f,
-					     global_pos.vy * 100.0f,
-					     global_pos.vz * 100.0f,
+					     (global_pos.alt - home.alt) * 1000.0f,
+					     global_pos.vel_n * 100.0f,
+					     global_pos.vel_e * 100.0f,
+					     global_pos.vel_d * 100.0f,
 					     _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
 }
 
@@ -410,23 +410,18 @@ l_local_position(const struct listener *l)
 void
 l_global_position_setpoint(const struct listener *l)
 {
-	struct mission_item_triplet_s triplet;
-	orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet);
+	struct position_setpoint_triplet_s triplet;
+	orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet);
 
-	uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
-	
-	if (!triplet.current_valid)
+	if (!triplet.current.valid)
 		return;
 
-	if (triplet.current.altitude_is_relative)
-		coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
-
 	if (gcs_link)
 		mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
-				coordinate_frame,
+				MAV_FRAME_GLOBAL,
 				(int32_t)(triplet.current.lat * 1e7d),
 				(int32_t)(triplet.current.lon * 1e7d),
-				(int32_t)(triplet.current.altitude * 1e3f),
+				(int32_t)(triplet.current.alt * 1e3f),
 				(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
 }
 
@@ -662,11 +657,9 @@ l_optical_flow(const struct listener *l)
 void
 l_home(const struct listener *l)
 {
-	struct home_position_s home;
-
 	orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
 
-	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f);
+	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
 }
 
 void
@@ -688,26 +681,6 @@ l_nav_cap(const struct listener *l)
 
 }
 
-void
-l_control_mode(const struct listener *l)
-{
-	orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode);
-
-	/* translate the current syste state to mavlink state and mode */
-	uint8_t mavlink_state = 0;
-	uint8_t mavlink_base_mode = 0;
-	uint32_t mavlink_custom_mode = 0;
-	get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
-	/* send heartbeat */
-	mavlink_msg_heartbeat_send(chan,
-				   mavlink_system.type,
-				   MAV_AUTOPILOT_PX4,
-				   mavlink_base_mode,
-				   mavlink_custom_mode,
-				   mavlink_state);
-}
-
 static void *
 uorb_receive_thread(void *arg)
 {
@@ -783,9 +756,9 @@ uorb_receive_start(void)
 	status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	orb_set_interval(status_sub, 300);		/* max 3.33 Hz updates */
 
-	/* --- CONTROL MODE --- */
-	mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	orb_set_interval(mavlink_subs.control_mode_sub, 300);		/* max 3.33 Hz updates */
+	/* --- POSITION SETPOINT TRIPLET --- */
+	mavlink_subs.position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	orb_set_interval(mavlink_subs.position_setpoint_triplet_sub, 0);		/* not polled, don't limit */
 
 	/* --- RC CHANNELS VALUE --- */
 	rc_sub = orb_subscribe(ORB_ID(rc_channels));
@@ -804,7 +777,7 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.local_pos_sub, 1000);	/* 1Hz active updates */
 
 	/* --- GLOBAL SETPOINT VALUE --- */
-	mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
+	mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	orb_set_interval(mavlink_subs.triplet_sub, 2000);	/* 0.5 Hz updates */
 	
 	/* --- LOCAL SETPOINT VALUE --- */

src/modules/mavlink/orb_topics.h

@@ -51,9 +51,8 @@
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -95,7 +94,7 @@ struct mavlink_subscriptions {
 	int home_sub;
 	int airspeed_sub;
 	int navigation_capabilities_sub;
-	int control_mode_sub;
+	int position_setpoint_triplet_sub;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;
@@ -112,8 +111,8 @@ extern struct navigation_capabilities_s nav_cap;
 /** Vehicle status */
 extern struct vehicle_status_s v_status;
 
-/** Vehicle control mode */
-extern struct vehicle_control_mode_s control_mode;
+/** Position setpoint triplet */
+extern struct position_setpoint_triplet_s pos_sp_triplet;
 
 /** RC channels */
 extern struct rc_channels_s rc;

src/modules/mavlink_onboard/orb_topics.h

@@ -50,7 +50,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>

src/modules/mc_att_control/mc_att_control_params.c

@@ -41,13 +41,16 @@
 
 #include <systemlib/param/param.h>
 
-PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f);
-PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
+PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
 PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
-PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
+PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);

src/modules/navigator/navigator_mission.cpp

@@ -52,8 +52,8 @@
 static const int ERROR = -1;
 
 
-Mission::Mission() : 
-	
+Mission::Mission() :
+
 	_offboard_dataman_id(-1),
 	_current_offboard_mission_index(0),
 	_current_onboard_mission_index(0),
@@ -65,7 +65,7 @@ Mission::Mission() :
 
 Mission::~Mission()
 {
-	
+
 }
 
 void
@@ -126,33 +126,39 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool
 {
 	/* try onboard mission first */
 	if (current_onboard_mission_available()) {
-		
+
 		const ssize_t len = sizeof(struct mission_item_s);
+
 		if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			return ERROR;
 		}
+
 		_current_mission_type = MISSION_TYPE_ONBOARD;
 		*onboard = true;
 		*index = _current_onboard_mission_index;
 
-	/* otherwise fallback to offboard */
+		/* otherwise fallback to offboard */
+
 	} else if (current_offboard_mission_available()) {
 
 		dm_item_t dm_current;
 
 		if (_offboard_dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
 		} else {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
 
 		const ssize_t len = sizeof(struct mission_item_s);
+
 		if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			_current_mission_type = MISSION_TYPE_NONE;
 			return ERROR;
 		}
+
 		_current_mission_type = MISSION_TYPE_OFFBOARD;
 		*onboard = false;
 		*index = _current_offboard_mission_index;
@@ -171,25 +177,29 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
 {
 	/* try onboard mission first */
 	if (next_onboard_mission_available()) {
-		
+
 		const ssize_t len = sizeof(struct mission_item_s);
+
 		if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			return ERROR;
 		}
 
-	/* otherwise fallback to offboard */
+		/* otherwise fallback to offboard */
+
 	} else if (next_offboard_mission_available()) {
 
 		dm_item_t dm_current;
 
 		if (_offboard_dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
 		} else {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
 
 		const ssize_t len = sizeof(struct mission_item_s);
+
 		if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			return ERROR;
@@ -244,14 +254,16 @@ void
 Mission::move_to_next()
 {
 	switch (_current_mission_type) {
-		case MISSION_TYPE_ONBOARD:
-			_current_onboard_mission_index++;
-			break;
-		case MISSION_TYPE_OFFBOARD:
-			_current_offboard_mission_index++;
-			break;
-		case MISSION_TYPE_NONE:
-		default:
-			break;
+	case MISSION_TYPE_ONBOARD:
+		_current_onboard_mission_index++;
+		break;
+
+	case MISSION_TYPE_OFFBOARD:
+		_current_offboard_mission_index++;
+		break;
+
+	case MISSION_TYPE_NONE:
+	default:
+		break;
 	}
 }

src/modules/navigator/navigator_params.c

@@ -55,8 +55,10 @@
 
 PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
 PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
-PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f);
+PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
 PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
 PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);	// default TAKEOFF altitude
 PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);		// slow descend from this altitude when landing
 PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);		// min altitude for going home in RTL mode
+PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);	// delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT
+PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);	// enable parachute deployment

src/modules/navigator/navigator_state.h

@@ -0,0 +1,21 @@
+/*
+ * navigator_state.h
+ *
+ *  Created on: 27.01.2014
+ *      Author: ton
+ */
+
+#ifndef NAVIGATOR_STATE_H_
+#define NAVIGATOR_STATE_H_
+
+typedef enum {
+	NAV_STATE_NONE = 0,
+	NAV_STATE_READY,
+	NAV_STATE_LOITER,
+	NAV_STATE_MISSION,
+	NAV_STATE_RTL,
+	NAV_STATE_LAND,
+	NAV_STATE_MAX
+} nav_state_t;
+
+#endif /* NAVIGATOR_STATE_H_ */

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -202,8 +202,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	bool landed = true;
 	hrt_abstime landed_time = 0;
 
-	bool flag_armed = false;
-
 	uint32_t accel_counter = 0;
 	uint32_t baro_counter = 0;
 
@@ -329,6 +327,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
+						global_pos.baro_valid = true;
 					}
 				}
 			}
@@ -379,17 +378,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			if (updated) {
 				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-
-				/* reset ground level on arm */
-				if (armed.armed && !flag_armed) {
-					flag_armed = armed.armed;
-					baro_offset -= z_est[0];
-					corr_baro = 0.0f;
-					local_pos.ref_alt -= z_est[0];
-					local_pos.ref_timestamp = t;
-					z_est[0] = 0.0f;
-					alt_avg = 0.0f;
-				}
 			}
 
 			/* sensor combined */
@@ -539,13 +527,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				if (gps.fix_type >= 3) {
 					/* hysteresis for GPS quality */
 					if (gps_valid) {
-						if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) {
+						if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) {
 							gps_valid = false;
 							mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
 						}
 
 					} else {
-						if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) {
+						if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) {
 							gps_valid = true;
 							mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
 						}
@@ -601,8 +589,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							corr_gps[2][1] = 0.0f;
 						}
 
-						w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m);
-						w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m);
+						w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m);
+						w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m);
 					}
 
 				} else {
@@ -637,6 +625,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		}
 
 		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
+		dt = fmaxf(fminf(0.02, dt), 0.005);
 		t_prev = t;
 
 		/* use GPS if it's valid and reference position initialized */
@@ -679,7 +668,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		if (use_gps_z) {
 			float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
 			baro_offset += offs_corr;
-			baro_counter += offs_corr;
+			corr_baro += offs_corr;
 		}
 
 		/* accelerometer bias correction */
@@ -835,32 +824,32 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
 
 			/* publish global position */
-			global_pos.valid = local_pos.xy_global;
+			global_pos.global_valid = local_pos.xy_global;
 
 			if (local_pos.xy_global) {
 				double est_lat, est_lon;
 				map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
-				global_pos.lat = (int32_t)(est_lat * 1e7d);
-				global_pos.lon = (int32_t)(est_lon * 1e7d);
+				global_pos.lat = est_lat;
+				global_pos.lon = est_lon;
 				global_pos.time_gps_usec = gps.time_gps_usec;
 			}
 
 			/* set valid values even if position is not valid */
 			if (local_pos.v_xy_valid) {
-				global_pos.vx = local_pos.vx;
-				global_pos.vy = local_pos.vy;
-			}
-
-			if (local_pos.z_valid) {
-				global_pos.relative_alt = -local_pos.z;
+				global_pos.vel_n = local_pos.vx;
+				global_pos.vel_e = local_pos.vy;
 			}
 
 			if (local_pos.z_global) {
 				global_pos.alt = local_pos.ref_alt - local_pos.z;
 			}
 
+			if (local_pos.z_valid) {
+				global_pos.baro_alt = baro_offset - local_pos.z;
+			}
+
 			if (local_pos.v_z_valid) {
-				global_pos.vz = local_pos.vz;
+				global_pos.vel_d = local_pos.vz;
 			}
 
 			global_pos.yaw = local_pos.yaw;

src/modules/position_estimator_inav/position_estimator_inav_params.c

@@ -40,7 +40,7 @@
 
 #include "position_estimator_inav_params.h"
 
-PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
 PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f);
 PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);

src/modules/px4iofirmware/adc.c

@@ -83,6 +83,14 @@ adc_init(void)
 {
 	adc_perf = perf_alloc(PC_ELAPSED, "adc");
 
+	/* put the ADC into power-down mode */
+	rCR2 &= ~ADC_CR2_ADON;
+	up_udelay(10);
+
+	/* bring the ADC out of power-down mode */
+	rCR2 |= ADC_CR2_ADON;
+	up_udelay(10);
+
 	/* do calibration if supported */
 #ifdef ADC_CR2_CAL
 	rCR2 |= ADC_CR2_RSTCAL;
@@ -96,41 +104,25 @@ adc_init(void)
 
 	if (rCR2 & ADC_CR2_CAL)
 		return -1;
-
 #endif
 
-	/* arbitrarily configure all channels for 55 cycle sample time */
-	rSMPR1 = 0b00000011011011011011011011011011;
+	/*
+	 * Configure sampling time.
+	 * 
+	 * For electrical protection reasons, we want to be able to have
+	 * 10K in series with ADC inputs that leave the board. At 12MHz this
+	 * means we need 28.5 cycles of sampling time (per table 43 in the
+	 * datasheet).
+	 */
+	rSMPR1 = 0b00000000011011011011011011011011;
 	rSMPR2 = 0b00011011011011011011011011011011;
 
-	/* XXX for F2/4, might want to select 12-bit mode? */
-	rCR1 = 0;
-
-	/* enable the temperature sensor / Vrefint channel if supported*/
-	rCR2 =
-#ifdef ADC_CR2_TSVREFE
-		/* enable the temperature sensor in CR2 */
-		ADC_CR2_TSVREFE |
-#endif
-		0;
-
-#ifdef ADC_CCR_TSVREFE
-	/* enable temperature sensor in CCR */
-	rCCR = ADC_CCR_TSVREFE;
-#endif
+	rCR2 |=	ADC_CR2_TSVREFE;		/* enable the temperature sensor / Vrefint channel */
 
 	/* configure for a single-channel sequence */
 	rSQR1 = 0;
 	rSQR2 = 0;
-	rSQR3 = 0;	/* will be updated with the channel each tick */
-
-	/* power-cycle the ADC and turn it on */
-	rCR2 &= ~ADC_CR2_ADON;
-	up_udelay(10);
-	rCR2 |= ADC_CR2_ADON;
-	up_udelay(10);
-	rCR2 |= ADC_CR2_ADON;
-	up_udelay(10);
+	rSQR3 = 0;	/* will be updated with the channel at conversion time */
 
 	return 0;
 }
@@ -141,11 +133,12 @@ adc_init(void)
 uint16_t
 adc_measure(unsigned channel)
 {
+
 	perf_begin(adc_perf);
 
 	/* clear any previous EOC */
-	if (rSR & ADC_SR_EOC)
-		rSR &= ~ADC_SR_EOC;
+	rSR = 0;
+	(void)rDR;
 
 	/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
 	rSQR3 = channel;
@@ -158,7 +151,6 @@ adc_measure(unsigned channel)
 
 		/* never spin forever - this will give a bogus result though */
 		if (hrt_elapsed_time(&now) > 100) {
-			debug("adc timeout");
 			perf_end(adc_perf);
 			return 0xffff;
 		}
@@ -166,6 +158,7 @@ adc_measure(unsigned channel)
 
 	/* read the result and clear EOC */
 	uint16_t result = rDR;
+	rSR = 0;
 
 	perf_end(adc_perf);
 	return result;

src/modules/px4iofirmware/controls.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -59,6 +59,11 @@ static perf_counter_t c_gather_ppm;
 void
 controls_init(void)
 {
+	/* no channels */
+	r_raw_rc_count = 0;
+	system_state.rc_channels_timestamp_received = 0;
+	system_state.rc_channels_timestamp_valid = 0;
+
 	/* DSM input (USART1) */
 	dsm_init("/dev/ttyS0");
 
@@ -97,26 +102,64 @@ controls_tick() {
 	/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
 	uint16_t rssi = 0;
 
+#ifdef ADC_RSSI
+	if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
+		unsigned counts = adc_measure(ADC_RSSI);
+		if (counts != 0xffff) {
+			/* use 1:1 scaling on 3.3V ADC input */
+			unsigned mV = counts * 3300 / 4096;
+
+			/* scale to 0..253 */
+			rssi = mV / 13;
+		}
+	}
+#endif
+
 	perf_begin(c_gather_dsm);
 	uint16_t temp_count = r_raw_rc_count;
 	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
 	if (dsm_updated) {
-		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
 		r_raw_rc_count = temp_count & 0x7fff;
 		if (temp_count & 0x8000)
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
 		else
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
+			r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 
-		rssi = 255;
 	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
-	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
+
+	bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
+
+	bool sbus_failsafe, sbus_frame_drop;
+	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
+
 	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
+
+		rssi = 255;
+
+		if (sbus_frame_drop) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
+			rssi = 100;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		}
+
+		if (sbus_failsafe) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+			rssi = 0;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+		}
+
 	}
+
 	perf_end(c_gather_sbus);
 
 	/*
@@ -125,13 +168,12 @@ controls_tick() {
 	 * disable the PPM decoder completely if we have S.bus signal.
 	 */
 	perf_begin(c_gather_ppm);
-	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
+	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
 	if (ppm_updated) {
 
-		/* XXX sample RSSI properly here */
-		rssi = 255;
-
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 	}
 	perf_end(c_gather_ppm);
 
@@ -139,6 +181,9 @@ controls_tick() {
 	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
 		r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
 
+	/* store RSSI */
+	r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
+
 	/*
 	 * In some cases we may have received a frame, but input has still
 	 * been lost.
@@ -150,97 +195,100 @@ controls_tick() {
 	 */
 	if (dsm_updated || sbus_updated || ppm_updated) {
 
-		/* update RC-received timestamp */
-		system_state.rc_channels_timestamp = hrt_absolute_time();
-
 		/* record a bitmask of channels assigned */
 		unsigned assigned_channels = 0;
 
-		/* map raw inputs to mapped inputs */
-		/* XXX mapping should be atomic relative to protocol */
-		for (unsigned i = 0; i < r_raw_rc_count; i++) {
+		/* update RC-received timestamp */
+		system_state.rc_channels_timestamp_received = hrt_absolute_time();
 
-			/* map the input channel */
-			uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
+		/* do not command anything in failsafe, kick in the RC loss counter */
+		if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
 
-			if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+			/* update RC-received timestamp */
+			system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
 
-				uint16_t raw = r_raw_rc_values[i];
+			/* map raw inputs to mapped inputs */
+			/* XXX mapping should be atomic relative to protocol */
+			for (unsigned i = 0; i < r_raw_rc_count; i++) {
 
-				int16_t scaled;
+				/* map the input channel */
+				uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
 
-				/*
-				 * 1) Constrain to min/max values, as later processing depends on bounds.
-				 */
-				if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
-					raw = conf[PX4IO_P_RC_CONFIG_MIN];
-				if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
-					raw = conf[PX4IO_P_RC_CONFIG_MAX];
+				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
 
-				/*
-				 * 2) Scale around the mid point differently for lower and upper range.
-				 *
-				 * This is necessary as they don't share the same endpoints and slope.
-				 *
-				 * First normalize to 0..1 range with correct sign (below or above center),
-				 * then scale to 20000 range (if center is an actual center, -10000..10000,
-				 * if parameters only support half range, scale to 10000 range, e.g. if
-				 * center == min 0..10000, if center == max -10000..0).
-				 *
-				 * As the min and max bounds were enforced in step 1), division by zero
-				 * cannot occur, as for the case of center == min or center == max the if
-				 * statement is mutually exclusive with the arithmetic NaN case.
-				 *
-				 * DO NOT REMOVE OR ALTER STEP 1!
-				 */
-				if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+					uint16_t raw = r_raw_rc_values[i];
 
-				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+					int16_t scaled;
 
-				} else {
-					/* in the configured dead zone, output zero */
-					scaled = 0;
-				}
+					/*
+					 * 1) Constrain to min/max values, as later processing depends on bounds.
+					 */
+					if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+						raw = conf[PX4IO_P_RC_CONFIG_MIN];
+					if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+						raw = conf[PX4IO_P_RC_CONFIG_MAX];
 
-				/* invert channel if requested */
-				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
-					scaled = -scaled;
+					/*
+					 * 2) Scale around the mid point differently for lower and upper range.
+					 *
+					 * This is necessary as they don't share the same endpoints and slope.
+					 *
+					 * First normalize to 0..1 range with correct sign (below or above center),
+					 * then scale to 20000 range (if center is an actual center, -10000..10000,
+					 * if parameters only support half range, scale to 10000 range, e.g. if
+					 * center == min 0..10000, if center == max -10000..0).
+					 *
+					 * As the min and max bounds were enforced in step 1), division by zero
+					 * cannot occur, as for the case of center == min or center == max the if
+					 * statement is mutually exclusive with the arithmetic NaN case.
+					 *
+					 * DO NOT REMOVE OR ALTER STEP 1!
+					 */
+					if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
 
-				/* and update the scaled/mapped version */
-				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-				if (mapped < PX4IO_CONTROL_CHANNELS) {
+					} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
 
-					/* invert channel if pitch - pulling the lever down means pitching up by convention */
-					if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
+					} else {
+						/* in the configured dead zone, output zero */
+						scaled = 0;
+					}
+
+					/* invert channel if requested */
+					if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
 						scaled = -scaled;
 
-					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-					assigned_channels |= (1 << mapped);
+					/* and update the scaled/mapped version */
+					unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+					if (mapped < PX4IO_CONTROL_CHANNELS) {
 
+						/* invert channel if pitch - pulling the lever down means pitching up by convention */
+						if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
+							scaled = -scaled;
+
+						r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+						assigned_channels |= (1 << mapped);
+
+					}
 				}
 			}
-		}
 
-		/* set un-assigned controls to zero */
-		for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
-			if (!(assigned_channels & (1 << i)))
-				r_rc_values[i] = 0;
-		}
+			/* set un-assigned controls to zero */
+			for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+				if (!(assigned_channels & (1 << i)))
+					r_rc_values[i] = 0;
+			}
 
-		/*
-		 * If we got an update with zero channels, treat it as 
-		 * a loss of input.
-		 *
-		 * This might happen if a protocol-based receiver returns an update
-		 * that contains no channels that we have mapped.
-		 */
-		if (assigned_channels == 0 || rssi == 0) {
-			rc_input_lost = true;
-		} else {
-			/* set RC OK flag */
+			/* set RC OK flag, as we got an update */
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+
+			/* if we have enough channels (5) to control the vehicle, the mapping is ok */
+			if (assigned_channels > 4) {
+				r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
+			} else {
+				r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
+			}
 		}
 
 		/*
@@ -253,7 +301,7 @@ controls_tick() {
 	 * If we haven't seen any new control data in 200ms, assume we
 	 * have lost input.
 	 */
-	if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
+	if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
 		rc_input_lost = true;
 
 		/* clear the input-kind flags here */
@@ -261,24 +309,32 @@ controls_tick() {
 			PX4IO_P_STATUS_FLAGS_RC_PPM |
 			PX4IO_P_STATUS_FLAGS_RC_DSM |
 			PX4IO_P_STATUS_FLAGS_RC_SBUS);
+
 	}
 
 	/*
 	 * Handle losing RC input
 	 */
-	if (rc_input_lost) {
 
+	/* this kicks in if the receiver is gone or the system went to failsafe */
+	if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
 		/* Clear the RC input status flag, clear manual override flag */
 		r_status_flags &= ~(
 			PX4IO_P_STATUS_FLAGS_OVERRIDE |
 			PX4IO_P_STATUS_FLAGS_RC_OK);
 
+		/* Mark all channels as invalid, as we just lost the RX */
+		r_rc_valid = 0;
+
 		/* Set the RC_LOST alarm */
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
+	}
 
-		/* Mark the arrays as empty */
+	/* this kicks in if the receiver is completely gone */
+	if (rc_input_lost) {
+
+		/* Set channel count to zero */
 		r_raw_rc_count = 0;
-		r_rc_valid = 0;
 	}
 
 	/*

src/modules/px4iofirmware/mixer.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -71,6 +71,7 @@ extern "C" {
 static bool mixer_servos_armed = false;
 static bool should_arm = false;
 static bool should_always_enable_pwm = false;
+static volatile bool in_mixer = false;
 
 /* selected control values and count for mixing */
 enum mixer_source {
@@ -95,6 +96,7 @@ static void mixer_set_failsafe();
 void
 mixer_tick(void)
 {
+
 	/* check that we are receiving fresh data from the FMU */
 	if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
 
@@ -199,13 +201,17 @@ mixer_tick(void)
 		}
 
 
-	} else if (source != MIX_NONE) {
+	} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
 		float	outputs[PX4IO_SERVO_COUNT];
 		unsigned mixed;
 
 		/* mix */
+
+		/* poor mans mutex */
+		in_mixer = true;
 		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
+		in_mixer = false;
 
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
@@ -217,14 +223,25 @@ mixer_tick(void)
 		}
 	}
 
-	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
+	/* set arming */
+	bool needs_to_arm = (should_arm || should_always_enable_pwm);
+
+	/* check any conditions that prevent arming */
+	if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
+		needs_to_arm = false;
+	}
+	if (!should_arm && !should_always_enable_pwm) {
+		needs_to_arm = false;
+	}
+
+	if (needs_to_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
 		mixer_servos_armed = true;
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
 		isr_debug(5, "> PWM enabled");
 
-	} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
+	} else if (!needs_to_arm && mixer_servos_armed) {
 		/* armed but need to disarm */
 		up_pwm_servo_arm(false);
 		mixer_servos_armed = false;
@@ -297,12 +314,17 @@ mixer_callback(uintptr_t handle,
 static char mixer_text[256];		/* large enough for one mixer */
 static unsigned mixer_text_length = 0;
 
-void
+int
 mixer_handle_text(const void *buffer, size_t length)
 {
 	/* do not allow a mixer change while safety off */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
-		return;
+		return 1;
+	}
+
+	/* abort if we're in the mixer */
+	if (in_mixer) {
+		return 1;
 	}
 
 	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
@@ -310,7 +332,7 @@ mixer_handle_text(const void *buffer, size_t length)
 	isr_debug(2, "mix txt %u", length);
 
 	if (length < sizeof(px4io_mixdata))
-		return;
+		return 0;
 
 	unsigned	text_length = length - sizeof(px4io_mixdata);
 
@@ -328,13 +350,16 @@ mixer_handle_text(const void *buffer, size_t length)
 	case F2I_MIXER_ACTION_APPEND:
 		isr_debug(2, "append %d", length);
 
+		/* disable mixing during the update */
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
+
 		/* check for overflow - this would be really fatal */
 		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
-			return;
+			return 0;
 		}
 
-		/* append mixer text and nul-terminate */
+		/* append mixer text and nul-terminate, guard against overflow */
 		memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
 		mixer_text_length += text_length;
 		mixer_text[mixer_text_length] = '\0';
@@ -369,6 +394,8 @@ mixer_handle_text(const void *buffer, size_t length)
 
 		break;
 	}
+
+	return 0;
 }
 
 static void

src/modules/px4iofirmware/protocol.h

@@ -111,7 +111,6 @@
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
 #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
-#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 13) /* DSM input is 11 bit data */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -128,8 +127,6 @@
 #define PX4IO_P_STATUS_VSERVO			6	/* [2] servo rail voltage in mV */
 #define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */
 #define PX4IO_P_STATUS_PRSSI			8	/* [2] RSSI PWM value */
-#define PX4IO_P_STATUS_NRSSI			9	/* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
-#define PX4IO_P_STATUS_RC_DATA			10	/* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
 
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -140,7 +137,17 @@
 /* array of raw RC input values, microseconds */
 #define PX4IO_PAGE_RAW_RC_INPUT		4
 #define PX4IO_P_RAW_RC_COUNT			0	/* number of valid channels */
-#define PX4IO_P_RAW_RC_BASE			1	/* CONFIG_RC_INPUT_COUNT channels from here */
+#define PX4IO_P_RAW_RC_FLAGS			1	/* RC detail status flags */
+#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP		(1 << 0) /* single frame drop */
+#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE		(1 << 1) /* receiver is in failsafe mode */
+#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11		(1 << 2) /* DSM decoding is 11 bit mode */
+#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK		(1 << 3) /* Channel mapping is ok */
+
+#define PX4IO_P_RAW_RC_NRSSI			2	/* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
+#define PX4IO_P_RAW_RC_DATA			3	/* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
+#define PX4IO_P_RAW_FRAME_COUNT			4	/* Number of total received frames (wrapping counter) */
+#define PX4IO_P_RAW_LOST_FRAME_COUNT		5	/* Number of total dropped frames (wrapping counter) */
+#define PX4IO_P_RAW_RC_BASE			6	/* CONFIG_RC_INPUT_COUNT channels from here */
 
 /* array of scaled RC input values, -10000..10000 */
 #define PX4IO_PAGE_RC_INPUT		5
@@ -157,6 +164,10 @@
 /* setup page */
 #define PX4IO_PAGE_SETUP		50
 #define PX4IO_P_SETUP_FEATURES			0
+#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /* enable S.Bus v1 output */
+#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /* enable S.Bus v2 output */
+#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /* enable PWM RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /* enable ADC RSSI parsing */
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
@@ -166,6 +177,7 @@
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
 #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
 #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
+#define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 7) /* If set, the system operates normally, but won't actuate any servos */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */

src/modules/px4iofirmware/px4io.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -196,6 +196,11 @@ user_start(int argc, char *argv[])
 	POWER_SERVO(true);
 #endif
 
+	/* turn off S.Bus out (if supported) */
+#ifdef ENABLE_SBUS_OUT
+	ENABLE_SBUS_OUT(false);
+#endif
+
 	/* start the safety switch handler */
 	safety_init();
 
@@ -205,6 +210,9 @@ user_start(int argc, char *argv[])
 	/* initialise the control inputs */
 	controls_init();
 
+	/* set up the ADC */
+	adc_init();
+
 	/* start the FMU interface */
 	interface_init();
 
@@ -223,24 +231,41 @@ user_start(int argc, char *argv[])
 	/* initialize PWM limit lib */
 	pwm_limit_init(&pwm_limit);
 
-#if 0
-	/* not enough memory, lock down */
-	if (minfo.mxordblk < 500) {
+	/*
+	 *    P O L I C E    L I G H T S
+	 *
+	 * Not enough memory, lock down.
+	 *
+	 * We might need to allocate mixers later, and this will
+	 * ensure that a developer doing a change will notice
+	 * that he just burned the remaining RAM with static
+	 * allocations. We don't want him to be able to
+	 * get past that point. This needs to be clearly
+	 * documented in the dev guide.
+         *
+	 */
+	if (minfo.mxordblk < 600) {
+
 		lowsyslog("ERR: not enough MEM");
 		bool phase = false;
 
-		if (phase) {
-			LED_AMBER(true);
-			LED_BLUE(false);
-		} else {
-			LED_AMBER(false);
-			LED_BLUE(true);
-		}
+		while (true) {
 
-		phase = !phase;
-		usleep(300000);
+			if (phase) {
+				LED_AMBER(true);
+				LED_BLUE(false);
+			} else {
+				LED_AMBER(false);
+				LED_BLUE(true);
+			}
+			up_udelay(250000);
+
+			phase = !phase;
+		}
 	}
-#endif
+
+	/* Start the failsafe led init */
+	failsafe_led_init();
 
 	/*
 	 * Run everything in a tight loop.
@@ -270,11 +295,12 @@ user_start(int argc, char *argv[])
 
                 check_reboot();
 
-#if 0
-		/* check for debug activity */
+		/* check for debug activity (default: none) */
 		show_debug_messages();
 
-		/* post debug state at ~1Hz */
+		/* post debug state at ~1Hz - this is via an auxiliary serial port
+		 * DEFAULTS TO OFF!
+		 */
 		if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
 
 			struct mallinfo minfo = mallinfo();
@@ -287,7 +313,6 @@ user_start(int argc, char *argv[])
 				  (unsigned)minfo.mxordblk);
 			last_debug_time = hrt_absolute_time();
 		}
-#endif
 	}
 }
 

src/modules/px4iofirmware/px4io.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -96,8 +96,9 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 
 #define r_raw_rc_count		r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
 #define r_raw_rc_values		(&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_raw_rc_flags		r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
 #define r_rc_valid		r_page_rc_input[PX4IO_P_RC_VALID]
-#define r_rc_values		(&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_rc_values		(&r_page_rc_input[PX4IO_P_RC_BASE])
 
 #define r_setup_features	r_page_setup[PX4IO_P_SETUP_FEATURES]
 #define r_setup_arming		r_page_setup[PX4IO_P_SETUP_ARMING]
@@ -115,7 +116,8 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
  */
 struct sys_state_s {
 
-	volatile uint64_t	rc_channels_timestamp;
+	volatile uint64_t	rc_channels_timestamp_received;
+	volatile uint64_t	rc_channels_timestamp_valid;
 
 	/**
 	 * Last FMU receive time, in microseconds since system boot
@@ -160,6 +162,7 @@ extern pwm_limit_t pwm_limit;
 
 # define PX4IO_RELAY_CHANNELS		0
 # define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+# define ENABLE_SBUS_OUT(_s)		stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
 
 # define VDD_SERVO_FAULT		(!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
 
@@ -177,12 +180,13 @@ extern pwm_limit_t pwm_limit;
  * Mixer
  */
 extern void	mixer_tick(void);
-extern void	mixer_handle_text(const void *buffer, size_t length);
+extern int	mixer_handle_text(const void *buffer, size_t length);
 
 /**
  * Safety switch/LED.
  */
 extern void	safety_init(void);
+extern void	failsafe_led_init(void);
 
 /**
  * FMU communications
@@ -213,7 +217,7 @@ extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
 extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
-extern bool	sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
+extern bool	sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
 
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;

src/modules/px4iofirmware/registers.c

@@ -89,7 +89,7 @@ uint16_t		r_page_status[] = {
 	[PX4IO_P_STATUS_IBATT]			= 0,
 	[PX4IO_P_STATUS_VSERVO]			= 0,
 	[PX4IO_P_STATUS_VRSSI]			= 0,
-	[PX4IO_P_STATUS_PRSSI]			= 0
+	[PX4IO_P_STATUS_PRSSI]			= 0,
 };
 
 /**
@@ -114,6 +114,12 @@ uint16_t		r_page_servos[PX4IO_SERVO_COUNT];
 uint16_t		r_page_raw_rc_input[] =
 {
 	[PX4IO_P_RAW_RC_COUNT]			= 0,
+	[PX4IO_P_RAW_RC_FLAGS]			= 0,
+	[PX4IO_P_RAW_RC_NRSSI]			= 0,
+	[PX4IO_P_RAW_RC_DATA]			= 0,
+	[PX4IO_P_RAW_FRAME_COUNT]		= 0,
+	[PX4IO_P_RAW_LOST_FRAME_COUNT]		= 0,
+	[PX4IO_P_RAW_RC_DATA]			= 0,
 	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
 };
 
@@ -142,7 +148,12 @@ uint16_t		r_page_scratch[32];
  */
 volatile uint16_t	r_page_setup[] =
 {
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	/* default to RSSI ADC functionality */
+	[PX4IO_P_SETUP_FEATURES]		= PX4IO_P_SETUP_FEATURES_ADC_RSSI,
+#else
 	[PX4IO_P_SETUP_FEATURES]		= 0,
+#endif
 	[PX4IO_P_SETUP_ARMING]			= 0,
 	[PX4IO_P_SETUP_PWM_RATES]		= 0,
 	[PX4IO_P_SETUP_PWM_DEFAULTRATE]		= 50,
@@ -160,14 +171,22 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
 };
 
-#define PX4IO_P_SETUP_FEATURES_VALID	(0)
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+#define PX4IO_P_SETUP_FEATURES_VALID	(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
+					 PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
+					 PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
+					 PX4IO_P_SETUP_FEATURES_PWM_RSSI)
+#else
+#define PX4IO_P_SETUP_FEATURES_VALID	0
+#endif
 #define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
 					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
 					 PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
 					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
 					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
-					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
+					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
+					 PX4IO_P_SETUP_ARMING_LOCKDOWN)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -380,7 +399,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 
 		/* handle text going to the mixer parser */
 	case PX4IO_PAGE_MIXERLOAD:
-		mixer_handle_text(values, num_values * sizeof(*values));
+		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+				    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+			return mixer_handle_text(values, num_values * sizeof(*values));
+		}
 		break;
 
 	default:
@@ -433,9 +455,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_FEATURES:
 
 			value &= PX4IO_P_SETUP_FEATURES_VALID;
-			r_setup_features = value;
 
-			/* no implemented feature selection at this point */
+			/* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
+
+			/* switch S.Bus output pin as needed */
+			#ifdef ENABLE_SBUS_OUT
+			ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
+
+			/* disable the conflicting options */
+			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+			}
+			#endif
+
+			/* disable the conflicting options with ADC RSSI */
+			if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
+					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+			}
+
+			/* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
+			if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
+					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+			}
+
+			/* apply changes */
+			r_setup_features = value;
 
 			break;
 
@@ -451,11 +499,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * lockup of the IO arming state.
 			 */
 
-			// XXX do not reset IO's safety state by FMU for now
-			// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
-			// 	r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
-			// }
-
 			if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 			}
@@ -507,8 +550,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 		case PX4IO_P_SETUP_REBOOT_BL:
 			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
-			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
-			    (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 				// don't allow reboot while armed
 				break;
 			}
@@ -538,8 +580,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		 * do not allow a RC config change while outputs armed
 		 */
 		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
-			(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
-			(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 			break;
 		}
 
@@ -599,7 +640,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
 					disabled = true;
-                } else if (REG_TO_SIGNED(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
+				} else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
 					count++;
 				}
 

src/modules/px4iofirmware/safety.c

@@ -83,7 +83,11 @@ safety_init(void)
 {
 	/* arrange for the button handler to be called at 10Hz */
 	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
+}
 
+void
+failsafe_led_init(void)
+{
 	/* arrange for the failsafe blinker to be called at 8Hz */
 	hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
 }
@@ -164,8 +168,8 @@ failsafe_blink(void *arg)
 	/* indicate that a serious initialisation error occured */
 	if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
 		LED_AMBER(true);
-	    return;
-    }
+		return;
+	}
 
 	static bool failsafe = false;
 

src/modules/px4iofirmware/sbus.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -87,7 +87,7 @@ static unsigned partial_frame_count;
 
 unsigned sbus_frame_drops;
 
-static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
 
 int
 sbus_init(const char *device)
@@ -118,7 +118,7 @@ sbus_init(const char *device)
 }
 
 bool
-sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
+sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_
 	 * decode it.
 	 */
 	partial_frame_count = 0;
-	return sbus_decode(now, values, num_values, rssi, max_channels);
+	return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
 }
 
 /*
@@ -215,14 +215,36 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
 };
 
 static bool
-sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
-	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
+	if ((frame[0] != 0x0f)) {
 		sbus_frame_drops++;
 		return false;
 	}
 
+	switch (frame[24]) {
+		case 0x00:
+		/* this is S.BUS 1 */
+		break;
+		case 0x03:
+		/* S.BUS 2 SLOT0: RX battery and external voltage */
+		break;
+		case 0x83:
+		/* S.BUS 2 SLOT1 */
+		break;
+		case 0x43:
+		case 0xC3:
+		case 0x23:
+		case 0xA3:
+		case 0x63:
+		case 0xE3:
+		break;
+		default:
+		/* we expect one of the bits above, but there are some we don't know yet */
+		break;
+	}
+
 	/* we have received something we think is a frame */
 	last_frame_time = frame_time;
 
@@ -267,20 +289,22 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
 	/* decode and handle failsafe and frame-lost flags */
 	if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
 		/* report that we failed to read anything valid off the receiver */
-		*rssi = 0;
-		return false;
+		*sbus_failsafe = true;
+		*sbus_frame_drop = true;
 	}
 	else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
-		/* set a special warning flag or try to calculate some kind of RSSI information - to be implemented 
+		/* set a special warning flag
 		 * 
 		 * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this 
 		 * condition as fail-safe greatly reduces the reliability and range of the radio link, 
 		 * e.g. by prematurely issueing return-to-launch!!! */
 
-		*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
+		*sbus_failsafe = false;
+		*sbus_frame_drop = true;
+	} else {
+		*sbus_failsafe = false;
+		*sbus_frame_drop = false;
 	}
 
-	*rssi = 255;
-
 	return true;
 }

src/modules/sdlog2/sdlog2.c

@@ -62,7 +62,6 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -73,7 +72,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
@@ -83,6 +82,7 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/esc_status.h>
+#include <uORB/topics/telemetry_status.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -740,7 +740,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	/* warning! using union here to save memory, elements should be used separately! */
 	union {
 		struct vehicle_command_s cmd;
-		struct vehicle_control_mode_s control_mode;
 		struct sensor_combined_s sensor;
 		struct vehicle_attitude_s att;
 		struct vehicle_attitude_setpoint_s att_sp;
@@ -750,7 +749,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_local_position_s local_pos;
 		struct vehicle_local_position_setpoint_s local_pos_sp;
 		struct vehicle_global_position_s global_pos;
-		struct mission_item_triplet_s triplet;
+		struct position_setpoint_triplet_s triplet;
 		struct vehicle_gps_position_s gps_pos;
 		struct vehicle_vicon_position_s vicon_pos;
 		struct optical_flow_s flow;
@@ -760,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct esc_status_s esc;
 		struct vehicle_global_velocity_setpoint_s global_vel_sp;
 		struct battery_status_s battery;
+		struct telemetry_status_s telemetry;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -767,7 +767,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	struct {
 		int cmd_sub;
 		int status_sub;
-		int control_mode_sub;
 		int sensor_sub;
 		int att_sub;
 		int att_sp_sub;
@@ -786,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int esc_sub;
 		int global_vel_sp_sub;
 		int battery_sub;
+		int telemetry_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -814,6 +814,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GVSP_s log_GVSP;
 			struct log_BATT_s log_BATT;
 			struct log_DIST_s log_DIST;
+			struct log_TELE_s log_TELE;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -847,12 +848,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
-	/* --- VEHICLE CONTROL MODE --- */
-	subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	fds[fdsc_count].fd = subs.control_mode_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
 	/* --- SENSORS COMBINED --- */
 	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	fds[fdsc_count].fd = subs.sensor_sub;
@@ -908,7 +903,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fdsc_count++;
 
 	/* --- GLOBAL POSITION SETPOINT--- */
-	subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
+	subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	fds[fdsc_count].fd = subs.triplet_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
@@ -955,6 +950,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- TELEMETRY STATUS --- */
+	subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
+	fds[fdsc_count].fd = subs.telemetry_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -1002,7 +1003,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* decide use usleep() or blocking poll() */
 		bool use_sleep = sleep_delay > 0 && logging_enabled;
 
-		/* poll all topics if logging enabled or only management (first 2) if not */
+		/* poll all topics if logging enabled or only management (first 3) if not */
 		int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout);
 
 		/* handle the poll result */
@@ -1064,11 +1065,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* --- VEHICLE STATUS --- */
 			if (fds[ifds++].revents & POLLIN) {
 				/* don't orb_copy, it's already done few lines above */
-				/* copy VEHICLE CONTROL MODE control mode here to construct STAT message */
-				orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode);
 				log_msg.msg_type = LOG_STAT_MSG;
-				log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state;
-				log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state;
+				log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
 				log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
 				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
 				log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
@@ -1078,7 +1076,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 			/* --- GPS POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
+				/* don't orb_copy, it's already done few lines above */
 				log_msg.msg_type = LOG_GPS_MSG;
 				log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec;
 				log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type;
@@ -1094,8 +1092,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(GPS);
 			}
 
-			ifds++;	// skip CONTROL MODE, already handled
-
 			/* --- SENSOR COMBINED --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
@@ -1252,29 +1248,29 @@ int sdlog2_thread_main(int argc, char *argv[])
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
 				log_msg.msg_type = LOG_GPOS_MSG;
-				log_msg.body.log_GPOS.lat = buf.global_pos.lat;
-				log_msg.body.log_GPOS.lon = buf.global_pos.lon;
+				log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7;
+				log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7;
 				log_msg.body.log_GPOS.alt = buf.global_pos.alt;
-				log_msg.body.log_GPOS.vel_n = buf.global_pos.vx;
-				log_msg.body.log_GPOS.vel_e = buf.global_pos.vy;
-				log_msg.body.log_GPOS.vel_d = buf.global_pos.vz;
+				log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
+				log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
+				log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
+				log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt;
+				log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0);
 				LOGBUFFER_WRITE_AND_COUNT(GPOS);
 			}
 
 			/* --- GLOBAL POSITION SETPOINT --- */
 			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet);
+				orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet);
 				log_msg.msg_type = LOG_GPSP_MSG;
-				log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative;
+				log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
 				log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
 				log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
-				log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude;
+				log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
 				log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
-				log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd;				
+				log_msg.body.log_GPSP.type = buf.triplet.current.type;
 				log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
 				log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
-				log_msg.body.log_GPSP.acceptance_radius = buf.triplet.current.acceptance_radius;
-				log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside;
 				log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
 				LOGBUFFER_WRITE_AND_COUNT(GPSP);
 			}
@@ -1361,6 +1357,20 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(BATT);
 			}
 
+			/* --- TELEMETRY --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry);
+				log_msg.msg_type = LOG_TELE_MSG;
+				log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
+				log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
+				log_msg.body.log_TELE.noise = buf.telemetry.noise;
+				log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
+				log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
+				log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
+				log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
+				LOGBUFFER_WRITE_AND_COUNT(TELE);
+			}
+
 			/* signal the other thread new data, but not yet unlock */
 			if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 				/* only request write if several packets can be written at once */

src/modules/sdlog2/sdlog2_messages.h

@@ -149,7 +149,6 @@ struct log_ATTC_s {
 #define LOG_STAT_MSG 10
 struct log_STAT_s {
 	uint8_t main_state;
-	uint8_t navigation_state;
 	uint8_t arming_state;
 	float battery_remaining;
 	uint8_t battery_warning;
@@ -205,21 +204,21 @@ struct log_GPOS_s {
 	float vel_n;
 	float vel_e;
 	float vel_d;
+	float baro_alt;
+	uint8_t flags;
 };
 
 /* --- GPSP - GLOBAL POSITION SETPOINT --- */
 #define LOG_GPSP_MSG 17
 struct log_GPSP_s {
-	uint8_t altitude_is_relative;
+	uint8_t nav_state;
 	int32_t lat;
 	int32_t lon;
-	float altitude;
+	float alt;
 	float yaw;
-	uint8_t nav_cmd;
+	uint8_t type;
 	float loiter_radius;
 	int8_t loiter_direction;
-	float acceptance_radius;
-	float time_inside;
 	float pitch_min;
 };
 
@@ -265,6 +264,18 @@ struct log_DIST_s {
 	uint8_t flags;
 };
 
+/* --- TELE - TELEMETRY STATUS --- */
+#define LOG_TELE_MSG 22
+struct log_TELE_s {
+    uint8_t rssi;
+    uint8_t remote_rssi;
+    uint8_t noise;
+    uint8_t remote_noise;
+    uint16_t rxerrors;
+    uint16_t fixed;
+    uint8_t txbuf;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -300,18 +311,20 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
+	LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
-	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
-	LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"),
+	LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
+	LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
 	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
+	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
+
 	/* system-level messages, ID >= 0x80 */
 	// FMT: don't write format of format message, it's useless
 	LOG_FORMAT(TIME, "Q", "StartTime"),

src/modules/sensors/sensor_params.c

@@ -381,14 +381,73 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
 #endif
 PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124);	/* scaling for 3DR power brick */
 
+/**
+ * Roll control channel mapping.
+ *
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for reading roll inputs from.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
+
+/**
+ * Pitch control channel mapping.
+ *
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for reading pitch inputs from.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
+
+/**
+ * Throttle control channel mapping.
+ *
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for reading throttle inputs from.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
+
+/**
+ * Yaw control channel mapping.
+ *
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for reading yaw inputs from.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
 
-PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
+/**
+ * Mode switch channel mapping.
+ *
+ * This is the main flight mode selector.
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for deciding about the main mode.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6);
+PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 

src/modules/sensors/sensors.cpp

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -633,37 +632,39 @@ Sensors::parameters_update()
 	if (!rc_valid)
 		warnx("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
 
+	const char *paramerr = "FAIL PARM LOAD";
+
 	/* channel mapping */
 	if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
-		warnx("Failed getting roll chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
-		warnx("Failed getting pitch chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
-		warnx("Failed getting yaw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
-		warnx("Failed getting throttle chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
-		warnx("Failed getting mode sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
-		warnx("Failed getting return sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
-		warnx("Failed getting assisted sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
-		warnx("Failed getting mission sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) {
@@ -671,7 +672,7 @@ Sensors::parameters_update()
 	}
 
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
-		warnx("Failed getting flaps chan index");
+		warnx(paramerr);
 	}
 
 	param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
@@ -738,12 +739,12 @@ Sensors::parameters_update()
 
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
-		warnx("Failed updating voltage scaling param");
+		warnx(paramerr);
 	}
 
 	/* scaling of ADC ticks to battery current */
 	if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
-		warnx("Failed updating current scaling param");
+		warnx(paramerr);
 	}
 
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
@@ -791,7 +792,6 @@ Sensors::accel_init()
 
 #endif
 
-		warnx("using system accel");
 		close(fd);
 	}
 }
@@ -831,7 +831,6 @@ Sensors::gyro_init()
 
 #endif
 
-		warnx("using system gyro");
 		close(fd);
 	}
 }
@@ -1272,6 +1271,9 @@ Sensors::rc_poll()
 
 		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
 
+		if (rc_input.rc_lost)
+			return;
+
 		struct manual_control_setpoint_s manual_control;
 		struct actuator_controls_s actuator_group_3;
 
@@ -1316,7 +1318,7 @@ Sensors::rc_poll()
 			channel_limit = _rc_max_chan_count;
 
 		/* we are accepting this message */
-		_rc_last_valid = rc_input.timestamp;
+		_rc_last_valid = rc_input.timestamp_last_signal;
 
 		/* Read out values from raw message */
 		for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1365,9 +1367,9 @@ Sensors::rc_poll()
 		}
 
 		_rc.chan_count = rc_input.channel_count;
-		_rc.timestamp = rc_input.timestamp;
+		_rc.timestamp = rc_input.timestamp_last_signal;
 
-		manual_control.timestamp = rc_input.timestamp;
+		manual_control.timestamp = rc_input.timestamp_last_signal;
 
 		/* roll input - rolling right is stick-wise and rotation-wise positive */
 		manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
@@ -1502,9 +1504,6 @@ void
 Sensors::task_main()
 {
 
-	/* inform about start */
-	warnx("Initializing..");
-
 	/* start individual sensors */
 	accel_init();
 	gyro_init();
@@ -1649,17 +1648,17 @@ int sensors_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (sensors::g_sensors != nullptr)
-			errx(0, "sensors task already running");
+			errx(0, "already running");
 
 		sensors::g_sensors = new Sensors;
 
 		if (sensors::g_sensors == nullptr)
-			errx(1, "sensors task alloc failed");
+			errx(1, "alloc failed");
 
 		if (OK != sensors::g_sensors->start()) {
 			delete sensors::g_sensors;
 			sensors::g_sensors = nullptr;
-			err(1, "sensors task start failed");
+			err(1, "start failed");
 		}
 
 		exit(0);
@@ -1667,7 +1666,7 @@ int sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "stop")) {
 		if (sensors::g_sensors == nullptr)
-			errx(1, "sensors task not running");
+			errx(1, "not running");
 
 		delete sensors::g_sensors;
 		sensors::g_sensors = nullptr;
@@ -1676,10 +1675,10 @@ int sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (sensors::g_sensors) {
-			errx(0, "task is running");
+			errx(0, "is running");
 
 		} else {
-			errx(1, "task is not running");
+			errx(1, "not running");
 		}
 	}
 

src/modules/systemlib/cpuload.c

@@ -93,8 +93,7 @@ void cpuload_initialize_once()
 #endif /* CONFIG_SCHED_WORKQUEUE */
 
 	// perform static initialization of "system" threads
-	for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++)
-	{
+	for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) {
 		system_load.tasks[system_load.total_count].start_time = now;
 		system_load.tasks[system_load.total_count].total_runtime = 0;
 		system_load.tasks[system_load.total_count].curr_start_time = 0;

src/modules/systemlib/cpuload.h

@@ -40,15 +40,15 @@ __BEGIN_DECLS
 #include <nuttx/sched.h>
 
 struct system_load_taskinfo_s {
-	uint64_t total_runtime;      ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
-	uint64_t curr_start_time;     ///< Start time of the current scheduling slot
-	uint64_t start_time;         ///< FIRST start time of task
-	FAR struct tcb_s *tcb;               ///<
-	bool valid;                  ///< Task is currently active / valid
+	uint64_t total_runtime;			///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
+	uint64_t curr_start_time;		///< Start time of the current scheduling slot
+	uint64_t start_time;			///< FIRST start time of task
+	FAR struct tcb_s *tcb;			///<
+	bool valid;						///< Task is currently active / valid
 };
 
 struct system_load_s {
-	uint64_t start_time;         ///< Global start time of measurements
+	uint64_t start_time;			///< Global start time of measurements
 	struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS];
 	uint8_t initialized;
 	int total_count;

src/modules/systemlib/mixer/mixer_load.c

@@ -52,7 +52,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 	/* open the mixer definition file */
 	fp = fopen(fname, "r");
 	if (fp == NULL) {
-		return 1;
+		warnx("file not found");
+		return -1;
 	}
 
 	/* read valid lines from the file into a buffer */
@@ -88,7 +89,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 
 		/* if the line is too long to fit in the buffer, bail */
 		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
-			return 1;
+			warnx("line too long");
+			return -1;
 		}
 
 		/* add the line to the buffer */

src/modules/uORB/objects_common.cpp

@@ -117,8 +117,8 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
 #include "topics/vehicle_bodyframe_speed_setpoint.h"
 ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s);
 
-#include "topics/mission_item_triplet.h"
-ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s);
+#include "topics/position_setpoint_triplet.h"
+ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
 
 #include "topics/vehicle_global_velocity_setpoint.h"
 ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);

src/modules/uORB/topics/home_position.h

@@ -62,7 +62,7 @@ struct home_position_s
 	//bool altitude_is_relative;	// TODO what means home relative altitude? we need clear definition of reference altitude then
 	double lat;				/**< Latitude in degrees 			*/
 	double lon;				/**< Longitude in degrees			*/
-	float altitude;				/**< Altitude in meters				*/
+	float alt;				/**< Altitude in meters				*/
 };
 
 /**

src/modules/uORB/topics/position_setpoint_triplet.h

@@ -45,32 +45,47 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
-
-#include "mission.h"
+#include <navigator/navigator_state.h>
 
 /**
  * @addtogroup topics
  * @{
  */
 
+enum SETPOINT_TYPE
+{
+	SETPOINT_TYPE_NORMAL = 0,	/**< normal setpoint */
+	SETPOINT_TYPE_LOITER,		/**< loiter setpoint */
+	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
+	SETPOINT_TYPE_LAND,			/**< land setpoint, altitude must be ignored, vehicle must descend until landing */
+	SETPOINT_TYPE_IDLE,			/**< do nothing, switch off motors or keep at idle speed (MC) */
+};
+
+struct position_setpoint_s
+{
+	bool valid;					/**< true if setpoint is valid */
+	enum SETPOINT_TYPE type;	/**< setpoint type to adjust behavior of position controller */
+	double lat;					/**< latitude, in deg */
+	double lon;					/**< longitude, in deg */
+	float alt;					/**< altitude AMSL, in m */
+	float yaw;					/**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
+	float loiter_radius;		/**< loiter radius (only for fixed wing), in m */
+	int8_t loiter_direction;	/**< loiter direction: 1 = CW, -1 = CCW */
+	float pitch_min;			/**< minimal pitch angle for fixed wing takeoff waypoints */
+};
+
 /**
  * Global position setpoint triplet in WGS84 coordinates.
  *
  * This are the three next waypoints (or just the next two or one).
  */
-struct mission_item_triplet_s
+struct position_setpoint_triplet_s
 {
-	bool previous_valid;
-	bool current_valid;				/**< flag indicating previous mission item is valid */
-	bool next_valid;					/**< flag indicating next mission item is valid */
+	struct position_setpoint_s previous;
+	struct position_setpoint_s current;
+	struct position_setpoint_s next;
 
-	struct mission_item_s previous;
-	struct mission_item_s current;
-	struct mission_item_s next;
-
-	int previous_index;
-	int current_index;
-	int next_index;
+	nav_state_t nav_state;		/**< navigation state */
 };
 
 /**
@@ -78,6 +93,6 @@ struct mission_item_triplet_s
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(mission_item_triplet);
+ORB_DECLARE(position_setpoint_triplet);
 
 #endif

src/modules/uORB/topics/telemetry_status.h

@@ -58,10 +58,10 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
 struct telemetry_status_s {
 	uint64_t timestamp;
     enum TELEMETRY_STATUS_RADIO_TYPE type;  /**< type of the radio hardware     */
-	unsigned rssi;              /**< local signal strength                      */
-    unsigned remote_rssi;       /**< remote signal strength                     */
-    unsigned rxerrors;          /**< receive errors                             */
-    unsigned fixed;             /**< count of error corrected packets           */
+    uint8_t rssi;              /**< local signal strength                      */
+    uint8_t remote_rssi;       /**< remote signal strength                     */
+    uint16_t rxerrors;          /**< receive errors                             */
+    uint16_t fixed;             /**< count of error corrected packets           */
     uint8_t noise;              /**< background noise level                     */
     uint8_t remote_noise;       /**< remote background noise level              */
     uint8_t txbuf;              /**< how full the tx buffer is as a percentage  */
@@ -73,4 +73,4 @@ struct telemetry_status_s {
 
 ORB_DECLARE(telemetry_status);
 
-#endif /* TOPIC_TELEMETRY_STATUS_H */
+#endif /* TOPIC_TELEMETRY_STATUS_H */

src/modules/uORB/topics/vehicle_control_mode.h

@@ -61,22 +61,10 @@
  * Encodes the complete system state and is set by the commander app.
  */
 
-typedef enum {
-	NAV_STATE_NONE = 0,
-	NAV_STATE_READY,
-	NAV_STATE_LOITER,
-	NAV_STATE_MISSION,
-	NAV_STATE_RTL,
-	NAV_STATE_MAX
-} nav_state_t;
-
 struct vehicle_control_mode_s
 {
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
-	main_state_t main_state;
-	nav_state_t nav_state;
-
 	bool flag_armed;
 
 	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
@@ -85,14 +73,14 @@ struct vehicle_control_mode_s
 	bool flag_system_hil_enabled;
 
 	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
-	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */
+	bool flag_control_auto_enabled;			/**< true if onboard autopilot should act */
 	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_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
-	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
-	bool flag_control_flighttermination_enabled;   /**< true if flighttermination is enabled */
+	bool flag_control_climb_rate_enabled;	/**< true if climb rate is controlled */
+	bool flag_control_termination_enabled;	/**< true if flighttermination is enabled */
 };
 
 /**

src/modules/uORB/topics/vehicle_global_position.h

@@ -54,25 +54,28 @@
  /**
  * Fused global position in WGS84.
  *
- * This struct contains the system's believ about its position. It is not the raw GPS
+ * This struct contains global position estimation. It is not the raw GPS
  * measurement (@see vehicle_gps_position). This topic is usually published by the position
  * estimator, which will take more sources of information into account than just GPS,
  * e.g. control inputs of the vehicle in a Kalman-filter implementation.
  */
 struct vehicle_global_position_s
 {
-	uint64_t timestamp;		/**< time of this estimate, in microseconds since system start */
-	uint64_t time_gps_usec; 	/**< GPS timestamp in microseconds							   */
-	bool valid;			/**< true if position satisfies validity criteria of estimator */
+	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start */
 
-	int32_t lat;			/**< Latitude in 1E7 degrees							 	   */
-	int32_t lon;			/**< Longitude in 1E7 degrees							 	   */
-	float alt;			/**< Altitude in meters									 	   */
-	float relative_alt;		/**< Altitude above home position in meters, 				   */
-	float vx; 			/**< Ground X velocity, m/s in NED				 			   */
-	float vy;			/**< Ground Y velocity, m/s in NED							   */
-	float vz;			/**< Ground Z velocity, m/s	in NED 							   */
-	float yaw; 			/**< Compass heading in radians -PI..+PI.					   */
+	bool global_valid;		/**< true if position satisfies validity criteria of estimator */
+	bool baro_valid;		/**< true if baro_alt is valid (vel_d is also valid in this case) */
+
+	uint64_t time_gps_usec; /**< GPS timestamp in microseconds					   */
+	double lat;			/**< Latitude in degrees							 	   */
+	double lon;			/**< Longitude in degrees							 	   */
+	float alt;			/**< Altitude AMSL in meters						 	   */
+	float vel_n; 		/**< Ground north velocity, m/s				 			   */
+	float vel_e;		/**< Ground east velocity, m/s							   */
+	float vel_d;		/**< Ground downside velocity, m/s						   */
+	float yaw; 			/**< Yaw in radians -PI..+PI.							   */
+
+	float baro_alt;		/**< Barometric altitude (not raw baro but fused with accelerometer) */
 };
 
 /**

src/modules/uORB/topics/vehicle_status.h

@@ -54,6 +54,8 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
+#include <navigator/navigator_state.h>
+
 /**
  * @addtogroup topics @{
  */
@@ -65,6 +67,7 @@ typedef enum {
 	MAIN_STATE_EASY,
 	MAIN_STATE_AUTO,
 	MAIN_STATE_OFFBOARD,
+	MAIN_STATE_MAX
 } main_state_t;
 
 typedef enum {
@@ -74,7 +77,8 @@ typedef enum {
 	ARMING_STATE_ARMED_ERROR,
 	ARMING_STATE_STANDBY_ERROR,
 	ARMING_STATE_REBOOT,
-	ARMING_STATE_IN_AIR_RESTORE
+	ARMING_STATE_IN_AIR_RESTORE,
+	ARMING_STATE_MAX
 } arming_state_t;
 
 typedef enum {
@@ -83,9 +87,12 @@ typedef enum {
 } hil_state_t;
 
 typedef enum {
-	FLIGHTTERMINATION_STATE_OFF = 0,
-	FLIGHTTERMINATION_STATE_ON
-} flighttermination_state_t;
+	FAILSAFE_STATE_NORMAL = 0,		/**< Normal operation */
+	FAILSAFE_STATE_RTL,				/**< Return To Launch */
+	FAILSAFE_STATE_LAND,			/**< Land without position control */
+	FAILSAFE_STATE_TERMINATION,		/**< Disable motors and use parachute, can't be recovered */
+	FAILSAFE_STATE_MAX
+} failsafe_state_t;
 
 typedef enum {
 	MODE_SWITCH_MANUAL = 0,
@@ -180,6 +187,7 @@ struct vehicle_status_s
 	uint64_t set_nav_state_timestamp;	/**< timestamp of latest change of set_nav_state */
 	arming_state_t arming_state;			/**< current arming state */
 	hil_state_t hil_state;					/**< current hil state */
+	failsafe_state_t failsafe_state;		/**< current failsafe state */
 
 	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
 	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */
@@ -208,6 +216,7 @@ struct vehicle_status_s
 
 	bool rc_signal_found_once;
 	bool rc_signal_lost;				/**< true if RC reception lost */
+	bool rc_input_blocked;				/**< set if RC input should be ignored */
 
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
@@ -231,8 +240,6 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
-
-	flighttermination_state_t flighttermination_state;
 };
 
 /**