mavlink: bugs fixed

3 files changed, 23 insertions, 16 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 4f4bf8691..f3246c380 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -146,7 +146,7 @@ Mavlink::Mavlink() :
 	   _baudrate(57600),
 	   _datarate(1000),
 	   _datarate_events(500),
-	   _rate_mult(0.0f),
+	   _rate_mult(1.0f),
 	   _mavlink_param_queue_index(0),
 	   mavlink_link_termination_allowed(false),
 	   _subscribe_to_stream(nullptr),
@@ -724,7 +724,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
 	_last_write_try_time = hrt_absolute_time();
 
 	/* check if there is space in the buffer, let it overflow else */
-	if (buf_free >= TX_BUFFER_GAP) {
+	if (buf_free < TX_BUFFER_GAP) {
 		warnx("SKIP msgid %i, %i bytes", msgid, packet_len);
 		/* no enough space in buffer to send */
 		count_txerr();
@@ -749,9 +749,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
 	uint16_t checksum;
 	crc_init(&checksum);
 	crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
-#if MAVLINK_CRC_EXTRA
 	crc_accumulate(mavlink_message_crcs[msgid], &checksum);
-#endif
 
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
@@ -1127,7 +1125,7 @@ Mavlink::update_rate_mult()
 		}
 	}
 
-	_rate_mult = ((float)_datarate - const_rate) / (rate - const_rate);
+	_rate_mult = ((float)_datarate - const_rate) / rate;
 }
 
 int
@@ -1250,8 +1248,6 @@ Mavlink::task_main(int argc, char *argv[])
 		break;
 	}
 
-	update_rate_mult();
-
 	warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
 
 	/* flush stdout in case MAVLink is about to take it over */
@@ -1328,7 +1324,7 @@ Mavlink::task_main(int argc, char *argv[])
 	switch (_mode) {
 	case MAVLINK_MODE_NORMAL:
 		configure_stream("SYS_STATUS", 1.0f);
-		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
+		/*configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
 		configure_stream("HIGHRES_IMU", 1.0f);
 		configure_stream("ATTITUDE", 10.0f);
 		configure_stream("VFR_HUD", 8.0f);
@@ -1340,6 +1336,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f);
 		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
+		*/
 		break;
 
 	case MAVLINK_MODE_CAMERA:
@@ -1353,9 +1350,6 @@ Mavlink::task_main(int argc, char *argv[])
 		break;
 	}
 
-	/* don't send parameters on startup without request */
-	_mavlink_param_queue_index = param_count();
-
 	float base_rate_mult = _datarate / 1000.0f;
 
 	MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult);
@@ -1374,6 +1368,9 @@ Mavlink::task_main(int argc, char *argv[])
 
 		hrt_abstime t = hrt_absolute_time();
 
+		update_rate_mult();
+		warnx("rate mult %f", (double)_rate_mult);
+
 		if (param_sub->update(&param_time, nullptr)) {
 			/* parameters updated */
 			mavlink_update_system();
@@ -1410,7 +1407,8 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		if (fast_rate_limiter.check(t)) {
-			_mission_manager->eventloop();
+			// TODO missions
+			//_mission_manager->eventloop();
 
 			if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
 				struct mavlink_logmessage msg;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index a8b4d11cc..20b1a966f 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -305,7 +305,7 @@ protected:
 		msg.custom_mode = 0;
 		get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
 		msg.type = mavlink_system.type;
-		msg.autopilot = mavlink_system.type;
+		msg.autopilot = MAV_AUTOPILOT_PX4;
 		msg.mavlink_version = 3;
 
 		_mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg);
@@ -352,7 +352,7 @@ protected:
 	{}
 
 	void subscribe() {
-		_cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+		_cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_command));
 	}
 
 	void send(const hrt_abstime t)
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index da3a55172..5b9e45d3e 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -71,12 +71,21 @@ int
 MavlinkStream::update(const hrt_abstime t)
 {
 	uint64_t dt = t - _last_sent;
-	unsigned int interval = _interval * _mavlink->get_rate_mult();
+	unsigned int interval = _interval;
+
+	if (!const_rate()) {
+		interval /= _mavlink->get_rate_mult();
+	}
 
 	if (dt > 0 && dt >= interval) {
 		/* interval expired, send message */
 		send(t);
-		_last_sent = t;
+		if (const_rate()) {
+			_last_sent = (t / _interval) * _interval;
+
+		} else {
+			_last_sent = t;
+		}
 
 		return 0;
 	}