Merge branch 'beta' into offboard2

src/modules/commander/commander.cpp

@@ -211,7 +211,7 @@ transition_result_t set_main_state_rc(struct vehicle_status_s *status);
 
 void set_control_mode();
 
-void print_reject_mode(const char *msg);
+void print_reject_mode(struct vehicle_status_s *current_status, const char *msg);
 
 void print_reject_arm(const char *msg);
 
@@ -370,6 +370,7 @@ int arm()
 	if (arming_res == TRANSITION_CHANGED) {
 		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
 		return 0;
+
 	} else {
 		return 1;
 	}
@@ -382,6 +383,7 @@ int disarm()
 	if (arming_res == TRANSITION_CHANGED) {
 		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
 		return 0;
+
 	} else {
 		return 1;
 	}
@@ -661,6 +663,8 @@ int commander_thread_main(int argc, char *argv[])
 	/* vehicle status topic */
 	memset(&status, 0, sizeof(status));
 	status.condition_landed = true;	// initialize to safe value
+	// We want to accept RC inputs as default
+	status.rc_input_blocked = false;
 	status.main_state = MAIN_STATE_MANUAL;
 	status.set_nav_state = NAV_STATE_NONE;
 	status.set_nav_state_timestamp = 0;
@@ -939,10 +943,12 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
+		static bool published_condition_landed_fw = false;
 		if (status.is_rotary_wing && status.condition_local_altitude_valid) {
 			if (status.condition_landed != local_position.landed) {
 				status.condition_landed = local_position.landed;
 				status_changed = true;
+				published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes
 
 				if (status.condition_landed) {
 					mavlink_log_critical(mavlink_fd, "#audio: LANDED");
@@ -951,6 +957,12 @@ int commander_thread_main(int argc, char *argv[])
 					mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
 				}
 			}
+		} else {
+			if (!published_condition_landed_fw) {
+				status.condition_landed = false; // Fixedwing does not have a landing detector currently
+				published_condition_landed_fw = true;
+				status_changed = true;
+			}
 		}
 
 		/* update battery status */
@@ -1109,7 +1121,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* start RC input check */
-		if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
+		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
@@ -1544,7 +1556,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
 {
 	/* main mode switch */
 	if (!isfinite(sp_man->mode_switch)) {
-		warnx("mode sw not finite");
+		/* default to manual if signal is invalid */
 		status->mode_switch = MODE_SWITCH_MANUAL;
 
 	} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
@@ -1634,7 +1646,7 @@ set_main_state_rc(struct vehicle_status_s *status)
 				break;	// changed successfully or already in this state
 
 			// else fallback to SEATBELT
-			print_reject_mode("EASY");
+			print_reject_mode(status, "EASY");
 		}
 
 		res = main_state_transition(status, MAIN_STATE_SEATBELT);
@@ -1643,7 +1655,7 @@ set_main_state_rc(struct vehicle_status_s *status)
 			break;	// changed successfully or already in this mode
 
 		if (status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
-			print_reject_mode("SEATBELT");
+			print_reject_mode(status, "SEATBELT");
 
 		// else fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
@@ -1657,7 +1669,7 @@ set_main_state_rc(struct vehicle_status_s *status)
 			break;	// changed successfully or already in this state
 
 		// else fallback to SEATBELT (EASY likely will not work too)
-		print_reject_mode("AUTO");
+		print_reject_mode(status, "AUTO");
 		res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
 		if (res != TRANSITION_DENIED)
@@ -1676,6 +1688,7 @@ set_main_state_rc(struct vehicle_status_s *status)
 }
 
 void
+
 set_control_mode()
 {
 	/* set vehicle_control_mode according to main state and failsafe state */
@@ -1772,16 +1785,25 @@ set_control_mode()
 }
 
 void
-print_reject_mode(const char *msg)
+print_reject_mode(struct vehicle_status_s *status, const char *msg)
 {
 	hrt_abstime t = hrt_absolute_time();
 
 	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
 		last_print_mode_reject_time = t;
 		char s[80];
-		sprintf(s, "#audio: warning: reject %s", msg);
+		sprintf(s, "#audio: REJECT %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
-		tune_negative();
+
+		// only buzz if armed, because else we're driving people nuts indoors
+		// they really need to look at the leds as well.
+		if (status->arming_state == ARMING_STATE_ARMED) {
+			tune_negative();
+		} else {
+
+			// Always show the led indication
+			led_negative();
+		}
 	}
 }
 
@@ -1929,7 +1951,15 @@ void *commander_low_prio_loop(void *arg)
 				} else if ((int)(cmd.param4) == 1) {
 					/* RC calibration */
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					calib_ret = do_rc_calibration(mavlink_fd);
+					/* disable RC control input completely */
+					status.rc_input_blocked = true;
+					calib_ret = OK;
+					mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN");
+
+				} else if ((int)(cmd.param4) == 2) {
+					/* RC trim calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_trim_calibration(mavlink_fd);
 
 				} else if ((int)(cmd.param5) == 1) {
 					/* accelerometer calibration */
@@ -1940,6 +1970,18 @@ void *commander_low_prio_loop(void *arg)
 					/* airspeed calibration */
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 					calib_ret = do_airspeed_calibration(mavlink_fd);
+				} else if ((int)(cmd.param4) == 0) {
+					/* RC calibration ended - have we been in one worth confirming? */
+					if (status.rc_input_blocked) {
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+						/* enable RC control input */
+						status.rc_input_blocked = false;
+						mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
+					}
+
+					/* this always succeeds */
+					calib_ret = OK;
+
 				}
 
 				if (calib_ret == OK)
@@ -1998,6 +2040,10 @@ void *commander_low_prio_loop(void *arg)
 				break;
 			}
 
+		case VEHICLE_CMD_START_RX_PAIR:
+		/* handled in the IO driver */
+		break;
+
 		default:
 			/* don't answer on unsupported commands, it will be done in main loop */
 			break;

src/modules/commander/commander_helper.cpp

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

src/modules/commander/commander_helper.h

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

src/modules/commander/rc_calibration.cpp

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

src/modules/commander/rc_calibration.h

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

src/modules/commander/state_machine_helper.cpp

@@ -394,6 +394,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 			break;
 
 		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;
@@ -404,6 +405,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 			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;

src/modules/dataman/dataman.c

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

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -785,26 +785,26 @@ 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(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+		math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
 		/* current waypoint (the one currently heading for) */
-		math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+		math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
 
 		/* previous waypoint */
 		math::Vector<2> prev_wp;
 
 		if (pos_sp_triplet.previous.valid) {
-			prev_wp(0) = pos_sp_triplet.previous.lat;
-			prev_wp(1) = pos_sp_triplet.previous.lon;
+			prev_wp(0) = (float)pos_sp_triplet.previous.lat;
+			prev_wp(1) = (float)pos_sp_triplet.previous.lon;
 
 		} else {
 			/*
 			 * No valid previous waypoint, go for the current wp.
 			 * This is automatically handled by the L1 library.
 			 */
-			prev_wp(0) = pos_sp_triplet.current.lat;
-			prev_wp(1) = pos_sp_triplet.current.lon;
+			prev_wp(0) = (float)pos_sp_triplet.current.lat;
+			prev_wp(1) = (float)pos_sp_triplet.current.lon;
 
 		}
 
@@ -1263,7 +1263,7 @@ FixedwingPositionControl::task_main()
 			// vehicle_baro_poll();
 
 			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);
+			math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
 
 			/*
 			 * Attempt to control position, on success (= sensors present and not in manual mode),

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -489,14 +489,18 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			//}
 		} else {
 			float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
+
 			if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
 				/* move yaw setpoint */
 				yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
+
 				if (_manual_control_sp.yaw > 0.0f) {
 					yaw_sp_move_rate -= YAW_DEADZONE;
+
 				} else {
 					yaw_sp_move_rate += YAW_DEADZONE;
 				}
+
 				yaw_sp_move_rate *= _params.rc_scale_yaw;
 				_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 				_v_att_sp.R_valid = false;
@@ -660,7 +664,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
 
 				if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
-					_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
 					_rates_int(i) = rate_i;
 				}
 			}

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -479,9 +479,11 @@ MulticopterPositionControl::select_alt(bool global)
 {
 	if (global != _use_global_alt) {
 		_use_global_alt = global;
+
 		if (global) {
 			/* switch from barometric to global altitude */
 			_alt_sp += _global_pos.alt - _global_pos.baro_alt;
+
 		} else {
 			/* switch from global to barometric altitude */
 			_alt_sp += _global_pos.baro_alt - _global_pos.alt;
@@ -589,6 +591,7 @@ MulticopterPositionControl::task_main()
 			if (_control_mode.flag_control_manual_enabled) {
 				/* select altitude source and update setpoint */
 				select_alt(_global_pos.global_valid);
+
 				if (!_use_global_alt) {
 					alt = _global_pos.baro_alt;
 				}
@@ -845,9 +848,10 @@ MulticopterPositionControl::task_main()
 
 					/* adjust limits for landing mode */
 					if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
-							_pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+					    _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 						/* limit max tilt and min lift when landing */
 						tilt_max = _params.land_tilt_max;
+
 						if (thr_min < 0.0f)
 							thr_min = 0.0f;
 					}
@@ -863,9 +867,11 @@ MulticopterPositionControl::task_main()
 						if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) {
 							/* absolute horizontal thrust */
 							float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+
 							if (thrust_sp_xy_len > 0.01f) {
 								/* max horizontal thrust for given vertical thrust*/
 								float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
+
 								if (thrust_sp_xy_len > thrust_xy_max) {
 									float k = thrust_xy_max / thrust_sp_xy_len;
 									thrust_sp(0) *= k;
@@ -874,15 +880,18 @@ MulticopterPositionControl::task_main()
 								}
 							}
 						}
+
 					} else {
 						/* thrust compensation for altitude only control mode */
 						float att_comp;
 
 						if (_att.R[2][2] > TILT_COS_MAX) {
 							att_comp = 1.0f / _att.R[2][2];
+
 						} else if (_att.R[2][2] > 0.0f) {
 							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
 							saturation_z = true;
+
 						} else {
 							att_comp = 1.0f;
 							saturation_z = true;

src/modules/navigator/navigator_main.cpp

@@ -688,8 +688,9 @@ Navigator::task_main()
 					/* RETURN switch, overrides MISSION switch */
 					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
 						/* switch to RTL if not already landed after RTL and home position set */
-						if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) &&
-								_vstatus.condition_home_position_valid) {
+						if (!(_rtl_state == RTL_STATE_DESCEND &&
+						      (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+						    _vstatus.condition_home_position_valid) {
 							dispatch(EVENT_RTL_REQUESTED);
 						}
 
@@ -746,8 +747,9 @@ Navigator::task_main()
 							break;
 
 						case NAV_STATE_RTL:
-							if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) &&
-									_vstatus.condition_home_position_valid) {
+							if (!(_rtl_state == RTL_STATE_DESCEND &&
+							      (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+							    _vstatus.condition_home_position_valid) {
 								dispatch(EVENT_RTL_REQUESTED);
 							}
 
@@ -1078,7 +1080,7 @@ Navigator::start_loiter()
 			mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
 		}
 
-		_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
+		_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
 	}
 
 	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
@@ -1443,14 +1445,7 @@ Navigator::check_mission_item_reached()
 	}
 
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		if (_vstatus.is_rotary_wing) {
-			return _vstatus.condition_landed;
-
-		} else {
-			/* For fw there is currently no landing detector:
-			 * make sure control is not stopped when overshooting the landing waypoint */
-			return false;
-		}
+		return _vstatus.condition_landed;
 	}
 
 	/* XXX TODO count turns */
@@ -1575,10 +1570,12 @@ Navigator::on_mission_item_reached()
 		if (_rtl_state == RTL_STATE_DESCEND) {
 			/* hovering above home position, land if needed or loiter */
 			mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
+
 			if (_mission_item.autocontinue) {
 				dispatch(EVENT_LAND_REQUESTED);
 
 			} else {
+				_reset_loiter_pos = false;
 				dispatch(EVENT_LOITER_REQUESTED);
 			}
 

src/modules/navigator/navigator_mission.cpp

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

src/modules/navigator/navigator_params.c

@@ -60,4 +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
+PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);	// delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT

src/modules/sensors/sensors.cpp

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -633,37 +632,39 @@ Sensors::parameters_update()
 	if (!rc_valid)
 		warnx("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
 
+	const char *paramerr = "FAIL PARM LOAD";
+
 	/* channel mapping */
 	if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
-		warnx("Failed getting roll chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
-		warnx("Failed getting pitch chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
-		warnx("Failed getting yaw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
-		warnx("Failed getting throttle chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
-		warnx("Failed getting mode sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
-		warnx("Failed getting return sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
-		warnx("Failed getting assisted sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
-		warnx("Failed getting mission sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) {
@@ -671,7 +672,7 @@ Sensors::parameters_update()
 	}
 
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
-		warnx("Failed getting flaps chan index");
+		warnx(paramerr);
 	}
 
 	param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
@@ -738,12 +739,12 @@ Sensors::parameters_update()
 
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
-		warnx("Failed updating voltage scaling param");
+		warnx(paramerr);
 	}
 
 	/* scaling of ADC ticks to battery current */
 	if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
-		warnx("Failed updating current scaling param");
+		warnx(paramerr);
 	}
 
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
@@ -1647,17 +1648,17 @@ int sensors_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (sensors::g_sensors != nullptr)
-			errx(0, "sensors task already running");
+			errx(0, "already running");
 
 		sensors::g_sensors = new Sensors;
 
 		if (sensors::g_sensors == nullptr)
-			errx(1, "sensors task alloc failed");
+			errx(1, "alloc failed");
 
 		if (OK != sensors::g_sensors->start()) {
 			delete sensors::g_sensors;
 			sensors::g_sensors = nullptr;
-			err(1, "sensors task start failed");
+			err(1, "start failed");
 		}
 
 		exit(0);
@@ -1665,7 +1666,7 @@ int sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "stop")) {
 		if (sensors::g_sensors == nullptr)
-			errx(1, "sensors task not running");
+			errx(1, "not running");
 
 		delete sensors::g_sensors;
 		sensors::g_sensors = nullptr;
@@ -1674,10 +1675,10 @@ int sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (sensors::g_sensors) {
-			errx(0, "task is running");
+			errx(0, "is running");
 
 		} else {
-			errx(1, "task is not running");
+			errx(1, "not running");
 		}
 	}
 

src/modules/uORB/topics/vehicle_status.h

@@ -216,6 +216,7 @@ struct vehicle_status_s
 
 	bool rc_signal_found_once;
 	bool rc_signal_lost;				/**< true if RC reception lost */
+	bool rc_input_blocked;				/**< set if RC input should be ignored */
 
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;