Merge branch 'beta' into offboard2

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/commander/commander.cpp

@@ -200,9 +200,11 @@ 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 set_control_mode();
 
 void print_reject_mode(const char *msg);
 
@@ -414,51 +416,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)
@@ -527,7 +523,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	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);
+				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
 				ret = true;
 
@@ -566,17 +562,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 }
 
 static struct vehicle_status_s status;
-
-/* armed topic */
+static struct vehicle_control_mode_s control_mode;
 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;
@@ -590,6 +583,28 @@ int commander_thread_main(int argc, char *argv[])
 	/* 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 +619,15 @@ 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));
-
 	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 +644,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);
 
@@ -703,10 +718,15 @@ int commander_thread_main(int argc, char *argv[])
 	struct manual_control_setpoint_s sp_man;
 	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));
+
 	/* 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));
@@ -820,12 +840,16 @@ 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);
-
 		if (updated) {
 			orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp);
 		}
 
+		orb_check(sp_offboard_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+		}
+
 		orb_check(sensor_sub, &updated);
 
 		if (updated) {
@@ -862,7 +886,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);
@@ -1018,19 +1042,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 +1064,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 (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 +1145,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 +1175,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 (offboard_sp.timestamp != 0 && hrt_absolute_time() < offboard_sp.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");
@@ -1189,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[])
 						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");
 						}
 					}
 
@@ -1211,19 +1277,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 && 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 +1300,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 +1394,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,133 +1501,235 @@ 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;
+		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) {
-		current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD;
+		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);
+	/* 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("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;
+		} else {
+			return res;
+		}
+	}
 
-		case MODE_SWITCH_ASSISTED:
-			if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
-				res = main_state_transition(current_status, MAIN_STATE_EASY);
+	/* 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;
 
-				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);
+	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 (EASY likely will not work too)
-			print_reject_mode("AUTO");
-			res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+			// else fallback to SEATBELT
+			print_reject_mode("EASY");
+		}
 
-			if (res != TRANSITION_DENIED)
-				break;	// changed successfully or already in this state
+		res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
-			// else fallback to MANUAL
-			res = main_state_transition(current_status, MAIN_STATE_MANUAL);
-			// TRANSITION_DENIED is not possible here
+		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("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("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
+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;
 	}
 
-	return res;
+	/* 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

src/modules/commander/state_machine_helper.cpp

@@ -63,7 +63,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 +220,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 +299,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 {
@@ -368,28 +373,49 @@ 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 +423,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/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 */
@@ -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};
 
 
@@ -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,56 @@ 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(pos_sp_triplet.current.lat, 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(pos_sp_triplet.current.lat, 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) = pos_sp_triplet.previous.lat;
+			prev_wp(1) = 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) = pos_sp_triplet.current.lat;
+			prev_wp(1) = 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) {
 
 			/* Horizontal landing control */
 			/* switch to heading hold for the last meters, continue heading hold after */
@@ -847,7 +845,7 @@ 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) {
+					if (pos_sp_triplet.previous.valid) {
 						target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
 					} else {
 						target_bearing = _att.yaw;
@@ -879,23 +877,23 @@ 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);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
+			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
 
 
 
-			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 */
 
@@ -914,12 +912,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.getFlareCurveAltitude(wp_distance, _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 +975,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 +1009,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 +1020,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 +1035,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 +1051,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 +1174,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 +1262,14 @@ FixedwingPositionControl::task_main()
 			vehicle_airspeed_poll();
 			// vehicle_baro_poll();
 
-			math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy);
+			math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
 			math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
 
 			/*
 			 * 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 +1283,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/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

@@ -664,13 +664,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)
@@ -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_main.cpp

@@ -85,7 +85,7 @@
 extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
 
 #define MIN_TAKEOFF_THROTTLE	0.3f
-#define YAW_DEADZONE	0.01f
+#define YAW_DEADZONE	0.05f
 #define RATES_I_LIMIT	0.5f
 
 class MulticopterAttitudeControl

src/modules/navigator/navigator_params.c

@@ -60,3 +60,4 @@ 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, 10.0f);	// delay after descend before landing

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 */
@@ -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/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

@@ -114,9 +114,20 @@ controls_tick() {
 
 	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);
+
 	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
 	}
+
+	/* switch S.Bus output pin as needed */
+	if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) {
+		#ifdef ENABLE_SBUS_OUT
+		ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS));
+		#endif
+	}
+
 	perf_end(c_gather_sbus);
 
 	/*

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);
 
@@ -297,12 +303,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 +321,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 +339,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 +383,8 @@ mixer_handle_text(const void *buffer, size_t length)
 
 		break;
 	}
+
+	return 0;
 }
 
 static void

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

@@ -160,6 +160,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 +178,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

src/modules/px4iofirmware/registers.c

@@ -89,7 +89,9 @@ 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,
+	[PX4IO_P_STATUS_NRSSI]			= 0,
+	[PX4IO_P_STATUS_RC_DATA]		= 0
 };
 
 /**
@@ -380,7 +382,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:
@@ -507,8 +512,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 +542,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 +602,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
@@ -218,11 +218,33 @@ static bool
 sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, 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;
 

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>
@@ -740,7 +739,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 +748,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;
@@ -767,7 +765,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;
@@ -847,12 +844,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 +899,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++;
@@ -1002,7 +993,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 +1055,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 +1066,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 +1082,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 +1238,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);
 			}

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;
 };
 
@@ -300,14 +299,14 @@ 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"),

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

@@ -791,7 +791,6 @@ Sensors::accel_init()
 
 #endif
 
-		warnx("using system accel");
 		close(fd);
 	}
 }
@@ -831,7 +830,6 @@ Sensors::gyro_init()
 
 #endif
 
-		warnx("using system gyro");
 		close(fd);
 	}
 }
@@ -1502,9 +1500,6 @@ void
 Sensors::task_main()
 {
 
-	/* inform about start */
-	warnx("Initializing..");
-
 	/* start individual sensors */
 	accel_init();
 	gyro_init();

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/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 */
@@ -231,8 +239,6 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
-
-	flighttermination_state_t flighttermination_state;
 };
 
 /**