float lookup_tan(TrigTable *t, float val)
{
float cos = lookup_cos(t, val);
if (cos == 0){
return FLT_MAX;
}
float sin = lookup_sin(t, val);
return sin/cos;
}
void complimentary_filter_predict_rad(int16_vect3 accel_raw, uint16_vect3 gyro_raw, int16_vect3 mag_raw, float_vect3* attitude, float_vect3* att_rates){
static float phi_acc_state = 0;
static float theta_acc_state = 0;
static float psi_mag_state = 0;
static float phi_gyro_state = 0;
static float theta_gyro_state = 0;
static float psi_gyro_state = 0;
static float phi_rate_state = 0;
static float theta_rate_state = 0;
static float psi_rate_state = 0;
float phi;
float theta;
float psi;
float phi_acc;
float theta_acc;
float psi_mag;
float phi_gyro;
float theta_gyro;
float psi_gyro;
static float psi_prev = 0;
// static float phi_rate_array[LENGTH_RATE_LOWPASS];
// static float theta_rate_array[LENGTH_RATE_LOWPASS];
// static float psi_rate_array[LENGTH_RATE_LOWPASS];
// static float phi_rate = 0;
// static float theta_rate = 0;
// static float psi_rate = 0;
// static uint32_t rate_idx = 0;
//unbias magnet sensor data
float_vect3 mag;
mag.x = (mag_raw.x-MAG_NEUTRALX)*MAG_SCALEX;
mag.y = (mag_raw.y-MAG_NEUTRALY)*MAG_SCALEY;
mag.z = mag_raw.z;
//unbias acccel
int32_vect3 accel;
accel.x = +accel_raw.x - ACCEL_NEUTRALX; //flipping coordinate system from z upward to downward
accel.y = -accel_raw.y + ACCEL_NEUTRALY;
accel.z = -accel_raw.z + ACCEL_NEUTRALZ;
//unbias gyro
int32_vect3 gyro;
gyro.x = gyro_raw.x - GYRO_NEUTRALX;
gyro.y = -gyro_raw.y + GYRO_NEUTRALY;
gyro.z = gyro_raw.z - GYRO_NEUTRALZ;
//scale gyro to rad/sec
float_vect3 att_rates_raw;
att_rates_raw.x = gyro.x * GYRO_SCALE_X;
att_rates_raw.y = gyro.y * GYRO_SCALE_Y;
att_rates_raw.z = gyro.z * GYRO_SCALE_Z;
//lowpass Accelerations (maybe later)
//lowpass magnet sensor data (maybe later)
//Nick Roll measurement build
float_vect3 attitude_tmp;
attitude_tmp.x = fast_atan2(accel.y,accel.z); //euler angle phi fast_atan2
attitude_tmp.y = -fast_atan2(accel.x,accel.z*(lookup_sin(attitude_tmp.x)+lookup_cos(attitude_tmp.x))); //euler angle theta
//lowpass nick roll
phi_acc = C_LOW*phi_acc_state + D_LOW*attitude_tmp.x;
phi_acc_state = A_LOW*phi_acc_state + B_LOW*attitude_tmp.x;
theta_acc = C_LOW*theta_acc_state + D_LOW*attitude_tmp.y;
theta_acc_state = A_LOW*theta_acc_state + B_LOW*attitude_tmp.y;
//integrate and highpass nick roll from gyro
phi_gyro = C_HIGH*phi_gyro_state + D_HIGH*att_rates_raw.x;
phi_gyro_state = A_HIGH*phi_gyro_state + B_HIGH*att_rates_raw.x;
theta_gyro = C_HIGH*theta_gyro_state + D_HIGH*att_rates_raw.y;
theta_gyro_state = A_HIGH*theta_gyro_state + B_HIGH*att_rates_raw.y;
//add up resulting high and lowpassed angles
phi = phi_acc + phi_gyro;
theta = theta_acc + theta_gyro;
//test from tilt compensation algorithm for 2 axis magnetic compass
mag.z=(sin(DIP_ANGLE)+mag.x*sin(theta)-mag.y*cos(theta)*sin(phi))/(cos(theta)*cos(phi));
//Transformation of maget vector to inertial frame and computation of angle in the x-y plane
attitude_tmp.z = fast_atan2(lookup_cos(theta)*mag.x + lookup_sin(phi)*lookup_sin(theta)*mag.y + lookup_cos(phi)*lookup_sin(theta)*mag.z , lookup_cos(phi)*mag.y - lookup_sin(phi)*mag.z);
// //asembly of angle with range [-PI,PI] to a continous function with range [-2*PI,2*PI]
if(psi_prev > PI/2 && attitude_tmp.z < psi_prev - PI) attitude_tmp.z += 2*PI;
else if(psi_prev < -PI/2 && attitude_tmp.z > psi_prev + PI) attitude_tmp.z -= 2*PI;
else if(attitude_tmp.z > PI/2 && psi_prev < attitude_tmp.z - PI) attitude_tmp.z -= 2*PI;
else if(attitude_tmp.z < -PI/2 && psi_prev > attitude_tmp.z +PI) attitude_tmp.z += 2*PI;
//Reset of angle to Zero if it reaches +-2*PI
if (attitude_tmp.z > 2*PI){
attitude_tmp.z -= 2*PI;
psi_mag_state=0;
}
else if(attitude_tmp.z < -2*PI){
attitude_tmp.z += 2*PI;
psi_mag_state=0;
}
psi_prev = attitude_tmp.z;
//lowpass psi_tmp
psi_mag = C_LOW*psi_mag_state + D_LOW*attitude_tmp.z;
psi_mag_state = A_LOW*psi_mag_state + B_LOW*attitude_tmp.z;
//integrate and highpass yaw from gyro
psi_gyro = C_HIGH*psi_gyro_state + D_HIGH*att_rates_raw.z;
psi_gyro_state = A_HIGH*psi_gyro_state + B_HIGH*att_rates_raw.z;
//add up resulting high and lowpassed angles
psi = psi_mag + psi_gyro;
// //Signal trasformation from range [-2*PI,2*PI] to [-PI,PI]
if(psi > PI) psi = psi -2*PI;
else if(psi < -PI) psi = psi +2*PI;
//output euler angles
(*attitude).x = phi;
(*attitude).y = theta;
(*attitude).z = psi;
//att_rates filtering for gyro rates output in SI units
//lowpass version (0.1sec delay)
(*att_rates).x= C_LOW*phi_rate_state + D_LOW*att_rates_raw.x;
phi_rate_state = A_LOW*phi_rate_state + B_LOW*att_rates_raw.x;
(*att_rates).y= C_LOW*theta_rate_state + D_LOW*att_rates_raw.y;
theta_rate_state = A_LOW*theta_rate_state + B_LOW*att_rates_raw.y;
(*att_rates).z= C_LOW*psi_rate_state + D_LOW*att_rates_raw.z;
psi_rate_state = A_LOW*psi_rate_state + B_LOW*att_rates_raw.z;
// (*att_rates).z= att_rates_raw.z;
// threshold values < |0.008|
// if (((*att_rates).z < 0.008) && (*att_rates).z > - 0.008){
// (*att_rates).z = 0;
// }
// //att_rates filtering for gyro rates output in SI units
// //moving average on gyros 0.05sec delay
// phi_rate -= phi_rate_array[rate_idx]/LENGTH_RATE_LOWPASS;
// theta_rate -= theta_rate_array[rate_idx]/LENGTH_RATE_LOWPASS;
// psi_rate -= psi_rate_array[rate_idx]/LENGTH_RATE_LOWPASS;
//
// phi_rate_array[rate_idx] = gyro.x;
// theta_rate_array[rate_idx] = gyro.y;
// psi_rate_array[rate_idx] = gyro.z;
//
// phi_rate += phi_rate_array[rate_idx]/LENGTH_RATE_LOWPASS;
// theta_rate += theta_rate_array[rate_idx]/LENGTH_RATE_LOWPASS;
// psi_rate += psi_rate_array[rate_idx]/LENGTH_RATE_LOWPASS;
// ++rate_idx;
// if(rate_idx>=LENGTH_RATE_LOWPASS) rate_idx=0;
//
// (*att_rates).x = phi_rate;
// (*att_rates).y = theta_rate;
// (*att_rates).z = psi_rate;
}
/**
* @brief Send one of the buffered messages
* @param pos data from vision
*/
void vision_buffer_handle_data(mavlink_vision_position_estimate_t* pos)
{
if (vision_buffer_index_write == vision_buffer_index_read)
{
//buffer empty
return;
}
vision_buffer_index_read = (vision_buffer_index_read + 1)
% VISION_BUFFER_COUNT;
//TODO: find data and process it
uint8_t for_count = 0;
uint8_t i = vision_buffer_index_read;
for (; (vision_buffer[i].time_captured < pos->usec)
&& (vision_buffer_index_write - i != 1); i = (i + 1)
% VISION_BUFFER_COUNT)
{
if (++for_count > VISION_BUFFER_COUNT)
{
debug_message_buffer("vision_buffer PREVENTED HANG");
break;
}
}
if (vision_buffer[i].time_captured == pos->usec)
{
//we found the right data
if (!isnumber(pos->x) || !isnumber(pos->y) || !isnumber(pos->z)
|| !isnumber(pos->roll) || !isnumber(pos->pitch) || !isnumber(pos->yaw)
|| pos->x == 0.0 || pos->y == 0.0 || pos->z == 0.0)
{
//reject invalid data
debug_message_buffer("vision_buffer invalid data (inf,nan,0) rejected");
}
else if (fabs(vision_buffer[i].ang.x - pos->roll)
< global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD]
&& fabs(vision_buffer[i].ang.y - pos->pitch)
< global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD])
{
// Update validity time
global_data.pos_last_valid = sys_time_clock_get_time_usec();
//Pack new vision_data package
global_data.vision_data.time_captured
= vision_buffer[i].time_captured;
global_data.vision_data.comp_end = sys_time_clock_get_unix_time();
//Set data from Vision directly
global_data.vision_data.ang.x = pos->roll;
global_data.vision_data.ang.y = pos->pitch;
global_data.vision_data.ang.z = pos->yaw;
global_data.vision_data.pos.x = pos->x;
global_data.vision_data.pos.y = pos->y;
global_data.vision_data.pos.z = pos->z;
// If yaw input from vision is enabled, feed vision
// directly into state estimator
global_data.vision_magnetometer_replacement.x = 200.0f*lookup_cos(pos->yaw);
global_data.vision_magnetometer_replacement.y = -200.0f*lookup_sin(pos->yaw);
global_data.vision_magnetometer_replacement.z = 0.f;
//If yaw goes to infinity (no idea why) set it to setpoint, next time will be better
if (global_data.attitude.z > 18.8495559 || global_data.attitude.z < -18.8495559)
{
global_data.attitude.z = global_data.yaw_pos_setpoint;
debug_message_buffer("vision_buffer CRITICAL FAULT yaw was bigger than 6 PI! prevented crash");
}
global_data.vision_data.new_data = 1;
//TODO correct also all buffer data needed if we are going to have overlapping vision data
}
else
{
//rejected outlayer
if (vision_buffer_reject_count++ % 16 == 0)
{
debug_message_buffer_sprintf("vision_buffer rejected outlier #%u",
vision_buffer_reject_count);
}
}
if (global_data.param[PARAM_SEND_SLOT_DEBUG_1] == 1)
{
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 202, global_data.attitude.z);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 210, pos->x);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 211, pos->y);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 212, pos->z);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 215, pos->yaw);
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 212, pos.z);
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 203, pos.r1);
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 204, pos.confidence);
}
if (for_count)
{
debug_message_buffer_sprintf(
"vision_buffer data found skipped %i data sets", for_count);
}
}
else
{
//we didn't find it
// debug_message_buffer("vision_buffer data NOT found");
if (for_count)
{
debug_message_buffer_sprintf(
"vision_buffer data NOT found skipped %i data sets",
for_count);
}
}
vision_buffer_index_read = i;//skip all images that are older;
// if (for_count)
// {
// debug_message_buffer_sprintf("vision_buffer skipped %i data sets",
// for_count);
// }
}
float lookup_cos(TrigTable *t, float val)
{
return lookup_sin(t, val + 90);
}
/**
* @brief Send one of the buffered messages
* @param pos data from vision
*/
void vision_buffer_handle_global_data(mavlink_global_vision_position_estimate_t* pos)
{
//#define PROJECT_GLOBAL_DATA_FORWARD
#ifdef PROJECT_GLOBAL_DATA_FORWARD
if (vision_buffer_index_write == vision_buffer_index_read)
{
//buffer empty
debug_message_buffer("ERROR VIS BUF: buffer empty");
return;
}
vision_buffer_index_read = (vision_buffer_index_read + 1) % VISION_BUFFER_COUNT;
//TODO: find data and process it
uint8_t for_count = 0;
uint8_t i = vision_buffer_index_read;
for (; (vision_buffer[i].time_captured < pos->usec)
&& (vision_buffer_index_write - i != 1); i = (i + 1)
% VISION_BUFFER_COUNT)
{
if (++for_count > VISION_BUFFER_COUNT) break;
}
if (vision_buffer[i].time_captured == pos->usec)
{
//we found the right data
if (!isnumber(pos->x) || !isnumber(pos->y) || !isnumber(pos->z)
|| !isnumber(pos->roll) || !isnumber(pos->pitch) || !isnumber(pos->yaw)
|| pos->x == 0.0 || pos->y == 0.0 || pos->z == 0.0)
{
//reject invalid data
debug_message_buffer("ERROR VIS BUF: invalid data (inf,nan,0) rejected");
}
else if (fabs(vision_buffer[i].ang.x - pos->roll) < global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD]
&& fabs(vision_buffer[i].ang.y - pos->pitch) < global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD])
{
// Update validity time
global_data.pos_last_valid = sys_time_clock_get_time_usec();
//Pack new vision_data package
global_data.vision_data_global.time_captured
= vision_buffer[i].time_captured;
global_data.vision_data_global.comp_end = sys_time_clock_get_unix_time();
// FIXME currently visodo is not allowed to run in parallel, else race condititions!
// Project position measurement
global_data.vision_data_global.pos.x = pos->x + (global_data.position.x - vision_buffer[i].pos.x);
global_data.vision_data_global.pos.y = pos->y + (global_data.position.y - vision_buffer[i].pos.y);
global_data.vision_data_global.pos.z = pos->z + (global_data.position.z - vision_buffer[i].pos.z);
// Set roll and pitch absolutely
global_data.vision_data_global.ang.x = pos->roll;
global_data.vision_data_global.ang.y = pos->pitch;
global_data.vision_data_global.ang.z = pos->yaw;
// Project yaw
//global_data.vision_data_global.ang.z = pos->yaw-vision_buffer[i].ang.z;
for (uint8_t j = (i+1) % VISION_BUFFER_COUNT; j != i; j = (j + 1) % VISION_BUFFER_COUNT)
{
vision_buffer[j].pos.x = vision_buffer[j].pos.x + (pos->x - vision_buffer[i].pos.x);
vision_buffer[j].pos.y = vision_buffer[j].pos.y + (pos->y - vision_buffer[i].pos.y);
vision_buffer[j].pos.z = vision_buffer[j].pos.z + (pos->z - vision_buffer[i].pos.z);
}
// If yaw input from vision is enabled, feed vision
// directly into state estimator
float lpYaw = pos->yaw*0.5f+global_data.attitude.z*0.5f;
global_data.vision_global_magnetometer_replacement.x = 200.0f*lookup_cos(lpYaw);
global_data.vision_global_magnetometer_replacement.y = -200.0f*lookup_sin(lpYaw);
global_data.vision_global_magnetometer_replacement.z = 0.f;
//If yaw goes to infinity (no idea why) set it to setpoint, next time will be better
if (global_data.attitude.z > 18.8495559 || global_data.attitude.z < -18.8495559)
{
global_data.attitude.z = global_data.yaw_pos_setpoint;
debug_message_buffer("vision_buffer CRITICAL FAULT yaw was bigger than 6 PI! prevented crash");
}
global_data.vision_data_global.new_data = 1;
global_data.state.global_vision_attitude_new_data = 1;
debug_message_buffer_sprintf("vision_buffer data found skipped %i data sets", for_count);
//TODO correct also all buffer data needed if we are going to have overlapping vision data
if (global_data.param[PARAM_SEND_SLOT_DEBUG_1] == 1)
{
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 202, global_data.attitude.z);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 220, pos->x);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 221, pos->y);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 222, pos->z);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 225, pos->yaw);
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 212, pos.z);
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 203, pos.r1);
//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 204, pos.confidence);
}
}
else
{
//rejected outlier
//if (vision_buffer_reject_count++ % 16 == 0)
{
debug_message_buffer_sprintf("vision_buffer rejected outlier #%u",
vision_buffer_reject_count);
}
}
}
else
{
//we didn't find it
debug_message_buffer_sprintf("vision_buffer data NOT found skipped %i data sets", for_count);
}
vision_buffer_index_read = i;//skip all images that are older;
#else
global_data.vision_data_global.pos.x = pos->x;
global_data.vision_data_global.pos.y = pos->y;
global_data.vision_data_global.pos.z = pos->z;
//Set data from Vision directly
global_data.vision_data_global.ang.x = pos->roll;
global_data.vision_data_global.ang.y = pos->pitch;
global_data.vision_data_global.ang.z = pos->yaw;
global_data.vision_data_global.new_data = 1;
global_data.state.global_vision_attitude_new_data = 1;
#endif
mavlink_msg_global_vision_position_estimate_send(MAVLINK_COMM_0, sys_time_clock_get_unix_loop_start_time(), global_data.vision_data_global.pos.x, global_data.vision_data_global.pos.y, global_data.vision_data_global.pos.z, global_data.vision_data_global.ang.x, global_data.vision_data_global.ang.y, global_data.vision_data_global.ang.z);
}
