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); }