ACKs for syslink radio parameters. Fixes #6005

src/modules/syslink/CMakeLists.txt

@@ -33,7 +33,7 @@
 px4_add_module(
 	MODULE modules__syslink
 	MAIN syslink
-	STACK_MAIN 2000
+	STACK_MAIN 1300
 	SRCS
 		syslink_main.cpp
 		syslink_bridge.cpp

src/modules/syslink/syslink_main.cpp

@@ -105,8 +105,9 @@ Syslink::Syslink() :
 	_rssi(RC_INPUT_RSSI_MAX),
 	_bstate(BAT_DISCHARGING)
 {
-	px4_sem_init(&radio_sem, 0, 0);
 	px4_sem_init(&memory_sem, 0, 0);
+
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 }
 
 
@@ -134,20 +135,7 @@ Syslink::set_datarate(uint8_t datarate)
 	msg.type = SYSLINK_RADIO_DATARATE;
 	msg.length = 1;
 	msg.data[0] = datarate;
-
+	return send_message(&msg);
-	if (send_message(&msg) != 0) {
-		return -1;
-	}
-
-	// Wait for a second
-//	struct timespec abstime = {1, 0};
-//	if(px4_sem_timedwait(&radio_sem, &abstime) != 0) {
-//		return -1;
-//	}
-
-
-
-	return OK;
 }
 
 int
@@ -190,6 +178,55 @@ Syslink::send_queued_raw_message()
 }
 
 
+void
+Syslink::update_params(bool force_set)
+{
+	param_t _param_radio_channel = param_find("SLNK_RADIO_CHAN");
+	param_t _param_radio_rate = param_find("SLNK_RADIO_RATE");
+	param_t _param_radio_addr1 = param_find("SLNK_RADIO_ADDR1");
+	param_t _param_radio_addr2 = param_find("SLNK_RADIO_ADDR2");
+
+
+	// reading parameter values into temp variables
+
+	uint32_t channel, rate, addr1, addr2;
+	uint64_t addr = 0;
+
+	param_get(_param_radio_channel, &channel);
+	param_get(_param_radio_rate, &rate);
+	param_get(_param_radio_addr1, &addr1);
+	param_get(_param_radio_addr2, &addr2);
+
+	memcpy(&addr, &addr2, 4); memcpy(((char *)&addr) + 4, &addr1, 4);
+
+
+	hrt_abstime t = hrt_absolute_time();
+
+	// request updates if needed
+
+	if (channel != this->_channel || force_set) {
+		this->_channel = channel;
+		set_channel(channel);
+		this->_params_update[0] = t;
+		this->_params_ack[0] = 0;
+	}
+
+	if (rate != this->_rate || force_set) {
+		this->_rate = rate;
+		set_datarate(rate);
+		this->_params_update[1] = t;
+		this->_params_ack[1] = 0;
+	}
+
+	if (addr != this->_addr || force_set) {
+		this->_addr = addr;
+		set_address(addr);
+		this->_params_update[2] = t;
+		this->_params_ack[2] = 0;
+	}
+
+
+}
 
 // 1M 8N1 serial connection to NRF51
 int
@@ -249,21 +286,6 @@ Syslink::task_main_trampoline(int argc, char *argv[])
 void
 Syslink::task_main()
 {
-	param_t _param_radio_channel = param_find("SLNK_RADIO_CHAN");
-	param_t _param_radio_rate = param_find("SLNK_RADIO_RATE");
-	param_t _param_radio_addr1 = param_find("SLNK_RADIO_ADDR1");
-	param_t _param_radio_addr2 = param_find("SLNK_RADIO_ADDR2");
-
-	uint32_t channel, rate, addr1, addr2;
-	uint64_t addr = 0;
-
-	param_get(_param_radio_channel, &channel);
-	param_get(_param_radio_rate, &rate);
-	param_get(_param_radio_addr1, &addr1);
-	param_get(_param_radio_addr2, &addr2);
-
-	memcpy(&addr, &addr2, 4); memcpy(((char *)&addr) + 4, &addr1, 4);
-
 	_bridge = new SyslinkBridge(this);
 	_bridge->init();
 
@@ -293,15 +315,13 @@ Syslink::task_main()
 
 	px4_arch_configgpio(GPIO_NRF_TXEN);
 
-	set_channel(channel);
+	px4_pollfd_struct_t fds[2];
-	set_datarate(rate);
-	set_address(addr);
-
-
-	px4_pollfd_struct_t fds[1];
 	fds[0].fd = _fd;
 	fds[0].events = POLLIN;
 
+	fds[1].fd = _params_sub;
+	fds[1].events = POLLIN;
+
 	int error_counter = 0;
 
 	char buf[64];
@@ -312,10 +332,11 @@ Syslink::task_main()
 
 	syslink_parse_init(&state);
 
+	// setup initial parameters
+	update_params(true);
 
 	while (_task_running) {
-		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
+		int poll_ret = px4_poll(fds, 2, 1000);
-		int poll_ret = px4_poll(fds, 1, 1000);
 
 		/* handle the poll result */
 		if (poll_ret == 0) {
@@ -342,6 +363,12 @@ Syslink::task_main()
 					}
 				}
 			}
+
+			if (fds[1].revents & POLLIN) {
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+				update_params(false);
+			}
 		}
 	}
 
@@ -419,8 +446,7 @@ Syslink::handle_message(syslink_message_t *msg)
 		_lastrxtime = t;
 
 	} else if ((msg->type & SYSLINK_GROUP) == SYSLINK_RADIO) {
-		radio_msg = *msg;
+		handle_radio(msg);
-		px4_sem_post(&radio_sem);
 
 	} else if ((msg->type & SYSLINK_GROUP) == SYSLINK_OW) {
 		memcpy(&_memory->msgbuf, msg, sizeof(syslink_message_t));
@@ -469,6 +495,36 @@ Syslink::handle_message(syslink_message_t *msg)
 		led_off(LED_TX);
 	}
 
+
+	// resend parameters if they haven't been acknowledged
+	if (_params_ack[0] == 0 && t - _params_update[0] > 10000) {
+		set_channel(_channel);
+
+	} else if (_params_ack[1] == 0 && t - _params_update[1] > 10000) {
+		set_datarate(_rate);
+
+	} else if (_params_ack[2] == 0 && t - _params_update[2] > 10000) {
+		set_address(_addr);
+	}
+
+}
+
+void
+Syslink::handle_radio(syslink_message_t *sys)
+{
+	hrt_abstime t = hrt_absolute_time();
+
+	// record acknowlegements to parameter messages
+	if (sys->type == SYSLINK_RADIO_CHANNEL) {
+		_params_ack[0] = t;
+
+	} else if (sys->type == SYSLINK_RADIO_DATARATE) {
+		_params_ack[1] = t;
+
+	} else if (sys->type == SYSLINK_RADIO_ADDRESS) {
+		_params_ack[2] = t;
+	}
+
 }
 
 void
@@ -598,7 +654,7 @@ Syslink::handle_raw_other(syslink_message_t *sys)
 		else if (c->channel == 1) { // Param read
 			// 0 is ok
 			c->data[1] = 0; // value
-			c->size = 1 + 3;
+			c->size = 1 + 2;
 			send_message(sys);
 		}
 
@@ -681,6 +737,14 @@ void status()
 	printf("- radio rx: %d p/s (%d null)\n", g_syslink->rxrate, g_syslink->nullrate);
 	printf("- radio tx: %d p/s\n\n", g_syslink->txrate);
 
+	printf("Parameter Status:\n");
+	const char *goodParam = "good";
+	const char *badParam = "fail!";
+	printf("- channel: %s\n", g_syslink->is_good(0) ? goodParam : badParam);
+	printf("- data rate: %s\n", g_syslink->is_good(1) != 0 ? goodParam : badParam);
+	printf("- address: %s\n\n", g_syslink->is_good(2) != 0 ? goodParam : badParam);
+
+
 	int deckfd = open(DECK_DEVICE_PATH, O_RDONLY);
 	int ndecks = 0; ioctl(deckfd, DECKIOGNUM, (unsigned long) &ndecks);
 	printf("Deck scan: (found %d)\n", ndecks);

src/modules/syslink/syslink_main.h

@@ -63,11 +63,11 @@ public:
 
 	int start();
 
-	// TODO: These should wait for an ACK
 	int set_datarate(uint8_t datarate);
 	int set_channel(uint8_t channel);
 	int set_address(uint64_t addr);
 
+	int is_good(int i) { return _params_ack != 0; }
 
 	int pktrate;
 	int nullrate;
@@ -83,6 +83,7 @@ private:
 
 	void handle_message(syslink_message_t *msg);
 	void handle_raw(syslink_message_t *sys);
+	void handle_radio(syslink_message_t *sys);
 
 	// Handles other types of messages that we don't really care about, but
 	// will be maintained with the bare minimum implementation for supporting
@@ -96,6 +97,7 @@ private:
 
 	int send_queued_raw_message();
 
+	void update_params(bool force_set);
 
 	int _syslink_task;
 	bool _task_running;
@@ -118,6 +120,14 @@ private:
 	SyslinkBridge *_bridge;
 	SyslinkMemory *_memory;
 
+	int _params_sub;
+
+	// Current parameter values
+	uint32_t _channel, _rate;
+	uint64_t _addr;
+	hrt_abstime _params_update[3]; // Time at which the parameters were updated
+	hrt_abstime _params_ack[3]; // Time at which the parameters were acknowledged by the nrf module
+
 	orb_advert_t _battery_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _cmd_pub;
@@ -129,10 +139,8 @@ private:
 	int32_t _rssi;
 	battery_state _bstate;
 
-	px4_sem_t radio_sem;
 	px4_sem_t memory_sem;
 
-	syslink_message_t radio_msg;
 	syslink_message_t memory_msg;
 
 	static int task_main_trampoline(int argc, char *argv[]);