add generic wait to transmit SDM120 request

lib/sensor_sdm120.c

@@ -127,36 +127,73 @@ void sensor_sdm120_setup(void)
 	sensor_sdm120_measurement_received = false;
 }
 
-void sensor_sdm120_measurement_request(uint8_t address, enum sensor_sdm120_measurement_type_t type)
+/** send request to electricity meter
+ *  @param[in] slave electricity meter slave device address
+ *  @param[in] function ModBus function: 0x03 read two 16 bits holding registers, 0x04 read two 16 bits input registers, 0x10 write two 16 bits holding registers
+ *  @param[in] address register start point address
+ *  @param[in] value value to store in holding register (if function 0x10 is used)
+ *  @return if request is correct and transmission started
+ */
+static bool sensor_sdm120_transmit_request(uint8_t slave, uint8_t function, uint16_t address, float value)
 {
 	if (tx_used!=0) { // transmission is ongoing
-		return;
+		return false;
 	}
-	if (type>=SENSOR_SDM120_MAX) { // invalid type
-		return;
+	if (slave==0) { // broadcast request are not supported
+		return false;
 	}
-	uint8_t packet[6]; // buffer to build ModBus message (without error check)
-	packet[0] = address; // set slave device address
-	packet[1] = 0x04; // set function to read 3X registers
-	packet[2] = register_input[type]>>8; // set high register address
-	packet[3] = register_input[type]; // set low register address
+	if (function!=0x03 && function!=0x04 && function!=0x10) { // function not supported
+		return false;
+	}
+	if (address%2) { // even register addresses are not supported by device
+		return false;
+	}
+	uint8_t packet[11]; // buffer to build ModBus message (without error check)
+	uint8_t packet_size = 0; // ModBus message size (without error check)
+	packet[0] = slave; // set slave device address
+	packet[1] = function; // set function
+	packet[2] = address>>8; // set high register address
+	packet[3] = address; // set low register address
 	packet[4] = 0; // set high number of registers to read
 	packet[5] = 2; // set low number of register to read (the measurement are encoded using 32 bits IEE745 float, and register hold 16 bits, thus we want to read 2 registers
-	uint16_t crc = crc_modbus(packet, LENGTH(packet)); // compute error check
-	for (uint8_t i=0; i<LENGTH(packet); i++) {
-		tx_buffer[LENGTH(packet)-i+1] = packet[i]; // copy packet to tx buffer in reverse order (this is how sending is implemented)
+	if (function==0x03 || function==0x04) { // read register
+		packet_size = 6; // set message size
+	} else if (function==0x10) { // write register
+		packet[6] = 4; // byte count (writing two 16 bits registers)
+		// store little endian encoded value in big endian encoded data
+		uint8_t* data = (uint8_t*)&value;
+		packet[7] = data[3];
+		packet[8] = data[2];
+		packet[9] = data[1];
+		packet[10] = data[0];
+		packet_size = 11; // set message size
+	}
+	uint16_t crc = crc_modbus(packet, packet_size); // compute error check
+	for (uint8_t i=0; i<packet_size; i++) {
+		tx_buffer[packet_size-i+1] = packet[i]; // copy packet to tx buffer in reverse order (this is how sending is implemented)
 	}
 	tx_buffer[1] = crc; // set low error check
 	tx_buffer[0] = crc>>8; // set high error check
-	tx_used = LENGTH(packet)+2; // set request size
+	tx_used = packet_size+2; // set request size
 	rx_used = 0; // reset reset buffer
 	sensor_sdm120_measurement_received = false; // reset measurement flag
 	gpio_set(GPIO(SENSOR_SDM120_REDE_PORT),GPIO(SENSOR_SDM120_REDE_PIN)); // enable driver output and disable receive output
 	USART_SR(USART(SENSOR_SDM120_USART)) &= USART_SR_TXE; // clear interrupt flag
 	usart_enable_tx_interrupt(USART(SENSOR_SDM120_USART)); // enable interrupt to send other bytes
 	usart_send(USART(SENSOR_SDM120_USART),tx_buffer[--tx_used]); // start transmission
+	return true;
 }
 
+bool sensor_sdm120_measurement_request(uint8_t slave, enum sensor_sdm120_measurement_type_t type)
+{
+	if (type>=SENSOR_SDM120_MAX) { // invalid type
+		return false;
+	}
+	return sensor_sdm120_transmit_request(slave, 0x04, register_input[type], 0);
+}
+
+
+
 float sensor_sdm120_measurement_decode(void)
 {
 	float measurement = NAN; // decoded measurement to return (invalid in the beginning)

lib/sensor_sdm120.h

@@ -44,10 +44,11 @@ enum sensor_sdm120_measurement_type_t {
 /** setup peripherals to communicate with electricity meter */
 void sensor_sdm120_setup(void);
 /** request measurement from electricity meter
- *  @param[in] address electricity meter device address
+ *  @param[in] slave electricity meter slave device address
  *  @param[in] type measurement type to request
+ *  @return if transmission started
  */
-void sensor_sdm120_measurement_request(uint8_t address, enum sensor_sdm120_measurement_type_t type);
+bool sensor_sdm120_measurement_request(uint8_t slave, enum sensor_sdm120_measurement_type_t type);
 /** decode received measurement
  *  @return decoded measurement (NaN if invalid or no new measurement has been received)
  */