void airspeed_ets_read_periodic( void ) {
#ifndef SITL
if (airspeed_ets_i2c_trans.status == I2CTransDone)
i2c_receive(&AIRSPEED_ETS_I2C_DEV, &airspeed_ets_i2c_trans, AIRSPEED_ETS_ADDR, 2);
#else // SITL
extern float sim_air_speed;
stateSetAirspeed_f(&sim_air_speed);
#endif //SITL
}
void airspeed_ets_read_periodic( void ) {
#ifndef SITL
if (!airspeed_ets_delay_done) {
if (SysTimeTimer(airspeed_ets_delay_time) < USEC_OF_SEC(AIRSPEED_ETS_START_DELAY)) return;
else airspeed_ets_delay_done = TRUE;
}
if (airspeed_ets_i2c_trans.status == I2CTransDone)
i2c_receive(&AIRSPEED_ETS_I2C_DEV, &airspeed_ets_i2c_trans, AIRSPEED_ETS_ADDR, 2);
#elif !defined USE_NPS
extern float sim_air_speed;
stateSetAirspeed_f(&sim_air_speed);
#endif //SITL
}
void pbn_read_event(void)
{
pbn_trans.status = I2CTransDone;
// Get raw values from buffer
pbn.airspeed_adc = ((uint16_t)(pbn_trans.buf[0]) << 8) | (uint16_t)(pbn_trans.buf[1]);
pbn.altitude_adc = ((uint16_t)(pbn_trans.buf[2]) << 8) | (uint16_t)(pbn_trans.buf[3]);
// Consider 0 as a wrong value
if (pbn.airspeed_adc == 0 || pbn.altitude_adc == 0) {
pbn.data_valid = false;
} else {
pbn.data_valid = true;
if (offset_cnt > 0) {
// IIR filter to compute an initial offset
#ifndef PBN_AIRSPEED_OFFSET
pbn.airspeed_offset = (PBN_OFFSET_FILTER * pbn.airspeed_offset + pbn.airspeed_adc) /
(PBN_OFFSET_FILTER + 1);
#else
pbn.airspeed_offset = PBN_AIRSPEED_OFFSET;
#endif
#ifndef PBN_ALTITUDE_OFFSET
pbn.altitude_offset = (PBN_OFFSET_FILTER * pbn.altitude_offset + pbn.altitude_adc) /
(PBN_OFFSET_FILTER + 1);
#else
pbn.altitude_offset = PBN_ALTITUDE_OFFSET;
#endif
// decrease init counter
--offset_cnt;
} else {
// Compute pressure
float pressure = PBN_ALTITUDE_SCALE * (float) pbn.altitude_adc + PBN_PRESSURE_OFFSET;
AbiSendMsgBARO_ABS(BARO_PBN_SENDER_ID, pressure);
// Compute airspeed and altitude
//pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28;
uint16_t diff = Max(pbn.airspeed_adc - pbn.airspeed_offset, 0);
float tmp_airspeed = sqrtf((float)diff * PBN_AIRSPEED_SCALE);
pbn.airspeed = (pbn.airspeed_filter * pbn.airspeed + tmp_airspeed) /
(pbn.airspeed_filter + 1.);
#if USE_AIRSPEED_PBN
stateSetAirspeed_f(pbn.airspeed);
#endif
pbn.altitude = PBN_ALTITUDE_SCALE * (float)(pbn.altitude_adc - pbn.altitude_offset);
}
}
}
void airspeed_amsys_read_periodic( void ) {
#ifndef SITL
if (airspeed_amsys_i2c_trans.status == I2CTransDone)
#ifndef MEASURE_AMSYS_TEMPERATURE
i2c_receive(&AIRSPEED_AMSYS_I2C_DEV, &airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 2);
#else
i2c_receive(&AIRSPEED_AMSYS_I2C_DEV, &airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 4);
#endif
#else // SITL
extern float sim_air_speed;
stateSetAirspeed_f(&sim_air_speed);
#endif //SITL
}
/* Parse the InterMCU message */
static inline void mag_pitot_parse_msg(void)
{
uint32_t now_ts = get_sys_time_usec();
/* Parse the mag-pitot message */
uint8_t msg_id = mp_msg_buf[1];
switch (msg_id) {
/* Got a magneto message */
case DL_IMCU_REMOTE_MAG: {
struct Int32Vect3 raw_mag;
// Read the raw magneto information
raw_mag.x = DL_IMCU_REMOTE_MAG_mag_x(mp_msg_buf);
raw_mag.y = DL_IMCU_REMOTE_MAG_mag_y(mp_msg_buf);
raw_mag.z = DL_IMCU_REMOTE_MAG_mag_z(mp_msg_buf);
// Rotate the magneto
struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&mag_pitot.imu_to_mag);
int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &raw_mag);
// Send and set the magneto IMU data
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MAG_PITOT_ID, now_ts, &imu.mag);
break;
}
/* Got a barometer message */
case DL_IMCU_REMOTE_BARO: {
float pitot_stat = DL_IMCU_REMOTE_BARO_pitot_stat(mp_msg_buf);
float pitot_temp = DL_IMCU_REMOTE_BARO_pitot_temp(mp_msg_buf);
AbiSendMsgBARO_ABS(IMU_MAG_PITOT_ID, pitot_stat);
AbiSendMsgTEMPERATURE(IMU_MAG_PITOT_ID, pitot_temp);
break;
}
/* Got an airspeed message */
case DL_IMCU_REMOTE_AIRSPEED: {
// Should be updated to differential pressure
float pitot_ias = DL_IMCU_REMOTE_AIRSPEED_pitot_IAS(mp_msg_buf);
stateSetAirspeed_f(pitot_ias);
break;
}
default:
break;
}
}
void airspeed_amsys_read_periodic( void ) {
#ifndef SITL
if (airspeed_amsys_i2c_trans.status == I2CTransDone) {
#ifndef MEASURE_AMSYS_TEMPERATURE
i2c_receive(&AIRSPEED_AMSYS_I2C_DEV, &airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 2);
#else
i2c_receive(&AIRSPEED_AMSYS_I2C_DEV, &airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 4);
#endif
}
#if USE_AIRSPEED
stateSetAirspeed_f(&airspeed_amsys);
#endif
#elif !defined USE_NPS
extern float sim_air_speed;
stateSetAirspeed_f(&sim_air_speed);
#endif //SITL
#ifndef AIRSPEED_AMSYS_SYNC_SEND
RunOnceEvery(10, airspeed_amsys_downlink());
#endif
}
void airspeed_amsys_read_event( void ) {
// Get raw airspeed from buffer
airspeed_amsys_raw = 0;
airspeed_amsys_raw = (airspeed_amsys_i2c_trans.buf[0]<<8) | airspeed_amsys_i2c_trans.buf[1];
#ifdef MEASURE_AMSYS_TEMPERATURE
tempAS_amsys_raw = (airspeed_amsys_i2c_trans.buf[2]<<8) | airspeed_amsys_i2c_trans.buf[3];
airspeed_temperature = (float)((float)(tempAS_amsys_raw-TEMPERATURE_AMSYS_OFFSET_MIN)/((float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)/TEMPERATURE_AMSYS_MAX)+TEMPERATURE_AMSYS_MIN);// Tmin=-25, Tmax=85
#endif
// Check if this is valid airspeed
if (airspeed_amsys_raw == 0)
airspeed_amsys_valid = FALSE;
else
airspeed_amsys_valid = TRUE;
// Continue only if a new airspeed value was received
if (airspeed_amsys_valid) {
// raw not under offest min
if (airspeed_amsys_raw<AIRSPEED_AMSYS_OFFSET_MIN)
airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MIN;
// raw not over offest max
if (airspeed_amsys_raw>AIRSPEED_AMSYS_OFFSET_MAX)
airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MAX;
// calculate raw to pressure
pressure_amsys = (float)(airspeed_amsys_raw-AIRSPEED_AMSYS_OFFSET_MIN)*AIRSPEED_AMSYS_MAXPRESURE/(float)(AIRSPEED_AMSYS_OFFSET_MAX-AIRSPEED_AMSYS_OFFSET_MIN);
airspeed_tmp = sqrtf(2*(pressure_amsys)*airspeed_scale/1.2041); //without offset
// Lowpass filter
airspeed_amsys = airspeed_filter * airspeed_old + (1 - airspeed_filter) * airspeed_tmp;
airspeed_old = airspeed_amsys;
#if USE_AIRSPEED
stateSetAirspeed_f(&airspeed_amsys);
#endif
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature);
#else
RunOnceEvery(10, DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature));
#endif
}
// Transaction has been read
airspeed_amsys_i2c_trans.status = I2CTransDone;
}
void pbn_read_event( void ) {
pbn_trans.status = I2CTransDone;
// Get raw values from buffer
airspeed_adc = ((uint16_t)(pbn_trans.buf[0]) << 8) | (uint16_t)(pbn_trans.buf[1]);
altitude_adc = ((uint16_t)(pbn_trans.buf[2]) << 8) | (uint16_t)(pbn_trans.buf[3]);
// Consider 0 as a wrong value
if (airspeed_adc == 0 || altitude_adc == 0) {
data_valid = FALSE;
}
else {
data_valid = TRUE;
if (offset_cnt > 0) {
// IIR filter to compute an initial offset
#ifndef PBN_AIRSPEED_OFFSET
airspeed_offset = (PBN_OFFSET_FILTER * airspeed_offset + airspeed_adc) / (PBN_OFFSET_FILTER + 1);
#else
airspeed_offset = PBN_AIRSPEED_OFFSET;
#endif
#ifndef PBN_ALTITUDE_OFFSET
altitude_offset = (PBN_OFFSET_FILTER * altitude_offset + altitude_adc) / (PBN_OFFSET_FILTER + 1);
#else
altitude_offset = PBN_ALTITUDE_OFFSET;
#endif
// decrease init counter
--offset_cnt;
}
else {
// Compute airspeed and altitude
//pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28;
uint16_t diff = Max(airspeed_adc-airspeed_offset, 0);
float tmp_airspeed = sqrtf((float)diff * PBN_AIRSPEED_SCALE);
pbn_altitude = PBN_ALTITUDE_SCALE*(float)(altitude_adc-altitude_offset);
pbn_airspeed = (airspeed_filter*pbn_airspeed + tmp_airspeed) / (airspeed_filter + 1.);
#if USE_AIRSPEED
stateSetAirspeed_f(&pbn_airspeed);
#endif
//alt_kalman(pbn_altitude);
}
}
}
void ms45xx_i2c_event(void)
{
/* Check if transaction is succesfull */
if (ms45xx_trans.status == I2CTransSuccess) {
/* 2 MSB of data are status bits, 0 = good data, 2 = already fetched, 3 = fault */
uint8_t status = (0xC0 & ms45xx_trans.buf[0]) >> 6;
if (status == 0) {
/* 14bit raw pressure */
uint16_t p_raw = 0x3FFF & (((uint16_t)(ms45xx_trans.buf[0]) << 8) |
(uint16_t)(ms45xx_trans.buf[1]));
/* Output is proportional to the difference between Port 1 and Port 2. Output
* swings positive when Port 1> Port 2. Output is 50% of total counts
* when Port 1=Port 2.
* p_diff = p_raw * scale - offset
*/
float p_diff = p_raw * ms45xx.pressure_scale - ms45xx.pressure_offset;
ms45xx.diff_pressure = update_butterworth_2_low_pass(&ms45xx_filter, p_diff);
/* 11bit raw temperature, 5 LSB bits not used */
uint16_t temp_raw = 0xFFE0 & (((uint16_t)(ms45xx_trans.buf[2]) << 8) |
(uint16_t)(ms45xx_trans.buf[3]));
temp_raw = temp_raw >> 5;
/* 0 = -50degC, 20147 = 150degC
* ms45xx_temperature in 0.1 deg Celcius
*/
ms45xx.temperature = ((uint32_t)temp_raw * 2000) / 2047 - 500;
// Send differential pressure via ABI
AbiSendMsgBARO_DIFF(MS45XX_SENDER_ID, ms45xx.diff_pressure);
// Send temperature as float in deg Celcius via ABI
float temp = ms45xx.temperature / 10.0f;
AbiSendMsgTEMPERATURE(MS45XX_SENDER_ID, temp);
// Compute airspeed
ms45xx.airspeed = sqrtf(Max(ms45xx.diff_pressure * ms45xx.airspeed_scale, 0));
#if USE_AIRSPEED_MS45XX
stateSetAirspeed_f(&ms45xx.airspeed);
#endif
if (ms45xx.sync_send) {
ms45xx_downlink(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
}
}
void airspeed_adc_update(void)
{
#ifndef SITL
airspeed_adc.val = buf_airspeed.sum / buf_airspeed.av_nb_sample;
float airspeed_unscaled = Max(airspeed_adc.val - airspeed_adc.offset, 0);
#ifdef AIRSPEED_ADC_QUADRATIC_SCALE
airspeed_adc.airspeed = airspeed_adc.scale * sqrtf(airspeed_unscaled);
#else
airspeed_adc.airspeed = airspeed_adc.scale * airspeed_unscaled;
#endif
#elif !defined USE_NPS
extern float sim_air_speed;
airspeed_adc.airspeed = sim_air_speed;
#endif //SITL
#if USE_AIRSPEED_ADC
stateSetAirspeed_f(&airspeed_adc.airspeed);
#endif
}
void airspeed_ets_read_event(void)
{
int n;
float airspeed_tmp = 0.0;
// Get raw airspeed from buffer
airspeed_ets_raw = ((uint16_t)(airspeed_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(airspeed_ets_i2c_trans.buf[0]);
// Check if this is valid airspeed
if (airspeed_ets_raw == 0) {
airspeed_ets_valid = false;
} else {
airspeed_ets_valid = true;
}
// Continue only if a new airspeed value was received
if (airspeed_ets_valid) {
#if !AIRSPEED_ETS_3RD_PARTY_MODE
// Calculate offset average if not done already
if (!airspeed_ets_offset_init) {
--airspeed_ets_cnt;
// Check if averaging completed
if (airspeed_ets_cnt == 0) {
// Calculate average
airspeed_ets_offset = (uint16_t)(airspeed_ets_offset_tmp / AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG);
// Limit offset
if (airspeed_ets_offset < AIRSPEED_ETS_OFFSET_MIN) {
airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MIN;
}
if (airspeed_ets_offset > AIRSPEED_ETS_OFFSET_MAX) {
airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MAX;
}
airspeed_ets_offset_init = true;
}
// Check if averaging needs to continue
else if (airspeed_ets_cnt <= AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG) {
airspeed_ets_offset_tmp += airspeed_ets_raw;
}
}
// Convert raw to m/s
#ifdef AIRSPEED_ETS_REVERSE
if (airspeed_ets_offset_init && airspeed_ets_raw < airspeed_ets_offset) {
airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_offset - airspeed_ets_raw)) - AIRSPEED_ETS_OFFSET;
}
#else
if (airspeed_ets_offset_init && airspeed_ets_raw > airspeed_ets_offset) {
airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_raw - airspeed_ets_offset)) - AIRSPEED_ETS_OFFSET;
}
#endif
else {
airspeed_tmp = 0.0;
}
//use raw value for sensor set to third-party mode
#else
airspeed_tmp = airspeed_ets_raw;
#endif //AIRSPEED_ETS_3RD_PARTY_MODE
// Airspeed should always be positive
if (airspeed_tmp < 0.0) {
airspeed_tmp = 0.0;
}
// Moving average
airspeed_ets_buffer[airspeed_ets_buffer_idx++] = airspeed_tmp;
if (airspeed_ets_buffer_idx >= AIRSPEED_ETS_NBSAMPLES_AVRG) {
airspeed_ets_buffer_idx = 0;
}
airspeed_ets = 0.0;
for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) {
airspeed_ets += airspeed_ets_buffer[n];
}
airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG;
#if USE_AIRSPEED_ETS
stateSetAirspeed_f(airspeed_ets);
#endif
#if AIRSPEED_ETS_SYNC_SEND
DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
#endif
} else {
airspeed_ets = 0.0;
}
#if AIRSPEED_ETS_SDLOG
#ifndef SITL
if (pprzLogFile != -1) {
if (!log_airspeed_ets_started) {
sdLogWriteLog(pprzLogFile, "AIRSPEED_ETS: raw offset airspeed(m/s) GPS_fix TOW(ms) Week Lat(1e7deg) Lon(1e7deg) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n");
log_airspeed_ets_started = true;
}
sdLogWriteLog(pprzLogFile, "airspeed_ets: %d %d %8.4f %d %d %d %d %d %d %d %d %d\n",
airspeed_ets_raw, airspeed_ets_offset, airspeed_ets,
gps.fix, gps.tow, gps.week,
gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl,
gps.gspeed, gps.course, -gps.ned_vel.z);
}
#endif
#endif
// Transaction has been read
airspeed_ets_i2c_trans.status = I2CTransDone;
}
void airspeed_amsys_read_event( void ) {
// Get raw airspeed from buffer
airspeed_amsys_raw = 0;
airspeed_amsys_raw = (airspeed_amsys_i2c_trans.buf[0]<<8) | airspeed_amsys_i2c_trans.buf[1];
#ifdef MEASURE_AMSYS_TEMPERATURE
tempAS_amsys_raw = (airspeed_amsys_i2c_trans.buf[2]<<8) | airspeed_amsys_i2c_trans.buf[3];
const float temp_off_scale = (float)(TEMPERATURE_AMSYS_MAX) /
(TEMPERATURE_AMSYS_OFFSET_MAX - TEMPERATURE_AMSYS_OFFSET_MIN);
// Tmin=-25, Tmax=85
airspeed_temperature = temp_off_scale * (tempAS_amsys_raw - TEMPERATURE_AMSYS_OFFSET_MIN) +
TEMPERATURE_AMSYS_MIN;
#endif
// Check if this is valid airspeed
if (airspeed_amsys_raw == 0)
airspeed_amsys_valid = FALSE;
else
airspeed_amsys_valid = TRUE;
// Continue only if a new airspeed value was received
if (airspeed_amsys_valid) {
// raw not under offest min
if (airspeed_amsys_raw < AIRSPEED_AMSYS_OFFSET_MIN)
airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MIN;
// raw not over offest max
if (airspeed_amsys_raw > AIRSPEED_AMSYS_OFFSET_MAX)
airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MAX;
// calculate raw to pressure
const float p_off_scale = (float)(AIRSPEED_AMSYS_MAXPRESURE) /
(AIRSPEED_AMSYS_OFFSET_MAX - AIRSPEED_AMSYS_OFFSET_MIN);
airspeed_amsys_p = p_off_scale * (airspeed_amsys_raw - AIRSPEED_AMSYS_OFFSET_MIN);
if (!airspeed_amsys_offset_init) {
--airspeed_amsys_cnt;
// Check if averaging completed
if (airspeed_amsys_cnt == 0) {
// Calculate average
airspeed_amsys_offset = airspeed_amsys_offset_tmp / AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG;
airspeed_amsys_offset_init = TRUE;
}
// Check if averaging needs to continue
else if (airspeed_amsys_cnt <= AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG) {
airspeed_amsys_offset_tmp += airspeed_amsys_p;
}
airspeed_amsys = 0.;
}
else {
airspeed_amsys_p = airspeed_amsys_p - airspeed_amsys_offset;
if (airspeed_amsys_p <= 0)
airspeed_amsys_p = 0.000000001;
// convert pressure to airspeed
airspeed_amsys_tmp = sqrtf(2 * airspeed_amsys_p * airspeed_scale / 1.2041);
// Lowpassfiltering
airspeed_amsys = airspeed_filter * airspeed_old +
(1.0 - airspeed_filter) * airspeed_amsys_tmp;
airspeed_old = airspeed_amsys;
//New value available
#if USE_AIRSPEED
stateSetAirspeed_f(&airspeed_amsys);
#endif
#ifdef AIRSPEED_AMSYS_SYNC_SEND
airspeed_amsys_downlink();
#endif
}
}
/*else {
airspeed_amsys = 0.0;
}*/
// Transaction has been read
airspeed_amsys_i2c_trans.status = I2CTransDone;
}
void airspeed_ets_read_event( void ) {
int n;
float airspeed_tmp = 0.0;
// Get raw airspeed from buffer
airspeed_ets_raw = ((uint16_t)(airspeed_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(airspeed_ets_i2c_trans.buf[0]);
// Check if this is valid airspeed
if (airspeed_ets_raw == 0)
airspeed_ets_valid = FALSE;
else
airspeed_ets_valid = TRUE;
// Continue only if a new airspeed value was received
if (airspeed_ets_valid) {
// Calculate offset average if not done already
if (!airspeed_ets_offset_init) {
--airspeed_ets_cnt;
// Check if averaging completed
if (airspeed_ets_cnt == 0) {
// Calculate average
airspeed_ets_offset = (uint16_t)(airspeed_ets_offset_tmp / AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG);
// Limit offset
if (airspeed_ets_offset < AIRSPEED_ETS_OFFSET_MIN)
airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MIN;
if (airspeed_ets_offset > AIRSPEED_ETS_OFFSET_MAX)
airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MAX;
airspeed_ets_offset_init = TRUE;
}
// Check if averaging needs to continue
else if (airspeed_ets_cnt <= AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG)
airspeed_ets_offset_tmp += airspeed_ets_raw;
}
// Convert raw to m/s
#ifdef AIRSPEED_ETS_REVERSE
if (airspeed_ets_offset_init && airspeed_ets_raw < airspeed_ets_offset)
airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_offset-airspeed_ets_raw) ) - AIRSPEED_ETS_OFFSET;
#else
if (airspeed_ets_offset_init && airspeed_ets_raw > airspeed_ets_offset)
airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_raw-airspeed_ets_offset) ) - AIRSPEED_ETS_OFFSET;
#endif
else
airspeed_tmp = 0.0;
// Airspeed should always be positive
if (airspeed_tmp < 0.0)
airspeed_tmp = 0.0;
// Moving average
airspeed_ets_buffer[airspeed_ets_buffer_idx++] = airspeed_tmp;
if (airspeed_ets_buffer_idx >= AIRSPEED_ETS_NBSAMPLES_AVRG)
airspeed_ets_buffer_idx = 0;
airspeed_ets = 0.0;
for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n)
airspeed_ets += airspeed_ets_buffer[n];
airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG;
#if USE_AIRSPEED
stateSetAirspeed_f(&airspeed_ets);
#endif
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
#endif
} else {
airspeed_ets = 0.0;
}
// Transaction has been read
airspeed_ets_i2c_trans.status = I2CTransDone;
}
void nps_autopilot_run_step(double time)
{
nps_electrical_run_step(time);
#if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK
if (nps_radio_control_available(time)) {
radio_control_feed();
main_event();
}
#endif
if (nps_sensors_gyro_available()) {
imu_feed_gyro_accel();
main_event();
}
if (nps_sensors_mag_available()) {
imu_feed_mag();
main_event();
}
if (nps_sensors_baro_available()) {
float pressure = (float) sensors.baro.value;
AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, pressure);
main_event();
}
if (nps_sensors_temperature_available()) {
AbiSendMsgTEMPERATURE(BARO_SIM_SENDER_ID, (float)sensors.temp.value);
}
#if USE_AIRSPEED
if (nps_sensors_airspeed_available()) {
stateSetAirspeed_f((float)sensors.airspeed.value);
}
#endif
#if USE_SONAR
if (nps_sensors_sonar_available()) {
float dist = (float) sensors.sonar.value;
AbiSendMsgAGL(AGL_SONAR_NPS_ID, dist);
uint16_t foo = 0;
DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist);
main_event();
}
#endif
#if USE_GPS
if (nps_sensors_gps_available()) {
gps_feed_value();
main_event();
}
#endif
if (nps_bypass_ahrs) {
sim_overwrite_ahrs();
}
if (nps_bypass_ins) {
sim_overwrite_ins();
}
handle_periodic_tasks();
/* scale final motor commands to 0-1 for feeding the fdm */
for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
autopilot.commands[i] = (double)motor_mixing.commands[i] / MAX_PPRZ;
}
}
void dl_parse_msg(void) {
datalink_time = 0;
uint8_t msg_id = IdOfMsg(dl_buffer);
#if 0 // not ready yet
uint8_t sender_id = SenderIdOfMsg(dl_buffer);
/* parse telemetry messages coming from other AC */
if (sender_id != 0) {
switch (msg_id) {
#ifdef TCAS
case DL_TCAS_RA:
{
if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && SenderIdOfMsg(dl_buffer) != AC_ID) {
uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer);
tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer);
}
}
#endif
}
return;
}
#endif
if (msg_id == DL_PING) {
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice)
} else
#ifdef TRAFFIC_INFO
if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) {
uint8_t id = DL_ACINFO_ac_id(dl_buffer);
float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer));
float a = MOfCm(DL_ACINFO_alt(dl_buffer));
float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
float s = MOfCm(DL_ACINFO_speed(dl_buffer));
float cl = MOfCm(DL_ACINFO_climb(dl_buffer));
uint32_t t = DL_ACINFO_itow(dl_buffer);
SetAcInfo(id, ux, uy, c, a, s, cl, t);
} else
#endif
#ifdef NAV
if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) {
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla;
lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7));
lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7));
struct UtmCoor_f utm;
utm.zone = nav_utm_zone0;
utm_of_lla_f(&utm, &lla);
nav_move_waypoint(wp_id, utm.east, utm.north, a);
/* Waypoint range is limited. Computes the UTM pos back from the relative
coordinates */
utm.east = waypoints[wp_id].x + nav_utm_east0;
utm.north = waypoints[wp_id].y + nav_utm_north0;
DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
} else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) {
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
SEND_NAVIGATION(DefaultChannel, DefaultDevice);
} else
#endif /** NAV */
#ifdef WIND_INFO
if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
struct FloatVect2 wind;
wind.x = DL_WIND_INFO_north(dl_buffer);
wind.y = DL_WIND_INFO_east(dl_buffer);
stateSetHorizontalWindspeed_f(&wind);
#if !USE_AIRSPEED
float airspeed = DL_WIND_INFO_airspeed(dl_buffer);
stateSetAirspeed_f(&airspeed);
#endif
#ifdef WIND_INFO_RET
DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind.y, &wind.x, stateGetAirspeed_f());
#endif
} else
#endif /** WIND_INFO */
#ifdef HITL
/** Infrared and GPS sensors are replaced by messages on the datalink */
if (msg_id == DL_HITL_INFRARED) {
/** This code simulates infrared.c:ir_update() */
infrared.roll = DL_HITL_INFRARED_roll(dl_buffer);
infrared.pitch = DL_HITL_INFRARED_pitch(dl_buffer);
infrared.top = DL_HITL_INFRARED_top(dl_buffer);
} else if (msg_id == DL_HITL_UBX) {
/** This code simulates gps_ubx.c:parse_ubx() */
if (gps_msg_received) {
gps_nb_ovrn++;
} else {
ubx_class = DL_HITL_UBX_class(dl_buffer);
ubx_id = DL_HITL_UBX_id(dl_buffer);
uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer);
uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer);
memcpy(ubx_msg_buf, ubx_payload, l);
gps_msg_received = TRUE;
}
} else
#endif
#ifdef DlSetting
if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
uint8_t i = DL_SETTING_index(dl_buffer);
float val = DL_SETTING_value(dl_buffer);
DlSetting(i, val);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
} else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
} else
#endif /** Else there is no dl_settings section in the flight plan */
#if USE_JOYSTICK
if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) {
JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer),
DL_JOYSTICK_RAW_pitch(dl_buffer),
DL_JOYSTICK_RAW_throttle(dl_buffer));
} else
#endif // USE_JOYSTICK
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
} else if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(
DL_RC_4CH_mode(dl_buffer),
DL_RC_4CH_throttle(dl_buffer),
DL_RC_4CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer),
DL_RC_4CH_yaw(dl_buffer));
} else
#endif // RC_DATALINK
{ /* Last else */
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
}