void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct DoubleRMat* body_to_imu) { if (time < mag->next_update) return; /* transform magnetic field to body frame */ struct DoubleVect3 h_body; double_quat_vmult(&h_body, &fdm.ltp_to_body_quat, &fdm.ltp_h); /* transform to imu frame */ struct DoubleVect3 h_imu; MAT33_VECT3_MUL(h_imu, *body_to_imu, h_body ); /* transform to sensor frame */ struct DoubleVect3 h_sensor; MAT33_VECT3_MUL(h_sensor, mag->imu_to_sensor_rmat, h_imu ); /* compute magnetometer reading */ MAT33_VECT3_MUL(mag->value, mag->sensitivity, h_sensor); VECT3_ADD(mag->value, mag->neutral); /* FIXME: ADD error reading */ /* round signal to account for adc discretisation */ DOUBLE_VECT3_ROUND(mag->value); /* saturate */ VECT3_BOUND_CUBE(mag->value, mag->min, mag->max); mag->next_update += NPS_MAG_DT; mag->data_available = TRUE; }
void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct DoubleRMat* body_to_imu) { if (time < gyro->next_update) return; /* transform body rates to IMU frame */ struct DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_inertial_rotvel); struct DoubleVect3 rate_imu; MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body ); /* compute gyros readings */ MAT33_VECT3_MUL(gyro->value, gyro->sensitivity, rate_imu); VECT3_ADD(gyro->value, gyro->neutral); /* compute gyro error readings */ struct DoubleVect3 gyro_error; VECT3_COPY(gyro_error, gyro->bias_initial); double_vect3_add_gaussian_noise(&gyro_error, &gyro->noise_std_dev); double_vect3_update_random_walk(&gyro->bias_random_walk_value, &gyro->bias_random_walk_std_dev, NPS_GYRO_DT, 5.); VECT3_ADD(gyro_error, gyro->bias_random_walk_value); struct DoubleVect3 gain = {NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR}; VECT3_EW_MUL(gyro_error, gyro_error, gain); VECT3_ADD(gyro->value, gyro_error); /* round signal to account for adc discretisation */ DOUBLE_VECT3_ROUND(gyro->value); /* saturate */ VECT3_BOUND_CUBE(gyro->value, gyro->min, gyro->max); gyro->next_update += NPS_GYRO_DT; gyro->data_available = TRUE; }
void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct DoubleRMat* body_to_imu) { if (time < accel->next_update) return; /* transform gravity to body frame */ struct DoubleVect3 g_body; FLOAT_QUAT_VMULT(g_body, fdm.ltp_to_body_quat, fdm.ltp_g); // printf(" g_body %f %f %f\n", g_body.x, g_body.y, g_body.z); // printf(" accel_fdm %f %f %f\n", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z); /* substract gravity to acceleration in body frame */ struct DoubleVect3 accelero_body; VECT3_DIFF(accelero_body, fdm.body_ecef_accel, g_body); // printf(" accelero body %f %f %f\n", accelero_body.x, accelero_body.y, accelero_body.z); /* transform to imu frame */ struct DoubleVect3 accelero_imu; MAT33_VECT3_MUL(accelero_imu, *body_to_imu, accelero_body ); /* compute accelero readings */ MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu); VECT3_ADD(accel->value, accel->neutral); /* Compute sensor error */ struct DoubleVect3 accelero_error; /* constant bias */ VECT3_COPY(accelero_error, accel->bias); /* white noise */ double_vect3_add_gaussian_noise(&accelero_error, &accel->noise_std_dev); /* scale */ struct DoubleVect3 gain = {NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ}; VECT3_EW_MUL(accelero_error, accelero_error, gain); /* add error */ VECT3_ADD(accel->value, accelero_error); /* round signal to account for adc discretisation */ DOUBLE_VECT3_ROUND(accel->value); /* saturate */ VECT3_BOUND_CUBE(accel->value, 0, accel->resolution); accel->next_update += NPS_ACCEL_DT; accel->data_available = TRUE; }
void guidance_indi_run(bool in_flight, int32_t heading) { //filter accel to get rid of noise //filter attitude to synchronize with accel guidance_indi_filter_attitude(); guidance_indi_filter_accel(); float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x; //+-ov.y/ (STABILIZATION_ATTITUDE_SP_MAX_THETA)*3.0; float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y; //+ ov.x/ (STABILIZATION_ATTITUDE_SP_MAX_PHI)*3.0; float speed_sp_x = pos_x_err*guidance_indi_pos_gain; float speed_sp_y = pos_y_err*guidance_indi_pos_gain; sp_accel.x = (speed_sp_x - stateGetSpeedNed_f()->x)*guidance_indi_speed_gain; sp_accel.y = (speed_sp_y - stateGetSpeedNed_f()->y)*guidance_indi_speed_gain; // sp_accel.x = (radio_control.values[RADIO_PITCH]/9600.0)*8.0; // sp_accel.y = -(radio_control.values[RADIO_ROLL]/9600.0)*8.0; // struct FloatMat33 Ga; guidance_indi_calcG(&Ga); MAT33_INV(Ga_inv, Ga); float altitude_sp = POS_FLOAT_OF_BFP(guidance_v_z_sp); float vertical_velocity_sp = guidance_indi_pos_gain*(altitude_sp - stateGetPositionNed_f()->z); // float vertical_velocity_rc_euler = -(stabilization_cmd[COMMAND_THRUST]-4500.0)/4500.0*2.0; float vertical_velocity_err_euler = vertical_velocity_sp - stateGetSpeedNed_f()->z; sp_accel.z = vertical_velocity_err_euler*guidance_indi_speed_gain; struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned.x, sp_accel.y -filt_accel_ned.y, 0.0}; Bound(a_diff.x, -6.0, 6.0); Bound(a_diff.y, -6.0, 6.0); Bound(a_diff.z, -9.0, 9.0); MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff); guidance_euler_cmd.phi = roll_filt + euler_cmd.x; guidance_euler_cmd.theta = pitch_filt + euler_cmd.y; guidance_euler_cmd.psi = 0;//stateGetNedToBodyEulers_f()->psi; //Bound euler angles to prevent flipping and keep upright Bound(guidance_euler_cmd.phi, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK); Bound(guidance_euler_cmd.theta, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK); stabilization_attitude_set_setpoint_rp_quat_f(in_flight, heading); }
void enu_of_ecef_vect_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef) { MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, *ecef); }
void enu_of_ecef_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef) { struct EcefCoor_f delta; VECT3_DIFF(delta, *ecef, def->ecef); MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, delta); }