void GCS_MAVLINK::send_scaled_pressure(AP_Baro &barometer) { uint32_t now = AP_HAL::millis(); float pressure = barometer.get_pressure(0); mavlink_msg_scaled_pressure_send( chan, now, pressure*0.01f, // hectopascal (pressure - barometer.get_ground_pressure(0))*0.01f, // hectopascal barometer.get_temperature(0)*100); // 0.01 degrees C if (barometer.num_instances() > 1) { pressure = barometer.get_pressure(1); mavlink_msg_scaled_pressure2_send( chan, now, pressure*0.01f, // hectopascal (pressure - barometer.get_ground_pressure(1))*0.01f, // hectopascal barometer.get_temperature(1)*100); // 0.01 degrees C } if (barometer.num_instances() > 2) { pressure = barometer.get_pressure(2); mavlink_msg_scaled_pressure3_send( chan, now, pressure*0.01f, // hectopascal (pressure - barometer.get_ground_pressure(2))*0.01f, // hectopascal barometer.get_temperature(2)*100); // 0.01 degrees C } }
void GCS_MAVLINK::send_scaled_pressure(AP_Baro &barometer) { uint32_t now = hal.scheduler->millis(); float pressure = barometer.get_pressure(0); mavlink_msg_scaled_pressure_send( chan, now, pressure*0.01f, // hectopascal (pressure - barometer.get_ground_pressure(0))*0.01f, // hectopascal barometer.get_temperature(0)*100); // 0.01 degrees C #if BARO_MAX_INSTANCES > 1 if (barometer.num_instances() > 1) { pressure = barometer.get_pressure(1); mavlink_msg_scaled_pressure2_send( chan, now, pressure*0.01f, // hectopascal (pressure - barometer.get_ground_pressure(1))*0.01f, // hectopascal barometer.get_temperature(1)*100); // 0.01 degrees C } #endif #if BARO_MAX_INSTANCES > 2 if (barometer.num_instances() > 2) { pressure = barometer.get_pressure(2); mavlink_msg_scaled_pressure3_send( chan, now, pressure*0.01f, // hectopascal (pressure - barometer.get_ground_pressure(2))*0.01f, // hectopascal barometer.get_temperature(2)*100); // 0.01 degrees C } #endif }
void GCS_MAVLINK::send_sensor_offsets(const AP_InertialSensor &ins, const Compass &compass, AP_Baro &barometer) { // run this message at a much lower rate - otherwise it // pointlessly wastes quite a lot of bandwidth static uint8_t counter; if (counter++ < 10) { return; } counter = 0; const Vector3f &mag_offsets = compass.get_offsets(0); const Vector3f &accel_offsets = ins.get_accel_offsets(0); const Vector3f &gyro_offsets = ins.get_gyro_offsets(0); mavlink_msg_sensor_offsets_send(chan, mag_offsets.x, mag_offsets.y, mag_offsets.z, compass.get_declination(), barometer.get_pressure(), barometer.get_temperature()*100, gyro_offsets.x, gyro_offsets.y, gyro_offsets.z, accel_offsets.x, accel_offsets.y, accel_offsets.z); }
void loop() { // run accumulate() at 50Hz and update() at 10Hz if((hal.scheduler->micros() - timer) > 20*1000UL) { timer = hal.scheduler->micros(); barometer.accumulate(); if (counter++ < 5) { return; } counter = 0; barometer.update(); uint32_t read_time = hal.scheduler->micros() - timer; float alt = barometer.get_altitude(); if (!barometer.healthy()) { hal.console->println("not healthy"); return; } hal.console->print("Pressure:"); hal.console->print(barometer.get_pressure()); hal.console->print(" Temperature:"); hal.console->print(barometer.get_temperature()); hal.console->print(" Altitude:"); hal.console->print(alt); hal.console->printf(" climb=%.2f t=%u", barometer.get_climb_rate(), (unsigned)read_time); hal.console->println(); } else { hal.scheduler->delay(1); } }
void GCS_MAVLINK::send_scaled_pressure(AP_Baro &barometer) { float pressure = barometer.get_pressure(); mavlink_msg_scaled_pressure_send( chan, hal.scheduler->millis(), pressure*0.01f, // hectopascal (pressure - barometer.get_ground_pressure())*0.01f, // hectopascal barometer.get_temperature()*100); // 0.01 degrees C }