Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages

8 files changed, 26 insertions, 30 deletions

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 41942aacd..293690d27 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -165,7 +165,7 @@ Airspeed::probe()
 	*/
 	_retries = 4;
 	int ret = measure();
-	_retries = 2;
+	_retries = 0;
 	return ret;
 }
 
@@ -381,7 +381,10 @@ Airspeed::cycle_trampoline(void *arg)
 	Airspeed *dev = (Airspeed *)arg;
 
 	dev->cycle();
-	dev->update_status();
+	// XXX we do not know if this is
+	// really helping - do not update the
+	// subsys state right now
+	//dev->update_status();
 }
 
 void
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 46db788a6..926a8db2a 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 
 	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
 
-		float id = _rate_error * dt;
+		float id = _rate_error * dt * scaler;
 
 		/*
 		 * anti-windup: do not allow integrator to increase if actuator is at limit
@@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = _bodyrate_setpoint * _k_ff * scaler +
+		_rate_error * _k_p * scaler * scaler
+		+ integrator_constrained;  //scaler is proportional to 1/airspeed
 //	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 //	warnx("roll: _last_output %.4f", (double)_last_output);
 	return math::constrain(_last_output, -1.0f, 1.0f);
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 9894a34d7..94bd26f03 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch,
 
 	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
 
-		float id = _rate_error * dt;
+		float id = _rate_error * dt * scaler;
 
 		/*
 		* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch,
 	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = _bodyrate_setpoint * _k_ff * scaler +
+		_rate_error * _k_p * scaler * scaler
+		+ integrator_constrained;  //scaler is proportional to 1/airspeed
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 13c967b0c..183621c57 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -129,7 +129,7 @@ extern struct system_load_s system_load;
 #define POSITION_TIMEOUT		(2 * 1000 * 1000)	/**< consider the local or global position estimate invalid after 600ms */
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
 #define RC_TIMEOUT			500000
-#define DL_TIMEOUT			5 * 1000* 1000
+#define DL_TIMEOUT			(10 * 1000 * 1000)
 #define OFFBOARD_TIMEOUT		500000
 #define DIFFPRESS_TIMEOUT		2000000
 
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index a203102ec..8b7fc4650 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -218,6 +218,8 @@ Mavlink::Mavlink() :
 		errx(1, "instance ID is out of range");
 		break;
 	}
+
+	_rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 }
 
 Mavlink::~Mavlink()
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 6b3c3accb..adff41812 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -114,7 +114,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
-	_radio_status_available(false),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
@@ -691,9 +690,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 		} else {
 			orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
 		}
-
-		/* this means that heartbeats alone won't be published to the radio status no more */
-		_radio_status_available = true;
 	}
 }
 
@@ -735,25 +731,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 
 			struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
 
-			hrt_abstime tnow = hrt_absolute_time();
-
-			/* always set heartbeat, publish only if telemetry link not up */
-			tstatus.heartbeat_time = tnow;
-
-			/* if no radio status messages arrive, lets at least publish that heartbeats were received */
-			if (!_radio_status_available) {
-
-				tstatus.timestamp = tnow;
-				/* telem_time indicates the timestamp of a telemetry status packet and we got none */
-				tstatus.telem_time = 0;
-				tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+			/* set heartbeat time and topic time and publish -
+			 * the telem status also gets updated on telemetry events
+			 */
+			tstatus.timestamp = hrt_absolute_time();
+			tstatus.heartbeat_time = tstatus.timestamp;
 
-				if (_telemetry_status_pub < 0) {
-					_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
+			if (_telemetry_status_pub < 0) {
+				_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
-				} else {
-					orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
-				}
+			} else {
+				orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
 			}
 		}
 	}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index ed47a3a63..10661fa88 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -155,7 +155,6 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
-	bool _radio_status_available;
 	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index a4f17eebd..e005bf9c1 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -331,7 +331,7 @@ mag(int argc, char *argv[])
 
 	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
 
-	if (len < 1.0f || len > 3.0f) {
+	if (len < 0.25f || len > 3.0f) {
 		warnx("MAG scale error!");
 		return ERROR;
 	}