void apogee_baro_event(void)
{
mpl3115_event(&apogee_baro);
if (apogee_baro.data_available && startup_cnt == 0) {
float pressure = ((float)apogee_baro.pressure / (1 << 2));
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
float temp = apogee_baro.temperature / 16.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
apogee_baro.data_available = false;
#if APOGEE_BARO_SDLOG
if (pprzLogFile != -1) {
if (!log_apogee_baro_started) {
sdLogWriteLog(pprzLogFile, "APOGEE_BARO: Pres(Pa) Temp(degC) GPS_fix TOW(ms) Week Lat(1e7deg) Lon(1e7deg) HMSL(mm) gpseed(cm/s) course(1e7deg) climb(cm/s)\n");
log_apogee_baro_started = true;
}
sdLogWriteLog(pprzLogFile, "apogee_baro: %9.4f %9.4f %d %d %d %d %d %d %d %d %d\n",
pressure,temp,
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
}
}
void baro_event(void)
{
if (sys_time.nb_sec > 1) {
ms5611_spi_event(&bb_ms5611);
if (bb_ms5611.data_available) {
float pressure = (float)bb_ms5611.data.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
float temp = bb_ms5611.data.temperature / 100.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
bb_ms5611.data_available = false;
#ifdef BARO_LED
RunOnceEvery(10, LED_TOGGLE(BARO_LED));
#endif
#if DEBUG
float fbaroms = bb_ms5611.data.pressure / 100.;
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
&bb_ms5611.data.d1, &bb_ms5611.data.d2,
&fbaroms, &temp);
#endif
}
}
}
void baro_bmp_event(void) {
bmp085_event(&baro_bmp);
if (baro_bmp.data_available) {
float tmp = baro_bmp.pressure / 101325.0; // pressure at sea level
tmp = pow(tmp, 0.190295);
baro_bmp_alt = 44330 * (1.0 - tmp);
float pressure = (float)baro_bmp.pressure;
AbiSendMsgBARO_ABS(BARO_BMP_SENDER_ID, &pressure);
baro_bmp.data_available = FALSE;
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &baro_bmp.up,
&baro_bmp.ut, &baro_bmp.pressure,
&baro_bmp.temperature);
#else
RunOnceEvery(10, DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice,
&baro_bmp.up, &baro_bmp.ut,
&baro_bmp.pressure,
&baro_bmp.temperature));
#endif
}
}
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 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 (nps_sensors_gps_available()) {
gps_feed_value();
main_event();
}
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;
}
}
/* 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 = pprzlink_get_msg_id(mp_msg_buf);
#if PPRZLINK_DEFAULT_VER == 2
// Check that the message is really a intermcu message
if (pprzlink_get_msg_class_id(mp_msg_buf) == DL_intermcu_CLASS_ID) {
#endif
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);
AbiSendMsgAIRSPEED(IMU_MAG_PITOT_ID, pitot_ias);
break;
}
default:
break;
}
#if PPRZLINK_DEFAULT_VER == 2
}
#endif
}
void apogee_baro_event(void)
{
mpl3115_event(&apogee_baro);
if (apogee_baro.data_available && startup_cnt == 0) {
float pressure = ((float)apogee_baro.pressure / (1 << 2));
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
float temp = apogee_baro.temperature / 16.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp);
apogee_baro.data_available = FALSE;
}
}
void navgo_baro_event(void) {
mcp355x_event();
if (mcp355x_data_available) {
if (startup_cnt == 0) {
// Send data when init phase is done
float pressure = NAVGO_BARO_SENS*(mcp355x_data+NAVGO_BARO_OFFSET);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
}
mcp355x_data_available = FALSE;
}
}
void baro_scp_event(void)
{
if (baro_scp_available == TRUE) {
float pressure = (float)baro_scp_pressure;
AbiSendMsgBARO_ABS(BARO_SCP_SENDER_ID, pressure);
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_SCP_STATUS(DefaultChannel, DefaultDevice, &baro_scp_pressure, &baro_scp_temperature);
#endif
baro_scp_available = FALSE;
}
}
void umarim_baro_event(void)
{
Ads1114Event();
if (BARO_ABS_ADS.data_available) {
if (startup_cnt == 0) {
float pressure = UMARIM_BARO_SENS * Ads1114GetValue(BARO_ABS_ADS);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
}
BARO_ABS_ADS.data_available = FALSE;
}
}
void baro_ets_read_event(void)
{
// Get raw altimeter from buffer
baro_ets_adc = ((uint16_t)(baro_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(baro_ets_i2c_trans.buf[0]);
// Check if this is valid altimeter
if (baro_ets_adc == 0) {
baro_ets_valid = FALSE;
} else {
baro_ets_valid = TRUE;
}
// Continue only if a new altimeter value was received
if (baro_ets_valid) {
// Calculate offset average if not done already
if (!baro_ets_offset_init) {
--baro_ets_cnt;
// Check if averaging completed
if (baro_ets_cnt == 0) {
// Calculate average
baro_ets_offset = (uint16_t)(baro_ets_offset_tmp / BARO_ETS_OFFSET_NBSAMPLES_AVRG);
// Limit offset
if (baro_ets_offset < BARO_ETS_OFFSET_MIN) {
baro_ets_offset = BARO_ETS_OFFSET_MIN;
}
if (baro_ets_offset > BARO_ETS_OFFSET_MAX) {
baro_ets_offset = BARO_ETS_OFFSET_MAX;
}
baro_ets_offset_init = TRUE;
}
// Check if averaging needs to continue
else if (baro_ets_cnt <= BARO_ETS_OFFSET_NBSAMPLES_AVRG) {
baro_ets_offset_tmp += baro_ets_adc;
}
}
// Convert raw to m/s
if (baro_ets_offset_init) {
baro_ets_altitude = ground_alt + BARO_ETS_ALT_SCALE * (float)(baro_ets_offset - baro_ets_adc);
// New value available
float pressure = BARO_ETS_SCALE * (float) baro_ets_adc + BARO_ETS_PRESSURE_OFFSET;
AbiSendMsgBARO_ABS(BARO_ETS_SENDER_ID, pressure);
#ifdef BARO_ETS_SYNC_SEND
DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude);
#endif
} else {
baro_ets_altitude = 0.0;
}
} else {
baro_ets_altitude = 0.0;
}
// Transaction has been read
baro_ets_i2c_trans.status = I2CTransDone;
}
void bmp_baro_event(void)
{
bmp085_event(&baro_bmp085);
if (baro_bmp085.data_available) {
float pressure = (float)baro_bmp085.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
baro_bmp085.data_available = FALSE;
#ifdef BARO_LED
RunOnceEvery(10, LED_TOGGLE(BARO_LED));
#endif
}
}
void baro_scp_event(void)
{
if (scp_trans.status == I2CTransSuccess) {
if (baro_scp_status == BARO_SCP_RD_TEMP) {
/* read two byte temperature */
baro_scp_temperature = scp_trans.buf[0] << 8;
baro_scp_temperature |= scp_trans.buf[1];
if (baro_scp_temperature & 0x2000) {
baro_scp_temperature |= 0xC000;
}
baro_scp_temperature *= 5;
/* start one byte msb pressure */
scp_trans.buf[0] = SCP1000_DATARD8;
baro_scp_status = BARO_SCP_RD_PRESS_0;
i2c_transceive(&SCP_I2C_DEV, &scp_trans, SCP1000_SLAVE_ADDR, 1, 1);
}
else if (baro_scp_status == BARO_SCP_RD_PRESS_0) {
/* read one byte pressure */
baro_scp_pressure = scp_trans.buf[0] << 16;
/* start two byte lsb pressure */
scp_trans.buf[0] = SCP1000_DATARD16;
baro_scp_status = BARO_SCP_RD_PRESS_1;
i2c_transceive(&SCP_I2C_DEV, &scp_trans, SCP1000_SLAVE_ADDR, 1, 2);
}
else if (baro_scp_status == BARO_SCP_RD_PRESS_1) {
/* read two byte pressure */
baro_scp_pressure |= scp_trans.buf[0] << 8;
baro_scp_pressure |= scp_trans.buf[1];
baro_scp_pressure *= 25;
float pressure = (float) baro_scp_pressure;
AbiSendMsgBARO_ABS(BARO_SCP_SENDER_ID, pressure);
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_SCP_STATUS(DefaultChannel, DefaultDevice, &baro_scp_pressure, &baro_scp_temperature);
#endif
baro_scp_status = BARO_SCP_IDLE;
}
else { baro_scp_status = BARO_SCP_IDLE; }
}
}
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 baro_mpl3115_read_event(void)
{
mpl3115_event(&baro_mpl);
if (baro_mpl.data_available) {
float pressure = (float)baro_mpl.pressure / (1 << 2);
AbiSendMsgBARO_ABS(BARO_MPL3115_SENDER_ID, pressure);
float temp = (float)baro_mpl.pressure / 16.0f;
AbiSendMsgTEMPERATURE(BARO_MPL3115_SENDER_ID, temp);
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_MPL3115_BARO(DefaultChannel, DefaultDevice, &baro_mpl.pressure, &baro_mpl.temperature, &baro_mpl.alt);
#endif
baro_mpl.data_available = FALSE;
}
}
void baro_event(void) {
bmp085_event(&baro_bmp085);
if (baro_bmp085.data_available) {
float pressure = (float)baro_bmp085.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
float temp = baro_bmp085.temperature / 10.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp);
baro_bmp085.data_available = FALSE;
#ifdef BARO_LED
RunOnceEvery(10,LED_TOGGLE(BARO_LED));
#endif
}
}
void lisa_l_baro_event(void)
{
if (baro_board.status == LBS_READING_ABS &&
baro_trans.status != I2CTransPending) {
baro_board.status = LBS_READ_ABS;
if (baro_trans.status == I2CTransSuccess) {
int16_t tmp = baro_trans.buf[0] << 8 | baro_trans.buf[1];
float pressure = LISA_L_BARO_SENS * (float)tmp;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
}
} else if (baro_board.status == LBS_READING_DIFF &&
baro_trans.status != I2CTransPending) {
baro_board.status = LBS_READ_DIFF;
if (baro_trans.status == I2CTransSuccess) {
int16_t tmp = baro_trans.buf[0] << 8 | baro_trans.buf[1];
float diff = LISA_L_DIFF_SENS * (float)tmp;
AbiSendMsgBARO_DIFF(BARO_BOARD_SENDER_ID, diff);
}
}
}
void baro_ms5611_event( void ) {
ms5611_i2c_event(&baro_ms5611);
if (baro_ms5611.data_available) {
float pressure = (float)baro_ms5611.data.pressure;
AbiSendMsgBARO_ABS(BARO_MS5611_SENDER_ID, &pressure);
baro_ms5611.data_available = FALSE;
baro_ms5611_alt = pprz_isa_altitude_of_pressure(pressure);
baro_ms5611_alt_valid = TRUE;
#ifdef SENSOR_SYNC_SEND
ftempms = baro_ms5611.data.temperature / 100.;
fbaroms = baro_ms5611.data.pressure / 100.;
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
&baro_ms5611.data.d1, &baro_ms5611.data.d2,
&fbaroms, &ftempms);
#endif
}
}
void baro_hca_read_event(void)
{
pBaroRaw = 0;
// Get raw altimeter from buffer
pBaroRaw = ((uint16_t)baro_hca_i2c_trans.buf[0] << 8) | baro_hca_i2c_trans.buf[1];
if (pBaroRaw == 0) {
baro_hca_valid = FALSE;
} else {
baro_hca_valid = TRUE;
}
if (baro_hca_valid) {
//Cut RAW Min and Max
if (pBaroRaw < BARO_HCA_MIN_OUT) {
pBaroRaw = BARO_HCA_MIN_OUT;
}
if (pBaroRaw > BARO_HCA_MAX_OUT) {
pBaroRaw = BARO_HCA_MAX_OUT;
}
float pressure = BARO_HCA_SCALE * (float)pBaroRaw + BARO_HCA_PRESSURE_OFFSET;
AbiSendMsgBARO_ABS(BARO_HCA_SENDER_ID, pressure);
}
baro_hca_i2c_trans.status = I2CTransDone;
uint16_t foo = 0;
float bar = 0;
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &pBaroRaw, &foo, &bar)
#else
RunOnceEvery(10, DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &pBaroRaw, &foo, &bar));
#endif
}
void baro_sim_periodic(void) {
float pressure = pprz_isa_pressure_of_altitude(gps.hmsl / 1000.0);
AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, &pressure);
}
void baro_amsys_read_event(void)
{
pBaroRaw = 0;
// Get raw altimeter from buffer
pBaroRaw = (baro_amsys_i2c_trans.buf[0] << 8) | baro_amsys_i2c_trans.buf[1];
#ifdef MEASURE_AMSYS_TEMPERATURE
tBaroRaw = (baro_amsys_i2c_trans.buf[2] << 8) | baro_amsys_i2c_trans.buf[3];
const float temp_off_scale = (float)(TEMPERATURE_AMSYS_MAX - TEMPERATURE_AMSYS_MIN) /
(TEMPERATURE_AMSYS_OFFSET_MAX - TEMPERATURE_AMSYS_OFFSET_MIN);
// Tmin=-25, Tmax=85
baro_amsys_temp = temp_off_scale * (tBaroRaw - TEMPERATURE_AMSYS_OFFSET_MIN) +
TEMPERATURE_AMSYS_MIN;
#endif
// Check if this is valid altimeter
if (pBaroRaw == 0) {
baro_amsys_valid = false;
} else {
baro_amsys_valid = true;
}
baro_amsys_adc = pBaroRaw;
// Continue only if a new altimeter value was received
//if (baro_amsys_valid && GpsFixValid()) {
if (baro_amsys_valid) {
//Cut RAW Min and Max
if (pBaroRaw < BARO_AMSYS_OFFSET_MIN) {
pBaroRaw = BARO_AMSYS_OFFSET_MIN;
}
if (pBaroRaw > BARO_AMSYS_OFFSET_MAX) {
pBaroRaw = BARO_AMSYS_OFFSET_MAX;
}
//Convert to pressure
baro_amsys_p = (float)(pBaroRaw - BARO_AMSYS_OFFSET_MIN) * BARO_AMSYS_MAX_PRESSURE / (float)(
BARO_AMSYS_OFFSET_MAX - BARO_AMSYS_OFFSET_MIN);
// Send pressure over ABI
AbiSendMsgBARO_ABS(BARO_AMSYS_SENDER_ID, baro_amsys_p);
// compute altitude localy
if (!baro_amsys_offset_init) {
--baro_amsys_cnt;
// Check if averaging completed
if (baro_amsys_cnt == 0) {
// Calculate average
baro_amsys_offset = (float)(baro_amsys_offset_tmp / BARO_AMSYS_OFFSET_NBSAMPLES_AVRG);
ref_alt_init = GROUND_ALT;
baro_amsys_offset_init = true;
// hight over Sea level at init point
//baro_amsys_offset_altitude = 288.15 / 0.0065 * (1 - pow((baro_amsys_p)/1013.25 , 1/5.255));
}
// Check if averaging needs to continue
else if (baro_amsys_cnt <= BARO_AMSYS_OFFSET_NBSAMPLES_AVRG) {
baro_amsys_offset_tmp += baro_amsys_p;
}
baro_amsys_altitude = 0.0;
} else {
// Lowpassfiltering and convert pressure to altitude
baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset - baro_amsys_p) * baro_scale /
(1.2041 * 9.81);
baro_old = baro_amsys_altitude;
//New value available
}
baro_amsys_abs_altitude = baro_amsys_altitude + ref_alt_init;
} /*else {
baro_amsys_abs_altitude = 0.0;
}*/
// Transaction has been read
baro_amsys_i2c_trans.status = I2CTransDone;
}
