Added "bottom distance" switch, multirotor_pos_control implemented: use bottom distance to surface to control altitde

10 files changed, 106 insertions, 41 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index a9f6a2351..064e80399 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1398,13 +1398,13 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
 
 	/* land switch */
 	if (!isfinite(sp_man->return_switch)) {
-		current_status->return_switch = RETURN_SWITCH_NONE;
+		current_status->return_switch = SWITCH_OFF;
 
 	} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
-		current_status->return_switch = RETURN_SWITCH_RETURN;
+		current_status->return_switch = SWITCH_ON;
 
 	} else {
-		current_status->return_switch = RETURN_SWITCH_NONE;
+		current_status->return_switch = SWITCH_OFF;
 	}
 
 	/* assisted switch */
@@ -1418,7 +1418,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
 		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
 	}
 
-	/* mission switch  */
+	/* mission switch */
 	if (!isfinite(sp_man->mission_switch)) {
 		current_status->mission_switch = MISSION_SWITCH_MISSION;
 
@@ -1428,6 +1428,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
 	} else {
 		current_status->mission_switch = MISSION_SWITCH_MISSION;
 	}
+
+	/* distance bottom switch */
+	if (!isfinite(sp_man->dist_bottom_switch)) {
+		current_status->dist_bottom_switch = SWITCH_OFF;
+
+	} else if (sp_man->dist_bottom_switch > STICK_ON_OFF_LIMIT) {
+		current_status->dist_bottom_switch = SWITCH_ON;
+
+	} else {
+		current_status->dist_bottom_switch = SWITCH_OFF;
+	}
 }
 
 transition_result_t
@@ -1548,7 +1559,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 			/* switch to AUTO mode */
 			if (status->rc_signal_found_once && !status->rc_signal_lost) {
 				/* act depending on switches when manual control enabled */
-				if (status->return_switch == RETURN_SWITCH_RETURN) {
+				if (status->return_switch == SWITCH_ON) {
 					/* RTL */
 					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 58f7238ff..579a0d3d1 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -424,6 +424,16 @@ navigation_state_transition(struct vehicle_status_s *status, navigation_state_t
 		}
 	}
 
+	bool use_dist_bottom_prev = control_mode->flag_use_dist_bottom;
+	control_mode->flag_use_dist_bottom = control_mode->flag_control_manual_enabled &&
+			control_mode->flag_control_altitude_enabled && status->dist_bottom_switch == SWITCH_ON;
+
+	if (ret == TRANSITION_NOT_CHANGED && control_mode->flag_use_dist_bottom != use_dist_bottom_prev) {
+		// TODO really, navigation state not changed, set this to force publishing control_mode
+		ret = TRANSITION_CHANGED;
+		navigation_state_changed = true;
+	}
+
 	return ret;
 }
 
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 7abeade80..f283a1eb4 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -229,14 +229,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	bool reset_auto_sp_xy = true;
 	bool reset_auto_sp_z = true;
 	bool reset_takeoff_sp = true;
+	bool reset_z_sp_dist = false;
+	float surface_offset = 0.0f;	// Z of ground level
+	float z_sp_dist = 0.0f;			// distance to ground setpoint (positive)
 
 	hrt_abstime t_prev = 0;
 	const float alt_ctl_dz = 0.2f;
 	const float pos_ctl_dz = 0.05f;
 
-	float ref_alt_prev = 0.0f;
-	hrt_abstime ref_surface_prev_t = 0;
 	uint64_t local_ref_timestamp = 0;
+	uint64_t surface_bottom_timestamp = 0;
 
 	PID_t xy_pos_pids[2];
 	PID_t xy_vel_pids[2];
@@ -250,7 +252,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	parameters_init(&params_h);
 	parameters_update(&params_h, &params);
 
-
 	for (int i = 0; i < 2; i++) {
 		pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
 		pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
@@ -345,28 +346,48 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			if (control_mode.flag_control_manual_enabled) {
 				/* manual control */
 				/* check for reference point updates and correct setpoint */
-				if (local_pos.ref_timestamp != ref_surface_prev_t) {
+				//if (local_pos.ref_timestamp != ref_prev_t) {
 					/* reference alt changed, don't follow large ground level changes in manual flight */
-					local_pos_sp.z += local_pos.ref_alt - ref_alt_prev;
-
+					//local_pos_sp.z += local_pos.ref_alt - ref_alt_prev;
 					// TODO also correct XY setpoint
-				}
+				//}
 
-				/* reset setpoints to current position if needed */
 				if (control_mode.flag_control_altitude_enabled) {
+					/* reset setpoints to current position if needed */
 					if (reset_man_sp_z) {
 						reset_man_sp_z = false;
 						local_pos_sp.z = local_pos.z;
+						reset_z_sp_dist = true;
 						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
 					}
 
+					/* if distance to surface is available, use it */
+					if (control_mode.flag_use_dist_bottom && local_pos.dist_bottom_valid) {
+						if (reset_z_sp_dist) {
+							reset_z_sp_dist = false;
+							surface_offset = local_pos.z + local_pos.dist_bottom;
+							z_sp_dist = -local_pos_sp.z + surface_offset;
+							mavlink_log_info(mavlink_fd, "[mpc] surface offset: %.2f", surface_offset);
+						} else {
+							if (local_pos.surface_bottom_timestamp != surface_bottom_timestamp) {
+								/* new surface level */
+								z_sp_dist += local_pos.z + local_pos.dist_bottom - surface_offset;
+							}
+							surface_offset = local_pos.z + local_pos.dist_bottom;
+						}
+						/* move altitude setpoint according to ground distance */
+						local_pos_sp.z = surface_offset - z_sp_dist;
+					}
+
 					/* move altitude setpoint with throttle stick */
 					float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
 
 					if (z_sp_ctl != 0.0f) {
 						sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
 						local_pos_sp.z += sp_move_rate[2] * dt;
+						z_sp_dist -= sp_move_rate[2] * dt;	// this will have effect only if distance sensor used
 
+						// TODO add z_sp_dist correction here
 						if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
 							local_pos_sp.z = local_pos.z + z_sp_offs_max;
 
@@ -673,13 +694,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			reset_auto_sp_z = true;
 		}
 
-		/* track local position reference even when not controlling position */
-		ref_surface_prev_t = local_pos.ref_timestamp;
-		ref_alt_prev = local_pos.ref_alt;
-
 		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
 		reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
 
+		/* reset distance setpoint if distance is not available */
+		if (!local_pos.dist_bottom_valid) {
+			reset_z_sp_dist = true;
+		}
+
+		surface_bottom_timestamp = local_pos.surface_bottom_timestamp;
+
 		/* run at approximately 100 Hz */
 		usleep(10000);
 	}
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 6df7c69b2..8cd161075 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -439,12 +439,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						} else {
 							/* new ground level */
 							surface_offset -= sonar_corr;
-							mavlink_log_info(mavlink_fd, "[inav] new surface level: %.3f", surface_offset);
-							alt_avg -= sonar_corr;	// TODO check this
 							sonar_corr = 0.0f;
 							sonar_corr_filtered = 0.0f;
 							sonar_valid_time = t;
 							sonar_valid = true;
+							local_pos.surface_bottom_timestamp = t;
+							mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset);
 						}
 
 					} else {
@@ -521,14 +521,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					if (gps_valid) {
 						if (gps.eph_m > 10.0f) {
 							gps_valid = false;
-							warnx("GPS signal lost");
 							mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
 						}
 
 					} else {
 						if (gps.eph_m < 5.0f) {
 							gps_valid = true;
-							warnx("GPS signal found");
 							mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
 						}
 					}
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 4d719e0e1..6f5c20d39 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -188,6 +188,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
 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_DIST_B_SW, 0);
 
 //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 085da905c..78b2fa8ee 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -315,6 +315,7 @@ private:
 		int rc_map_return_sw;
 		int rc_map_assisted_sw;
 		int rc_map_mission_sw;
+		int rc_map_dist_bottom_sw;
 
 //		int rc_map_offboard_ctrl_mode_sw;
 
@@ -360,6 +361,7 @@ private:
 		param_t rc_map_return_sw;
 		param_t rc_map_assisted_sw;
 		param_t rc_map_mission_sw;
+		param_t rc_map_dist_bottom_sw;
 
 //		param_t rc_map_offboard_ctrl_mode_sw;
 
@@ -578,6 +580,7 @@ Sensors::Sensors() :
 	/* optional mode switches, not mapped per default */
 	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
 	_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
+	_parameter_handles.rc_map_dist_bottom_sw = param_find("RC_MAP_DIST_B_SW");
 
 //	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
 
@@ -726,6 +729,10 @@ Sensors::parameters_update()
 		warnx("Failed getting mission sw chan index");
 	}
 
+	if (param_get(_parameter_handles.rc_map_dist_bottom_sw, &(_parameters.rc_map_dist_bottom_sw)) != OK) {
+		warnx("Failed getting distance bottom sw chan index");
+	}
+
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
 		warnx("Failed getting flaps chan index");
 	}
@@ -754,6 +761,7 @@ Sensors::parameters_update()
 	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
 	_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
 	_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
+	_rc.function[DIST_BOTTOM] = _parameters.rc_map_dist_bottom_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
 
@@ -1330,6 +1338,8 @@ Sensors::ppm_poll()
 		manual_control.return_switch = NAN;
 		manual_control.assisted_switch = NAN;
 		manual_control.mission_switch = NAN;
+		manual_control.dist_bottom_switch = NAN;
+
 //		manual_control.auto_offboard_input_switch = NAN;
 
 		manual_control.flaps = NAN;
@@ -1442,6 +1452,9 @@ Sensors::ppm_poll()
 		/* mission switch input */
 		manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 
+		/* distance bottom switch input */
+		manual_control.dist_bottom_switch = limit_minus_one_to_one(_rc.chan[_rc.function[DIST_BOTTOM]].scaled);
+
 		/* flaps */
 		if (_rc.function[FLAPS] >= 0) {
 
@@ -1460,6 +1473,10 @@ Sensors::ppm_poll()
 			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 		}
 
+		if (_rc.function[DIST_BOTTOM] >= 0) {
+			manual_control.dist_bottom_switch = limit_minus_one_to_one(_rc.chan[_rc.function[DIST_BOTTOM]].scaled);
+		}
+
 //		if (_rc.function[OFFBOARD_MODE] >= 0) {
 //			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
 //		}
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index eac6d6e98..6e14d4bee 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -60,6 +60,7 @@ struct manual_control_setpoint_s {
 	float return_switch;			/**< land 2 position switch (mandatory): land, no effect */
 	float assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
 	float mission_switch;		/**< mission 2 position switch (optional): mission, loiter */
+	float dist_bottom_switch;		/**< distance bottom 2 position switch (optional): off, on */
 
 	/**
 	 * Any of the channels below may not be available and be set to NaN
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 5a8580143..8b952c28f 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -63,20 +63,21 @@
 enum RC_CHANNELS_FUNCTION
 {
   THROTTLE = 0,
-  ROLL     = 1,
-  PITCH    = 2,
-  YAW      = 3,
-  MODE = 4,
-  RETURN = 5,
-  ASSISTED = 6,
-  MISSION = 7,
-  OFFBOARD_MODE = 8,
-  FLAPS   = 9,
-  AUX_1   = 10,
-  AUX_2   = 11,
-  AUX_3   = 12,
-  AUX_4   = 13,
-  AUX_5   = 14,
+  ROLL,
+  PITCH,
+  YAW,
+  MODE,
+  RETURN,
+  ASSISTED,
+  MISSION,
+  DIST_BOTTOM,
+  OFFBOARD_MODE,
+  FLAPS,
+  AUX_1,
+  AUX_2,
+  AUX_3,
+  AUX_4,
+  AUX_5,
   RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 093c6775d..cd004844c 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -79,6 +79,7 @@ struct vehicle_control_mode_s
 	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_use_dist_bottom;				/**< true if bottom distance sensor should be used for altitude control */
 
 	bool flag_control_auto_enabled;		// TEMP
 	uint8_t auto_state;	// TEMP navigation state for AUTO modes
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 6bc63cbae..96ae9cbce 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -107,15 +107,15 @@ typedef enum {
 } assisted_switch_pos_t;
 
 typedef enum {
-	RETURN_SWITCH_NONE = 0,
-	RETURN_SWITCH_RETURN
-} return_switch_pos_t;
-
-typedef enum {
 	MISSION_SWITCH_NONE = 0,
 	MISSION_SWITCH_MISSION
 } mission_switch_pos_t;
 
+typedef enum {
+	SWITCH_OFF = 0,
+	SWITCH_ON
+} on_off_switch_pos_t;
+
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
 	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@@ -187,9 +187,10 @@ struct vehicle_status_s
 	bool is_rotary_wing;
 
 	mode_switch_pos_t mode_switch;
-	return_switch_pos_t return_switch;
+	on_off_switch_pos_t return_switch;
 	assisted_switch_pos_t assisted_switch;
 	mission_switch_pos_t mission_switch;
+	on_off_switch_pos_t dist_bottom_switch;
 
 	bool condition_battery_voltage_valid;
 	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */