Sam Walsh / Mbed 2 deprecated Nucleo_F746ZG_Ethernet_MQTT_Ultrasound

Ultrasound timeout demo

Dependencies:   F7_Ethernet HCSR04 MQTT mbed

Fork of Nucleo_F746ZG_Ethernet by Dieter Graef

main.cpp@5:8eba0e0f29e3, 2017-01-30 (annotated)

Committer:
EmbeddedSam
Date:
Mon Jan 30 11:29:12 2017 +0000
Revision:
5:8eba0e0f29e3
Parent:
3:e5716d0d5e36 
WORKING WITHOUT SENSORS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EmbeddedSam 3:e5716d0d5e36 1 /*
EmbeddedSam 3:e5716d0d5e36 2 STM32 Nucleo MQTT Ultrasound Demonstration
EmbeddedSam 3:e5716d0d5e36 3 Author : Sam Walsh
EmbeddedSam 3:e5716d0d5e36 4 Date: 24/10/16
EmbeddedSam 3:e5716d0d5e36 5 Platform: STM32F746ZG NUCLEO (ARM Cortex M7 @ 216MHz)
EmbeddedSam 3:e5716d0d5e36 6
EmbeddedSam 3:e5716d0d5e36 7 Description: This code exercises the ethernet and digital IO on the F7 Nucleo
EmbeddedSam 3:e5716d0d5e36 8 by connecting to an MQTT broker automatically through the ethernet and then
EmbeddedSam 3:e5716d0d5e36 9 publishing values from a sensor (ultrasound sensor HCSR04)
EmbeddedSam 3:e5716d0d5e36 10
EmbeddedSam 3:e5716d0d5e36 11 Although not fully tested it is publishing values from the sensor to the MQTT broker
EmbeddedSam 3:e5716d0d5e36 12 (Raspberry Pi 3 connected on a 100Mb LAN) at a rate of about 10Hz, this is limited
EmbeddedSam 3:e5716d0d5e36 13 by the timeout functionality I have added to the default HCSR04 software - it usually
EmbeddedSam 3:e5716d0d5e36 14 waits for an echo indefinitely which leads to lags in program, so I added some timeouts (10ms)
EmbeddedSam 3:e5716d0d5e36 15 to each echo to stop that. These could be tweaked to gain more time I think.
EmbeddedSam 3:e5716d0d5e36 16
EmbeddedSam 3:e5716d0d5e36 17 I have benchmarked the Nucleo to be able to send messages to the Pi at a rate of about 16k per second
EmbeddedSam 3:e5716d0d5e36 18 with a payload of just "0.5", it is the ultrasound limiting the speed of transfer
EmbeddedSam 3:e5716d0d5e36 19
EmbeddedSam 3:e5716d0d5e36 20 <samuel.walsh@manchester.ac.uk>
EmbeddedSam 3:e5716d0d5e36 21 */
EmbeddedSam 3:e5716d0d5e36 22
DieterGraef 0:f9b6112278fe 23 #include "mbed.h"
DieterGraef 0:f9b6112278fe 24 #include "rtos.h"
DieterGraef 0:f9b6112278fe 25 #include "EthernetInterface.h"
DieterGraef 0:f9b6112278fe 26 #include <stdio.h>
EmbeddedSam 3:e5716d0d5e36 27 #include "hcsr04.h"
DieterGraef 0:f9b6112278fe 28
EmbeddedSam 3:e5716d0d5e36 29 #define MQTTCLIENT_QOS2 1
EmbeddedSam 3:e5716d0d5e36 30 #include "MQTTEthernet.h"
EmbeddedSam 3:e5716d0d5e36 31 #include "MQTTClient.h"
EmbeddedSam 3:e5716d0d5e36 32
EmbeddedSam 5:8eba0e0f29e3 33 HCSR04 UltrasonicSensor_FrontLeft(D2,D3); //Trigger,Echo
EmbeddedSam 5:8eba0e0f29e3 34 HCSR04 UltrasonicSensor_FrontCentre(D4,D5); //Trigger,Echo
EmbeddedSam 5:8eba0e0f29e3 35 HCSR04 UltrasonicSensor_FrontRight(D6,D7); //Trigger,Echo
EmbeddedSam 5:8eba0e0f29e3 36
EmbeddedSam 5:8eba0e0f29e3 37
EmbeddedSam 5:8eba0e0f29e3 38 HCSR04 UltrasonicSensor_BackLeft(PG_5,PG_6); //Trigger,Echo
EmbeddedSam 5:8eba0e0f29e3 39 HCSR04 UltrasonicSensor_BackCentre(PE_0,PG_8); //Trigger,Echo
EmbeddedSam 5:8eba0e0f29e3 40 HCSR04 UltrasonicSensor_BackRight(PF_15,PF_11); //Trigger,Echo
EmbeddedSam 5:8eba0e0f29e3 41 Serial pc(USBTX, USBRX);
EmbeddedSam 5:8eba0e0f29e3 42
EmbeddedSam 3:e5716d0d5e36 43 float distance;
EmbeddedSam 3:e5716d0d5e36 44
DieterGraef 0:f9b6112278fe 45 EthernetInterface eth;
DieterGraef 0:f9b6112278fe 46
DieterGraef 0:f9b6112278fe 47 DigitalOut led1(LED1);
DieterGraef 0:f9b6112278fe 48
EmbeddedSam 3:e5716d0d5e36 49 int arrivedcount = 0;
EmbeddedSam 3:e5716d0d5e36 50
EmbeddedSam 3:e5716d0d5e36 51 //Callback function for arriving messages
EmbeddedSam 3:e5716d0d5e36 52 void messageArrived(MQTT::MessageData& md)
EmbeddedSam 3:e5716d0d5e36 53 {
EmbeddedSam 3:e5716d0d5e36 54 MQTT::Message &message = md.message;
EmbeddedSam 3:e5716d0d5e36 55 printf("\n\rMessage arrived: qos %d, retained %d, dup %d, packetid %d\n", message.qos, message.retained, message.dup, message.id);
EmbeddedSam 3:e5716d0d5e36 56 printf("\n\rPayload %.*s\n", message.payloadlen, (char*)message.payload);
EmbeddedSam 3:e5716d0d5e36 57 ++arrivedcount;
EmbeddedSam 3:e5716d0d5e36 58 }
EmbeddedSam 3:e5716d0d5e36 59
DieterGraef 0:f9b6112278fe 60 int main()
DieterGraef 0:f9b6112278fe 61 {
EmbeddedSam 3:e5716d0d5e36 62 printf("\n\n*** Ethernet MQTT Demo for STM32F746ZG***\r\n");
EmbeddedSam 3:e5716d0d5e36 63 printf("Starting to connect to Ethernet\r\n");
EmbeddedSam 3:e5716d0d5e36 64
EmbeddedSam 3:e5716d0d5e36 65 MQTTEthernet ipstack = MQTTEthernet();
EmbeddedSam 3:e5716d0d5e36 66
EmbeddedSam 3:e5716d0d5e36 67 MQTT::Client<MQTTEthernet, Countdown> client = MQTT::Client<MQTTEthernet, Countdown>(ipstack);
EmbeddedSam 3:e5716d0d5e36 68
EmbeddedSam 3:e5716d0d5e36 69 //This is just my MQTT broker, this could be anything//
EmbeddedSam 5:8eba0e0f29e3 70 char* hostname = "192.168.1.2";
EmbeddedSam 3:e5716d0d5e36 71 int port = 1883;
EmbeddedSam 3:e5716d0d5e36 72 printf("Connecting to %s:%d\n", hostname, port);
EmbeddedSam 3:e5716d0d5e36 73 int rc = ipstack.connect(hostname, port);
EmbeddedSam 3:e5716d0d5e36 74 if (rc != 0)
EmbeddedSam 3:e5716d0d5e36 75 printf("rc from TCP connect is %d\n", rc);
EmbeddedSam 3:e5716d0d5e36 76
EmbeddedSam 3:e5716d0d5e36 77 MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
EmbeddedSam 3:e5716d0d5e36 78
EmbeddedSam 3:e5716d0d5e36 79 data.MQTTVersion = 3;
EmbeddedSam 3:e5716d0d5e36 80 data.clientID.cstring = "mbed-sample";
EmbeddedSam 3:e5716d0d5e36 81 data.username.cstring = "testuser";
EmbeddedSam 3:e5716d0d5e36 82 data.password.cstring = "testpassword";
EmbeddedSam 3:e5716d0d5e36 83
EmbeddedSam 3:e5716d0d5e36 84
EmbeddedSam 3:e5716d0d5e36 85 //Connect to broker
EmbeddedSam 3:e5716d0d5e36 86 printf("Connecting to MQTT Broker\r\n");
EmbeddedSam 3:e5716d0d5e36 87 if ((rc = client.connect(data)) != 0)
EmbeddedSam 3:e5716d0d5e36 88 printf("rc from MQTT connect is %d\n", rc);
EmbeddedSam 3:e5716d0d5e36 89
DieterGraef 0:f9b6112278fe 90
EmbeddedSam 3:e5716d0d5e36 91 //Start building Message
EmbeddedSam 3:e5716d0d5e36 92 MQTT::Message message;
EmbeddedSam 3:e5716d0d5e36 93 // QoS 0
EmbeddedSam 3:e5716d0d5e36 94 char buf[100];
EmbeddedSam 3:e5716d0d5e36 95 message.qos = MQTT::QOS0;
EmbeddedSam 3:e5716d0d5e36 96 message.retained = false;
EmbeddedSam 3:e5716d0d5e36 97 message.dup = false;
EmbeddedSam 3:e5716d0d5e36 98
EmbeddedSam 3:e5716d0d5e36 99 //Publish ultrasonic distance
EmbeddedSam 3:e5716d0d5e36 100 while(1)
EmbeddedSam 3:e5716d0d5e36 101 {
EmbeddedSam 3:e5716d0d5e36 102 //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
EmbeddedSam 5:8eba0e0f29e3 103 distance = UltrasonicSensor_FrontLeft.distance();
EmbeddedSam 5:8eba0e0f29e3 104 wait_ms(10);
EmbeddedSam 5:8eba0e0f29e3 105 if(distance == 17182){
EmbeddedSam 5:8eba0e0f29e3 106 //this is just 999999 error code ran through distance calc, i should change this but
EmbeddedSam 5:8eba0e0f29e3 107 printf("\n\rOut of range -> echo timed out");
EmbeddedSam 5:8eba0e0f29e3 108 distance = 99999;
EmbeddedSam 5:8eba0e0f29e3 109 }
EmbeddedSam 5:8eba0e0f29e3 110 else{
EmbeddedSam 5:8eba0e0f29e3 111 printf("\n\rDistance is: %0.2f", distance);
EmbeddedSam 5:8eba0e0f29e3 112 }
EmbeddedSam 5:8eba0e0f29e3 113
EmbeddedSam 5:8eba0e0f29e3 114 sprintf(buf, "%d", int(distance));
EmbeddedSam 5:8eba0e0f29e3 115 message.payload = (void*)buf;
EmbeddedSam 5:8eba0e0f29e3 116 message.payloadlen = strlen(buf)+1;
EmbeddedSam 5:8eba0e0f29e3 117 rc = client.publish("RobotSensors/Ultrasound/FrontLeft", message);
EmbeddedSam 5:8eba0e0f29e3 118
EmbeddedSam 5:8eba0e0f29e3 119 //Read Front Centre
EmbeddedSam 5:8eba0e0f29e3 120 distance = UltrasonicSensor_FrontCentre.distance();
EmbeddedSam 5:8eba0e0f29e3 121 wait_ms(10);
EmbeddedSam 3:e5716d0d5e36 122 if(distance == 17182){
EmbeddedSam 5:8eba0e0f29e3 123 //this is just 999999 error code ran through distance calc, i should change this but
EmbeddedSam 3:e5716d0d5e36 124 printf("\n\rOut of range -> echo timed out");
EmbeddedSam 5:8eba0e0f29e3 125 distance = 99999;
EmbeddedSam 5:8eba0e0f29e3 126 }
EmbeddedSam 5:8eba0e0f29e3 127 else{
EmbeddedSam 5:8eba0e0f29e3 128 printf("\n\rDistance is: %0.2f", distance);
EmbeddedSam 5:8eba0e0f29e3 129 }
EmbeddedSam 5:8eba0e0f29e3 130
EmbeddedSam 5:8eba0e0f29e3 131 sprintf(buf, "%d", int(distance));
EmbeddedSam 5:8eba0e0f29e3 132 message.payload = (void*)buf;
EmbeddedSam 5:8eba0e0f29e3 133 message.payloadlen = strlen(buf)+1;
EmbeddedSam 5:8eba0e0f29e3 134 rc = client.publish("RobotSensors/Ultrasound/FrontCentre", message);
EmbeddedSam 5:8eba0e0f29e3 135
EmbeddedSam 5:8eba0e0f29e3 136 //Read Front Right
EmbeddedSam 5:8eba0e0f29e3 137 distance = UltrasonicSensor_FrontRight.distance();
EmbeddedSam 5:8eba0e0f29e3 138 wait_ms(10);
EmbeddedSam 5:8eba0e0f29e3 139 if(distance == 17182){
EmbeddedSam 5:8eba0e0f29e3 140 //this is just 999999 error code ran through distance calc, i should change this but
EmbeddedSam 5:8eba0e0f29e3 141 printf("\n\rOut of range -> echo timed out");
EmbeddedSam 5:8eba0e0f29e3 142 distance = 99999;
DieterGraef 0:f9b6112278fe 143 }
EmbeddedSam 3:e5716d0d5e36 144 else{
EmbeddedSam 3:e5716d0d5e36 145 printf("\n\rDistance is: %0.2f", distance);
DieterGraef 0:f9b6112278fe 146 }
EmbeddedSam 3:e5716d0d5e36 147
EmbeddedSam 5:8eba0e0f29e3 148 sprintf(buf, "%d", int(distance));
EmbeddedSam 5:8eba0e0f29e3 149 message.payload = (void*)buf;
EmbeddedSam 5:8eba0e0f29e3 150 message.payloadlen = strlen(buf)+1;
EmbeddedSam 5:8eba0e0f29e3 151 rc = client.publish("RobotSensors/Ultrasound/FrontRight", message);
EmbeddedSam 5:8eba0e0f29e3 152
EmbeddedSam 5:8eba0e0f29e3 153
EmbeddedSam 5:8eba0e0f29e3 154
EmbeddedSam 5:8eba0e0f29e3 155 distance = UltrasonicSensor_BackLeft.distance();
EmbeddedSam 5:8eba0e0f29e3 156 wait_ms(10);
EmbeddedSam 5:8eba0e0f29e3 157 if(distance == 17182){
EmbeddedSam 5:8eba0e0f29e3 158 //this is just 999999 error code ran through distance calc, i should change this but
EmbeddedSam 5:8eba0e0f29e3 159 printf("\n\rOut of range -> echo timed out");
EmbeddedSam 5:8eba0e0f29e3 160 distance = 99999;
EmbeddedSam 5:8eba0e0f29e3 161 }
EmbeddedSam 5:8eba0e0f29e3 162 else{
EmbeddedSam 5:8eba0e0f29e3 163 printf("\n\rDistance is: %0.2f", distance);
EmbeddedSam 5:8eba0e0f29e3 164 }
EmbeddedSam 5:8eba0e0f29e3 165
EmbeddedSam 5:8eba0e0f29e3 166 sprintf(buf, "%d", int(distance));
EmbeddedSam 3:e5716d0d5e36 167 message.payload = (void*)buf;
EmbeddedSam 3:e5716d0d5e36 168 message.payloadlen = strlen(buf)+1;
EmbeddedSam 5:8eba0e0f29e3 169 rc = client.publish("RobotSensors/Ultrasound/BackLeft", message);
EmbeddedSam 5:8eba0e0f29e3 170
EmbeddedSam 5:8eba0e0f29e3 171 //Read Front Centre
EmbeddedSam 5:8eba0e0f29e3 172 distance = UltrasonicSensor_BackCentre.distance();
EmbeddedSam 5:8eba0e0f29e3 173 wait_ms(10);
EmbeddedSam 5:8eba0e0f29e3 174 if(distance == 17182){
EmbeddedSam 5:8eba0e0f29e3 175 //this is just 999999 error code ran through distance calc, i should change this but
EmbeddedSam 5:8eba0e0f29e3 176 printf("\n\rOut of range -> echo timed out");
EmbeddedSam 5:8eba0e0f29e3 177 distance = 99999;
EmbeddedSam 5:8eba0e0f29e3 178 }
EmbeddedSam 5:8eba0e0f29e3 179 else{
EmbeddedSam 5:8eba0e0f29e3 180 printf("\n\rDistance is: %0.2f", distance);
EmbeddedSam 5:8eba0e0f29e3 181 }
EmbeddedSam 5:8eba0e0f29e3 182
EmbeddedSam 5:8eba0e0f29e3 183 sprintf(buf, "%d", int(distance));
EmbeddedSam 5:8eba0e0f29e3 184 message.payload = (void*)buf;
EmbeddedSam 5:8eba0e0f29e3 185 message.payloadlen = strlen(buf)+1;
EmbeddedSam 5:8eba0e0f29e3 186 rc = client.publish("RobotSensors/Ultrasound/BackCentre", message);
EmbeddedSam 5:8eba0e0f29e3 187
EmbeddedSam 5:8eba0e0f29e3 188 //Read Front Right
EmbeddedSam 5:8eba0e0f29e3 189 distance = UltrasonicSensor_BackRight.distance();
EmbeddedSam 5:8eba0e0f29e3 190 wait_ms(10);
EmbeddedSam 5:8eba0e0f29e3 191 if(distance == 17182){
EmbeddedSam 5:8eba0e0f29e3 192 //this is just 999999 error code ran through distance calc, i should change this but
EmbeddedSam 5:8eba0e0f29e3 193 printf("\n\rOut of range -> echo timed out");
EmbeddedSam 5:8eba0e0f29e3 194 distance = 99999;
EmbeddedSam 5:8eba0e0f29e3 195 }
EmbeddedSam 5:8eba0e0f29e3 196 else{
EmbeddedSam 5:8eba0e0f29e3 197 printf("\n\rDistance is: %0.2f", distance);
EmbeddedSam 5:8eba0e0f29e3 198 }
EmbeddedSam 5:8eba0e0f29e3 199
EmbeddedSam 5:8eba0e0f29e3 200 sprintf(buf, "%d", int(distance));
EmbeddedSam 5:8eba0e0f29e3 201 message.payload = (void*)buf;
EmbeddedSam 5:8eba0e0f29e3 202 message.payloadlen = strlen(buf)+1;
EmbeddedSam 5:8eba0e0f29e3 203 rc = client.publish("RobotSensors/Ultrasound/BackRight", message);
DieterGraef 0:f9b6112278fe 204 }
EmbeddedSam 3:e5716d0d5e36 205 //client.yield(100);
EmbeddedSam 3:e5716d0d5e36 206 // if ((rc = client.unsubscribe(topic)) != 0)
EmbeddedSam 3:e5716d0d5e36 207 // printf("rc from unsubscribe was %d\n", rc);
EmbeddedSam 3:e5716d0d5e36 208 //
EmbeddedSam 3:e5716d0d5e36 209 // if ((rc = client.disconnect()) != 0)
EmbeddedSam 3:e5716d0d5e36 210 // printf("rc from disconnect was %d\n", rc);
EmbeddedSam 3:e5716d0d5e36 211 //
EmbeddedSam 3:e5716d0d5e36 212 // ipstack.disconnect();
EmbeddedSam 3:e5716d0d5e36 213
DieterGraef 0:f9b6112278fe 214
DieterGraef 0:f9b6112278fe 215 }