Added seatbelt fixedwing controller.

apps/controllib/fixedwing.cpp

@@ -86,113 +86,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
 	_yawDamper.update(rCmd, r);
 }
 
-BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_psi2Phi(this, "PSI2PHI"),
-	_phi2P(this, "PHI2P"),
-	_phiLimit(this, "PHI_LIM")
-{
-}
-
-BlockHeadingHold::~BlockHeadingHold() {};
-
-float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p)
-{
-	float psiError = _wrap_pi(psiCmd - psi);
-	float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
-	return _phi2P.update(phiCmd - phi);
-}
-
-BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_v2Theta(this, "V2THE"),
-	_theta2Q(this, "THE2Q"),
-	_theLimit(this, "THE"),
-	_vLimit(this, "V")
-{
-}
-
-BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {};
-
-float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q)
-{
-	// negative sign because nose over to increase speed
-	float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
-	return _theta2Q.update(thetaCmd - theta);
-}
-
-BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_v2Thr(this, "V2THR")
-{
-}
-
-BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {};
-
-float BlockVelocityHoldFrontside::update(float vCmd, float v)
-{
-	return _v2Thr.update(vCmd - v);
-}
-
-BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_h2Thr(this, "H2THR"),
-	_throttle(0)
-{
-}
-
-BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {};
-
-void BlockAltitudeHoldBackside::update(float hCmd, float h)
-{
-	_throttle = _h2Thr.update(hCmd - h);
-}
-
-BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_h2Theta(this, "H2THE"),
-	_theta2Q(this, "THE2Q")
-{
-}
-
-BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {};
-
-float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q)
-{
-	float thetaCmd = _h2Theta.update(hCmd - h);
-	return _theta2Q.update(thetaCmd - theta);
-}
-
-BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
-		const char *name,
-		BlockStabilization *stabilization) :
-	SuperBlock(parent, name),
-	_stabilization(stabilization),
-	_headingHold(this, ""),
-	_velocityHold(this, ""),
-	_altitudeHold(this, ""),
-	_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
-	_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
-	_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
-	_trimThr(this, "TRIM_THR", true) /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
-{
-}
-
-BlockBacksideAutopilot::~BlockBacksideAutopilot() {};
-
-void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd,
-				    float h, float v,
-				    float phi, float theta, float psi,
-				    float p, float q, float r)
-{
-	_altitudeHold.update(hCmd, h);
-	_stabilization->update(
-		_headingHold.update(psiCmd, phi, psi, p),
-		_velocityHold.update(vCmd, v, theta, q),
-		rCmd,
-		p, q, r);
-}
-
 BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
 	_xtYawLimit(this, "XT2YAW"),
@@ -251,9 +144,33 @@ BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
 BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
 	BlockUorbEnabledAutopilot(parent, name),
 	_stabilization(this, ""), // no name needed, already unique
-	_backsideAutopilot(this, "", &_stabilization),
+
+	// heading hold
+	_psi2Phi(this, "PSI2PHI"),
+	_phi2P(this, "PHI2P"),
+	_phiLimit(this, "PHI_LIM"),
+
+	// velocity hold
+	_v2Theta(this, "V2THE"),
+	_theta2Q(this, "THE2Q"),
+	_theLimit(this, "THE"),
+	_vLimit(this, "V"),
+
+	// altitude/roc hold
+	_h2Thr(this, "H2THR"),
+	_roc2Thr(this, "H2THR"),
+
+	// guidance block
 	_guide(this, ""),
+
+	// block params
+	_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
+	_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
+	_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
+	_trimThr(this, "TRIM_THR", true), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
+
 	_vCmd(this, "V_CMD"),
+	_rocMax(this, "ROC_MAX"),
 	_attPoll(),
 	_lastPosCmd(),
 	_timeStamp(0)
@@ -305,7 +222,7 @@ void BlockMultiModeBacksideAutopilot::update()
 
 	// handle autopilot modes
 	if (_status.state_machine == SYSTEM_STATE_AUTO ||
-		_status.state_machine == SYSTEM_STATE_STABILIZED) {
+	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
 
 		// update guidance
 		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
@@ -313,19 +230,32 @@ void BlockMultiModeBacksideAutopilot::update()
 		// calculate velocity, XXX should be airspeed, but using ground speed for now
 		float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
 
-		// commands
+		// altitude hold
+		float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
+
+		// heading hold
+		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
+		float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
+		float pCmd = _phi2P.update(phiCmd - _att.roll);
+
+		// velocity hold
+		// negative sign because nose over to increase speed
+		float thetaCmd = _theLimit.update(-_v2Theta.update(
+				_vLimit.update(_vCmd.get()) - v));
+		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
+
+		// yaw rate cmd
 		float rCmd = 0;
 
-		_backsideAutopilot.update(
+		// stabilization
-			_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
+		_stabilization.update(pCmd, qCmd, rCmd,
-			_pos.alt, v,
+				      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
-			_att.roll, _att.pitch, _att.yaw,
+
-			_att.rollspeed, _att.pitchspeed, _att.yawspeed
+		// output
-		);
+		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
-		_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
+		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
-		_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
+		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
-		_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
+		_actuators.control[CH_THR] = dThrottle + _trimThr.get();
-		_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
 
 		// XXX limit throttle to manual setting (safety) for now.
 		// If it turns out to be confusing, it can be removed later once
@@ -336,14 +266,12 @@ void BlockMultiModeBacksideAutopilot::update()
 		if (!_status.flag_hil_enabled) {
 			/* limit to value of manual throttle */
 			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-							_actuators.control[CH_THR] : _manual.throttle;
+						     _actuators.control[CH_THR] : _manual.throttle;
 		}
 
-
 	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
 
 		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-
 			_actuators.control[CH_AIL] = _manual.roll;
 			_actuators.control[CH_ELV] = _manual.pitch;
 			_actuators.control[CH_RDR] = _manual.yaw;
@@ -351,8 +279,60 @@ void BlockMultiModeBacksideAutopilot::update()
 
 		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
 
+			// calculate velocity, XXX should be airspeed, but using ground speed for now
+			float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
+
+			// throttle channel -> rate of climb
+			// TODO, might want to put a gain on this, otherwise commanding
+			// from +1 -> -1 m/s for rate of climb
+			//float dThrottle = _roc2Thr.update(
+			//_rocMax.get()*_manual.throttle - _pos.vz);
+
+			// roll channel -> bank angle
+			float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
+			float pCmd = _phi2P.update(phiCmd - _att.roll);
+
+			// pitch channel -> velocity
+			// negative sign because nose over to increase speed
+			float vCmd = _manual.pitch * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
+			float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
+			float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
+
+			// yaw rate cmd
+			float rCmd = 0;
+
+			// stabilization
+			_stabilization.update(pCmd, qCmd, rCmd,
+					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+			// output
+			_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+			_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+			_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+
+			// currenlty using manual throttle
+			// XXX if you enable this watch out, vz might be very noisy
+			//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
+			_actuators.control[CH_THR] = _manual.throttle;
+
+			// XXX limit throttle to manual setting (safety) for now.
+			// If it turns out to be confusing, it can be removed later once
+			// a first binary release can be targeted.
+			// This is not a hack, but a design choice.
+
+			/* do not limit in HIL */
+			if (!_status.flag_hil_enabled) {
+				/* limit to value of manual throttle */
+				_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+							     _actuators.control[CH_THR] : _manual.throttle;
+			}
+		}
+
+		// body rates controller, disabled for now
+		else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
+
 			_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
-			_att.rollspeed, _att.pitchspeed, _att.yawspeed);
+					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
 
 			_actuators.control[CH_AIL] = _stabilization.getAileron();
 			_actuators.control[CH_ELV] = _stabilization.getElevator();

apps/controllib/fixedwing.hpp

@@ -232,26 +232,6 @@ public:
 	float getRudder() { return _yawDamper.getRudder(); }
 };
 
-/**
- * Heading hold autopilot block.
- * Aircraft Control and Simulation, Stevens and Lewis
- * Heading hold, pg. 348
- */
-class __EXPORT BlockHeadingHold : public SuperBlock
-{
-private:
-	BlockP _psi2Phi;
-	BlockP _phi2P;
-	BlockLimitSym _phiLimit;
-public:
-	BlockHeadingHold(SuperBlock *parent, const char *name);
-	virtual ~BlockHeadingHold();
-	/**
-	 * returns pCmd
-	 */
-	float update(float psiCmd, float phi, float psi, float p);
-};
-
 /**
  * Frontside/ Backside Control Systems
  *
@@ -269,106 +249,6 @@ public:
  * than frontside at high speeds.
  */
 
-/**
- * Backside velocity hold autopilot block.
- * v -> theta -> q -> elevator
- */
-class __EXPORT BlockVelocityHoldBackside : public SuperBlock
-{
-private:
-	BlockPID _v2Theta;
-	BlockPID _theta2Q;
-	BlockLimit _theLimit;
-	BlockLimit _vLimit;
-public:
-	BlockVelocityHoldBackside(SuperBlock *parent, const char *name);
-	virtual ~BlockVelocityHoldBackside();
-	/**
-	 * returns qCmd
-	 */
-	float update(float vCmd, float v, float theta, float q);
-};
-
-/**
- * Frontside velocity hold autopilot block.
- * v -> throttle
- */
-class __EXPORT BlockVelocityHoldFrontside : public SuperBlock
-{
-private:
-	BlockPID _v2Thr;
-public:
-	BlockVelocityHoldFrontside(SuperBlock *parent, const char *name);
-	virtual ~BlockVelocityHoldFrontside();
-	/**
-	 * returns throttle
-	 */
-	float update(float vCmd, float v);
-};
-
-/**
- * Backside altitude hold autopilot block.
- * h -> throttle
- */
-class __EXPORT BlockAltitudeHoldBackside : public SuperBlock
-{
-private:
-	BlockPID _h2Thr;
-	float _throttle;
-public:
-	BlockAltitudeHoldBackside(SuperBlock *parent, const char *name);
-	virtual ~BlockAltitudeHoldBackside();
-	void update(float hCmd, float h);
-	float getThrottle() { return _throttle; }
-};
-
-/**
- * Frontside altitude hold autopilot block.
- * h -> theta > q -> elevator
- */
-class __EXPORT BlockAltitudeHoldFrontside : public SuperBlock
-{
-private:
-	BlockPID _h2Theta;
-	BlockPID _theta2Q;
-public:
-	BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name);
-	virtual ~BlockAltitudeHoldFrontside();
-	/**
-	 * return qCmd
-	 */
-	float update(float hCmd, float h, float theta, float q);
-};
-
-/**
- * Backside autopilot
- */
-class __EXPORT BlockBacksideAutopilot : public SuperBlock
-{
-private:
-	BlockStabilization *_stabilization;
-	BlockHeadingHold _headingHold;
-	BlockVelocityHoldBackside _velocityHold;
-	BlockAltitudeHoldBackside _altitudeHold;
-	BlockParam<float> _trimAil;
-	BlockParam<float> _trimElv;
-	BlockParam<float> _trimRdr;
-	BlockParam<float> _trimThr;
-public:
-	BlockBacksideAutopilot(SuperBlock *parent,
-			       const char *name,
-			       BlockStabilization *stabilization);
-	virtual ~BlockBacksideAutopilot();
-	void update(float hCmd, float vCmd, float rCmd, float psiCmd,
-		    float h, float v,
-		    float phi, float theta, float psi,
-		    float p, float q, float r);
-	float getRudder() { return _stabilization->getRudder() + _trimRdr.get(); }
-	float getAileron() { return _stabilization->getAileron() + _trimAil.get(); }
-	float getElevator() { return _stabilization->getElevator() + _trimElv.get(); }
-	float getThrottle() { return _altitudeHold.getThrottle() + _trimThr.get(); }
-};
-
 /**
  * Waypoint Guidance block
  */
@@ -416,10 +296,34 @@ public:
 class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
 {
 private:
+	// stabilization
 	BlockStabilization _stabilization;
-	BlockBacksideAutopilot _backsideAutopilot;
+
+	// heading hold
+	BlockP _psi2Phi;
+	BlockP _phi2P;
+	BlockLimitSym _phiLimit;
+
+	// velocity hold
+	BlockPID _v2Theta;
+	BlockPID _theta2Q;
+	BlockLimit _theLimit;
+	BlockLimit _vLimit;
+
+	// altitude/ roc hold
+	BlockPID _h2Thr;
+	BlockPID _roc2Thr;
+
+	// guidance
 	BlockWaypointGuidance _guide;
+
+	// block params
+	BlockParam<float> _trimAil;
+	BlockParam<float> _trimElv;
+	BlockParam<float> _trimRdr;
+	BlockParam<float> _trimThr;
 	BlockParam<float> _vCmd;
+	BlockParam<float> _rocMax;
 
 	struct pollfd _attPoll;
 	vehicle_global_position_setpoint_s _lastPosCmd;

apps/examples/control_demo/params.c

@@ -56,4 +56,9 @@ PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f);       // minimum commanded velocity
 PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f);       // commanded velocity
 PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f);       // maximum commanded velocity
 
+// rate of climb
+// this is what rate of climb is commanded (in m/s) 
+// when the throttle stick is fully defelcted in simple mode
+PARAM_DEFINE_FLOAT(FWB_ROC_MAX, 1.0f);
+
 PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f);     // trim throttle (0,1)