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