Add new Airspeed flight mode (only fixed-wing)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

msg/commander_state.msg

@@ -16,7 +16,8 @@ uint8 MAIN_STATE_AUTO_LAND          = 11
 uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12
 uint8 MAIN_STATE_AUTO_PRECLAND      = 13
 uint8 MAIN_STATE_ORBIT              = 14
-uint8 MAIN_STATE_MAX                = 15
+uint8 MAIN_STATE_AIRSPEED           = 15
+uint8 MAIN_STATE_MAX                = 16
 
 uint8 main_state		    	# main state machine
 

msg/vehicle_control_mode.msg

@@ -15,3 +15,4 @@ 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_termination_enabled		# true if flighttermination is enabled
+bool flag_control_airspeed_enabled		# true if airspeed is controlled (only fixed-wing)

msg/vehicle_status.msg

@@ -32,7 +32,7 @@ uint8 NAVIGATION_STATE_AUTO_RTL = 5		# Auto return to launch mode
 uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 	# Auto land on engine failure
 uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9	# Auto land on gps failure (e.g. open loop loiter down)
 uint8 NAVIGATION_STATE_ACRO = 10		# Acro mode
-uint8 NAVIGATION_STATE_UNUSED = 11		# Free slot
+uint8 NAVIGATION_STATE_AIRSPEED = 11		# Airspeed control mode (only fixed-wing)
 uint8 NAVIGATION_STATE_DESCEND = 12		# Descend mode (no position control)
 uint8 NAVIGATION_STATE_TERMINATION = 13		# Termination mode
 uint8 NAVIGATION_STATE_OFFBOARD = 14

msg/vehicle_status_flags.msg

@@ -14,6 +14,7 @@ bool condition_local_position_valid		# set to true by the commander app if the q
 bool condition_local_velocity_valid		# set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation
 bool condition_global_position_valid		# set to true by the commander app if the quality of the global position estimate is good enough to use for navigation
 bool condition_home_position_valid		# indicates a valid home position (a valid home position is not always a valid launch)
+bool condition_airspeed_valid			# set to true if airspeed mesurements are valid
 bool condition_power_input_valid                # set if input power is valid
 bool condition_battery_healthy                # set if battery is available and not low
 bool condition_escs_error		      # set to true if one or more ESCs reporting esc_status are offline

src/modules/commander/Commander.cpp

@@ -342,6 +342,9 @@ int Commander::custom_command(int argc, char *argv[])
 			} else if (!strcmp(argv[1], "stabilized")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_STABILIZED);
 
+			} else if (!strcmp(argv[1], "airspeed")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AIRSPEED);
+
 			} else if (!strcmp(argv[1], "auto:takeoff")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF);
@@ -802,6 +805,11 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 					/* OFFBOARD */
 					main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, _internal_state);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AIRSPEED) {
+					/* AIRSPEED */
+					main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AIRSPEED, _status_flags, _internal_state);
+					printf("main_ret: %i\n", main_ret);
 				}
 
 			} else {
@@ -3227,6 +3235,13 @@ Commander::update_control_mode()
 		_vehicle_control_mode.flag_control_attitude_enabled = true;
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_AIRSPEED:
+		_vehicle_control_mode.flag_control_manual_enabled = true;
+		_vehicle_control_mode.flag_control_rates_enabled = true;
+		_vehicle_control_mode.flag_control_attitude_enabled = true;
+		_vehicle_control_mode.flag_control_airspeed_enabled = true;
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
 		_vehicle_control_mode.flag_control_manual_enabled = true;
 		_vehicle_control_mode.flag_control_rates_enabled = true;
@@ -3886,6 +3901,8 @@ void Commander::estimator_check()
 	_status_flags.condition_angular_velocity_valid = (hrt_elapsed_time(&angular_velocity.timestamp) < 1_s)
 			&& PX4_ISFINITE(angular_velocity.xyz[0]) && PX4_ISFINITE(angular_velocity.xyz[1])
 			&& PX4_ISFINITE(angular_velocity.xyz[2]);
+
+	_status_flags.condition_airspeed_valid = true; // TODO: check airspeed status
 }
 
 void Commander::UpdateEstimateValidity()
@@ -4108,7 +4125,7 @@ The commander module contains the state machine for mode switching and failsafe
 	PRINT_MODULE_USAGE_COMMAND("land");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode");
-	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland",
+	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|airspeed|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland",
 			"Flight mode", false);
 	PRINT_MODULE_USAGE_COMMAND("pair");
 	PRINT_MODULE_USAGE_COMMAND("lockdown");

src/modules/commander/px4_custom_mode.h

@@ -51,6 +51,7 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
 	PX4_CUSTOM_MAIN_MODE_STABILIZED,
 	PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
+	PX4_CUSTOM_MAIN_MODE_AIRSPEED,
 	PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
 };
 

src/modules/commander/state_machine_helper.cpp

@@ -300,6 +300,15 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
 		ret = TRANSITION_CHANGED;
 		break;
 
+	case commander_state_s::MAIN_STATE_AIRSPEED:
+
+		/* need at minimum airspeed estimate */
+		if (status_flags.condition_airspeed_valid) {
+			ret = TRANSITION_CHANGED;
+		}
+
+		break;
+
 	case commander_state_s::MAIN_STATE_ALTCTL:
 
 		/* need at minimum altitude estimate */
@@ -457,6 +466,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 	case commander_state_s::MAIN_STATE_ACRO:
 	case commander_state_s::MAIN_STATE_MANUAL:
 	case commander_state_s::MAIN_STATE_STAB:
+	case commander_state_s::MAIN_STATE_AIRSPEED:
 	case commander_state_s::MAIN_STATE_ALTCTL:
 
 		// Require RC for all manual modes
@@ -478,6 +488,10 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 				status.nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
 				break;
 
+			case commander_state_s::MAIN_STATE_AIRSPEED:
+				status.nav_state = vehicle_status_s::NAVIGATION_STATE_AIRSPEED;
+				break;
+
 			case commander_state_s::MAIN_STATE_ALTCTL:
 				status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
 				break;

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -393,6 +393,10 @@ void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state)
 		command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_AIRSPEED:
+		command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AIRSPEED;
+		break;
+
 	default: //vehicle_status_s::NAVIGATION_STATE_POSCTL
 		command.param2 = (float)PX4_CUSTOM_MAIN_MODE_POSCTL;
 		break;

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -256,6 +256,11 @@ FixedwingPositionControl::manual_control_setpoint_poll()
 		_manual_control_setpoint_altitude = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f);
 		_manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f;
 	}
+
+	// in airspeed mode the airspeed setpoint is taken from the pitch stick
+	if (!_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_airspeed_enabled) {
+		_manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f;
+	}
 }
 
 
@@ -1117,6 +1122,33 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 		_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
 		_att_sp.yaw_body = 0;
 
+	} else if (_control_mode.flag_control_airspeed_enabled) {
+		/* AIRSPEED CONTROL: pitch stick moves airspeed setpoint, throttle stick sets throttle */
+
+		_control_mode_current = FW_POSCTRL_MODE_AIRSPEED;
+
+		// if we assume that user is taking off then help by demanding altitude setpoint well above ground
+		// and set limit to pitch angle to prevent steering into ground
+		// this will only affect planes and not VTOL
+		float pitch_limit_min = _param_fw_p_lim_min.get();
+		// do_takeoff_help(&_hold_alt, &pitch_limit_min);
+
+		_tecs.set_speed_weight(2.0f);
+
+		tecs_update_pitch_throttle(now, _current_altitude,
+					   get_demanded_airspeed(),
+					   radians(_param_fw_p_lim_min.get()),
+					   radians(_param_fw_p_lim_max.get()),
+					   0.0f,
+					   1.0f,
+					   _param_fw_thr_cruise.get(),
+					   false,
+					   pitch_limit_min,
+					   tecs_status_s::TECS_MODE_NORMAL);
+
+		_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
+		_att_sp.yaw_body = 0;
+
 	} else {
 		_control_mode_current = FW_POSCTRL_MODE_OTHER;
 
@@ -1155,6 +1187,9 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 
 		_att_sp.thrust_body[0] = 0.0f;
 
+	} else if (_control_mode_current == FW_POSCTRL_MODE_AIRSPEED) {
+		_att_sp.thrust_body[0] = _manual_control_setpoint.z;
+
 	} else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
 		_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
 
@@ -1794,7 +1829,8 @@ FixedwingPositionControl::Run()
 			if (_control_mode.flag_control_position_enabled ||
 			    _control_mode.flag_control_velocity_enabled ||
 			    _control_mode.flag_control_acceleration_enabled ||
-			    _control_mode.flag_control_altitude_enabled) {
+			    _control_mode.flag_control_altitude_enabled ||
+			    _control_mode.flag_control_airspeed_enabled) {
 
 				const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 				q.copyTo(_att_sp.q_d);
@@ -1926,13 +1962,14 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 					      || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
 
 	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
-	bool in_air_alt_control = (!_landed &&
-				   (_control_mode.flag_control_auto_enabled ||
-				    _control_mode.flag_control_velocity_enabled ||
-				    _control_mode.flag_control_altitude_enabled));
+	const bool in_air_use_tecs = (!_landed &&
+				      (_control_mode.flag_control_auto_enabled ||
+				       _control_mode.flag_control_velocity_enabled ||
+				       _control_mode.flag_control_altitude_enabled ||
+				       _control_mode.flag_control_airspeed_enabled));
 
 	/* update TECS vehicle state estimates */
-	_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_alt_control,
+	_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_use_tecs,
 					     _current_altitude, _local_pos.vz);
 
 	/* scale throttle cruise by baro pressure */

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -261,6 +261,7 @@ private:
 	enum FW_POSCTRL_MODE {
 		FW_POSCTRL_MODE_AUTO,
 		FW_POSCTRL_MODE_POSITION,
+		FW_POSCTRL_MODE_AIRSPEED,
 		FW_POSCTRL_MODE_ALTITUDE,
 		FW_POSCTRL_MODE_OTHER
 	} _control_mode_current{FW_POSCTRL_MODE_OTHER};		///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.

src/modules/mavlink/mavlink_messages.cpp

@@ -285,6 +285,10 @@ union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED;
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_AIRSPEED:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AIRSPEED;
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;

src/modules/navigator/navigator_main.cpp

@@ -667,6 +667,7 @@ Navigator::run()
 		case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
 		case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
 		case vehicle_status_s::NAVIGATION_STATE_STAB:
+		case vehicle_status_s::NAVIGATION_STATE_AIRSPEED:
 		default:
 			navigation_mode_new = nullptr;
 			_can_loiter_at_sp = false;