mavlink: use command to set SiK ID

This removes the param MAV_SIK_RADIO_ID and replaces it with a MAVLink
command to set the radio's net ID.

Using a command has the benefit that we get feedback whether the change
has been accepted. The code now also reads back the bytes after doing
the config in order to check for the radio's OK feedback.

Previously, the commands were just sent blindly and there is the chance
they did not actually get through.

Signed-off-by: Julian Oes <julian@oes.ch>

src/modules/mavlink/mavlink_receiver.cpp

@@ -3213,6 +3213,13 @@ MavlinkReceiver::run()
 				/* non-blocking read. read may return negative values */
 				nread = ::read(fds[0].fd, buf, sizeof(buf));
 
+				parse_for_ok(nread, (const char *)buf);
+
+				if (waiting_for_ok()) {
+					// Don't bother with the rest at the moment, we only wait for an ok.
+					continue;
+				}
+
 				if (nread == -1 && errno == ENOTCONN) { // Not connected (can happen for USB)
 					usleep(100000);
 				}
@@ -3579,3 +3586,62 @@ void MavlinkReceiver::stop()
 	_should_exit.store(true);
 	pthread_join(_thread, nullptr);
 }
+
+void MavlinkReceiver::parse_for_ok(int nread, const char *buf)
+{
+	if (nread < 1) {
+		return;
+	}
+
+	for (int i = 0; i < nread; ++i) {
+		switch (_parse_state) {
+		case OkParseState::None:
+			// Nothing to do.
+			break;
+
+		case OkParseState::Waiting:
+			if (buf[i] == 'O') {
+				_parse_state = OkParseState::GotO;
+			}
+
+			break;
+
+		case OkParseState::GotO:
+			if (buf[i] == 'K') {
+				_parse_state = OkParseState::GotK;
+
+			} else {
+				_parse_state = OkParseState::None;
+			}
+
+			break;
+
+		case OkParseState::GotK:
+			break;
+
+		default:
+			break;
+
+
+		};
+	}
+}
+
+void MavlinkReceiver::wait_for_ok()
+{
+	_parse_state = OkParseState::Waiting;
+}
+
+bool MavlinkReceiver::got_ok()
+{
+	if (_parse_state == OkParseState::GotK) {
+		_parse_state = OkParseState::None;
+		return true;
+	}
+
+	return false;
+}
+bool MavlinkReceiver::waiting_for_ok() const
+{
+	return _parse_state == OkParseState::Waiting;
+}