John Lam / Mbed 2 deprecated Test2Boards

F742ZG

Dependencies:   mbed X_NUCLEO_IKS01A2 mbed-rtos

Dependents:   ros_button_2021 test test2

main.txt@1:54d03a42740a, 2021-06-12 (annotated)

Committer:
howanglam3
Date:
Sat Jun 12 16:01:51 2021 +0000
Revision:
1:54d03a42740a
Parent:
main.cpp@0:4e088cbb2dbf 
use as library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nirnakern 0:4e088cbb2dbf 1 #include "mbed.h"
nirnakern 0:4e088cbb2dbf 2 #include "rtos/rtos.h"
nirnakern 0:4e088cbb2dbf 3 #include "Teseo-LIV3F.h"
nirnakern 0:4e088cbb2dbf 4 #include <cstdlib>
nirnakern 0:4e088cbb2dbf 5 #include <vector>
nirnakern 0:4e088cbb2dbf 6 #include "XNucleoIKS01A2.h"
nirnakern 0:4e088cbb2dbf 7
nirnakern 0:4e088cbb2dbf 8 /*cose per gps*/
nirnakern 0:4e088cbb2dbf 9 static Thread t1;
nirnakern 0:4e088cbb2dbf 10 static Thread t2;
nirnakern 0:4e088cbb2dbf 11
nirnakern 0:4e088cbb2dbf 12 static Mutex data_access;
nirnakern 0:4e088cbb2dbf 13
nirnakern 0:4e088cbb2dbf 14 static Timer system_timer;
nirnakern 0:4e088cbb2dbf 15 static Serial usb_serial(USBTX, USBRX, 9600);
nirnakern 0:4e088cbb2dbf 16 static Serial *serial_debug= &usb_serial;
nirnakern 0:4e088cbb2dbf 17
nirnakern 0:4e088cbb2dbf 18
nirnakern 0:4e088cbb2dbf 19 static Teseo_LIV3F Teseo(D8, D13, D6, PE_7, PE_8, &usb_serial);
nirnakern 0:4e088cbb2dbf 20
nirnakern 0:4e088cbb2dbf 21
nirnakern 0:4e088cbb2dbf 22 struct gps_coordinate{
nirnakern 0:4e088cbb2dbf 23 float lat;
nirnakern 0:4e088cbb2dbf 24 char lat_cardinal;
nirnakern 0:4e088cbb2dbf 25 float longitud;
nirnakern 0:4e088cbb2dbf 26 char long_cardinal;
nirnakern 0:4e088cbb2dbf 27 bool valid;
nirnakern 0:4e088cbb2dbf 28
nirnakern 0:4e088cbb2dbf 29
nirnakern 0:4e088cbb2dbf 30
nirnakern 0:4e088cbb2dbf 31 };
nirnakern 0:4e088cbb2dbf 32
nirnakern 0:4e088cbb2dbf 33 gps_coordinate myPosition;
nirnakern 0:4e088cbb2dbf 34
nirnakern 0:4e088cbb2dbf 35
nirnakern 0:4e088cbb2dbf 36
nirnakern 0:4e088cbb2dbf 37 /*cose per sensori*/
nirnakern 0:4e088cbb2dbf 38
nirnakern 0:4e088cbb2dbf 39 static Thread t3;
nirnakern 0:4e088cbb2dbf 40
nirnakern 0:4e088cbb2dbf 41 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
nirnakern 0:4e088cbb2dbf 42
nirnakern 0:4e088cbb2dbf 43
nirnakern 0:4e088cbb2dbf 44 /* Retrieve the composing elements of the expansion board */
nirnakern 0:4e088cbb2dbf 45 static LSM303AGRMagSensor *magnetometer = mems_expansion_board->magnetometer;
nirnakern 0:4e088cbb2dbf 46 static HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor;
nirnakern 0:4e088cbb2dbf 47 static LPS22HBSensor *press_temp = mems_expansion_board->pt_sensor;
nirnakern 0:4e088cbb2dbf 48 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
nirnakern 0:4e088cbb2dbf 49 static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
nirnakern 0:4e088cbb2dbf 50
nirnakern 0:4e088cbb2dbf 51 void f1 (){
nirnakern 0:4e088cbb2dbf 52
nirnakern 0:4e088cbb2dbf 53 enum nmea_msg_id id;
nirnakern 0:4e088cbb2dbf 54
nirnakern 0:4e088cbb2dbf 55
nirnakern 0:4e088cbb2dbf 56
nirnakern 0:4e088cbb2dbf 57
nirnakern 0:4e088cbb2dbf 58
nirnakern 0:4e088cbb2dbf 59 Teseo.Reset(serial_debug);
nirnakern 0:4e088cbb2dbf 60 Teseo.startListener(serial_debug);
nirnakern 0:4e088cbb2dbf 61
nirnakern 0:4e088cbb2dbf 62 while(1) {
nirnakern 0:4e088cbb2dbf 63 osEvent evt = Teseo.queue.get();
nirnakern 0:4e088cbb2dbf 64 if (evt.status == osEventMessage) {
nirnakern 0:4e088cbb2dbf 65 struct teseo_msg *message = (struct teseo_msg *)evt.value.p;
nirnakern 0:4e088cbb2dbf 66 if (message->len){
nirnakern 0:4e088cbb2dbf 67 id = Teseo.MsgDetect(message->buf, message->len, serial_debug);
nirnakern 0:4e088cbb2dbf 68
nirnakern 0:4e088cbb2dbf 69 }
nirnakern 0:4e088cbb2dbf 70
nirnakern 0:4e088cbb2dbf 71 if (id==1){
nirnakern 0:4e088cbb2dbf 72
nirnakern 0:4e088cbb2dbf 73
nirnakern 0:4e088cbb2dbf 74
nirnakern 0:4e088cbb2dbf 75
nirnakern 0:4e088cbb2dbf 76 //da qui converto il messaggio in coordinate//
nirnakern 0:4e088cbb2dbf 77 data_access.lock();
nirnakern 0:4e088cbb2dbf 78
nirnakern 0:4e088cbb2dbf 79 vector<char> pos;
nirnakern 0:4e088cbb2dbf 80
nirnakern 0:4e088cbb2dbf 81
nirnakern 0:4e088cbb2dbf 82 //latitudine
nirnakern 0:4e088cbb2dbf 83 // printf ("latitudine=");
nirnakern 0:4e088cbb2dbf 84
nirnakern 0:4e088cbb2dbf 85 int cursor;
nirnakern 0:4e088cbb2dbf 86 for (int i=7;;i++){
nirnakern 0:4e088cbb2dbf 87 cursor=i;
nirnakern 0:4e088cbb2dbf 88 if (message->buf[i] == ',')
nirnakern 0:4e088cbb2dbf 89 break;
nirnakern 0:4e088cbb2dbf 90 pos.push_back(message->buf[i]);
nirnakern 0:4e088cbb2dbf 91
nirnakern 0:4e088cbb2dbf 92
nirnakern 0:4e088cbb2dbf 93 }
nirnakern 0:4e088cbb2dbf 94
nirnakern 0:4e088cbb2dbf 95
nirnakern 0:4e088cbb2dbf 96 //ma quanto è cool? conversione da vector ad array e poi a float
nirnakern 0:4e088cbb2dbf 97 myPosition.lat= atof (&pos[0]);
nirnakern 0:4e088cbb2dbf 98 // printf ("%f",myPosition.lat);
nirnakern 0:4e088cbb2dbf 99 // printf (" ");
nirnakern 0:4e088cbb2dbf 100
nirnakern 0:4e088cbb2dbf 101 myPosition.lat_cardinal= message->buf[cursor+1];
nirnakern 0:4e088cbb2dbf 102 // printf ("%c ",myPosition.lat_cardinal);
nirnakern 0:4e088cbb2dbf 103
nirnakern 0:4e088cbb2dbf 104 pos.clear();
nirnakern 0:4e088cbb2dbf 105
nirnakern 0:4e088cbb2dbf 106
nirnakern 0:4e088cbb2dbf 107 // printf ("longitudine=");
nirnakern 0:4e088cbb2dbf 108
nirnakern 0:4e088cbb2dbf 109
nirnakern 0:4e088cbb2dbf 110 for (int i=cursor+3;;i++){
nirnakern 0:4e088cbb2dbf 111 cursor=i;
nirnakern 0:4e088cbb2dbf 112 if (message->buf[i] == ',')
nirnakern 0:4e088cbb2dbf 113 break;
nirnakern 0:4e088cbb2dbf 114 pos.push_back(message->buf[i]);
nirnakern 0:4e088cbb2dbf 115
nirnakern 0:4e088cbb2dbf 116
nirnakern 0:4e088cbb2dbf 117 }
nirnakern 0:4e088cbb2dbf 118
nirnakern 0:4e088cbb2dbf 119
nirnakern 0:4e088cbb2dbf 120
nirnakern 0:4e088cbb2dbf 121 myPosition.longitud= atof (&pos[0]);
nirnakern 0:4e088cbb2dbf 122 // printf ("%f",myPosition.longitud);
nirnakern 0:4e088cbb2dbf 123 // printf (" ");
nirnakern 0:4e088cbb2dbf 124
nirnakern 0:4e088cbb2dbf 125 myPosition.long_cardinal= message->buf[cursor+1];
nirnakern 0:4e088cbb2dbf 126 // printf ("%c ",myPosition.long_cardinal);
nirnakern 0:4e088cbb2dbf 127
nirnakern 0:4e088cbb2dbf 128 for (int i=cursor+3;;i++){
nirnakern 0:4e088cbb2dbf 129 cursor=i;
nirnakern 0:4e088cbb2dbf 130 if (message->buf[i] == 'A' ){
nirnakern 0:4e088cbb2dbf 131 myPosition.valid=true;
nirnakern 0:4e088cbb2dbf 132 break;
nirnakern 0:4e088cbb2dbf 133 }else if (message->buf[i] == 'V' ) {
nirnakern 0:4e088cbb2dbf 134 myPosition.valid=false;
nirnakern 0:4e088cbb2dbf 135 break;
nirnakern 0:4e088cbb2dbf 136
nirnakern 0:4e088cbb2dbf 137 }
nirnakern 0:4e088cbb2dbf 138
nirnakern 0:4e088cbb2dbf 139
nirnakern 0:4e088cbb2dbf 140
nirnakern 0:4e088cbb2dbf 141 }
nirnakern 0:4e088cbb2dbf 142
nirnakern 0:4e088cbb2dbf 143 if (myPosition.valid==true){
nirnakern 0:4e088cbb2dbf 144 // printf ("Valid");
nirnakern 0:4e088cbb2dbf 145 }
nirnakern 0:4e088cbb2dbf 146
nirnakern 0:4e088cbb2dbf 147
nirnakern 0:4e088cbb2dbf 148 if (myPosition.valid==false){
nirnakern 0:4e088cbb2dbf 149 //printf ("Not Valid");
nirnakern 0:4e088cbb2dbf 150 }
nirnakern 0:4e088cbb2dbf 151
nirnakern 0:4e088cbb2dbf 152
nirnakern 0:4e088cbb2dbf 153 data_access.unlock();
nirnakern 0:4e088cbb2dbf 154
nirnakern 0:4e088cbb2dbf 155 // printf ("\n\r");
nirnakern 0:4e088cbb2dbf 156
nirnakern 0:4e088cbb2dbf 157 }
nirnakern 0:4e088cbb2dbf 158
nirnakern 0:4e088cbb2dbf 159
nirnakern 0:4e088cbb2dbf 160 Teseo.mpool.free(message);
nirnakern 0:4e088cbb2dbf 161 }
nirnakern 0:4e088cbb2dbf 162
nirnakern 0:4e088cbb2dbf 163 }
nirnakern 0:4e088cbb2dbf 164
nirnakern 0:4e088cbb2dbf 165 }
nirnakern 0:4e088cbb2dbf 166
nirnakern 0:4e088cbb2dbf 167
nirnakern 0:4e088cbb2dbf 168
nirnakern 0:4e088cbb2dbf 169 void f2 (){
nirnakern 0:4e088cbb2dbf 170 Thread::wait (1000);
nirnakern 0:4e088cbb2dbf 171
nirnakern 0:4e088cbb2dbf 172 Thread::wait (2000);
nirnakern 0:4e088cbb2dbf 173 while (true){
nirnakern 0:4e088cbb2dbf 174 Thread::wait (3000);
nirnakern 0:4e088cbb2dbf 175 data_access.lock();
nirnakern 0:4e088cbb2dbf 176
nirnakern 0:4e088cbb2dbf 177 printf ("lat:%f %c, lon:%f %c, %d\n\r",myPosition.lat,myPosition.lat_cardinal, myPosition.longitud, myPosition.long_cardinal, myPosition.valid );
nirnakern 0:4e088cbb2dbf 178
nirnakern 0:4e088cbb2dbf 179
nirnakern 0:4e088cbb2dbf 180
nirnakern 0:4e088cbb2dbf 181 data_access.unlock();
nirnakern 0:4e088cbb2dbf 182 }
nirnakern 0:4e088cbb2dbf 183
nirnakern 0:4e088cbb2dbf 184 }
nirnakern 0:4e088cbb2dbf 185
nirnakern 0:4e088cbb2dbf 186
nirnakern 0:4e088cbb2dbf 187 void accgyro_thread(){
nirnakern 0:4e088cbb2dbf 188
nirnakern 0:4e088cbb2dbf 189 uint8_t id;
nirnakern 0:4e088cbb2dbf 190 float value1, value2;
nirnakern 0:4e088cbb2dbf 191 char buffer1[32], buffer2[32];
nirnakern 0:4e088cbb2dbf 192 int32_t axes[3];
nirnakern 0:4e088cbb2dbf 193 int32_t axesOld[3];
nirnakern 0:4e088cbb2dbf 194 float odr =208.0f;
nirnakern 0:4e088cbb2dbf 195
nirnakern 0:4e088cbb2dbf 196
nirnakern 0:4e088cbb2dbf 197 /* Enable all sensors */
nirnakern 0:4e088cbb2dbf 198
nirnakern 0:4e088cbb2dbf 199 accelerometer->enable();
nirnakern 0:4e088cbb2dbf 200 acc_gyro->enable_x();
nirnakern 0:4e088cbb2dbf 201 acc_gyro->enable_g();
nirnakern 0:4e088cbb2dbf 202 acc_gyro->set_g_odr (odr);
nirnakern 0:4e088cbb2dbf 203
nirnakern 0:4e088cbb2dbf 204 while (true){
nirnakern 0:4e088cbb2dbf 205
nirnakern 0:4e088cbb2dbf 206 Thread::wait (5000);
nirnakern 0:4e088cbb2dbf 207
nirnakern 0:4e088cbb2dbf 208
nirnakern 0:4e088cbb2dbf 209 acc_gyro->get_g_axes(axes);
nirnakern 0:4e088cbb2dbf 210 printf("LSM6DSL [gyro/mdps]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
nirnakern 0:4e088cbb2dbf 211
nirnakern 0:4e088cbb2dbf 212
nirnakern 0:4e088cbb2dbf 213 acc_gyro->get_x_axes(axes);
nirnakern 0:4e088cbb2dbf 214 printf("LSM6DSL [acc/mg]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
nirnakern 0:4e088cbb2dbf 215
nirnakern 0:4e088cbb2dbf 216
nirnakern 0:4e088cbb2dbf 217
nirnakern 0:4e088cbb2dbf 218 }
nirnakern 0:4e088cbb2dbf 219
nirnakern 0:4e088cbb2dbf 220
nirnakern 0:4e088cbb2dbf 221 }
nirnakern 0:4e088cbb2dbf 222
nirnakern 0:4e088cbb2dbf 223
nirnakern 0:4e088cbb2dbf 224
nirnakern 0:4e088cbb2dbf 225
nirnakern 0:4e088cbb2dbf 226 int main() {
nirnakern 0:4e088cbb2dbf 227
nirnakern 0:4e088cbb2dbf 228 t1.start(f1);
nirnakern 0:4e088cbb2dbf 229
nirnakern 0:4e088cbb2dbf 230 t2.start (f2);
nirnakern 0:4e088cbb2dbf 231
nirnakern 0:4e088cbb2dbf 232 t3.start (accgyro_thread);
nirnakern 0:4e088cbb2dbf 233
nirnakern 0:4e088cbb2dbf 234
nirnakern 0:4e088cbb2dbf 235
nirnakern 0:4e088cbb2dbf 236
nirnakern 0:4e088cbb2dbf 237 }