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 aoa_pwm_update(void) { static float prev_aoa = 0.0f; // raw duty cycle in usec uint32_t duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL); // remove some offset if needed aoa_pwm.raw = duty_raw - AOA_PWM_OFFSET; // FIXME for some reason, the last value of the MA3 encoder is not 4096 but 4097 // this case is not handled since we don't care about angles close to +- 180 deg aoa_pwm.angle = AOA_SIGN * (((float)aoa_pwm.raw * aoa_pwm.sens) - aoa_pwm.offset - AOA_ANGLE_OFFSET); // filter angle aoa_pwm.angle = aoa_pwm.filter * prev_aoa + (1.0f - aoa_pwm.filter) * aoa_pwm.angle; prev_aoa = aoa_pwm.angle; #if USE_AOA stateSetAngleOfAttack_f(aoa_adc.angle); #endif #if SEND_SYNC_AOA RunOnceEvery(10, DOWNLINK_SEND_AOA(DefaultChannel, DefaultDevice, &aoa_pwm.raw, &aoa_pwm.angle)); #endif #if LOG_AOA if(pprzLogFile != -1) { if (!log_started) { sdLogWriteLog(pprzLogFile, "AOA_PWM: ANGLE(deg) RAW(int16)\n"); log_started = true; } else { float angle = DegOfRad(aoa_pwm.angle); sdLogWriteLog(pprzLogFile, "AOA_PWM: %.3f %d\n", angle, aoa_pwm.raw); } } #endif }
void temod_event(void) { if (tmd_trans.status == I2CTransSuccess) { uint16_t tmd_temperature; /* read two byte temperature */ tmd_temperature = tmd_trans.buf[0] << 8; tmd_temperature |= tmd_trans.buf[1]; ftmd_temperature = (tmd_temperature / TEMOD_TYPE) - 32.; DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &tmd_temperature, &ftmd_temperature); tmd_trans.status = I2CTransDone; #if TEMP_TEMOD_SDLOG if (pprzLogFile != -1) { if (!log_temod_started) { sdLogWriteLog(pprzLogFile, "TEMOD: Temp(degC) GPS_fix TOW(ms) Week Lat(1e7deg) Lon(1e7deg) HMSL(mm) gspeed(cm/s) course(1e7deg) climb(cm/s)\n"); log_temod_started = true; } else { sdLogWriteLog(pprzLogFile, "temod: %9.4f %d %d %d %d %d %d %d %d %d\n", ftmd_temperature, 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 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 }
void humid_sht_periodic(void) { uint8_t error = 0, checksum; if (humid_sht_status == SHT_IDLE) { /* init humidity read */ s_connectionreset(); s_start_measure(HUMI); humid_sht_status = SHT_MEASURING_HUMID; } else if (humid_sht_status == SHT_MEASURING_HUMID) { /* get data */ error += s_read_measure(&humidsht, &checksum); if (error != 0) { s_connectionreset(); s_start_measure(HUMI); //restart //LED_TOGGLE(2); } else { error += s_start_measure(TEMP); humid_sht_status = SHT_MEASURING_TEMP; } } else if (humid_sht_status == SHT_MEASURING_TEMP) { /* get data */ error += s_read_measure(&tempsht, &checksum); if (error != 0) { s_connectionreset(); s_start_measure(TEMP); //restart //LED_TOGGLE(2); } else { calc_sht(humidsht, tempsht, &fhumidsht, &ftempsht); humid_sht_available = true; s_connectionreset(); s_start_measure(HUMI); humid_sht_status = SHT_MEASURING_HUMID; DOWNLINK_SEND_SHT_STATUS(DefaultChannel, DefaultDevice, &humidsht, &tempsht, &fhumidsht, &ftempsht); humid_sht_available = false; #if SHT_SDLOG if (pprzLogFile != -1) { if (!log_sht_started) { sdLogWriteLog(pprzLogFile, "SHT75: Humid(pct) Temp(degC) GPS_fix TOW(ms) Week Lat(1e7deg) Lon(1e7deg) HMSL(mm) gspeed(cm/s) course(1e7deg) climb(cm/s)\n"); log_sht_started = true; } sdLogWriteLog(pprzLogFile, "sht75: %9.4f %9.4f %d %d %d %d %d %d %d %d %d\n", fhumidsht, ftempsht, 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 ahrs_propagate(void) { struct NedCoor_f accel; struct FloatRates body_rates; struct FloatEulers eulers; // realign all the filter if needed // a complete init cycle is required if (ins_impl.reset) { ins_impl.reset = FALSE; ins.status = INS_UNINIT; ahrs.status = AHRS_UNINIT; init_invariant_state(); } // fill command vector struct Int32Rates gyro_meas_body; INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, imu.body_to_imu_rmat, imu.gyro); RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body); struct Int32Vect3 accel_meas_body; INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body); // update correction gains error_output(&ins_impl); // propagate model struct inv_state new_state; runge_kutta_4_float((float*)&new_state, (float*)&ins_impl.state, INV_STATE_DIM, (float*)&ins_impl.cmd, INV_COMMAND_DIM, invariant_model, dt); ins_impl.state = new_state; // normalize quaternion FLOAT_QUAT_NORMALIZE(ins_impl.state.quat); // set global state FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat); #if INS_UPDATE_FW_ESTIMATOR // Some stupid lines of code for neutrals eulers.phi -= ins_roll_neutral; eulers.theta -= ins_pitch_neutral; stateSetNedToBodyEulers_f(&eulers); #else stateSetNedToBodyQuat_f(&ins_impl.state.quat); #endif RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias); stateSetBodyRates_f(&body_rates); stateSetPositionNed_f(&ins_impl.state.pos); stateSetSpeedNed_f(&ins_impl.state.speed); // untilt accel and remove gravity FLOAT_QUAT_RMAT_B2N(accel, ins_impl.state.quat, ins_impl.cmd.accel); FLOAT_VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as)); FLOAT_VECT3_ADD(accel, A); stateSetAccelNed_f(&accel); //------------------------------------------------------------// RunOnceEvery(3,{ DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice, &ins_impl.state.quat.qi, &eulers.phi, &eulers.theta, &eulers.psi, &ins_impl.state.speed.x, &ins_impl.state.speed.y, &ins_impl.state.speed.z, &ins_impl.state.pos.x, &ins_impl.state.pos.y, &ins_impl.state.pos.z, &ins_impl.state.bias.p, &ins_impl.state.bias.q, &ins_impl.state.bias.r, &ins_impl.state.as, &ins_impl.state.hb, &ins_impl.meas.baro_alt, &ins_impl.meas.pos_gps.z) }); #if LOG_INVARIANT_FILTER if (pprzLogFile.fs != NULL) { if (!log_started) { // log file header sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n"); log_started = TRUE; } else { sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", ins_impl.cmd.rates.p, ins_impl.cmd.rates.q, ins_impl.cmd.rates.r, ins_impl.cmd.accel.x, ins_impl.cmd.accel.y, ins_impl.cmd.accel.z, ins_impl.meas.pos_gps.x, ins_impl.meas.pos_gps.y, ins_impl.meas.pos_gps.z, ins_impl.meas.speed_gps.x, ins_impl.meas.speed_gps.y, ins_impl.meas.speed_gps.z, ins_impl.meas.mag.x, ins_impl.meas.mag.y, ins_impl.meas.mag.z, ins_impl.meas.baro_alt, ins_impl.state.quat.qi, ins_impl.state.quat.qx, ins_impl.state.quat.qy, ins_impl.state.quat.qz, ins_impl.state.bias.p, ins_impl.state.bias.q, ins_impl.state.bias.r, ins_impl.state.speed.x, ins_impl.state.speed.y, ins_impl.state.speed.z, ins_impl.state.pos.x, ins_impl.state.pos.y, ins_impl.state.pos.z, ins_impl.state.hb, ins_impl.state.as); } } #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 ahrs_propagate(float dt) { struct FloatVect3 accel; struct FloatRates body_rates; // realign all the filter if needed // a complete init cycle is required if (ins_impl.reset) { ins_impl.reset = FALSE; ins.status = INS_UNINIT; ahrs.status = AHRS_UNINIT; init_invariant_state(); } // fill command vector struct Int32Rates gyro_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, &imu.gyro); RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body); struct Int32Vect3 accel_meas_body; int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body); // update correction gains error_output(&ins_impl); // propagate model struct inv_state new_state; runge_kutta_4_float((float *)&new_state, (float *)&ins_impl.state, INV_STATE_DIM, (float *)&ins_impl.cmd, INV_COMMAND_DIM, invariant_model, dt); ins_impl.state = new_state; // normalize quaternion FLOAT_QUAT_NORMALIZE(ins_impl.state.quat); // set global state stateSetNedToBodyQuat_f(&ins_impl.state.quat); RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias); stateSetBodyRates_f(&body_rates); stateSetPositionNed_f(&ins_impl.state.pos); stateSetSpeedNed_f(&ins_impl.state.speed); // untilt accel and remove gravity struct FloatQuat q_b2n; float_quat_invert(&q_b2n, &ins_impl.state.quat); float_quat_vmult(&accel, &q_b2n, &ins_impl.cmd.accel); VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as)); VECT3_ADD(accel, A); stateSetAccelNed_f((struct NedCoor_f *)&accel); //------------------------------------------------------------// #if SEND_INVARIANT_FILTER struct FloatEulers eulers; FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat); RunOnceEvery(3, { pprz_msg_send_INV_FILTER(trans, dev, AC_ID, &ins_impl.state.quat.qi, &eulers.phi, &eulers.theta, &eulers.psi, &ins_impl.state.speed.x, &ins_impl.state.speed.y, &ins_impl.state.speed.z, &ins_impl.state.pos.x, &ins_impl.state.pos.y, &ins_impl.state.pos.z, &ins_impl.state.bias.p, &ins_impl.state.bias.q, &ins_impl.state.bias.r, &ins_impl.state.as, &ins_impl.state.hb, &ins_impl.meas.baro_alt, &ins_impl.meas.pos_gps.z) }); #endif #if LOG_INVARIANT_FILTER if (pprzLogFile.fs != NULL) { if (!log_started) { // log file header sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n"); log_started = TRUE; } else { sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", ins_impl.cmd.rates.p, ins_impl.cmd.rates.q, ins_impl.cmd.rates.r, ins_impl.cmd.accel.x, ins_impl.cmd.accel.y, ins_impl.cmd.accel.z, ins_impl.meas.pos_gps.x, ins_impl.meas.pos_gps.y, ins_impl.meas.pos_gps.z, ins_impl.meas.speed_gps.x, ins_impl.meas.speed_gps.y, ins_impl.meas.speed_gps.z, ins_impl.meas.mag.x, ins_impl.meas.mag.y, ins_impl.meas.mag.z, ins_impl.meas.baro_alt, ins_impl.state.quat.qi, ins_impl.state.quat.qx, ins_impl.state.quat.qy, ins_impl.state.quat.qz, ins_impl.state.bias.p, ins_impl.state.bias.q, ins_impl.state.bias.r, ins_impl.state.speed.x, ins_impl.state.speed.y, ins_impl.state.speed.z, ins_impl.state.pos.x, ins_impl.state.pos.y, ins_impl.state.pos.z, ins_impl.state.hb, ins_impl.state.as); } } #endif }
/** Periodic function */ void meteo_stick_periodic(void) { // Read ADC #ifdef MS_PRESSURE_SLAVE_IDX ads1220_periodic(&meteo_stick.pressure); #endif #ifdef MS_DIFF_PRESSURE_SLAVE_IDX ads1220_periodic(&meteo_stick.diff_pressure); #endif #ifdef MS_TEMPERATURE_SLAVE_IDX ads1220_periodic(&meteo_stick.temperature); #endif // Read PWM #ifdef MS_HUMIDITY_PWM_INPUT meteo_stick.humidity_period = pwm_input_period_tics[MS_HUMIDITY_PWM_INPUT]; meteo_stick.current_humidity = get_humidity(meteo_stick.humidity_period); #endif #if USE_MS_EEPROM if (meteo_stick.eeprom.data_available) { // Extract calibration data if (!mtostk_populate_cal_from_buffer(&meteo_stick.calib, (uint8_t *)(meteo_stick.eeprom.rx_buf + 3))) { // Extraction failed // Force number of calibration to 0 for all sensors int i; for (i = 0; i < MTOSTK_NUM_SENSORS; i++) { meteo_stick.calib.params[i].num_temp = 0; } } } else if (meteo_stick.eeprom.spi_trans.status == SPITransDone) { // Load reading request (reading 1Kb from address 0x0) eeprom25AA256_read(&meteo_stick.eeprom, 0x0, 1024); } #endif // Log data #if LOG_MS if (pprzLogFile != -1) { if (!log_ptu_started) { #if USE_MS_EEPROM if (meteo_stick.eeprom.data_available) { // Print calibration data in the log header sdLogWriteLog(pprzLogFile, "# Calibration data (UUID: %s)\n#\n", meteo_stick.calib.uuid); int i, j, k; for (i = 0; i < MTOSTK_NUM_SENSORS; i++) { sdLogWriteLog(pprzLogFile, "# Sensor: %d, time: %d, num_temp: %d, num_coeff: %d\n", i, meteo_stick.calib.params[i].timestamp, meteo_stick.calib.params[i].num_temp, meteo_stick.calib.params[i].num_coeff); if (meteo_stick.calib.params[i].timestamp == 0) { continue; // No calibration } for (j = 0; j < meteo_stick.calib.params[i].num_temp; j++) { sdLogWriteLog(pprzLogFile, "# Reference temp: %.2f\n", meteo_stick.calib.params[i].temps[j]); sdLogWriteLog(pprzLogFile, "# Coeffs:"); for (k = 0; k < meteo_stick.calib.params[i].num_coeff; k++) { sdLogWriteLog(pprzLogFile, " %.5f", meteo_stick.calib.params[i].coeffs[j][k]); } sdLogWriteLog(pprzLogFile, "\n"); } } sdLogWriteLog(pprzLogFile, "#\n"); sdLogWriteLog(pprzLogFile, "P(adc) T(adc) H(ticks) P_diff(adc) P(hPa) T(C) H(\%) CAS(m/s) FIX TOW(ms) WEEK Lat(1e7rad) Lon(1e7rad) HMSL(mm) GS(cm/s) course(1e7rad) VZ(cm/s)\n"); log_ptu_started = TRUE; } #else sdLogWriteLog(pprzLogFile, "P(adc) T(adc) H(ticks) P_diff(adc) P(hPa) T(C) H(\%) CAS(m/s) FIX TOW(ms) WEEK Lat(1e7rad) Lon(1e7rad) HMSL(mm) GS(cm/s) course(1e7rad) VZ(cm/s)\n"); log_ptu_started = TRUE; #endif } else {