Merged with master

4 files changed, 59 insertions, 156 deletions

diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp
index 1cc28b9dd..7be38015c 100644
--- a/src/modules/controllib/fixedwing.cpp
+++ b/src/modules/controllib/fixedwing.cpp
@@ -56,9 +56,9 @@ BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
 
 BlockYawDamper::~BlockYawDamper() {};
 
-void BlockYawDamper::update(float rCmd, float r)
+void BlockYawDamper::update(float rCmd, float r, float outputScale)
 {
-	_rudder = _r2Rdr.update(rCmd -
+	_rudder = outputScale*_r2Rdr.update(rCmd -
 				_rWashout.update(_rLowPass.update(r)));
 }
 
@@ -77,13 +77,13 @@ BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
 BlockStabilization::~BlockStabilization() {};
 
 void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
-				float p, float q, float r)
+				float p, float q, float r, float outputScale)
 {
-	_aileron = _p2Ail.update(
+	_aileron = outputScale*_p2Ail.update(
 			   pCmd - _pLowPass.update(p));
-	_elevator = _q2Elv.update(
+	_elevator = outputScale*_q2Elv.update(
 			    qCmd - _qLowPass.update(q));
-	_yawDamper.update(rCmd, r);
+	_yawDamper.update(rCmd, r, outputScale);
 }
 
 BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
@@ -156,21 +156,21 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
 	_theLimit(this, "THE"),
 	_vLimit(this, "V"),
 
-	// altitude/roc hold
+	// altitude/climb rate hold
 	_h2Thr(this, "H2THR"),
-	_roc2Thr(this, "ROC2THR"),
+	_cr2Thr(this, "CR2THR"),
 
 	// 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) */
+	_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"), 	/* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
+	_trimV(this, "TRIM_V"), 	/* FWB_ specific trim velocity (full name : FWB_TRIM_V) */
 
 	_vCmd(this, "V_CMD"),
-	_rocMax(this, "ROC_MAX"),
+	_crMax(this, "CR_MAX"),
 	_attPoll(),
 	_lastPosCmd(),
 	_timeStamp(0)
@@ -228,7 +228,15 @@ void BlockMultiModeBacksideAutopilot::update()
 		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
 
 		// 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);
+		// for the purpose of control we will limit the velocity feedback between
+		// the min/max velocity
+		float v = _vLimit.update(sqrtf(
+					_pos.vx * _pos.vx +
+					_pos.vy * _pos.vy +
+					_pos.vz * _pos.vz));
+
+		// limit velocity command between min/max velocity
+		float vCmd = _vLimit.update(_vCmd.get());
 
 		// altitude hold
 		float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
@@ -240,16 +248,19 @@ void BlockMultiModeBacksideAutopilot::update()
 
 		// velocity hold
 		// negative sign because nose over to increase speed
-		float thetaCmd = _theLimit.update(-_v2Theta.update(
-				_vLimit.update(_vCmd.get()) - v));
+		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
 		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
 
 		// yaw rate cmd
 		float rCmd = 0;
 
 		// stabilization
+		float velocityRatio = _trimV.get()/v;
+		float outputScale = velocityRatio*velocityRatio;
+		// this term scales the output based on the dynamic pressure change from trim
 		_stabilization.update(pCmd, qCmd, rCmd,
-				      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+				      _att.rollspeed, _att.pitchspeed, _att.yawspeed,
+				      outputScale);
 
 		// output
 		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
@@ -280,13 +291,18 @@ 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);
+			// for the purpose of control we will limit the velocity feedback between
+			// the min/max velocity
+			float v = _vLimit.update(sqrtf(
+						_pos.vx * _pos.vx +
+						_pos.vy * _pos.vy +
+						_pos.vz * _pos.vz));
 
 			// pitch 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.pitch - _pos.vz);
+			//float dThrottle = _cr2Thr.update(
+			//_crMax.get()*_manual.pitch - _pos.vz);
 
 			// roll channel -> bank angle
 			float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
@@ -294,8 +310,10 @@ void BlockMultiModeBacksideAutopilot::update()
 
 			// throttle channel -> velocity
 			// negative sign because nose over to increase speed
-			float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
-			float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
+			float vCmd = _vLimit.update(_manual.throttle *
+					(_vLimit.getMax() - _vLimit.getMin()) +
+					_vLimit.getMin());
+			float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
 			float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
 
 			// yaw rate cmd
diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/controllib/fixedwing.hpp
index 281cbb4cb..53d0cf893 100644
--- a/src/modules/controllib/fixedwing.hpp
+++ b/src/modules/controllib/fixedwing.hpp
@@ -193,7 +193,7 @@ public:
 	 * good idea to declare a member to store the temporary
 	 * variable.
 	 */
-	void update(float rCmd, float r);
+	void update(float rCmd, float r, float outputScale = 1.0);
 
 	/**
 	 * Rudder output value accessor
@@ -226,7 +226,8 @@ public:
 	BlockStabilization(SuperBlock *parent, const char *name);
 	virtual ~BlockStabilization();
 	void update(float pCmd, float qCmd, float rCmd,
-		    float p, float q, float r);
+		    float p, float q, float r,
+		    float outputScale = 1.0);
 	float getAileron() { return _aileron; }
 	float getElevator() { return _elevator; }
 	float getRudder() { return _yawDamper.getRudder(); }
@@ -310,9 +311,9 @@ private:
 	BlockLimit _theLimit;
 	BlockLimit _vLimit;
 
-	// altitude/ roc hold
+	// altitude/ climb rate hold
 	BlockPID _h2Thr;
-	BlockPID _roc2Thr;
+	BlockPID _cr2Thr;
 
 	// guidance
 	BlockWaypointGuidance _guide;
@@ -322,8 +323,9 @@ private:
 	BlockParam<float> _trimElv;
 	BlockParam<float> _trimRdr;
 	BlockParam<float> _trimThr;
+	BlockParam<float> _trimV;
 	BlockParam<float> _vCmd;
-	BlockParam<float> _rocMax;
+	BlockParam<float> _crMax;
 
 	struct pollfd _attPoll;
 	vehicle_global_position_setpoint_s _lastPosCmd;
diff --git a/src/modules/fixedwing_backside/params.c b/src/modules/fixedwing_backside/params.c
index 428b779b1..db30af416 100644
--- a/src/modules/fixedwing_backside/params.c
+++ b/src/modules/fixedwing_backside/params.c
@@ -59,13 +59,14 @@ 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 pitch stick is fully defelcted in simple mode
-PARAM_DEFINE_FLOAT(FWB_ROC_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f);
 
-//  rate of climb -> thr
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_P, 0.01f);   // rate of climb to throttle PID
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_I, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_D, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_D_LP, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_I_MAX, 0.0f);
+//  climb rate -> thr
+PARAM_DEFINE_FLOAT(FWB_CR2THR_P, 0.01f);   // rate of climb to throttle PID
+PARAM_DEFINE_FLOAT(FWB_CR2THR_I, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_CR2THR_D, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_CR2THR_D_LP, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_CR2THR_I_MAX, 0.0f);
 
-PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f);     // trim throttle (0,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f);    // trim throttle (0,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_V, 12.0f);     // trim velocity, m/s
diff --git a/src/modules/mavlink/mavlink_receiver.c b/src/modules/mavlink/mavlink_receiver.c
index e62e4dcc4..a42612f9e 100644
--- a/src/modules/mavlink/mavlink_receiver.c
+++ b/src/modules/mavlink/mavlink_receiver.c
@@ -308,82 +308,6 @@ handle_message(mavlink_message_t *msg)
 
 		uint64_t timestamp = hrt_absolute_time();
 
-		/* TODO, set ground_press/ temp during calib */
-		static const float ground_press = 1013.25f; // mbar
-		static const float ground_tempC = 21.0f;
-		static const float ground_alt = 0.0f;
-		static const float T0 = 273.15f;
-		static const float R = 287.05f;
-		static const float g = 9.806f;
-
-		if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) {
-
-			mavlink_raw_imu_t imu;
-			mavlink_msg_raw_imu_decode(msg, &imu);
-
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
-
-			/* sensors general */
-			hil_sensors.timestamp = imu.time_usec;
-
-			/* hil gyro */
-			static const float mrad2rad = 1.0e-3f;
-			hil_sensors.gyro_counter = hil_counter;
-			hil_sensors.gyro_raw[0] = imu.xgyro;
-			hil_sensors.gyro_raw[1] = imu.ygyro;
-			hil_sensors.gyro_raw[2] = imu.zgyro;
-			hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad;
-			hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad;
-			hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad;
-
-			/* accelerometer */
-			hil_sensors.accelerometer_counter = hil_counter;
-			static const float mg2ms2 = 9.8f / 1000.0f;
-			hil_sensors.accelerometer_raw[0] = imu.xacc;
-			hil_sensors.accelerometer_raw[1] = imu.yacc;
-			hil_sensors.accelerometer_raw[2] = imu.zacc;
-			hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc;
-			hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc;
-			hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc;
-			hil_sensors.accelerometer_mode = 0; // TODO what is this?
-			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
-
-			/* adc */
-			hil_sensors.adc_voltage_v[0] = 0;
-			hil_sensors.adc_voltage_v[1] = 0;
-			hil_sensors.adc_voltage_v[2] = 0;
-
-			/* magnetometer */
-			float mga2ga = 1.0e-3f;
-			hil_sensors.magnetometer_counter = hil_counter;
-			hil_sensors.magnetometer_raw[0] = imu.xmag;
-			hil_sensors.magnetometer_raw[1] = imu.ymag;
-			hil_sensors.magnetometer_raw[2] = imu.zmag;
-			hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga;
-			hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga;
-			hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga;
-			hil_sensors.magnetometer_range_ga = 32.7f; // int16
-			hil_sensors.magnetometer_mode = 0; // TODO what is this
-			hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
-
-			/* publish */
-			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
-
-			// increment counters
-			hil_counter += 1 ;
-			hil_frames += 1 ;
-
-			// output
-			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil imu at %d hz\n", hil_frames/10);
-				old_timestamp = timestamp;
-				hil_frames = 0;
-			}
-		}
-
 		if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) {
 
 			mavlink_highres_imu_t imu;
@@ -437,13 +361,9 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.magnetometer_mode = 0; // TODO what is this
 			hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
 
+			/* baro */
 			hil_sensors.baro_pres_mbar = imu.abs_pressure;
-
-			float tempC =  imu.temperature;
-			float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
-			float h =  ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure);
-
-			hil_sensors.baro_alt_meter = h;
+			hil_sensors.baro_alt_meter = imu.pressure_alt;
 			hil_sensors.baro_temp_celcius = imu.temperature;
 
 			hil_sensors.gyro_counter = hil_counter;
@@ -516,44 +436,6 @@ handle_message(mavlink_message_t *msg)
 			}
 		}
 
-		if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) {
-
-			mavlink_raw_pressure_t press;
-			mavlink_msg_raw_pressure_decode(msg, &press);
-
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
-
-			/* sensors general */
-			hil_sensors.timestamp = press.time_usec;
-
-			/* baro */
-
-			float tempC =  press.temperature / 100.0f;
-			float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
-			float h =  ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs);
-			hil_sensors.baro_counter = hil_counter;
-			hil_sensors.baro_pres_mbar = press.press_abs;
-			hil_sensors.baro_alt_meter = h;
-			hil_sensors.baro_temp_celcius = tempC;
-
-			/* publish */
-			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
-
-			// increment counters
-			hil_counter += 1 ;
-			hil_frames += 1 ;
-
-			// output
-			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil pressure at %d hz\n", hil_frames/10);
-				old_timestamp = timestamp;
-				hil_frames = 0;
-			}
-		}
-
 		if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
 
 			mavlink_hil_state_t hil_state;