void parse_mf_daq_msg(void)
{
mf_daq.nb = dl_buffer[2];
if (mf_daq.nb > 0) {
if (mf_daq.nb > MF_DAQ_SIZE) {
mf_daq.nb = MF_DAQ_SIZE;
}
// Store data struct directly from dl_buffer
float *buf = (float*)(dl_buffer+3);
memcpy(mf_daq.values, buf, mf_daq.nb * sizeof(float));
// Log on SD card
if (log_started) {
DOWNLINK_SEND_PAYLOAD_FLOAT(pprzlog_tp, chibios_sdlog, mf_daq.nb, mf_daq.values);
DOWNLINK_SEND_MF_DAQ_STATE(pprzlog_tp, chibios_sdlog,
&autopilot_flight_time,
&stateGetBodyRates_f()->p,
&stateGetBodyRates_f()->q,
&stateGetBodyRates_f()->r,
&stateGetNedToBodyEulers_f()->phi,
&stateGetNedToBodyEulers_f()->theta,
&stateGetNedToBodyEulers_f()->psi,
&stateGetAccelNed_f()->x,
&stateGetAccelNed_f()->y,
&stateGetAccelNed_f()->z,
&stateGetSpeedEnu_f()->x,
&stateGetSpeedEnu_f()->y,
&stateGetSpeedEnu_f()->z,
&stateGetPositionLla_f()->lat,
&stateGetPositionLla_f()->lon,
&stateGetPositionLla_f()->alt,
&stateGetHorizontalWindspeed_f()->y,
&stateGetHorizontalWindspeed_f()->x);
}
}
}
void mf_daq_send_report(void)
{
// Send report over normal telemetry
if (mf_daq.nb > 0) {
DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 9, mf_daq.values);
}
// Test if log is started
if (pprzLogFile != -1) {
if (log_started == FALSE) {
// Log MD5SUM once
DOWNLINK_SEND_ALIVE(pprzlog_tp, chibios_sdlog, 16, MD5SUM);
log_started = true;
}
// Log GPS for time reference
uint8_t foo = 0;
int16_t climb = -gps.ned_vel.z;
int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
struct UtmCoor_f utm = *stateGetPositionUtm_f();
int32_t east = utm.east * 100;
int32_t north = utm.north * 100;
DOWNLINK_SEND_GPS(pprzlog_tp, chibios_sdlog, &gps.fix,
&east, &north, &course, &gps.hmsl, &gps.gspeed, &climb,
&gps.week, &gps.tow, &utm.zone, &foo);
}
}
void mf_ptu_periodic(void)
{
// Read ADC
pressure_adc = pressure_buf.sum / pressure_buf.av_nb_sample;
temp_adc = temp_buf.sum / temp_buf.av_nb_sample;
// Read PWM
humid_period = USEC_OF_PWM_INPUT_TICKS(pwm_input_period_tics[PWM_INPUT_CHANNEL_HUMIDITY]);
// Log data
#if LOG_PTU
if (pprzLogFile != -1) {
if (!log_ptu_started) {
sdLogWriteLog(pprzLogFile,
"P(adc) T(adc) H(usec) GPS_fix TOW(ms) Week Lat(1e7rad) Lon(1e7rad) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n");
log_ptu_started = true;
} else {
sdLogWriteLog(pprzLogFile, "%d %d %d %d %d %d %d %d %d %d %d %d\n",
pressure_adc, temp_adc, humid_period,
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
// Send data
#if SEND_PTU
#define PTU_DATA_SIZE 3
float ptu_data[PTU_DATA_SIZE];
ptu_data[0] = (float)(PTU_PRESSURE_SCALE * ((int16_t)pressure_adc - PTU_PRESSURE_OFFSET));
ptu_data[1] = (float)(PTU_TEMPERATURE_SCALE * ((int16_t)temp_adc - PTU_TEMPERATURE_OFFSET));
ptu_data[2] = (float)(PTU_HUMIDTY_SCALE * ((int16_t)humid_period - PTU_HUMIDTY_OFFSET));
DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, PTU_DATA_SIZE, ptu_data);
#endif
}
static inline void meteo_stick_send_data(void)
{
float ptu_data[MS_DATA_SIZE];
ptu_data[0] = meteo_stick.current_pressure;
ptu_data[1] = meteo_stick.current_temperature;
ptu_data[2] = meteo_stick.current_humidity;
ptu_data[3] = meteo_stick.current_airspeed;
DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, MS_DATA_SIZE, ptu_data);
}
