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);
}
}
}
//low pass the accelerometer measurements with a second order filter to remove noise from vibrations
void guidance_indi_filter_accel(void)
{
VECT3_ADD_SCALED(filt_accel_ned, filt_accel_ned_d, 1.0/PERIODIC_FREQUENCY);
// filt_accelzbody = filt_accelzbody + filt_accelzbodyd / PERIODIC_FREQUENCY; //also do body z accel
VECT3_ADD_SCALED(filt_accel_ned_d, filt_accel_ned_dd, 1.0/PERIODIC_FREQUENCY);
// filt_accelzbodyd = filt_accelzbodyd + filt_accelzbodydd / PERIODIC_FREQUENCY; //also do body z accel
filt_accel_ned_dd.x = -filt_accel_ned_d.x * 2 * filter_zeta * filter_omega + (stateGetAccelNed_f()->x - filt_accel_ned.x) * filter_omega*filter_omega;
filt_accel_ned_dd.y = -filt_accel_ned_d.y * 2 * filter_zeta * filter_omega + (stateGetAccelNed_f()->y - filt_accel_ned.y) * filter_omega*filter_omega;
filt_accel_ned_dd.z = -filt_accel_ned_d.z * 2 * filter_zeta * filter_omega + (stateGetAccelNed_f()->z - filt_accel_ned.z) * filter_omega*filter_omega;
// filt_accelzbodydd= -filt_accelzbodyd * 2 * filter_zeta * filter_omega + (accel_meas_body_f.z - filt_accelzbody) * filter_omega*filter_omega;
}
void autopilot_check_in_flight(bool motors_on)
{
if (autopilot_in_flight) {
if (autopilot_in_flight_counter > 0) {
/* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
(fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
(fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot_in_flight = false;
}
} else { /* thrust, speed or accel not above min threshold, reset counter */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
}
} else { /* currently not in flight */
if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
motors_on) {
/* if thrust above min threshold, assume in_flight.
* Don't check for velocity and acceleration above threshold here...
*/
if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
autopilot_in_flight_counter++;
if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) {
autopilot_in_flight = true;
}
} else { /* currently not in_flight and thrust below threshold, reset counter */
autopilot_in_flight_counter = 0;
}
}
}
}
void nav_catapult_highrate_module(void)
{
if (nav_catapult.status == NAV_CATAPULT_UNINIT || nav_catapult.status == NAV_CATAPULT_ARMED) {
nav_catapult.timer = 0;
// nothing more to do
return;
}
// increase timer
nav_catapult.timer++;
// wait for acceleration
if (nav_catapult.status == NAV_CATAPULT_WAIT_ACCEL) {
// launch detection filter
if (nav_catapult.timer < NAV_CATAPULT_ACCELERATION_DETECTION) {
// several consecutive measurements above threshold
#ifndef SITL
struct FloatVect3 *accel_ned = (struct FloatVect3 *)stateGetAccelNed_f();
struct FloatRMat *ned_to_body = stateGetNedToBodyRMat_f();
struct FloatVect3 accel_body;
float_rmat_transp_vmult(&accel_body, ned_to_body, accel_ned);
if (accel_body.x < nav_catapult.accel_threshold * 9.81) {
// accel is low, reset timer
nav_catapult.timer = 0;
return;
}
#else
if (launch != 1) {
// wait for simulated launch
nav_catapult.timer = 0;
return;
}
#endif
}
// launch was detected: Motor Delay Counter
else if (nav_catapult.timer >= nav_catapult.motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
// turn on motor
NavVerticalThrottleMode(MAX_PPRZ * nav_catapult.initial_throttle);
launch = 1;
// go to next stage
nav_catapult.status = NAV_CATAPULT_MOTOR_ON;
}
}
// reaching timeout and function still running
// shuting it down
if (nav_catapult.timer > NAV_CATAPULT_TIMEOUT * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
nav_catapult.status = NAV_CATAPULT_UNINIT;
nav_catapult_nav_catapult_highrate_module_status = MODULES_STOP;
}
}
void mf_daq_send_state(void)
{
// Send aircraft state to DAQ board
DOWNLINK_SEND_MF_DAQ_STATE(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
&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);
}
