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 double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3* std_dev, double dt, double thau) {
struct DoubleVect3 drw;
double_vect3_get_gaussian_noise(&drw, std_dev);
struct DoubleVect3 tmp;
VECT3_SMUL(tmp, *rw, (-1./thau));
VECT3_ADD(drw, tmp);
VECT3_SMUL(drw, drw, dt);
VECT3_ADD(*rw, drw);
}
void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) {
if (time < gps->next_update)
return;
/*
* simulate speed sensor
*/
struct DoubleVect3 cur_speed_reading;
VECT3_COPY(cur_speed_reading, fdm.ecef_ecef_vel);
/* add a gaussian noise */
double_vect3_add_gaussian_noise(&cur_speed_reading, &gps->speed_noise_std_dev);
/* store that for later and retrieve a previously stored data */
UpdateSensorLatency(time, &cur_speed_reading, &gps->speed_history, gps->speed_latency, &gps->ecef_vel);
/*
* simulate position sensor
*/
/* compute gps error readings */
struct DoubleVect3 pos_error;
VECT3_COPY(pos_error, gps->pos_bias_initial);
/* add a gaussian noise */
double_vect3_add_gaussian_noise(&pos_error, &gps->pos_noise_std_dev);
/* update random walk bias and add it to error*/
double_vect3_update_random_walk(&gps->pos_bias_random_walk_value, &gps->pos_bias_random_walk_std_dev, NPS_GPS_DT, 5.);
VECT3_ADD(pos_error, gps->pos_bias_random_walk_value);
/* add error to current pos reading */
struct DoubleVect3 cur_pos_reading;
VECT3_COPY(cur_pos_reading, fdm.ecef_pos);
VECT3_ADD(cur_pos_reading, pos_error);
/* store that for later and retrieve a previously stored data */
UpdateSensorLatency(time, &cur_pos_reading, &gps->pos_history, gps->pos_latency, &gps->ecef_pos);
/*
* simulate lla pos
*/
/* convert current ecef reading to lla */
struct LlaCoor_d cur_lla_reading;
lla_of_ecef_d(&cur_lla_reading, (EcefCoor_d*) &cur_pos_reading);
/* store that for later and retrieve a previously stored data */
UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, gps->pos_latency, &gps->lla_pos);
double cur_hmsl_reading = fdm.hmsl;
UpdateSensorLatency_Single(time, &cur_hmsl_reading, &gps->hmsl_history, gps->pos_latency, &gps->hmsl);
gps->next_update += NPS_GPS_DT;
gps->data_available = TRUE;
}
void ahrs_aligner_run(void) {
RATES_ADD(gyro_sum, imu.gyro);
VECT3_ADD(accel_sum, imu.accel);
VECT3_ADD(mag_sum, imu.mag);
ref_sensor_samples[samples_idx] = imu.accel.z;
samples_idx++;
#ifdef AHRS_ALIGNER_LED
RunOnceEvery(50, {LED_TOGGLE(AHRS_ALIGNER_LED);});
//进行ahrs校准器的运行，
void ahrs_aligner_run(void) {
RATES_ADD(gyro_sum, imu.gyro);
VECT3_ADD(accel_sum, imu.accel);
VECT3_ADD(mag_sum, imu.mag);
ref_sensor_samples[samples_idx] = imu.accel.z;//该数组大小为60（PERIDIC FREQUENCY）
samples_idx++;//samples_idx从0开始
#ifdef AHRS_ALIGNER_LED
RunOnceEvery(50, {LED_TOGGLE(AHRS_ALIGNER_LED)});//如果定义了ahrs校准器的指示灯时会让该灯以固定频率闪烁
#endif
if (samples_idx >= SAMPLES_NB) {
int32_t avg_ref_sensor = accel_sum.z;
if ( avg_ref_sensor >= 0)
avg_ref_sensor += SAMPLES_NB / 2;
else
avg_ref_sensor -= SAMPLES_NB / 2;
avg_ref_sensor /= SAMPLES_NB;
//噪声的误差计算
ahrs_aligner.noise = 0;
int i;
for (i=0; i<SAMPLES_NB; i++) {
int32_t diff = ref_sensor_samples[i] - avg_ref_sensor;
ahrs_aligner.noise += abs(diff);
}
//存储平均值(60次)到ahrs校准的lp_xxx
RATES_SDIV(ahrs_aligner.lp_gyro, gyro_sum, SAMPLES_NB);
VECT3_SDIV(ahrs_aligner.lp_accel, accel_sum, SAMPLES_NB);
VECT3_SDIV(ahrs_aligner.lp_mag, mag_sum, SAMPLES_NB);
//清零
INT_RATES_ZERO(gyro_sum);
INT_VECT3_ZERO(accel_sum);
INT_VECT3_ZERO(mag_sum);
samples_idx = 0;
if (ahrs_aligner.noise < LOW_NOISE_THRESHOLD)
ahrs_aligner.low_noise_cnt++;
else
if ( ahrs_aligner.low_noise_cnt > 0)
ahrs_aligner.low_noise_cnt--;
if (ahrs_aligner.low_noise_cnt > LOW_NOISE_TIME) {
ahrs_aligner.status = AHRS_ALIGNER_LOCKED;//如果ahrs校准器的噪声（noise）值低于阈值的次数为5次，那么ahrs校准器将关闭
#ifdef AHRS_ALIGNER_LED
LED_ON(AHRS_ALIGNER_LED);//ahrs校准器关闭的话，对应的led灯就会关闭
#endif
}
}
}
void imu_krooz_event( void )
{
if (imu_krooz.mpu_eoc) {
mpu60x0_i2c_read(&imu_krooz.mpu);
imu_krooz.mpu_eoc = FALSE;
}
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_krooz.mpu);
if (imu_krooz.mpu.data_available) {
RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
imu_krooz.meas_nb++;
imu_krooz.mpu.data_available = FALSE;
}
if (imu_krooz.hmc_eoc) {
hmc58xx_read(&imu_krooz.hmc);
imu_krooz.hmc_eoc = FALSE;
}
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
imu_krooz.hmc.data_available = FALSE;
imu_krooz.mag_valid = TRUE;
}
}
void imu_periodic( void )
{
// Start reading the latest gyroscope data
if (!imu_krooz.mpu.config.initialized)
mpu60x0_i2c_start_configure(&imu_krooz.mpu);
if (!imu_krooz.hmc.initialized)
hmc58xx_start_configure(&imu_krooz.hmc);
if (imu_krooz.meas_nb) {
RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
VECT3_ASSIGN(imu.accel_unscaled, -imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_krooz.gyr_valid = TRUE;
imu_krooz.acc_valid = TRUE;
}
//RunOnceEvery(10,imu_krooz_downlink_raw());
}
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 imu_krooz_event(void)
{
if (imu_krooz.mpu_eoc) {
mpu60x0_i2c_read(&imu_krooz.mpu);
imu_krooz.mpu_eoc = false;
}
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_krooz.mpu);
if (imu_krooz.mpu.data_available) {
RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
imu_krooz.meas_nb++;
imu_krooz.mpu.data_available = false;
}
if (imu_krooz.hmc_eoc) {
hmc58xx_read(&imu_krooz.hmc);
imu_krooz.hmc_eoc = false;
}
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
imu_krooz.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, get_sys_time_usec(), &imu.mag);
}
}
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;
}
static inline void ahrs_lowpass_accel(void) {
// get latest measurement
ACCELS_FLOAT_OF_BFP(bafl_accel_last, imu.accel);
// low pass it
VECT3_ADD(bafl_accel_measure, bafl_accel_last);
VECT3_SDIV(bafl_accel_measure, bafl_accel_measure, 2);
#ifdef BAFL_DEBUG
bafl_phi_accel = atan2f( -bafl_accel_measure.y, -bafl_accel_measure.z);
bafl_theta_accel = atan2f(bafl_accel_measure.x, sqrtf(bafl_accel_measure.y*bafl_accel_measure.y + bafl_accel_measure.z*bafl_accel_measure.z));
#endif
}
/** Convert a local ENU position to ECEF.
* @param[out] ecef ECEF position in cm
* @param[in] def local coordinate system definition
* @param[in] enu ENU position in meter << #INT32_POS_FRAC
*/
void ecef_of_enu_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
{
/* enu_cm = (enu * 100) >> INT32_POS_FRAC
* to loose less range:
* enu_cm = (enu * 25) >> (INT32_POS_FRAC-2)
* which puts max enu input Q23.8 range to 8388km / 25 = 335km
*/
struct EnuCoor_i enu_cm;
VECT3_SMUL(enu_cm, *enu, 25);
INT32_VECT3_RSHIFT(enu_cm, enu_cm, INT32_POS_FRAC - 2);
ecef_of_enu_vect_i(ecef, def, &enu_cm);
VECT3_ADD(*ecef, def->ecef);
}
void imu_periodic(void)
{
// Start reading the latest gyroscope data
if (!imu_krooz.mpu.config.initialized) {
mpu60x0_i2c_start_configure(&imu_krooz.mpu);
}
if (!imu_krooz.hmc.initialized) {
hmc58xx_start_configure(&imu_krooz.hmc);
}
uint32_t now_ts = get_sys_time_usec();
if (imu_krooz.meas_nb) {
RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb,
imu_krooz.rates_sum.p / imu_krooz.meas_nb,
imu_krooz.rates_sum.r / imu_krooz.meas_nb);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
}
if (imu_krooz.meas_nb_acc.x && imu_krooz.meas_nb_acc.y && imu_krooz.meas_nb_acc.z) {
imu.accel_unscaled.x = 65536 - imu_krooz.accel_sum.x / imu_krooz.meas_nb_acc.x;
imu.accel_unscaled.y = 65536 - imu_krooz.accel_sum.y / imu_krooz.meas_nb_acc.y;
imu.accel_unscaled.z = imu_krooz.accel_sum.z / imu_krooz.meas_nb_acc.z;
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered);
INT_VECT3_ZERO(imu_krooz.accel_sum);
INT_VECT3_ZERO(imu_krooz.meas_nb_acc);
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
RunOnceEvery(128, {axis_nb = 5;});
/* compute the mean of the last n accel measurements */
static inline void b2_hff_compute_accel_body_mean(uint8_t n) {
struct Int32Vect3 sum;
int i, j;
INT_VECT3_ZERO(sum);
if (n > 1) {
if (n > acc_body.n) {
n = acc_body.n;
}
for (i = 1; i <= n; i++) {
j = (acc_body.w - i) > 0 ? (acc_body.w - i) : (acc_body.w - i + acc_body.size);
VECT3_ADD(sum, acc_body.buf[j]);
}
VECT3_SDIV(acc_body_mean, sum, n);
} else {
VECT3_COPY(acc_body_mean, sum);
}
}
void imu_krooz_event( void )
{
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_krooz.mpu);
if (imu_krooz.mpu.data_available) {
RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
imu_krooz.meas_nb++;
imu_krooz.mpu.data_available = FALSE;
}
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
VECT3_COPY(imu.mag_unscaled, imu_krooz.hmc.data.vect);
#if KROOZ_USE_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
#endif
imu_krooz.hmc.data_available = FALSE;
imu_krooz.mag_valid = TRUE;
}
}
struct DoubleEulers sigma_euler_from_sigma_q(struct DoubleQuat q, struct DoubleQuat sigma_q){
struct DoubleVect3 v_q, v_sigma, temporary_result;
struct DoubleEulers sigma_eu;
QUAT_IMAGINARY_PART(q, v_q);
QUAT_IMAGINARY_PART(sigma_q, v_sigma);
if(DOUBLE_VECT3_NORM(v_sigma)>0.5){
EULERS_ASSIGN(sigma_eu, M_PI_2, M_PI_2, M_PI_2);
return sigma_eu;
}
VECT3_CROSS_PRODUCT(temporary_result, v_q, v_sigma);
VECT3_SMUL(v_q, v_q, sigma_q.qi);
VECT3_SMUL(v_sigma, v_sigma, q.qi);
VECT3_ADD(temporary_result, v_sigma);
VECT3_SUB(temporary_result, v_q);
VECT3_TO_EULERS(temporary_result, sigma_eu);
return sigma_eu;
}
void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt)
{
// check if we had at least one propagation since last update
if (ahrs_fc.accel_cnt == 0) {
return;
}
/* last column of roation matrix = ltp z-axis in imu-frame */
struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 2),
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 2),
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2)
};
struct FloatVect3 imu_accel_float;
ACCELS_FLOAT_OF_BFP(imu_accel_float, *accel);
struct FloatVect3 residual;
struct FloatVect3 pseudo_gravity_measurement;
if (ahrs_fc.correct_gravity && ahrs_fc.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
const struct FloatVect3 vel_tangential_body = {ahrs_fc.ltp_vel_norm, 0.0, 0.0};
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
struct FloatVect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);
/* convert centrifugal acceleration from body to imu frame */
struct FloatVect3 acc_c_imu;
float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu);
} else {
VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
}
VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
/* FIR filtered pseudo_gravity_measurement */
#define FIR_FILTER_SIZE 8
static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1);
VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
if (ahrs_fc.gravity_heuristic_factor) {
/* heuristic on acceleration (gravity estimate) norm */
/* Factor how strongly to change the weight.
* e.g. for gravity_heuristic_factor 30:
* <0.66G = 0, 1G = 1.0, >1.33G = 0
*/
const float g_meas_norm = float_vect3_norm(&filtered_gravity_measurement) / 9.81;
ahrs_fc.weight = 1.0 - ahrs_fc.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
Bound(ahrs_fc.weight, 0.15, 1.0);
} else {
ahrs_fc.weight = 1.0;
}
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * ahrs_fc.accel_cnt
* with ahrs_fc.accel_cnt beeing the number of propagations since last update
*/
const float gravity_rate_update_gain = -2 * ahrs_fc.accel_zeta * ahrs_fc.accel_omega *
ahrs_fc.weight * ahrs_fc.accel_cnt / 9.81;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual, gravity_rate_update_gain);
// reset accel propagation counter
ahrs_fc.accel_cnt = 0;
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2 * dt
*/
const float gravity_bias_update_gain = ahrs_fc.accel_omega * ahrs_fc.accel_omega *
ahrs_fc.weight * ahrs_fc.weight * dt / 9.81;
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual, gravity_bias_update_gain);
/* FIXME: saturate bias */
}
static void test_enu_of_ecef_int(void) {
printf("\n--- enu_of_ecef int ---\n");
struct EcefCoor_f ref_coor_f = { 4624497.0 , 116475.0, 4376563.0};
struct LtpDef_f ltp_def_f;
ltp_def_from_ecef_f(<p_def_f, &ref_coor_f);
struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_f.x)),
rint(CM_OF_M(ref_coor_f.y)),
rint(CM_OF_M(ref_coor_f.z))};
printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z);
struct LtpDef_i ltp_def_i;
ltp_def_from_ecef_i(<p_def_i, &ref_coor_i);
printf("lla0 : (%d %d %d) (%f,%f,%f)\n", ltp_def_i.lla.lat, ltp_def_i.lla.lon, ltp_def_i.lla.alt,
DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)),
DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)),
M_OF_CM((double)ltp_def_i.lla.alt));
#define STEP 1000.
#define RANGE 100000.
double sum_err = 0;
struct FloatVect3 max_err;
FLOAT_VECT3_ZERO(max_err);
struct FloatVect3 offset;
for (offset.x=-RANGE; offset.x<=RANGE; offset.x+=STEP) {
for (offset.y=-RANGE; offset.y<=RANGE; offset.y+=STEP) {
for (offset.z=-RANGE; offset.z<=RANGE; offset.z+=STEP) {
struct EcefCoor_f my_ecef_point_f = ref_coor_f;
VECT3_ADD(my_ecef_point_f, offset);
struct EnuCoor_f my_enu_point_f;
enu_of_ecef_point_f(&my_enu_point_f, <p_def_f, &my_ecef_point_f);
#if DEBUG
printf("ecef to enu float : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n",
my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z,
my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z );
#endif
struct EcefCoor_i my_ecef_point_i = { rint(CM_OF_M(my_ecef_point_f.x)),
rint(CM_OF_M(my_ecef_point_f.y)),
rint(CM_OF_M(my_ecef_point_f.z))};;
struct EnuCoor_i my_enu_point_i;
enu_of_ecef_point_i(&my_enu_point_i, <p_def_i, &my_ecef_point_i);
#if DEBUG
// printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z);
printf("ecef to enu int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n",
M_OF_CM((double)my_ecef_point_i.x),
M_OF_CM((double)my_ecef_point_i.y),
M_OF_CM((double)my_ecef_point_i.z),
M_OF_CM((double)my_enu_point_i.x),
M_OF_CM((double)my_enu_point_i.y),
M_OF_CM((double)my_enu_point_i.z));
#endif
float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x);
if (fabs(ex) > max_err.x) max_err.x = fabs(ex);
float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y);
if (fabs(ey) > max_err.y) max_err.y = fabs(ey);
float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z);
if (fabs(ez) > max_err.z) max_err.z = fabs(ez);
sum_err += ex*ex + ey*ey + ez*ez;
}
}
}
double nb_samples = (2*RANGE / STEP + 1) * (2*RANGE / STEP + 1) * (2*RANGE / STEP + 1);
printf("enu_of_ecef int/float comparison:\n");
printf("error max (%f,%f,%f) m\n", max_err.x, max_err.y, max_err.z );
printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples );
printf("\n");
}
void CN_vector_escape_velocity(void)
{
/////VECTOR METHOD VARIABLES//////
//Constants
//Parameters for Butterworth filter
float A_butter = -0.8541;//-0.7265;
float B_butter[2] = {0.0730 , 0.0730 };//{0.1367, 0.1367};
int8_t disp_count = 0;
//Initalize
//float fp_angle;
//float total_vel;
float Ca;
float Cv;
float angle_ver = 0;
float angle_hor = 0;
Repulsionforce_Kan.x = 0;
Repulsionforce_Kan.y = 0;
Repulsionforce_Kan.z = 0;
//struct FloatRMat T;
//struct FloatEulers current_attitude = {0,0,0};
struct FloatVect3 Total_Kan = {0, 0, 0};
//Tuning variables
//float force_max = 200;
/////ESCAPE METHOD VARIABLES//////
//Constants
int8_t min_disparity = 45;
int8_t number_of_buffer = 20;
float bias = 2 * M_PI;
vref_max = 0.1; //CHECK!
//init
float available_heading[size_matrix[0] * size_matrix[2]];
float diff_available_heading[size_matrix[0] * size_matrix[2]];
float new_heading_diff = 1000;
float new_heading = 1000;
int8_t i = 0;
int8_t i_buffer = 0;
float Distance_est;
float V_total = 0;
///////////////////////////////////
heading_goal_ref = heading_goal_f - stateGetNedToBodyEulers_f()->psi;
//FLOAT_ANGLE_NORMALIZE(heading_goal_ref);
//Calculate Attractforce_goal
Attractforce_goal.x = cos(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
Attractforce_goal.y = sin(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
Attractforce_goal.z = 0;
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
/ 2; //Check if bodyframe is correct with current_heading correction
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
angle_ver = 0.5 * stereo_fow[1] + stereo_fow[1] / size_matrix[1] - stereo_fow[1] / size_matrix[1] / 2;
for (int i2 = 0; i2 < 4; i2++) {
angle_ver = angle_ver - stereo_fow[1] / size_matrix[1];
Ca = cos((angle_hor - heading_goal_ref) * Cfreq) * erf(1 * sqrt(VECT2_NORM2(pos_diff)));
if (Ca < 0) {
Ca = 0;
}
//TODO make dependent on speed: total_vel/vref_max
Cv = F1 + F2 * Ca;
if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
Distance_est = (baseline * (float)focal / ((float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
size_matrix[2] + i3]) - 18.0) / 1000;
disp_count++;
Repulsionforce_Kan.x = Repulsionforce_Kan.x - pow(Cv / (Distance_est + Dist_offset),
2) * cos(angle_hor) * cos(angle_ver);
Repulsionforce_Kan.y = Repulsionforce_Kan.y - pow(Cv / (Distance_est + Dist_offset),
2) * sin(angle_hor) * cos(angle_ver);
Repulsionforce_Kan.z = Repulsionforce_Kan.z - pow(Cv / (Distance_est + Dist_offset), 2) * sin(angle_ver);
}
}
}
}
//Normalize for ammount entries in Matrix
VECT3_SMUL(Repulsionforce_Kan, Repulsionforce_Kan, 1.0 / (float)disp_count);
if (repulsionforce_filter_flag == 1) {
Repulsionforce_Used.x = B_butter[0] * Repulsionforce_Kan.x + B_butter[1] * Repulsionforce_Kan_old.x - A_butter *
filter_repforce_old.x;
Repulsionforce_Used.y = B_butter[0] * Repulsionforce_Kan.y + B_butter[1] * Repulsionforce_Kan_old.y - A_butter *
filter_repforce_old.y;
Repulsionforce_Used.z = B_butter[0] * Repulsionforce_Kan.z + B_butter[1] * Repulsionforce_Kan_old.z - A_butter *
filter_repforce_old.z;
VECT3_COPY(Repulsionforce_Kan_old, Repulsionforce_Kan);
VECT3_COPY(filter_repforce_old, Repulsionforce_Used);
}
else {
VECT3_COPY(Repulsionforce_Used, Repulsionforce_Kan);
}
VECT3_SMUL(Repulsionforce_Used, Repulsionforce_Used, Ko);
VECT3_SMUL(Attractforce_goal, Attractforce_goal, Kg);
//Total force
VECT3_ADD(Total_Kan, Repulsionforce_Used);
VECT3_ADD(Total_Kan, Attractforce_goal);
//set variable for stabilization_opticflow
printf("RepulsionforceX: %f AttractX: %f \n", Repulsionforce_Used.x, Attractforce_goal.x);
printf("RepulsionforceY: %f AttractY: %f \n", Repulsionforce_Used.y, Attractforce_goal.y);
//set write values for logger
//Attractforce_goal_send.x = Attractforce_goal.x;
//Attractforce_goal_send.y = Attractforce_goal.y;
//Repulsionforce_Kan_send.x = Repulsionforce_Used.x;
//Repulsionforce_Kan_send.y = Repulsionforce_Used.y;
V_total = sqrt(pow(Total_Kan.x, 2) + pow(Total_Kan.y, 2));
if (V_total < v_min && sqrt(VECT2_NORM2(pos_diff)) > 1) {
escape_flag = 1;
}
else if (V_total > v_max && obstacle_flag == 0) {
escape_flag = 0;
}
printf("V_total: %f", V_total);
printf("escape_flag: %i, obstacle_flag: %i, set_bias: %i", escape_flag, obstacle_flag, set_bias);
printf("escape_flag: %i, obstacle_flag: %i, set_bias: %i", escape_flag, obstacle_flag, set_bias);
if (escape_flag == 0) {
ref_pitch = Total_Kan.x;
ref_roll = Total_Kan.y;
} else if (escape_flag == 1) {
heading_goal_ref = heading_goal_f - stateGetNedToBodyEulers_f()->psi;
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2];
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
available_heading[i] = angle_hor;
diff_available_heading[i] = heading_goal_ref - available_heading[i];
FLOAT_ANGLE_NORMALIZE(diff_available_heading[i]);
i++;
}
}
i = 0;
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
for (int i2 = 0; i2 < 4; i2++) {
if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
//distance_est = (baseline*(float)focal/((float)READimageBuffer_offset[i1*size_matrix[1]+i2*size_matrix[0]*size_matrix[2] + i3])-18.0)/1000;
diff_available_heading[i] = 100;
for (int i_diff = -number_of_buffer; i_diff <= number_of_buffer + 1; i_diff++) {
i_buffer = i + i_diff;
if (i_buffer < 1) {
i_buffer = i_buffer + size_matrix[0] * size_matrix[2];
}
if (i_buffer > size_matrix[0]*size_matrix[2]) {
i_buffer = i_buffer - size_matrix[0] * size_matrix[2];
}
diff_available_heading[i_buffer] = 100;
}
}
}
i++;
}
}
//set bias
if (set_bias == 1) {
for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
if (diff_available_heading[i100] <= 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
diff_available_heading[i100] = diff_available_heading[i100] - bias;
}
}
} else if (set_bias == 2) {
for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
if (diff_available_heading[i100] > 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
diff_available_heading[i100] = diff_available_heading[i100] + bias;
}
}
}
for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
if (fabs(diff_available_heading[i100]) < fabs(new_heading_diff)) {
new_heading_diff = diff_available_heading[i100];
new_heading = available_heading[i100];
}
}
if (obstacle_flag == 0 && fabs(new_heading_diff) > (2 * M_PI / 36.0)) {
obstacle_flag = 1;
if (new_heading_diff > 0) {
set_bias = 1;
direction = -1;
} else if (new_heading_diff <= 0) {
set_bias = 2;
direction = 1;
}
}
if (fabs(new_heading_diff) <= (2 * M_PI / 36.0)) {
if (waypoint_rot == 0) {
obstacle_flag = 0;
set_bias = 0;
} else {
waypoint_rot = waypoint_rot - direction * 0.05 * M_PI;
}
}
if (fabs(new_heading_diff) >= 0.5 * M_PI) {
waypoint_rot = waypoint_rot + direction * 0.25 * M_PI;
}
//vref_max should be low
speed_pot = vref_max * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
if (speed_pot <= 0) {
speed_pot = 0;
}
new_heading_old = new_heading;
#if PRINT_STUFF
for (int i100 = 0; i100 < (size_matrix[0] * size_matrix[2]); i100++) {
printf("%i diff_available_heading: %f available_heading: %f \n", i100, diff_available_heading[i100],
available_heading[i100]);
}
printf("new_heading: %f current_heading: %f speed_pot: %f\n", new_heading, stateGetNedToBodyEulers_f()->psi,
speed_pot);
printf("set_bias: %i obstacle_flag: %i", set_bias, obstacle_flag);
#endif
ref_pitch = cos(new_heading) * speed_pot;
ref_roll = sin(new_heading) * speed_pot;
}
}
void ecef_of_enu_point_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct EnuCoor_d* enu) {
MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef, *enu);
VECT3_ADD(*ecef, def->ecef);
}
void CN_vector_velocity(void)
{
//Constants
//Parameters for Butterworth filter
float A_butter = -0.8541;//-0.7265;
float B_butter[2] = {0.0730 , 0.0730 };//{0.1367, 0.1367};
//Initalize
int8_t disp_count = 0;
float escape_angle = 0;
float y_goal_frame;
//float total_vel;
float Distance_est;
float Ca;
float Cv;
float angle_ver = 0;
float angle_hor = 0;
//struct FloatVect3 Repulsionforce_Kan = {0,0,0};
Repulsionforce_Kan.x = 0;
Repulsionforce_Kan.y = 0;
Repulsionforce_Kan.z = 0;
//struct FloatVect3 Attractforce_goal = {0,0,0};
//Attractforce_goal.x = 0;
//Attractforce_goal.y = 0;
//Attractforce_goal.z = 0;
//struct FloatRMat T;
//struct FloatEulers current_attitude = {0,0,0};
struct FloatVect3 Total_Kan = {0, 0, 0};
//Tuning variables
//float force_max = 200;
int8_t min_disparity = 45;
//Flight plath angle calculation
// TODO make algorithm dependent on angle of obstacle.....
// total_vel = pow((OF_Result_Vy*OF_Result_Vy + OF_Result_Vx*OF_Result_Vx),0.5);
// if (total_vel>vmin){
// fp_angle = atan2(OF_Result_Vx,OF_Result_Vy);
// }
// else{
// fp_angle = 0;
// }
heading_goal_ref = heading_goal_f - stateGetNedToBodyEulers_f()->psi;
//FLOAT_ANGLE_NORMALIZE(heading_goal_ref);
//Calculate Attractforce_goal size = 1;
Attractforce_goal.x = cos(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
Attractforce_goal.y = sin(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
Attractforce_goal.z = 0;
//printf("current_heading, %f heading_goal_f: %f, heading_goal_ref: %f",stateGetNedToBodyEulers_f()->psi, heading_goal_f, heading_goal_ref);
//Transform to body frame
//current_attitude.psi = stateGetNedToBodyEulers_f()->psi;Attractforce_already in body frame, check when using data form Roland
//float_rmat_of_eulers_312(&T, ¤t_attitude);
//MAT33_VECT3_MUL(Attractforce_goal, T, Attractforce_goal);
//Attractforce_goal_send.x = Attractforce_goal.x;
//Attractforce_goal_send.y = Attractforce_goal.y;
//Attractforce_goal_send.z = Attractforce_goal.z;
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
/ 2; //Check if bodyframe is correct with current_heading correction
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
angle_ver = 0.5 * stereo_fow[1] + stereo_fow[1] / size_matrix[1] - stereo_fow[1] / size_matrix[1] / 2;
for (int i2 = 0; i2 < 4; i2++) {
angle_ver = angle_ver - stereo_fow[1] / size_matrix[1];
Ca = cos((angle_hor - heading_goal_ref) * Cfreq) * erf(1 * sqrt(VECT2_NORM2(pos_diff)));
if (Ca < 0) {
Ca = 0;
}
//TODO make dependent on speed: total_vel/vref_max
Cv = F1 + F2 * Ca;
if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
Distance_est = (baseline * (float)focal / ((float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
size_matrix[2] + i3]) - 18.0) / 1000;
disp_count++;
Repulsionforce_Kan.x = Repulsionforce_Kan.x - pow(Cv / (Distance_est + Dist_offset),
2) * cos(angle_hor) * cos(angle_ver);
Repulsionforce_Kan.y = Repulsionforce_Kan.y - pow(Cv / (Distance_est + Dist_offset),
2) * sin(angle_hor) * cos(angle_ver);
Repulsionforce_Kan.z = Repulsionforce_Kan.z - pow(Cv / (Distance_est + Dist_offset), 2) * sin(angle_ver);
printf("rep.x %f index %d %d %d disp: %d cv: %f angle_hor: %f angle_ver: %f \n", Repulsionforce_Kan.x, i1, i2, i3,
stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3], Cv, angle_hor, angle_ver);
}
// else {
// Distance_est = 2000;
// }
//
// if(pow(Cv/(READimageBuffer_offset[i1*size_matrix[1]+i2*size_matrix[0]*size_matrix[2] + i3]-0.2),2)<force_max){
// Repulsionforce_Kan.x = Repulsionforce_Kan.x - pow(Cv/(Distance_est+Dist_offset),2)*cos(angle_hor)*cos(angle_ver);
// Repulsionforce_Kan.y = Repulsionforce_Kan.y - pow(Cv/(Distance_est+Dist_offset),2)*sin(angle_hor)*cos(angle_ver);
// Repulsionforce_Kan.z = Repulsionforce_Kan.z - pow(Cv/(Distance_est+Dist_offset),2)*sin(angle_ver);
// }
// else{
// Repulsionforce_Kan.x = Repulsionforce_Kan.x - force_max*sin(angle_hor)*cos(angle_ver);
// Repulsionforce_Kan.y = Repulsionforce_Kan.y - force_max*cos(angle_hor)*cos(angle_ver);
// Repulsionforce_Kan.z = Repulsionforce_Kan.z - force_max*sin(angle_ver);
// }
}
}
}
//Normalize for ammount entries in Matrix
//Repulsionforce_Kan = Repulsionforce_Kan/(float)(size_matrix[1]*size_matrix[2]);
VECT3_SMUL(Repulsionforce_Kan, Repulsionforce_Kan, 1.0 / (float)disp_count);
printf("After multiplication: %f", Repulsionforce_Kan.x);
if (repulsionforce_filter_flag == 1) {
Repulsionforce_Used.x = B_butter[0] * Repulsionforce_Kan.x + B_butter[1] * Repulsionforce_Kan_old.x - A_butter *
filter_repforce_old.x;
Repulsionforce_Used.y = B_butter[0] * Repulsionforce_Kan.y + B_butter[1] * Repulsionforce_Kan_old.y - A_butter *
filter_repforce_old.y;
Repulsionforce_Used.z = B_butter[0] * Repulsionforce_Kan.z + B_butter[1] * Repulsionforce_Kan_old.z - A_butter *
filter_repforce_old.z;
VECT3_COPY(Repulsionforce_Kan_old, Repulsionforce_Kan);
VECT3_COPY(filter_repforce_old, Repulsionforce_Used);
}
else {
VECT3_COPY(Repulsionforce_Used, Repulsionforce_Kan);
}
VECT3_SMUL(Repulsionforce_Used, Repulsionforce_Used, Ko);
VECT3_SMUL(Attractforce_goal, Attractforce_goal, Kg);
//Total force
VECT3_ADD(Total_Kan, Repulsionforce_Used);
VECT3_ADD(Total_Kan, Attractforce_goal);
//set variable for stabilization_opticflow
//printf("RepulsionforceX: %f AttractX: %f \n",Repulsionforce_Used.x,Attractforce_goal.x);
//printf("RepulsionforceY: %f AttractY: %f \n",Repulsionforce_Used.y,Attractforce_goal.y);
//hysteris
if (sqrt(VECT2_NORM2(Total_Kan)) < V_hys_low && sqrt(VECT2_NORM2(pos_diff)) > 1) {
hysteris_flag = 1;
//x_goal_frame= cos() * Total_Kan.x + sin() * Total_Kan.y;
y_goal_frame = -sin(heading_goal_ref) * Total_Kan.x + cos(heading_goal_ref) * Total_Kan.y;
if (y_goal_frame >= 0) {
escape_angle = 0.5 * M_PI;
} else if (y_goal_frame < 0) {
escape_angle = -0.5 * M_PI;
}
}
if (sqrt(VECT2_NORM2(Total_Kan)) > V_hys_high || sqrt(VECT2_NORM2(pos_diff)) < 1) {
hysteris_flag = 0;
}
if (hysteris_flag == 1) {
Total_Kan.x = cos(heading_goal_ref + escape_angle) * V_hys_high;
Total_Kan.y = -sin(heading_goal_ref + escape_angle) * V_hys_high;
}
ref_pitch = Total_Kan.x;
ref_roll = Total_Kan.y;
printf("ref_pitch: %f ref_roll: %f disp_count: %d", ref_pitch, ref_roll, disp_count);
//set write values for logger
//Attractforce_goal_send.x = Attractforce_goal.x;
//Attractforce_goal_send.y = Attractforce_goal.y;
//Repulsionforce_Kan_send.x = Repulsionforce_Used.x;
//Repulsionforce_Kan_send.y = Repulsionforce_Used.y;
}
void ahrs_update_accel(void) {
/* last column of roation matrix = ltp z-axis in imu-frame */
struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)};
struct FloatVect3 imu_accel_float;
ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);
struct FloatVect3 residual;
struct FloatVect3 pseudo_gravity_measurement;
if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
struct FloatVect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body);
/* convert centrifugal acceleration from body to imu frame */
struct FloatVect3 acc_c_imu;
FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu);
} else {
VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
}
FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
/* FIR filtered pseudo_gravity_measurement */
#define FIR_FILTER_SIZE 8
static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1);
VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
if (ahrs_impl.gravity_heuristic_factor) {
/* heuristic on acceleration (gravity estimate) norm */
/* Factor how strongly to change the weight.
* e.g. for gravity_heuristic_factor 30:
* <0.66G = 0, 1G = 1.0, >1.33G = 0
*/
const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81;
ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
Bound(ahrs_impl.weight, 0.15, 1.0);
}
else {
ahrs_impl.weight = 1.0;
}
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
*/
const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega *
ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / (AHRS_CORRECT_FREQUENCY * 9.81);
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
*/
const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega *
ahrs_impl.weight * ahrs_impl.weight / (AHRS_CORRECT_FREQUENCY * 9.81);
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain);
/* FIXME: saturate bias */
}
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
}
static inline void on_gyro_accel_event( void ) {
#ifdef AHRS_CPU_LED
LED_ON(AHRS_CPU_LED);
#endif
// Run aligner on raw data as it also makes averages.
if (ahrs.status == AHRS_UNINIT) {
ImuScaleGyro(imu);
ImuScaleAccel(imu);
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
ahrs_align();
return;
}
#if PERIODIC_FREQUENCY == 60
ImuScaleGyro(imu);
ImuScaleAccel(imu);
ahrs_propagate();
ahrs_update_accel();
ahrs_update_fw_estimator();
#else //PERIODIC_FREQUENCY
static uint8_t _reduced_propagation_rate = 0;
static uint8_t _reduced_correction_rate = 0;
static struct Int32Vect3 acc_avg;
static struct Int32Rates gyr_avg;
RATES_ADD(gyr_avg, imu.gyro_unscaled);
VECT3_ADD(acc_avg, imu.accel_unscaled);
_reduced_propagation_rate++;
if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY))
{
}
else
{
_reduced_propagation_rate = 0;
RATES_SDIV(imu.gyro_unscaled, gyr_avg, (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY) );
INT_RATES_ZERO(gyr_avg);
ImuScaleGyro(imu);
ahrs_propagate();
_reduced_correction_rate++;
if (_reduced_correction_rate >= (AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY))
{
_reduced_correction_rate = 0;
VECT3_SDIV(imu.accel_unscaled, acc_avg, (PERIODIC_FREQUENCY / AHRS_CORRECT_FREQUENCY) );
INT_VECT3_ZERO(acc_avg);
ImuScaleAccel(imu);
ahrs_update_accel();
ahrs_update_fw_estimator();
}
}
#endif //PERIODIC_FREQUENCY
#ifdef AHRS_CPU_LED
LED_OFF(AHRS_CPU_LED);
#endif
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
new_ins_attitude = 1;
#endif
}
/** Convert a point in local ENU to ECEF.
* @param[out] ecef ECEF point in cm
* @param[in] def local coordinate system definition
* @param[in] enu ENU point in cm
*/
void ecef_of_enu_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
{
ecef_of_enu_vect_i(ecef, def, enu);
VECT3_ADD(*ecef, def->ecef);
}
