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 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());
}
/** 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);
}
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 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;});
/// Utility function: converts lla (int) to local point (float)
bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
{
// return FALSE if there is no valid local coordinate system
if (!state.ned_initialized_i) {
return FALSE;
}
// change geoid alt to ellipsoid alt
lla->alt = lla->alt - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt;
//Compute ENU components from LLA with respect to ltp origin
struct EnuCoor_i tmp_enu_point_i;
enu_of_lla_point_i(&tmp_enu_point_i, &state.ned_origin_i, lla);
struct EnuCoor_f tmp_enu_point_f;
// result of enu_of_lla_point_i is in cm, convert to float in m
VECT3_SMUL(tmp_enu_point_f, tmp_enu_point_i, 0.01);
//Bound the new waypoint with max distance from home
struct FloatVect2 home;
home.x = waypoint_get_x(WP_HOME);
home.y = waypoint_get_y(WP_HOME);
struct FloatVect2 vect_from_home;
VECT2_DIFF(vect_from_home, tmp_enu_point_f, home);
//Saturate the mission wp not to overflow max_dist_from_home
//including a buffer zone before limits
float dist_to_home = float_vect2_norm(&vect_from_home);
dist_to_home += BUFFER_ZONE_DIST;
if (dist_to_home > MAX_DIST_FROM_HOME) {
VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
}
// set new point
VECT2_SUM(*point, home, vect_from_home);
point->z = tmp_enu_point_f.z;
return TRUE;
}
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 */
}
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 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 gx3_packet_read_message(void) {
ahrs_impl.gx3_accel.x = bef(&ahrs_impl.gx3_packet.msg_buf[1]);
ahrs_impl.gx3_accel.y = bef(&ahrs_impl.gx3_packet.msg_buf[5]);
ahrs_impl.gx3_accel.z = bef(&ahrs_impl.gx3_packet.msg_buf[9]);
ahrs_impl.gx3_rate.p = bef(&ahrs_impl.gx3_packet.msg_buf[13]);
ahrs_impl.gx3_rate.q = bef(&ahrs_impl.gx3_packet.msg_buf[17]);
ahrs_impl.gx3_rate.r = bef(&ahrs_impl.gx3_packet.msg_buf[21]);
ahrs_impl.gx3_rmat.m[0] = bef(&ahrs_impl.gx3_packet.msg_buf[25]);
ahrs_impl.gx3_rmat.m[1] = bef(&ahrs_impl.gx3_packet.msg_buf[29]);
ahrs_impl.gx3_rmat.m[2] = bef(&ahrs_impl.gx3_packet.msg_buf[33]);
ahrs_impl.gx3_rmat.m[3] = bef(&ahrs_impl.gx3_packet.msg_buf[37]);
ahrs_impl.gx3_rmat.m[4] = bef(&ahrs_impl.gx3_packet.msg_buf[41]);
ahrs_impl.gx3_rmat.m[5] = bef(&ahrs_impl.gx3_packet.msg_buf[45]);
ahrs_impl.gx3_rmat.m[6] = bef(&ahrs_impl.gx3_packet.msg_buf[49]);
ahrs_impl.gx3_rmat.m[7] = bef(&ahrs_impl.gx3_packet.msg_buf[53]);
ahrs_impl.gx3_rmat.m[8] = bef(&ahrs_impl.gx3_packet.msg_buf[57]);
ahrs_impl.gx3_time = (uint32_t)(ahrs_impl.gx3_packet.msg_buf[61] << 24 |
ahrs_impl.gx3_packet.msg_buf[62] << 16 | ahrs_impl.gx3_packet.msg_buf[63] << 8 | ahrs_impl.gx3_packet.msg_buf[64]);
ahrs_impl.gx3_chksm = GX3_CHKSM(ahrs_impl.gx3_packet.msg_buf);
ahrs_impl.gx3_freq = 62500.0 / (float)(ahrs_impl.gx3_time - ahrs_impl.gx3_ltime);
ahrs_impl.gx3_ltime = ahrs_impl.gx3_time;
// Acceleration
VECT3_SMUL(ahrs_impl.gx3_accel, ahrs_impl.gx3_accel, 9.80665); // Convert g into m/s2
ACCELS_BFP_OF_REAL(imu.accel, ahrs_impl.gx3_accel); // for backwards compatibility with fixed point interface
imuf.accel = ahrs_impl.gx3_accel;
// Rates
struct FloatRates body_rate;
imuf.gyro = ahrs_impl.gx3_rate;
/* compute body rates */
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imuf.body_to_imu);
FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, imuf.gyro);
/* Set state */
stateSetBodyRates_f(&body_rate);
// Attitude
struct FloatRMat ltp_to_body_rmat;
FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_impl.gx3_rmat, *body_to_imu_rmat);
#if AHRS_USE_GPS_HEADING && USE_GPS
struct FloatEulers ltp_to_body_eulers;
float_eulers_of_rmat(<p_to_body_eulers, <p_to_body_rmat);
float course_f = (float)DegOfRad(gps.course / 1e7);
if (course_f > 180.0) {
course_f -= 360.0;
}
ltp_to_body_eulers.psi = (float)RadOfDeg(course_f);
stateSetNedToBodyEulers_f(<p_to_body_eulers);
#else // !AHRS_USE_GPS_HEADING
#ifdef IMU_MAG_OFFSET
struct FloatEulers ltp_to_body_eulers;
float_eulers_of_rmat(<p_to_body_eulers, <p_to_body_rmat);
ltp_to_body_eulers.psi -= ahrs_impl.mag_offset;
stateSetNedToBodyEulers_f(<p_to_body_eulers);
#else
stateSetNedToBodyRMat_f(<p_to_body_rmat);
#endif // IMU_MAG_OFFSET
#endif // !AHRS_USE_GPS_HEADING
}
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
}
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) {
/* cmd_x is positive to north = negative pitch
* cmd_y is positive to east = positive roll
*
* orientation vector describing simultaneous rotation of roll/pitch
*/
const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0};
/* quaternion from that orientation vector */
struct FloatQuat q_rp;
float_quat_of_orientation_vect(&q_rp, &ov);
/* as rotation matrix */
struct FloatRMat R_rp;
float_rmat_of_quat(&R_rp, &q_rp);
/* body x-axis (before heading command) is first column */
struct FloatVect3 b_x;
VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]);
/* body z-axis (thrust vect) is last column */
struct FloatVect3 thrust_vect;
VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]);
/// @todo optimize yaw angle calculation
/*
* Instead of using the psi setpoint angle to rotate around the body z-axis,
* calculate the real angle needed to align the projection of the body x-axis
* onto the horizontal plane with the psi setpoint.
*
* angle between two vectors a and b:
* angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n))
* where the normal n is the thrust vector (i.e. both a and b lie in that plane)
*/
// desired heading vect in earth x-y plane
const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0};
/* projection of desired heading onto body x-y plane
* b = v - dot(v,n)*n
*/
float dot = VECT3_DOT_PRODUCT(psi_vect, thrust_vect);
struct FloatVect3 dotn;
VECT3_SMUL(dotn, thrust_vect, dot);
// b = v - dot(v,n)*n
struct FloatVect3 b;
VECT3_DIFF(b, psi_vect, dotn);
dot = VECT3_DOT_PRODUCT(b_x, b);
struct FloatVect3 cross;
VECT3_CROSS_PRODUCT(cross, b_x, b);
// norm of the cross product
float nc = FLOAT_VECT3_NORM(cross);
// angle = atan2(norm(cross(a,b)), dot(a,b))
float yaw2 = atan2(nc, dot) / 2.0;
// negative angle if needed
// sign(dot(cross(a,b), n)
float dot_cross_ab = VECT3_DOT_PRODUCT(cross, thrust_vect);
if (dot_cross_ab < 0) {
yaw2 = -yaw2;
}
/* quaternion with yaw command */
struct FloatQuat q_yaw;
QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
/* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
float_quat_comp(quat, &q_rp, &q_yaw);
float_quat_normalize(quat);
float_quat_wrap_shortest(quat);
}
void UM6_packet_read_message(void)
{
if ((UM6_status == UM6Running) && PacketLength > 11) {
switch (PacketAddr) {
case IMU_UM6_GYRO_PROC:
UM6_rate.p =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0610352;
UM6_rate.q =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0610352;
UM6_rate.r =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0610352;
RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec
RATES_BFP_OF_REAL(imu.gyro, UM6_rate);
break;
case IMU_UM6_ACCEL_PROC:
UM6_accel.x =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000183105;
UM6_accel.y =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000183105;
UM6_accel.z =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000183105;
VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2
ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int
break;
case IMU_UM6_MAG_PROC:
UM6_mag.x =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000305176;
UM6_mag.y =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000305176;
UM6_mag.z =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000305176;
// Assume the same units for magnetic field
// Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss).
MAGS_BFP_OF_REAL(imu.mag, UM6_mag);
break;
case IMU_UM6_EULER:
UM6_eulers.phi =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0109863;
UM6_eulers.theta =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0109863;
UM6_eulers.psi =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0109863;
EULERS_SMUL(UM6_eulers, UM6_eulers, 0.0174532925); //Convert deg to rad
EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers);
break;
case IMU_UM6_QUAT:
UM6_quat.qi =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0000335693;
UM6_quat.qx =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0000335693;
UM6_quat.qy =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0000335693;
UM6_quat.qz =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 6] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 7]))) * 0.0000335693;
QUAT_BFP_OF_REAL(ahrs_impl.ltp_to_imu_quat, UM6_quat);
break;
default:
break;
}
}
}