File content as of revision 34:8393ded26b4f:

#include "main.h"

DigitalOut myled(LED);
//D12 TRIGGER D11 ECHO
HCSR04 sensor(D12, D11);
/* Instantiate the expansion board */
XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
volatile int mems_event = 0;
volatile int notification = 0;

/* Sensor initialization */
HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor;
LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;

int main() { 
    /*
     * inits the Serial or USBSerial when available (230400 baud).
     * If the serial uart is not is not connected it swiches to USB Serial
     * blinking LED means USBSerial detected, waiting for a connect.
     * It waits up to 30 seconds for a USB terminal connections 
     */
    InitSerial(30*1000, &myled); 
    hum_temp->enable();
    
    /* Enable LSM6DSL accelerometer */
    acc_gyro->enable_x();
    /* Enable 6D Orientation. */
    acc_gyro->enable_6d_orientation();
    
    print_stuff();
    bool empty = true;
    //WAIT
    wait(30.0f);
    while(1){
        
        char distance[4], empty_distance[4], temperature[4];//Message to be sent
        get_distance(distance);
        get_temperature(temperature);
        notification = send_orientation(); 
        
        if(empty) {
            memcpy(empty_distance, distance, 4);
            SendAndBack((uint8_t*)distance, (uint8_t*)empty_distance, (uint8_t*)temperature, notification); //invia due volte
            empty = false;
            }
        else{
            SendAndBack((uint8_t*)distance, (uint8_t*)empty_distance, (uint8_t*)temperature, notification);
        }
        
    }
}

void get_distance(char message[]){
    //Ultrasonic measurement
    long distance = 401;
    while (distance >= 400) distance = sensor.distance();
    dprintf("distance  %d  \n",distance);
    
    sprintf(message, "%d", distance);
}

void get_temperature(char message[]){
    float value;
    hum_temp->get_temperature(&value);
    dprintf("temperature %f", value);
    
    sprintf(message, "%f", value);
    dprintf(message);
    
}

/* Print the orientation. */
bool send_orientation() {
  uint8_t xl = 0;
  uint8_t xh = 0;
  uint8_t yl = 0;
  uint8_t yh = 0;
  uint8_t zl = 0;
  uint8_t zh = 0;
  
  acc_gyro->get_6d_orientation_xl(&xl);
  acc_gyro->get_6d_orientation_xh(&xh);
  acc_gyro->get_6d_orientation_yl(&yl);
  acc_gyro->get_6d_orientation_yh(&yh);
  acc_gyro->get_6d_orientation_zl(&zl);
  acc_gyro->get_6d_orientation_zh(&zh);
  
  
  if ( xl == 0 && yl == 0 && zl == 0 && xh == 0 && yh == 0 && zh == 1 ) {
    return false;
  }
  
  else {
    return true;
  }
}