FlightTask: apply gear switch depending on task

msg/vehicle_constraints.msg

@@ -7,4 +7,7 @@ float32 speed_up	# in meters/sec
 float32 speed_down	# in meters/sec
 float32 tilt    # in radians [0, PI]
 
-bool landing_gear_up # true when up, false when down
+int8 GEAR_DOWN = -1
+int8 GEAR_UP = 1
+int8 GEAR_KEEP = 0
+int8 landing_gear # Down, UP or KEEP

src/lib/FlightTasks/tasks/FlightTask.cpp

@@ -81,5 +81,5 @@ void FlightTask::_setDefaultConstraints()
 	_constraints.speed_up = MPC_Z_VEL_MAX_UP.get();
 	_constraints.speed_down = MPC_Z_VEL_MAX_DN.get();
 	_constraints.tilt = math::radians(MPC_TILTMAX_AIR.get());
-	_constraints.landing_gear_up = false;
+	_constraints.landing_gear = vehicle_constraints_s::GEAR_KEEP;
 }

src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp

@@ -127,6 +127,7 @@ void FlightTaskAutoLine::_generateLandSetpoints()
 	// set constraints
 	_constraints.tilt = MPC_TILTMAX_LND.get();
 	_constraints.speed_down = MPC_LAND_SPEED.get();
+	_constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
 }
 
 void FlightTaskAutoLine::_generateTakeoffSetpoints()
@@ -137,6 +138,8 @@ void FlightTaskAutoLine::_generateTakeoffSetpoints()
 
 	// set constraints
 	_constraints.speed_up = MPC_TKO_SPEED.get();
+	_constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
+
 }
 
 void FlightTaskAutoLine::_generateVelocitySetpoints()
@@ -153,6 +156,12 @@ void FlightTaskAutoLine::_generateSetpoints()
 	_updateInternalWaypoints();
 	_generateAltitudeSetpoints();
 	_generateXYsetpoints();
+
+	// during mission and reposition, raise the landing gears but only
+	// if altitude is high enough
+	if (_highEnoughForLandingGear()) {
+		_constraints.landing_gear = vehicle_constraints_s::GEAR_UP;
+	}
 }
 
 void FlightTaskAutoLine::_updateInternalWaypoints()
@@ -526,6 +535,12 @@ void FlightTaskAutoLine::_updateAltitudeAboveGround()
 	}
 }
 
+bool FlightTaskAutoLine::_highEnoughForLandingGear()
+{
+	// return true if altitude is above two meters
+	return _alt_above_ground > 2.0f;
+}
+
 void FlightTaskAutoLine::updateParams()
 {
 	FlightTaskAuto::updateParams();

src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp

@@ -83,7 +83,7 @@ protected:
 	void _generateLandSetpoints();
 	void _generateVelocitySetpoints();
 	void _generateTakeoffSetpoints();
-	void _updateInternalWaypoints(); /* Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
+	void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
 	void _generateSetpoints(); /**< Generate velocity and position setpoint for following line. */
 	void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */
 	void _updateAltitudeAboveGround(); /**< Computes altitude above ground based on sensors available. */
@@ -91,8 +91,9 @@ protected:
 	void updateParams() override; /**< See ModuleParam class */
 
 private:
-	float _getVelocityFromAngle(const float angle); /** Computes the speed at target depending on angle. */
-	void _reset(); /** Resets member variables to current vehicle state */
+	float _getVelocityFromAngle(const float angle); /**< Computes the speed at target depending on angle. */
+	bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
+	void _reset(); /**< Resets member variables to current vehicle state */
 	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
 
 };

src/lib/FlightTasks/tasks/FlightTaskManual.cpp

@@ -81,12 +81,38 @@ bool FlightTaskManual::_evaluateSticks()
 		_sticks_expo(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _stick_dz.get());
 		_sticks_expo(2) = math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _stick_dz.get());
 
+		// Only switch the landing gear up if the user switched from gear down to gear up.
+		// If the user had the switch in the gear up position and took off ignore it
+		// until he toggles the switch to avoid retracting the gear immediately on takeoff.
+		int8_t gear_switch = _sub_manual_control_setpoint->get().gear_switch;
+
+		if (!_constraints.landing_gear) {
+			if (gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
+				_applyGearSwitch(gear_switch);
+			}
+
+		} else {
+			_applyGearSwitch(gear_switch);
+		}
+
 		return true;
 
 	} else {
 		/* Timeout: set all sticks to zero */
 		_sticks.zero();
 		_sticks_expo.zero();
+		_constraints.landing_gear = vehicle_constraints_s::GEAR_KEEP;
 		return false;
 	}
 }
+
+void FlightTaskManual::_applyGearSwitch(uint8_t gswitch)
+{
+	if (gswitch == manual_control_setpoint_s::SWITCH_POS_OFF) {
+		_constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
+	}
+
+	if (gswitch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		_constraints.landing_gear = vehicle_constraints_s::GEAR_UP;
+	}
+}

src/lib/FlightTasks/tasks/FlightTaskManual.hpp

@@ -63,9 +63,11 @@ protected:
 	matrix::Vector3f _sticks_expo; /**< modified manual sticks using expo function*/
 
 	float stickDeadzone() const { return _stick_dz.get(); }
+
 private:
 
 	bool _evaluateSticks(); /**< checks and sets stick inputs */
+	void _applyGearSwitch(uint8_t gswitch); /**< Sets gears according to switch */
 
 	uORB::Subscription<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};