Merge branch 'master' of github.com:PX4/Firmware into gpio_led_fmuv2

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -477,6 +477,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						dt = 0.005f;
 						parameters_update(&ekf_param_handles, &ekf_params);
 
+						/* update mag declination rotation matrix */
+						R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
+
 						x_aposteriori_k[0] = z_k[0];
 						x_aposteriori_k[1] = z_k[1];
 						x_aposteriori_k[2] = z_k[2];

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -166,6 +166,15 @@ private:
 		float airspeed_min;
 		float airspeed_trim;
 		float airspeed_max;
+
+		float trim_roll;
+		float trim_pitch;
+		float trim_yaw;
+		float rollsp_offset_deg;			/**< Roll Setpoint Offset in deg */
+		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
+		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
+		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -197,6 +206,12 @@ private:
 		param_t airspeed_min;
 		param_t airspeed_trim;
 		param_t airspeed_max;
+
+		param_t trim_roll;
+		param_t trim_pitch;
+		param_t trim_yaw;
+		param_t rollsp_offset_deg;
+		param_t pitchsp_offset_deg;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -335,6 +350,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 	_parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");
 
+	_parameter_handles.trim_roll = param_find("TRIM_ROLL");
+	_parameter_handles.trim_pitch = param_find("TRIM_PITCH");
+	_parameter_handles.trim_yaw = param_find("TRIM_YAW");
+	_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
+	_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -395,6 +416,15 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
 	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
 
+	param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
+	param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
+	param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
+	param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
+	param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
+	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
+	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
+
+
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
 	_pitch_ctrl.set_k_p(_parameters.p_p);
@@ -648,13 +678,13 @@ FixedwingAttitudeControl::task_main()
 				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
 				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
 
-				float roll_sp = 0.0f;
-				float pitch_sp = 0.0f;
+				float roll_sp = _parameters.rollsp_offset_rad;
+				float pitch_sp = _parameters.pitchsp_offset_rad;
 				float throttle_sp = 0.0f;
 
 				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
-					roll_sp = _att_sp.roll_body;
-					pitch_sp = _att_sp.pitch_body;
+					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
+					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
@@ -670,9 +700,13 @@ FixedwingAttitudeControl::task_main()
 					 * With this mapping the stick angle is a 1:1 representation of
 					 * the commanded attitude. If more than 45 degrees are desired,
 					 * a scaling parameter can be applied to the remote.
+					 *
+					 * The trim gets subtracted here from the manual setpoint to get
+					 * the intended attitude setpoint. Later, after the rate control step the
+					 * trim is added again to the control signal.
 					 */
-					roll_sp = _manual.roll * 0.75f;
-					pitch_sp = _manual.pitch * 0.75f;
+					roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad;
+					pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad;
 					throttle_sp = _manual.throttle;
 					_actuators.control[4] = _manual.flaps;
 
@@ -685,7 +719,7 @@ FixedwingAttitudeControl::task_main()
 					att_sp.timestamp = hrt_absolute_time();
 					att_sp.roll_body = roll_sp;
 					att_sp.pitch_body = pitch_sp;
-					att_sp.yaw_body = 0.0f;
+					att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
 					att_sp.thrust = throttle_sp;
 
 					/* lazily publish the setpoint only once available */
@@ -719,12 +753,12 @@ FixedwingAttitudeControl::task_main()
 							speed_body_u, speed_body_v, speed_body_w,
 							_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
 
-					/* Run attitude RATE controllers which need the desired attitudes from above */
+					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
 					float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
 							_att.rollspeed, _att.yawspeed,
 							_yaw_ctrl.get_desired_rate(),
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f;
+					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
 					if (!isfinite(roll_u)) {
 						warnx("roll_u %.4f", roll_u);
 					}
@@ -733,7 +767,7 @@ FixedwingAttitudeControl::task_main()
 							_att.pitchspeed, _att.yawspeed,
 							_yaw_ctrl.get_desired_rate(),
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f;
+					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
 					if (!isfinite(pitch_u)) {
 						warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
 								pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
@@ -743,7 +777,7 @@ FixedwingAttitudeControl::task_main()
 							_att.pitchspeed, _att.yawspeed,
 							_pitch_ctrl.get_desired_rate(),
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f;
+					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
 					if (!isfinite(yaw_u)) {
 						warnx("yaw_u %.4f", yaw_u);
 					}

src/modules/fw_att_control/fw_att_control_params.c

@@ -176,3 +176,13 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
 // @Description		If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
 // @Range		0.0 to 30
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
+
+// @DisplayName		Roll Setpoint Offset
+// @Description		An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe
+// @Range		-90.0 to 90.0
+PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
+
+// @DisplayName		Pitch Setpoint Offset
+// @Description		An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
+// @Range		-90.0 to 90.0
+PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -186,7 +186,7 @@ private:
 	float target_bearing;
 
 	/* Launch detection */
-	LaunchDetector launchDetector;
+	launchdetection::LaunchDetector launchDetector;
 
 	/* throttle and airspeed states */
 	float _airspeed_error;				///< airspeed error to setpoint in m/s
@@ -350,12 +350,12 @@ private:
 	/*
 	 * Reset takeoff state
 	 */
-	int reset_takeoff_state();
+	void reset_takeoff_state();
 
 	/*
 	 * Reset landing state
 	 */
-	int reset_landing_state();
+	void reset_landing_state();
 };
 
 namespace l1_control
@@ -989,6 +989,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				} else	{
 					/* no takeoff detection --> fly */
 					launch_detected = true;
+					warnx("launchdetection off");
 				}
 			}
 
@@ -1311,14 +1312,14 @@ FixedwingPositionControl::task_main()
 	_exit(0);
 }
 
-int FixedwingPositionControl::reset_takeoff_state()
+void FixedwingPositionControl::reset_takeoff_state()
 {
 	launch_detected = false;
 	usePreTakeoffThrust = false;
 	launchDetector.reset();
 }
 
-int FixedwingPositionControl::reset_landing_state()
+void FixedwingPositionControl::reset_landing_state()
 {
 	land_noreturn_horizontal = false;
 	land_noreturn_vertical = false;

src/modules/navigator/geofence.cpp

@@ -55,11 +55,13 @@
 #endif
 static const int ERROR = -1;
 
-Geofence::Geofence() : _fence_pub(-1),
+Geofence::Geofence() :
+		SuperBlock(NULL, "GF"),
+		_fence_pub(-1),
 		_altitude_min(0),
 		_altitude_max(0),
 		_verticesCount(0),
-		param_geofence_on(NULL, "GF_ON", false)
+		param_geofence_on(this, "ON")
 {
 	/* Load initial params */
 	updateParams();
@@ -292,8 +294,3 @@ int Geofence::clearDm()
 {
 	dm_clear(DM_KEY_FENCE_POINTS);
 }
-
-void Geofence::updateParams()
-{
-	param_geofence_on.update();
-}

src/modules/navigator/geofence.h

@@ -41,11 +41,13 @@
 #define GEOFENCE_H_
 
 #include <uORB/topics/fence.h>
+#include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
 #define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt"
 
-class Geofence {
+class Geofence : public control::SuperBlock
+{
 private:
 	orb_advert_t	_fence_pub;			/**< publish fence topic */
 
@@ -85,8 +87,6 @@ public:
 	int loadFromFile(const char *filename);
 
 	bool isEmpty() {return _verticesCount == 0;}
-
-	void updateParams();
 };
 
 

src/modules/position_estimator_inav/inertial_filter.c

@@ -15,10 +15,7 @@ void inertial_filter_predict(float dt, float x[3])
 
 void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
 {
-	float ewdt = w * dt;
-	if (ewdt > 1.0f)
-		ewdt = 1.0f;	// prevent over-correcting
-	ewdt *= e;
+	float ewdt = e * w * dt;
 	x[i] += ewdt;
 
 	if (i == 0) {

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -623,7 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		}
 
 		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
-		dt = fmaxf(fminf(0.02, dt), 0.005);
+		dt = fmaxf(fminf(0.02, dt), 0.002);		// constrain dt from 2 to 20 ms
 		t_prev = t;
 
 		/* use GPS if it's valid and reference position initialized */

src/modules/sdlog2/logbuffer.c

@@ -74,8 +74,9 @@ bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
 	// bytes available to write
 	int available = lb->read_ptr - lb->write_ptr - 1;
 
-	if (available < 0)
+	if (available < 0) {
 		available += lb->size;
+	}
 
 	if (size > available) {
 		// buffer overflow

src/modules/sdlog2/sdlog2_messages.h

@@ -267,13 +267,13 @@ struct log_DIST_s {
 /* --- TELE - TELEMETRY STATUS --- */
 #define LOG_TELE_MSG 22
 struct log_TELE_s {
-    uint8_t rssi;
-    uint8_t remote_rssi;
-    uint8_t noise;
-    uint8_t remote_noise;
-    uint16_t rxerrors;
-    uint16_t fixed;
-    uint8_t txbuf;
+	uint8_t rssi;
+	uint8_t remote_rssi;
+	uint8_t noise;
+	uint8_t remote_noise;
+	uint16_t rxerrors;
+	uint16_t fixed;
+	uint8_t txbuf;
 };
 
 /********** SYSTEM MESSAGES, ID > 0x80 **********/