Ultrasound timeout demo
Dependencies: F7_Ethernet HCSR04 MQTT mbed
Fork of Nucleo_F746ZG_Ethernet by
main.cpp
- Committer:
- EmbeddedSam
- Date:
- 2017-01-30
- Revision:
- 5:8eba0e0f29e3
- Parent:
- 3:e5716d0d5e36
File content as of revision 5:8eba0e0f29e3:
/* STM32 Nucleo MQTT Ultrasound Demonstration Author : Sam Walsh Date: 24/10/16 Platform: STM32F746ZG NUCLEO (ARM Cortex M7 @ 216MHz) Description: This code exercises the ethernet and digital IO on the F7 Nucleo by connecting to an MQTT broker automatically through the ethernet and then publishing values from a sensor (ultrasound sensor HCSR04) Although not fully tested it is publishing values from the sensor to the MQTT broker (Raspberry Pi 3 connected on a 100Mb LAN) at a rate of about 10Hz, this is limited by the timeout functionality I have added to the default HCSR04 software - it usually waits for an echo indefinitely which leads to lags in program, so I added some timeouts (10ms) to each echo to stop that. These could be tweaked to gain more time I think. I have benchmarked the Nucleo to be able to send messages to the Pi at a rate of about 16k per second with a payload of just "0.5", it is the ultrasound limiting the speed of transfer <samuel.walsh@manchester.ac.uk> */ #include "mbed.h" #include "rtos.h" #include "EthernetInterface.h" #include <stdio.h> #include "hcsr04.h" #define MQTTCLIENT_QOS2 1 #include "MQTTEthernet.h" #include "MQTTClient.h" HCSR04 UltrasonicSensor_FrontLeft(D2,D3); //Trigger,Echo HCSR04 UltrasonicSensor_FrontCentre(D4,D5); //Trigger,Echo HCSR04 UltrasonicSensor_FrontRight(D6,D7); //Trigger,Echo HCSR04 UltrasonicSensor_BackLeft(PG_5,PG_6); //Trigger,Echo HCSR04 UltrasonicSensor_BackCentre(PE_0,PG_8); //Trigger,Echo HCSR04 UltrasonicSensor_BackRight(PF_15,PF_11); //Trigger,Echo Serial pc(USBTX, USBRX); float distance; EthernetInterface eth; DigitalOut led1(LED1); int arrivedcount = 0; //Callback function for arriving messages void messageArrived(MQTT::MessageData& md) { MQTT::Message &message = md.message; printf("\n\rMessage arrived: qos %d, retained %d, dup %d, packetid %d\n", message.qos, message.retained, message.dup, message.id); printf("\n\rPayload %.*s\n", message.payloadlen, (char*)message.payload); ++arrivedcount; } int main() { printf("\n\n*** Ethernet MQTT Demo for STM32F746ZG***\r\n"); printf("Starting to connect to Ethernet\r\n"); MQTTEthernet ipstack = MQTTEthernet(); MQTT::Client<MQTTEthernet, Countdown> client = MQTT::Client<MQTTEthernet, Countdown>(ipstack); //This is just my MQTT broker, this could be anything// char* hostname = "192.168.1.2"; int port = 1883; printf("Connecting to %s:%d\n", hostname, port); int rc = ipstack.connect(hostname, port); if (rc != 0) printf("rc from TCP connect is %d\n", rc); MQTTPacket_connectData data = MQTTPacket_connectData_initializer; data.MQTTVersion = 3; data.clientID.cstring = "mbed-sample"; data.username.cstring = "testuser"; data.password.cstring = "testpassword"; //Connect to broker printf("Connecting to MQTT Broker\r\n"); if ((rc = client.connect(data)) != 0) printf("rc from MQTT connect is %d\n", rc); //Start building Message MQTT::Message message; // QoS 0 char buf[100]; message.qos = MQTT::QOS0; message.retained = false; message.dup = false; //Publish ultrasonic distance while(1) { //I have not fully benchmarked this loop but by eye it seems to be publishing sensor data about 10Hz TODO:Speed it up as much as possible and properly characterise sensor update rates distance = UltrasonicSensor_FrontLeft.distance(); wait_ms(10); if(distance == 17182){ //this is just 999999 error code ran through distance calc, i should change this but printf("\n\rOut of range -> echo timed out"); distance = 99999; } else{ printf("\n\rDistance is: %0.2f", distance); } sprintf(buf, "%d", int(distance)); message.payload = (void*)buf; message.payloadlen = strlen(buf)+1; rc = client.publish("RobotSensors/Ultrasound/FrontLeft", message); //Read Front Centre distance = UltrasonicSensor_FrontCentre.distance(); wait_ms(10); if(distance == 17182){ //this is just 999999 error code ran through distance calc, i should change this but printf("\n\rOut of range -> echo timed out"); distance = 99999; } else{ printf("\n\rDistance is: %0.2f", distance); } sprintf(buf, "%d", int(distance)); message.payload = (void*)buf; message.payloadlen = strlen(buf)+1; rc = client.publish("RobotSensors/Ultrasound/FrontCentre", message); //Read Front Right distance = UltrasonicSensor_FrontRight.distance(); wait_ms(10); if(distance == 17182){ //this is just 999999 error code ran through distance calc, i should change this but printf("\n\rOut of range -> echo timed out"); distance = 99999; } else{ printf("\n\rDistance is: %0.2f", distance); } sprintf(buf, "%d", int(distance)); message.payload = (void*)buf; message.payloadlen = strlen(buf)+1; rc = client.publish("RobotSensors/Ultrasound/FrontRight", message); distance = UltrasonicSensor_BackLeft.distance(); wait_ms(10); if(distance == 17182){ //this is just 999999 error code ran through distance calc, i should change this but printf("\n\rOut of range -> echo timed out"); distance = 99999; } else{ printf("\n\rDistance is: %0.2f", distance); } sprintf(buf, "%d", int(distance)); message.payload = (void*)buf; message.payloadlen = strlen(buf)+1; rc = client.publish("RobotSensors/Ultrasound/BackLeft", message); //Read Front Centre distance = UltrasonicSensor_BackCentre.distance(); wait_ms(10); if(distance == 17182){ //this is just 999999 error code ran through distance calc, i should change this but printf("\n\rOut of range -> echo timed out"); distance = 99999; } else{ printf("\n\rDistance is: %0.2f", distance); } sprintf(buf, "%d", int(distance)); message.payload = (void*)buf; message.payloadlen = strlen(buf)+1; rc = client.publish("RobotSensors/Ultrasound/BackCentre", message); //Read Front Right distance = UltrasonicSensor_BackRight.distance(); wait_ms(10); if(distance == 17182){ //this is just 999999 error code ran through distance calc, i should change this but printf("\n\rOut of range -> echo timed out"); distance = 99999; } else{ printf("\n\rDistance is: %0.2f", distance); } sprintf(buf, "%d", int(distance)); message.payload = (void*)buf; message.payloadlen = strlen(buf)+1; rc = client.publish("RobotSensors/Ultrasound/BackRight", message); } //client.yield(100); // if ((rc = client.unsubscribe(topic)) != 0) // printf("rc from unsubscribe was %d\n", rc); // // if ((rc = client.disconnect()) != 0) // printf("rc from disconnect was %d\n", rc); // // ipstack.disconnect(); }