Merged seatbelt_multirotor_new

7 files changed, 79 insertions, 61 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 66b9272de..4f4907dc8 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -799,9 +799,9 @@ int commander_thread_main(int argc, char *argv[])
 				status.condition_landed = local_position.landed;
 				status_changed = true;
 				if (status.condition_landed) {
-					mavlink_log_info(mavlink_fd, "[cmd] LANDED");
+					mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
 				} else {
-					mavlink_log_info(mavlink_fd, "[cmd] IN AIR");
+					mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
 				}
 			}
 		}
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index c711b1803..dcdc03281 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -666,12 +666,12 @@ l_airspeed(const struct listener *l)
 	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
 
 	float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
+	uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
 	float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
-	float alt = global_pos.alt;
-	float climb = global_pos.vz;
+	float alt = global_pos.relative_alt;
+	float climb = -global_pos.vz;
 
-	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed,
-		((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb);
+	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
 }
 
 static void *
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index 2d16d4921..09955ec50 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -81,6 +81,8 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
 static bool thread_should_exit;
 static int mc_task;
 static bool motor_test_mode = false;
+static const float min_takeoff_throttle = 0.3f;
+static const float yaw_deadzone = 0.01f;
 
 static int
 mc_thread_main(int argc, char *argv[])
@@ -144,13 +146,17 @@ mc_thread_main(int argc, char *argv[])
 	/* welcome user */
 	warnx("starting");
 
+	/* store last control mode to detect mode switches */
+	bool flag_control_manual_enabled = false;
+	bool flag_control_attitude_enabled = false;
 	bool control_yaw_position = true;
 	bool reset_yaw_sp = true;
+	bool failsafe_first_time = true;
 
 	/* prepare the handle for the failsafe throttle */
 	param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
 	float failsafe_throttle = 0.0f;
-
+	param_get(failsafe_throttle_handle, &failsafe_throttle);
 
 	while (!thread_should_exit) {
 
@@ -172,7 +178,7 @@ mc_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(parameter_update), param_sub, &update);
 
 				/* update parameters */
-				// XXX no params here yet
+				param_get(failsafe_throttle_handle, &failsafe_throttle);
 			}
 
 			/* only run controller if attitude changed */
@@ -204,6 +210,9 @@ mc_thread_main(int argc, char *argv[])
 				/* get a local copy of the current sensor values */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
+				/* set flag to safe value */
+				control_yaw_position = true;
+
 				/* define which input is the dominating control input */
 				if (control_mode.flag_control_offboard_enabled) {
 					/* offboard inputs */
@@ -221,41 +230,41 @@ mc_thread_main(int argc, char *argv[])
 						att_sp.yaw_body = offboard_sp.p3;
 						att_sp.thrust = offboard_sp.p4;
 						att_sp.timestamp = hrt_absolute_time();
-						/* STEP 2: publish the result to the vehicle actuators */
+						/* publish the result to the vehicle actuators */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 					}
 
 				} else if (control_mode.flag_control_manual_enabled) {
-					/* direct manual input */
+					/* manual input */
 					if (control_mode.flag_control_attitude_enabled) {
 
-						static bool rc_loss_first_time = true;
-
+						/* control attitude, update attitude setpoint depending on mode */
 						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
 						if (control_mode.failsave_highlevel) {
+							failsafe_first_time = false;
+
 							if (!control_mode.flag_control_velocity_enabled) {
-								/* Don't reset attitude setpoint in position control mode, it's handled by position controller. */
+								/* don't reset attitude setpoint in position control mode, it's handled by position controller. */
 								att_sp.roll_body = 0.0f;
 								att_sp.pitch_body = 0.0f;
+							}
 
-								if (!control_mode.flag_control_climb_rate_enabled) {
-									/* Don't touch throttle in modes with altitude hold, it's handled by position controller.
-									 *
-									 * Only go to failsafe throttle if last known throttle was
-									 * high enough to create some lift to make hovering state likely.
-									 *
-									 * This is to prevent that someone landing, but not disarming his
-									 * multicopter (throttle = 0) does not make it jump up in the air
-									 * if shutting down his remote.
-									 */
-									if (isfinite(manual.throttle) && manual.throttle > 0.2f) {	// TODO use landed status instead of throttle
-										/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
-										param_get(failsafe_throttle_handle, &failsafe_throttle);
-										att_sp.thrust = failsafe_throttle;
-
-									} else {
-										att_sp.thrust = 0.0f;
-									}
+							if (!control_mode.flag_control_climb_rate_enabled) {
+								/* don't touch throttle in modes with altitude hold, it's handled by position controller.
+								 *
+								 * Only go to failsafe throttle if last known throttle was
+								 * high enough to create some lift to make hovering state likely.
+								 *
+								 * This is to prevent that someone landing, but not disarming his
+								 * multicopter (throttle = 0) does not make it jump up in the air
+								 * if shutting down his remote.
+								 */
+								if (isfinite(manual.throttle) && manual.throttle > min_takeoff_throttle) {	// TODO use landed status instead of throttle
+									/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
+									att_sp.thrust = failsafe_throttle;
+
+								} else {
+									att_sp.thrust = 0.0f;
 								}
 							}
 
@@ -263,46 +272,50 @@ mc_thread_main(int argc, char *argv[])
 							 * since if the pilot regains RC control, he will be lost regarding
 							 * the current orientation.
 							 */
-							if (rc_loss_first_time)
-								att_sp.yaw_body = att.yaw;
-
-							rc_loss_first_time = false;
+							if (failsafe_first_time) {
+								reset_yaw_sp = true;
+							}
 
 						} else {
-							rc_loss_first_time = true;
+							failsafe_first_time = true;
 
 							/* control yaw in all manual / assisted modes */
 							/* set yaw if arming or switching to attitude stabilized mode */
-							if (!control_mode.flag_armed) {
+
+							if (!flag_control_manual_enabled || !flag_control_attitude_enabled || !control_mode.flag_armed) {
 								reset_yaw_sp = true;
 							}
 
 							/* only move setpoint if manual input is != 0 */
-							if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) {	// TODO use landed status instead of throttle
-								rates_sp.yaw = manual.yaw;
+
+							// TODO review yaw restpoint reset
+							if ((manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) && manual.throttle > min_takeoff_throttle) {
+								/* control yaw rate */
 								control_yaw_position = false;
-								reset_yaw_sp = true;
+								rates_sp.yaw = manual.yaw;
+								reset_yaw_sp = true;	// has no effect on control, just for beautiful log
 
 							} else {
-								if (reset_yaw_sp) {
-									att_sp.yaw_body = att.yaw;
-									reset_yaw_sp = false;
-								}
 								control_yaw_position = true;
 							}
 
 							if (!control_mode.flag_control_velocity_enabled) {
-								/* don't update attitude setpoint in position control mode */
+								/* update attitude setpoint if not in position control mode */
 								att_sp.roll_body = manual.roll;
 								att_sp.pitch_body = manual.pitch;
 
 								if (!control_mode.flag_control_climb_rate_enabled) {
-									/* don't set throttle in altitude hold modes */
+									/* pass throttle directly if not in altitude control mode */
 									att_sp.thrust = manual.throttle;
 								}
 							}
 
-							att_sp.timestamp = hrt_absolute_time();
+						}
+
+						/* reset yaw setpint to current position if needed */
+						if (reset_yaw_sp) {
+							att_sp.yaw_body = att.yaw;
+							reset_yaw_sp = false;
 						}
 
 						if (motor_test_mode) {
@@ -311,10 +324,11 @@ mc_thread_main(int argc, char *argv[])
 							att_sp.pitch_body = 0.0f;
 							att_sp.yaw_body = 0.0f;
 							att_sp.thrust = 0.1f;
-							att_sp.timestamp = hrt_absolute_time();
 						}
 
-						/* STEP 2: publish the controller output */
+						att_sp.timestamp = hrt_absolute_time();
+
+						/* publish the controller output */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 
 					} else {
@@ -357,6 +371,7 @@ mc_thread_main(int argc, char *argv[])
 					rates[1] = att.pitchspeed;
 					rates[2] = att.yawspeed;
 					multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
+
 				} else {
 					/* rates controller disabled, set actuators to zero for safety */
 					actuators.control[0] = 0.0f;
@@ -364,6 +379,7 @@ mc_thread_main(int argc, char *argv[])
 					actuators.control[2] = 0.0f;
 					actuators.control[3] = 0.0f;
 				}
+
 				actuators.timestamp = hrt_absolute_time();
 				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 760e598bc..1e20f9de9 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -609,7 +609,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (local_pos.v_xy_valid) {
 				global_pos.vx = local_pos.vx;
 				global_pos.vy = local_pos.vy;
-				global_pos.hdg = atan2f(local_pos.vy, local_pos.vx);
+				global_pos.hdg = atan2f(local_pos.vy, local_pos.vx);	// TODO is it correct?
 			}
 
 			if (local_pos.z_valid) {
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 7f8648d95..2b21bb5a5 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1182,6 +1182,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	pthread_mutex_destroy(&logbuffer_mutex);
 	pthread_cond_destroy(&logbuffer_cond);
 
+	free(lb.data);
+
 	warnx("exiting.");
 
 	thread_running = false;
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index d19a7aec3..b45317dbe 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -174,8 +174,8 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
 
 PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
-PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6);
-PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6);
 PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
 
 //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index f036c7223..822c323cf 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -62,17 +62,17 @@
 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							  */
+	uint64_t time_gps_usec; /**< GPS timestamp in microseconds							   */
 	bool valid;				/**< true if position satisfies validity criteria of estimator */
 
-	int32_t lat;			/**< Latitude in 1E7 degrees							LOGME */
-	int32_t lon;			/**< Longitude in 1E7 degrees							LOGME */
-	float alt;				/**< Altitude in meters									LOGME */
-	float relative_alt;		/**< Altitude above home position in meters, 			LOGME */
-	float vx; 				/**< Ground X Speed (Latitude), m/s in ENU				LOGME */
-	float vy;				/**< Ground Y Speed (Longitude), m/s in ENU				LOGME */
-	float vz;				/**< Ground Z Speed (Altitude), m/s	in ENU				LOGME */
-	float hdg; 				/**< Compass heading in radians -PI..+PI.					  */
+	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 hdg; 				/**< Compass heading in radians -PI..+PI.					   */
 
 };