// calibrate the airspeed. This must be called at least once before
// the get_airspeed() interface can be used
void AP_Airspeed::calibrate(bool in_startup)
{
float sum = 0;
uint8_t count = 0;
if (!_enable) {
return;
}
if (in_startup && _skip_cal) {
return;
}
// discard first reading
get_pressure();
for (uint8_t i = 0; i < 10; i++) {
hal.scheduler->delay(100);
float p = get_pressure();
if (_healthy) {
sum += p;
count++;
}
}
if (count == 0) {
// unhealthy sensor
hal.console->println_P(PSTR("Airspeed sensor unhealthy"));
_offset.set(0);
return;
}
float raw = sum/count;
_offset.set_and_save(raw);
_airspeed = 0;
_raw_airspeed = 0;
}
// return current altitude estimate relative to time that calibrate()
// was called. Returns altitude in meters
// note that this relies on read() being called regularly to get new data
float AP_Baro::get_altitude(void)
{
float scaling, temp;
if (_last_altitude_t == _last_update) {
// no new information
return _altitude + _alt_offset;
}
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
// on AVR use a less exact, but faster, calculation
scaling = (float)_ground_pressure / (float)get_pressure();
temp = ((float)_ground_temperature) + 273.15f;
_altitude = logf(scaling) * temp * 29.271267f;
#else
// on faster CPUs use a more exact calculation
scaling = (float)get_pressure() / (float)_ground_pressure;
temp = ((float)_ground_temperature) + 273.15f;
// This is an exact calculation that is within +-2.5m of the standard atmosphere tables
// in the troposphere (up to 11,000 m amsl).
_altitude = 153.8462f * temp * (1.0f - expf(0.190259f * logf(scaling)));
#endif
_last_altitude_t = _last_update;
// ensure the climb rate filter is updated
_climb_rate_filter.update(_altitude, _last_update);
return _altitude + _alt_offset;
}
static void parse_cylinder_keyvalue(void *_cylinder, const char *key, const char *value)
{
cylinder_t *cylinder = _cylinder;
if (!strcmp(key, "vol")) {
cylinder->type.size = get_volume(value);
return;
}
if (!strcmp(key, "workpressure")) {
cylinder->type.workingpressure = get_pressure(value);
return;
}
/* This is handled by the "get_utf8()" */
if (!strcmp(key, "description"))
return;
if (!strcmp(key, "o2")) {
cylinder->gasmix.o2 = get_fraction(value);
return;
}
if (!strcmp(key, "he")) {
cylinder->gasmix.he = get_fraction(value);
return;
}
if (!strcmp(key, "start")) {
cylinder->start = get_pressure(value);
return;
}
if (!strcmp(key, "end")) {
cylinder->end = get_pressure(value);
return;
}
report_error("Unknown cylinder key/value pair (%s/%s)", key, value);
}
// calibrate the barometer. This must be called at least once before
// the altitude() or climb_rate() interfaces can be used
void AP_Baro::calibrate()
{
float ground_pressure = 0;
float ground_temperature = 0;
{
uint32_t tstart = hal.scheduler->millis();
while (ground_pressure == 0 || !healthy) {
read(); // Get initial data from absolute pressure sensor
if (hal.scheduler->millis() - tstart > 500) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [1]\r\n"));
}
ground_pressure = get_pressure();
ground_temperature = get_temperature();
hal.scheduler->delay(20);
}
}
// let the barometer settle for a full second after startup
// the MS5611 reads quite a long way off for the first second,
// leading to about 1m of error if we don't wait
for (uint8_t i = 0; i < 10; i++) {
uint32_t tstart = hal.scheduler->millis();
do {
read();
if (hal.scheduler->millis() - tstart > 500) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [2]\r\n"));
}
} while (!healthy);
ground_pressure = get_pressure();
ground_temperature = get_temperature();
hal.scheduler->delay(100);
}
// now average over 5 values for the ground pressure and
// temperature settings
for (uint16_t i = 0; i < 5; i++) {
uint32_t tstart = hal.scheduler->millis();
do {
read();
if (hal.scheduler->millis() - tstart > 500) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [3]\r\n"));
}
} while (!healthy);
ground_pressure = (ground_pressure * 0.8) + (get_pressure() * 0.2);
ground_temperature = (ground_temperature * 0.8) +
(get_temperature() * 0.2);
hal.scheduler->delay(100);
}
_ground_pressure.set_and_save(ground_pressure);
_ground_temperature.set_and_save(ground_temperature / 10.0f);
}
static void cmd_press(BaseSequentialStream *chp, int argc, char *argv[]) {
uint8_t address = (uint8_t) strtol(argv[1],NULL, 16);///used for hex conversion
uint8_t value = (uint8_t) strtol(argv[2],NULL, 16);
if(*argv[0] == 'r'){
if(*argv[1] == 'a') {
uint8_t i = 0;//these three used for table display of registers.
uint8_t j = 0;
uint8_t k = 0;
chprintf((BaseSequentialStream*)&SD1, "Pressure: %d\n\r ", (int)get_pressure());
chprintf((BaseSequentialStream*)&SD1, " 0 1 2 3 4 5 6 7 8 9 A B C D E F \n\r");
for (i=0; i<4; i++){
chprintf((BaseSequentialStream*)&SD1, "0x%02x0 |", i);//rows of table
for(j=0; j<16; j++){
chprintf((BaseSequentialStream*)&SD1, " %03x ", pressure_read_register(k));//columns of table
k++;
}
chprintf((BaseSequentialStream*)&SD1, "\n\r");
}
}
chprintf((BaseSequentialStream*)&SD1, "Read Byte = 0x%02x\n\r",pressure_read_register(address));
chprintf((BaseSequentialStream*)&SD1, "read byte = %d \n\r",pressure_read_register(address));
} else if ( *argv[0] == 'w') {
pressure_write_register(address, value);
chprintf((BaseSequentialStream*)&SD1, "Wrote Byte = %02x at 0x%02x\n\r", value, address);
}
}
// read the airspeed sensor
void AP_Airspeed::read(void)
{
float airspeed_pressure;
if (!_enable) {
return;
}
airspeed_pressure = get_pressure() - _offset;
/*
we support different pitot tube setups so used can choose if
they want to be able to detect pressure on the static port
*/
switch ((enum pitot_tube_order)_tube_order.get()) {
case PITOT_TUBE_ORDER_NEGATIVE:
airspeed_pressure = -airspeed_pressure;
// no break
case PITOT_TUBE_ORDER_POSITIVE:
if (airspeed_pressure < -32) {
// we're reading more than about -8m/s. The user probably has
// the ports the wrong way around
_healthy = false;
}
break;
case PITOT_TUBE_ORDER_AUTO:
default:
airspeed_pressure = fabsf(airspeed_pressure);
break;
}
airspeed_pressure = max(airspeed_pressure, 0);
_last_pressure = airspeed_pressure;
_raw_airspeed = sqrtf(airspeed_pressure * _ratio);
_airspeed = 0.7f * _airspeed + 0.3f * _raw_airspeed;
_last_update_ms = hal.scheduler->millis();
}
static void print_info(const struct md *md)
{
double energy = md->potential_energy;
double invariant = md->get_invariant(md);
double temperature = get_temperature(md);
msg("%30s %16.10lf\n", "POTENTIAL ENERGY", energy);
msg("%30s %16.10lf\n", "INVARIANT", invariant);
msg("%30s %16.10lf\n", "TEMPERATURE", temperature);
if (cfg_get_enum(md->state->cfg, "ensemble") == ENSEMBLE_TYPE_NPT) {
double pressure = get_pressure(md) / BAR_TO_AU;
msg("%30s %16.10lf\n", "PRESSURE", pressure);
}
if (cfg_get_bool(md->state->cfg, "enable_pbc")) {
double x = md->box.x * BOHR_RADIUS;
double y = md->box.y * BOHR_RADIUS;
double z = md->box.z * BOHR_RADIUS;
msg("%30s %9.3lf %9.3lf %9.3lf\n", "PERIODIC BOX SIZE", x, y, z);
}
msg("\n\n");
}
int main(){
DI();//disable interrupts or hell breaks loose during hardware config
construct_system();//bind device addresses
init_system();//init devices
//clear all interrupts
clearall_interrupts();
EI();//enable interrupts: we have UART_recieve
//set the lcd contrast
//lcd_contrast(0x42);
lcd_contrast(0x32);
//clear the lcd
clearram();
//show boot up message
show_bootup();
//clear boot message
clearram();
//screen layout
screen_layout();
send(1, 225);
send(1, 255);
send(1, 2);
while(1)
{
get_temperature1();
get_temperature2();
get_light();
get_pressure();
get_humidity();
get_soilwetness();
display();
delay_ms(800);
}
return 0;
}
// return current altitude estimate relative to time that calibrate()
// was called. Returns altitude in meters
// note that this relies on read() being called regularly to get new data
float AP_Baro::get_altitude(void)
{
if (_ground_pressure == 0) {
// called before initialisation
return 0;
}
if (_last_altitude_t == _last_update) {
// no new information
return _altitude + _alt_offset;
}
float pressure = get_pressure();
float alt = get_altitude_difference(_ground_pressure, pressure);
// record that we have consumed latest data
_last_altitude_t = _last_update;
// sanity check altitude
if (isnan(alt) || isinf(alt)) {
_flags.alt_ok = false;
} else {
_altitude = alt;
_flags.alt_ok = true;
}
// ensure the climb rate filter is updated
_climb_rate_filter.update(_altitude, _last_update);
return _altitude + _alt_offset;
}
// read one airspeed sensor
void AP_Airspeed::read(uint8_t i)
{
float airspeed_pressure;
if (!enabled(i) || !sensor[i]) {
return;
}
bool prev_healthy = state[i].healthy;
float raw_pressure = get_pressure(i);
if (state[i].cal.start_ms != 0) {
update_calibration(i, raw_pressure);
}
airspeed_pressure = raw_pressure - param[i].offset;
// remember raw pressure for logging
state[i].corrected_pressure = airspeed_pressure;
// filter before clamping positive
if (!prev_healthy) {
// if the previous state was not healthy then we should not
// use an IIR filter, otherwise a bad reading will last for
// some time after the sensor becomees healthy again
state[i].filtered_pressure = airspeed_pressure;
} else {
state[i].filtered_pressure = 0.7f * state[i].filtered_pressure + 0.3f * airspeed_pressure;
}
/*
we support different pitot tube setups so user can choose if
they want to be able to detect pressure on the static port
*/
switch ((enum pitot_tube_order)param[i].tube_order.get()) {
case PITOT_TUBE_ORDER_NEGATIVE:
state[i].last_pressure = -airspeed_pressure;
state[i].raw_airspeed = sqrtf(MAX(-airspeed_pressure, 0) * param[i].ratio);
state[i].airspeed = sqrtf(MAX(-state[i].filtered_pressure, 0) * param[i].ratio);
break;
case PITOT_TUBE_ORDER_POSITIVE:
state[i].last_pressure = airspeed_pressure;
state[i].raw_airspeed = sqrtf(MAX(airspeed_pressure, 0) * param[i].ratio);
state[i].airspeed = sqrtf(MAX(state[i].filtered_pressure, 0) * param[i].ratio);
break;
case PITOT_TUBE_ORDER_AUTO:
default:
state[i].last_pressure = fabsf(airspeed_pressure);
state[i].raw_airspeed = sqrtf(fabsf(airspeed_pressure) * param[i].ratio);
state[i].airspeed = sqrtf(fabsf(state[i].filtered_pressure) * param[i].ratio);
break;
}
if (state[i].last_pressure < -32) {
// we're reading more than about -8m/s. The user probably has
// the ports the wrong way around
state[i].healthy = false;
}
state[i].last_update_ms = AP_HAL::millis();
}
// calibrate the airspeed. This must be called at least once before
// the get_airspeed() interface can be used
void AP_Airspeed::calibrate()
{
float sum = 0;
uint8_t c;
if (!_enable) {
return;
}
// discard first reading
get_pressure();
for (c = 0; c < 10; c++) {
hal.scheduler->delay(100);
sum += get_pressure();
}
float raw = sum/c;
_offset.set_and_save(raw);
_airspeed = 0;
_raw_airspeed = 0;
}
// read the airspeed sensor
void AP_Airspeed::read(void)
{
float airspeed_pressure;
if (!_enable) {
return;
}
float raw = get_pressure();
airspeed_pressure = max(raw - _offset, 0);
_raw_airspeed = sqrtf(airspeed_pressure * _ratio);
_airspeed = 0.7f * _airspeed + 0.3f * _raw_airspeed;
}
/**
update the barometer calibration
this updates the baro ground calibration to the current values. It
can be used before arming to keep the baro well calibrated
*/
void AP_Baro::update_calibration()
{
float pressure = get_pressure();
_ground_pressure.set(pressure);
float last_temperature = _ground_temperature;
_ground_temperature.set(get_calibration_temperature());
if (fabsf(last_temperature - _ground_temperature) > 3) {
// reset _EAS2TAS to force it to recalculate. This happens
// when a digital airspeed sensor comes online
_EAS2TAS = 0;
}
}
// return current scale factor that converts from equivalent to true airspeed
// valid for altitudes up to 10km AMSL
// assumes standard atmosphere lapse rate
float AP_Baro::get_EAS2TAS(void)
{
if ((fabs(_altitude - _last_altitude_EAS2TAS) < 100.0f) && (_EAS2TAS != 0.0f)) {
// not enough change to require re-calculating
return _EAS2TAS;
}
float tempK = ((float)_ground_temperature) + 273.15f - 0.0065f * _altitude;
_EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (287.26f * tempK)));
_last_altitude_EAS2TAS = _altitude;
return _EAS2TAS;
}
// return current scale factor that converts from equivalent to true airspeed
// valid for altitudes up to 10km AMSL
// assumes standard atmosphere lapse rate
float AP_Baro::get_EAS2TAS(void)
{
float altitude = get_altitude();
if ((fabsf(altitude - _last_altitude_EAS2TAS) < 100.0f) && !is_zero(_EAS2TAS)) {
// not enough change to require re-calculating
return _EAS2TAS;
}
float tempK = get_calibration_temperature() + 273.15f - 0.0065f * altitude;
_EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (287.26f * tempK)));
_last_altitude_EAS2TAS = altitude;
return _EAS2TAS;
}
/**
* Retrieve sensor data and fill in struct
* params package struct to store them
* return status 1 failed T, 2 failed P, 3 all failed
*/
int get_temperature_pressure_package(SFE_BMP180* pressure, TempPackage* package)
{
char status;
double T,P;
int sensor_status = 0;
// You must first get a temperature measurement to perform a pressure reading.
sensor_status += get_tempurature(pressure, package);
// Start a pressure measurement:
sensor_status += get_pressure(pressure, package);
return sensor_status;
}
// calibrate the barometer. This must be called at least once before
// the altitude() or climb_rate() interfaces can be used
void AP_Baro::calibrate(void (*callback)(unsigned long t))
{
float ground_pressure = 0;
float ground_temperature = 0;
while (ground_pressure == 0 || !healthy) {
read(); // Get initial data from absolute pressure sensor
ground_pressure = get_pressure();
ground_temperature = get_temperature();
callback(20);
}
// let the barometer settle for a full second after startup
// the MS5611 reads quite a long way off for the first second,
// leading to about 1m of error if we don't wait
for (uint16_t i = 0; i < 10; i++) {
do {
read();
} while (!healthy);
ground_pressure = get_pressure();
ground_temperature = get_temperature();
callback(100);
}
// now average over 5 values for the ground pressure and
// temperature settings
for (uint16_t i = 0; i < 5; i++) {
do {
read();
} while (!healthy);
ground_pressure = ground_pressure * 0.8 + get_pressure() * 0.2;
ground_temperature = ground_temperature * 0.8 + get_temperature() * 0.2;
callback(100);
}
_ground_pressure.set_and_save(ground_pressure);
_ground_temperature.set_and_save(ground_temperature / 10.0f);
}
// read the airspeed sensor
void AP_Airspeed::read(void)
{
float airspeed_pressure;
if (!enabled()) {
return;
}
float raw_pressure = get_pressure();
if (_cal.start_ms != 0) {
update_calibration(raw_pressure);
}
airspeed_pressure = raw_pressure - _offset;
// remember raw pressure for logging
_corrected_pressure = airspeed_pressure;
// filter before clamping positive
_filtered_pressure = 0.7f * _filtered_pressure + 0.3f * airspeed_pressure;
/*
we support different pitot tube setups so user can choose if
they want to be able to detect pressure on the static port
*/
switch ((enum pitot_tube_order)_tube_order.get()) {
case PITOT_TUBE_ORDER_NEGATIVE:
_last_pressure = -airspeed_pressure;
_raw_airspeed = sqrtf(MAX(-airspeed_pressure, 0) * _ratio);
_airspeed = sqrtf(MAX(-_filtered_pressure, 0) * _ratio);
break;
case PITOT_TUBE_ORDER_POSITIVE:
_last_pressure = airspeed_pressure;
_raw_airspeed = sqrtf(MAX(airspeed_pressure, 0) * _ratio);
_airspeed = sqrtf(MAX(_filtered_pressure, 0) * _ratio);
if (airspeed_pressure < -32) {
// we're reading more than about -8m/s. The user probably has
// the ports the wrong way around
_healthy = false;
}
break;
case PITOT_TUBE_ORDER_AUTO:
default:
_last_pressure = fabsf(airspeed_pressure);
_raw_airspeed = sqrtf(fabsf(airspeed_pressure) * _ratio);
_airspeed = sqrtf(fabsf(_filtered_pressure) * _ratio);
break;
}
_last_update_ms = AP_HAL::millis();
}
/*
update the barometer calibration
this updates the baro ground calibration to the current values. It
can be used before arming to keep the baro well calibrated
*/
void AP_Baro::update_calibration()
{
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
sensors[i].ground_pressure.set_and_notify(get_pressure(i));
}
float last_temperature = sensors[i].ground_temperature;
sensors[i].ground_temperature.set_and_notify(get_calibration_temperature(i));
if (fabsf(last_temperature - sensors[i].ground_temperature) > 3) {
// reset _EAS2TAS to force it to recalculate. This happens
// when a digital airspeed sensor comes online
_EAS2TAS = 0;
}
}
}
// calibrate the airspeed. This must be called at least once before
// the get_airspeed() interface can be used
void AP_Airspeed::calibrate(bool in_startup)
{
if (!_enable) {
return;
}
if (in_startup && _skip_cal) {
return;
}
// discard first reading
get_pressure();
_cal.start_ms = AP_HAL::millis();
_cal.count = 0;
_cal.sum = 0;
_cal.read_count = 0;
}
static THD_FUNCTION(meas_thd, arg) {
(void)arg;
uint32_t pressure;
/* For now just measure pressure */
ms5611_configure(&I2CD1);
while(true) {
chThdSleepMilliseconds(80);
pressure = get_pressure();
}
(void) pressure;
}
// return current scale factor that converts from equivalent to true airspeed
// valid for altitudes up to 10km AMSL
// assumes standard atmosphere lapse rate
float AP_Baro::get_EAS2TAS(void)
{
float altitude = get_altitude();
if ((fabsf(altitude - _last_altitude_EAS2TAS) < 25.0f) && !is_zero(_EAS2TAS)) {
// not enough change to require re-calculating
return _EAS2TAS;
}
// only estimate lapse rate for the difference from the ground location
// provides a more consistent reading then trying to estimate a complete
// ISA model atmosphere
float tempK = get_ground_temperature() + C_TO_KELVIN - ISA_LAPSE_RATE * altitude;
_EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (ISA_GAS_CONSTANT * tempK)));
_last_altitude_EAS2TAS = altitude;
return _EAS2TAS;
}
static void cmd_alti(BaseSequentialStream *chp, int argc, char *argv[]) {
//=(1-(A18/1013.25)^0.190284)*145366.45
double altitude_ft = (1-((get_pressure()/1013.25),0.190284))*145366.45; //converts to Altitude measurement
//took away pow
int final_ft = (int) altitude_ft;//converts above value to int for printing purposes
if(*argv[0] == 'f'){
chprintf((BaseSequentialStream*)&SD1, "Altitude ft = %d \n\r",final_ft);
}
else if (*argv[0] == 'm'){
//=D18/3.280839895
int altitude_m = (int)final_ft/3.280839895;//converts to meters, then to an int for printing purposes.
chprintf((BaseSequentialStream*)&SD1, "Altitude meters = %d \n\r",altitude_m);
}
}
/* FIXME! We should do the array thing here too. */
static void parse_sample_keyvalue(void *_sample, const char *key, const char *value)
{
struct sample *sample = _sample;
if (!strcmp(key, "sensor")) {
sample->sensor = atoi(value);
return;
}
if (!strcmp(key, "ndl")) {
sample->ndl = get_duration(value);
return;
}
if (!strcmp(key, "tts")) {
sample->tts = get_duration(value);
return;
}
if (!strcmp(key, "in_deco")) {
sample->in_deco = atoi(value);
return;
}
if (!strcmp(key, "stoptime")) {
sample->stoptime = get_duration(value);
return;
}
if (!strcmp(key, "stopdepth")) {
sample->stopdepth = get_depth(value);
return;
}
if (!strcmp(key, "cns")) {
sample->cns = atoi(value);
return;
}
if (!strcmp(key, "po2")) {
pressure_t p = get_pressure(value);
sample->setpoint.mbar = p.mbar;
return;
}
if (!strcmp(key, "heartbeat")) {
sample->heartbeat = atoi(value);
return;
}
if (!strcmp(key, "bearing")) {
sample->bearing.degrees = atoi(value);
return;
}
report_error("Unexpected sample key/value pair (%s/%s)", key, value);
}
void do_second_halfstep_kick(void)
{
int i, j;
int ti_step, tstart, tend;
#ifdef PMGRID
if(All.PM_Ti_endstep == All.Ti_Current) /* need to do long-range kick */
{
ti_step = All.PM_Ti_endstep - All.PM_Ti_begstep;
tstart = All.PM_Ti_begstep + ti_step / 2;
tend = tstart + ti_step / 2;
apply_long_range_kick(tstart, tend);
}
#endif
for(i = FirstActiveParticle; i >= 0; i = NextActiveParticle[i])
{
ti_step = P[i].TimeBin ? (((integertime) 1) << P[i].TimeBin) : 0;
tstart = P[i].Ti_begstep + ti_step / 2; /* midpoint of step */
tend = P[i].Ti_begstep + ti_step; /* end of step */
do_the_kick(i, tstart, tend, P[i].Ti_current);
/* after completion of a full step, set the predication values of SPH quantities
* to the current values. They will then predicted along in drift operations
*/
if(P[i].Type == 0)
{
for(j=0;j<3 ;j++)
SphP[i].VelPred[j] = P[i].Vel[j];
SphP[i].EntropyPred = SphP[i].Entropy;
#ifdef EOS_DEGENERATE
for(j = 0; j < 3; j++)
SphP[i].xnucPred[j] = SphP[i].xnuc[j];
#endif
SphP[i].Pressure = get_pressure(i);
set_predicted_sph_quantities_for_extra_physics(i);
}
}
}
// return current altitude estimate relative to time that calibrate()
// was called. Returns altitude in meters
// note that this relies on read() being called regularly to get new data
float AP_Baro::get_altitude(void)
{
if (_ground_pressure == 0) {
// called before initialisation
return 0;
}
if (_last_altitude_t == _last_update) {
// no new information
return _altitude + _alt_offset;
}
_altitude = get_altitude_difference(_ground_pressure, get_pressure());
_last_altitude_t = _last_update;
// ensure the climb rate filter is updated
_climb_rate_filter.update(_altitude, _last_update);
return _altitude + _alt_offset;
}
/*
update the barometer calibration
this updates the baro ground calibration to the current values. It
can be used before arming to keep the baro well calibrated
*/
void AP_Baro::update_calibration()
{
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
sensors[i].ground_pressure.set(get_pressure(i));
}
// don't notify the GCS too rapidly or we flood the link
uint32_t now = AP_HAL::millis();
if (now - _last_notify_ms > 10000) {
sensors[i].ground_pressure.notify();
_last_notify_ms = now;
}
}
// always update the guessed ground temp
_guessed_ground_temperature = get_external_temperature();
// force EAS2TAS to recalculate
_EAS2TAS = 0;
}
/*
update the barometer calibration
this updates the baro ground calibration to the current values. It
can be used before arming to keep the baro well calibrated
*/
void AP_Baro::update_calibration()
{
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
sensors[i].ground_pressure.set(get_pressure(i));
}
float last_temperature = sensors[i].ground_temperature;
sensors[i].ground_temperature.set(get_calibration_temperature(i));
// don't notify the GCS too rapidly or we flood the link
uint32_t now = AP_HAL::millis();
if (now - _last_notify_ms > 10000) {
sensors[i].ground_pressure.notify();
sensors[i].ground_temperature.notify();
_last_notify_ms = now;
}
if (fabsf(last_temperature - sensors[i].ground_temperature) > 3) {
// reset _EAS2TAS to force it to recalculate. This happens
// when a digital airspeed sensor comes online
_EAS2TAS = 0;
}
}
}
// return current altitude estimate relative to time that calibrate()
// was called. Returns altitude in meters
// note that this relies on read() being called regularly to get new data
float AP_Baro::get_altitude(void)
{
float scaling, temp;
if (_last_altitude_t == _last_update) {
// no new information
return _altitude;
}
// this has no filtering of the pressure values, use a separate
// filter if you want a smoothed value. The AHRS driver wants
// unsmoothed values
scaling = (float)_ground_pressure / (float)get_pressure();
temp = ((float)_ground_temperature) + 273.15f;
_altitude = log(scaling) * temp * 29.271267f;
_last_altitude_t = _last_update;
// ensure the climb rate filter is updated
_climb_rate_filter.update(_altitude, _last_update);
return _altitude;
}
static THD_FUNCTION(logThread,arg) {
UNUSED(arg);
while(TRUE){
// int i =0;
while (isLogging && counter < 3500) {
// for( i; i<(sizeof(press_ft)); i++){
double press = get_pressure();
chprintf((BaseSequentialStream*)&SD1, "1. %04d\n\r ", (int)press);//time stamp.
//=(1-(A18/1013.25)^0.190284)*145366.45
double altitude_ft = (1-((press/1013.25),0.190284))*145366.45;//took out "pow"
//converts to Altitude measurement
//converts above value to int for printing purposes
int final_ft = (int) altitude_ft;
chprintf((BaseSequentialStream*)&SD1, "2. %04d\n\r ", final_ft);//time stamp.
press_ft[counter]= final_ft;
// int final_m = (int)final_ft/3.280839895;
//converts to meters, then to an int for printing purposes.
int final_m = (int) final_ft/3.280839895;
chprintf((BaseSequentialStream*)&SD1, "3. %04d\n\r ",final_m);//time stamp.
chprintf((BaseSequentialStream*)&SD1, "array. %04d\n\r ",press_ft[counter]);//aray value
chprintf((BaseSequentialStream*)&SD1, "iterator. %04d\n\r ",counter);//i.
counter++;
chThdSleepMilliseconds(1); //escape for scheduler.
// chThdSleepMilliseconds(1); //escape for scheduler.
}
chThdSleepMilliseconds(1); //escape for scheduler.
}
return 0;
}
