Move autosave into the 1-second timeout check.

1 files changed, 152 insertions, 155 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index dadb5ea6e..35f538b23 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2549,216 +2549,213 @@ void *commander_low_prio_loop(void *arg)
 	fds[0].events = POLLIN;
 
 	while (!thread_should_exit) {
-		/* wait for up to 200ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
+		/* wait for up to 1000ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
 
 		/* timed out - periodic check for thread_should_exit, etc. */
 		if (pret == 0) {
-			continue;
-		}
+			/* trigger a param autosave if required */
+			if (_param_autosave) {
+				if (_param_autosave_timeout > 0 && hrt_elapsed_time(&_param_autosave_timeout) > 200000ULL) {
+					int ret = param_save_default();
+
+					if (ret == OK) {
+						mavlink_and_console_log_info(mavlink_fd, "settings autosaved");
 
+					} else {
+						mavlink_and_console_log_critical(mavlink_fd, "settings save error");
+					}
+
+					_param_autosave = false;
+					_param_autosave_timeout = 0;
+				} else {
+					_param_autosave_timeout = hrt_absolute_time();
+				}
+			}
+		} else if (pret < 0) {
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
-		if (pret < 0) {
 			warn("poll error %d, %d", pret, errno);
 			continue;
-		}
+		} else {
 
-		/* if we reach here, we have a valid command */
-		orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+			/* if we reach here, we have a valid command */
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
-		/* ignore commands the high-prio loop handles */
-		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
-		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
-		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
-		    cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
-			continue;
-		}
+			/* ignore commands the high-prio loop handles */
+			if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
+			    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
+			    cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
+			    cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
+				continue;
+			}
 
-		/* only handle low-priority commands here */
-		switch (cmd.command) {
+			/* only handle low-priority commands here */
+			switch (cmd.command) {
 
-		case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
-			if (is_safe(&status, &safety, &armed)) {
+			case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+				if (is_safe(&status, &safety, &armed)) {
+
+					if (((int)(cmd.param1)) == 1) {
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+						usleep(100000);
+						/* reboot */
+						systemreset(false);
 
-				if (((int)(cmd.param1)) == 1) {
-					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					usleep(100000);
-					/* reboot */
-					systemreset(false);
+					} else if (((int)(cmd.param1)) == 3) {
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+						usleep(100000);
+						/* reboot to bootloader */
+						systemreset(true);
 
-				} else if (((int)(cmd.param1)) == 3) {
-					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					usleep(100000);
-					/* reboot to bootloader */
-					systemreset(true);
+					} else {
+						answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+					}
 
 				} else {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 				}
 
-			} else {
-				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
-			}
+				break;
 
-			break;
+			case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
 
-		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
+					int calib_ret = ERROR;
 
-				int calib_ret = ERROR;
+					/* try to go to INIT/PREFLIGHT arming state */
+					if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
+							true /* fRunPreArmChecks */, mavlink_fd)) {
+						answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+						break;
+					}
 
-				/* try to go to INIT/PREFLIGHT arming state */
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
-						true /* fRunPreArmChecks */, mavlink_fd)) {
-					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
-					break;
-				}
+					if ((int)(cmd.param1) == 1) {
+						/* gyro calibration */
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+						calib_ret = do_gyro_calibration(mavlink_fd);
 
-				if ((int)(cmd.param1) == 1) {
-					/* gyro calibration */
-					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					calib_ret = do_gyro_calibration(mavlink_fd);
+					} else if ((int)(cmd.param2) == 1) {
+						/* magnetometer calibration */
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+						calib_ret = do_mag_calibration(mavlink_fd);
 
-				} else if ((int)(cmd.param2) == 1) {
-					/* magnetometer calibration */
-					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					calib_ret = do_mag_calibration(mavlink_fd);
+					} else if ((int)(cmd.param3) == 1) {
+						/* zero-altitude pressure calibration */
+						answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 
-				} else if ((int)(cmd.param3) == 1) {
-					/* zero-altitude pressure calibration */
-					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+					} else if ((int)(cmd.param4) == 1) {
+						/* RC calibration */
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+						/* 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 */
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+						calib_ret = do_accel_calibration(mavlink_fd);
 
-				} else if ((int)(cmd.param4) == 1) {
-					/* RC calibration */
-					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					/* 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 */
-					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					calib_ret = do_accel_calibration(mavlink_fd);
-
-				} else if ((int)(cmd.param6) == 1) {
-					/* 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) {
+					} else if ((int)(cmd.param6) == 1) {
+						/* airspeed calibration */
 						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");
+						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;
+
 					}
 
-					/* this always succeeds */
-					calib_ret = OK;
+					if (calib_ret == OK) {
+						tune_positive(true);
 
-				}
+					} else {
+						tune_negative(true);
+					}
 
-				if (calib_ret == OK) {
-					tune_positive(true);
+					arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
-				} else {
-					tune_negative(true);
+					break;
 				}
 
-				arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
+			case VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
-				break;
-			}
+					if (((int)(cmd.param1)) == 0) {
+						int ret = param_load_default();
 
-		case VEHICLE_CMD_PREFLIGHT_STORAGE: {
+						if (ret == OK) {
+							mavlink_log_info(mavlink_fd, "settings loaded");
+							answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
-				if (((int)(cmd.param1)) == 0) {
-					int ret = param_load_default();
+						} else {
+							mavlink_log_critical(mavlink_fd, "settings load ERROR");
 
-					if (ret == OK) {
-						mavlink_log_info(mavlink_fd, "settings loaded");
-						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+							/* convenience as many parts of NuttX use negative errno */
+							if (ret < 0) {
+								ret = -ret;
+							}
 
-					} else {
-						mavlink_log_critical(mavlink_fd, "settings load ERROR");
+							if (ret < 1000) {
+								mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
+							}
 
-						/* convenience as many parts of NuttX use negative errno */
-						if (ret < 0) {
-							ret = -ret;
+							answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 						}
 
-						if (ret < 1000) {
-							mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
-						}
+					} else if (((int)(cmd.param1)) == 1) {
+						int ret = param_save_default();
 
-						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
-					}
+						if (ret == OK) {
+							mavlink_log_info(mavlink_fd, "settings saved");
+							answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
-				} else if (((int)(cmd.param1)) == 1) {
-					int ret = param_save_default();
+						} else {
+							mavlink_log_critical(mavlink_fd, "settings save error");
 
-					if (ret == OK) {
-						mavlink_log_info(mavlink_fd, "settings saved");
-						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+							/* convenience as many parts of NuttX use negative errno */
+							if (ret < 0) {
+								ret = -ret;
+							}
 
-					} else {
-						mavlink_log_critical(mavlink_fd, "settings save error");
+							if (ret < 1000) {
+								mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
+							}
 
-						/* convenience as many parts of NuttX use negative errno */
-						if (ret < 0) {
-							ret = -ret;
+							answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 						}
-
-						if (ret < 1000) {
-							mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
-						}
-
-						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
+
+					break;
 				}
 
+			case VEHICLE_CMD_START_RX_PAIR:
+				/* handled in the IO driver */
 				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;
-		}
 
-		/* trigger a param autosave if required */
-		if (_param_autosave) {
-			if (_param_autosave_timeout > 0 && hrt_elapsed_time(&_param_autosave_timeout) > 200000ULL) {
-				int ret = param_save_default();
-
-				if (ret == OK) {
-					mavlink_and_console_log_info(mavlink_fd, "settings autosaved");
-
-				} else {
-					mavlink_and_console_log_critical(mavlink_fd, "settings save error");
-				}
-
-				_param_autosave = false;
-				_param_autosave_timeout = 0;
-			} else {
-				_param_autosave_timeout = hrt_absolute_time();
+			default:
+				/* don't answer on unsupported commands, it will be done in main loop */
+				break;
 			}
-		}
 
-		/* send any requested ACKs */
-		if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
-		    && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
-			/* send acknowledge command */
-			// XXX TODO
+			/* send any requested ACKs */
+			if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
+			    && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
+				/* send acknowledge command */
+				// XXX TODO
+			}
 		}
 	}