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);
float temp = baro_bmp.temperature / 10.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
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
}
}
/* 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 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 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 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);
float temp = baro_ms5611.data.temperature / 100.0f;
AbiSendMsgTEMPERATURE(BARO_MS5611_SENDER_ID, &temp);
baro_ms5611.data_available = FALSE;
baro_ms5611_alt = pprz_isa_altitude_of_pressure(pressure);
baro_ms5611_alt_valid = TRUE;
#ifdef SENSOR_SYNC_SEND
fbaroms = baro_ms5611.data.pressure / 100.;
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
&baro_ms5611.data.d1, &baro_ms5611.data.d2,
&fbaroms, &temp);
#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 (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;
}
}
