void back_to_destination(const vec3d *v) {
double power = 0.0;
double steering = 0.0;
char msg[100];
vec3d dir;
double dis;
double targetVel;
v3sub(v,&loc,&dir);
dis = v3length(&dir);
if (dis!=0.0) v3normalize(&dir);
targetVel = dis > 15 ? 15 : dis;
if (v3dot(&dir,&front)<0.0) {
steering = 0.3 * v3dot(&dir,&left);
power = -300*(targetVel - v3length(&vel));
power = power > 300 ? 300 : power;
power = power < -300 ? -300 : power;
} else {
steering = 1.0;
power = 150;
}
sprintf(msg,"drive %.16f %.16f",power,steering);
my_send(msg);
}
void IMU_GetAircraftSpaceState( vec3 *imu_north, vec3 *imu_down, vec3 *rates )
// Fill in this function and return sensor values for the IMU algorithm to use:
// imu_north - Normalized magnetometer vector
// imu_down - Normalized accelerometer, with optional centrepetal force correction applied.
// rates - rads/sec gyro rates
//
//+Gy = + pitch (Nose Up)
//+Gx = + roll (Right wing down)
//+Gz = + yaw (Nose right)
//
//
//AX=1.0 = -90 pitch
//AX=-1.0 = 90 pitch
//Ay=1.0 =+ 90 Roll
//Ay -1.0 = -90 Roll
//
//Az -1.0 = Inverted
//Az 1.0 = level
//
//
//Mx 0.5, My=0; Mz=-0.86 -> Heading ~ 0
//Mx 0, My =0.5, Mz=-0.86 ->Heading ~ -90
//Mx -0.5, My=0; Mz=-0.86 -> Heading ~ 180
//Mx 0, My =-0.5, Mz=-0.86 ->Heading ~ +90
//
{
// TODO: You need to read your ADC, apply calibration tables, and return these values.
vec3 tmp;
v3set(rates,lGx,lGy,lGz);
v3set(&tmp,lAx,lAy,lAz);
v3normalize(imu_down,&tmp);
if (
(lMy!=0) ||
(lMx!=0) ||
(lMz!=0)
)
{v3set(&tmp,lMx,lMy,lMz);
v3normalize(imu_north,&tmp);
bDoMag=true;
}
else
{
bDoMag=false;
}
}
void IMU_Update( float dt ,float slew_scale)
// Main IMU sensor fusion function. Call frequently.
// R_estimate - Estimated vehicle orientation (in/out)
// dt - timestep, in seconds.
{
quat erector;
vec3 imu_north, imu_down, rates;
// Read our sensors and get engineering units...
IMU_GetAircraftSpaceState( &imu_north, &imu_down, &rates);
// Euler Integrate our angluar rate gyros into R
IMU_IntegrateGyros( dt, &rates, &g_gyroFrame);
// Get the down vector in world space.
//IE rotate from measured IMU down to what it would be in world space
//If the gryo frame was without error.
vec3 v_measured_down_world;
Quat_RotateVec3( &g_gyroFrame, &v_measured_down_world, &imu_down);
// Get erector quaternion from measured down and "reference" down.
//G World down shoudl probably be adjusted with GPS measured acceleratiosn to give
//a proper value accouting for accelearations etc...
if( IMU_GetErectorQuat( &erector, &g_gyroFrame, &v_measured_down_world, &g_world_down , ERECT_RPS*slew_scale, dt) )
{
// If a correction is needed, apply the erector rotation to our estimate.
Quat_Multiply( &g_gyroFrame, &erector, &g_gyroFrame );
}
if(bDoMag)
{
// Get the north vector in world space, flattened on the X-Y plane
vec3 v_measured_north_world;
Quat_RotateVec3( &g_gyroFrame, &v_measured_north_world, &imu_north);
v_measured_north_world.z = 0.0f;
v3normalize(&v_measured_north_world,&v_measured_north_world);
// Get erector quaternion from measured north and "reference" north.
if( IMU_GetErectorQuat( &erector, &g_gyroFrame, &v_measured_north_world, &g_world_north , NORTH_RPS*slew_scale, dt) )
{
// If a correction is needed, apply the erector rotation to our estimate.
Quat_Multiply( &g_gyroFrame, &erector, &g_gyroFrame );
}
}
}
void IMU_IntegrateGyros(float dt, vec3 *rates, quat *pGyroFrame)
// Integrate gyros into our gyro frame.
{
// Get rotation axis from gyros, and rate in rads/sec
if ((rates->x==0.0f) && (rates->y==0.0f) && (rates->z==0.0f)) return;
vec3 axis;
float rate = v3normalize(&axis,rates);
NANTest("rate",rate);
// Get a quat for the displacement this timestep did.
quat q_dot;
Quat_SetAxisAndAngle( &q_dot, &axis, rate * dt);
NANTest("z",q_dot.z);
NANTest("x",q_dot.x);
NANTest("y",q_dot.y);
NANTest("z",q_dot.z);
Quat_Multiply(pGyroFrame, pGyroFrame, &q_dot);
Quat_Normalize( pGyroFrame);
}
bool IMU_GetErectorQuat( quat *q_erect_out, quat *frame, vec3 *v_measured, vec3 *v_reference, float rads_sec, float dt)
// Calculates the quaternion needed to rotate the measured vector to the reference vector (world space) at a fixed correction rate
// Returns TRUE if correction is necessary, or FALSE if it is below the threshold of caring.
{
// Get the rotation axis we'll use to erect.
// (Normalize returns the length. We do a lower limit. No sense in correcting if we're close)
vec3 c;
v3cross(&c,v_measured, v_reference);
if( v3normalize(&c,&c) > 1.0f * RADIANS_PER_DEGREE)
{
// Get the angle between the two vectors, and clamp to that limit. We don't want to overshoot.
// Angles are always positive since the rotation angle flips appropriately.
float rads = rads_sec * g_rps_scale * dt;
float a = fabs(clamped_acos(v3dot(v_measured, v_reference)));
if((rads>a) && (rads>0))
{
rads=a;
}
else
if((rads<-a) && (rads<0))
{
rads=-a;
}
//ULIMIT(rads,a);
// Get the quat that rotates our sensor toward the world vector by the specified amount
Quat_SetAxisAndAngle( q_erect_out, &c, rads);
return true;
}
return false;
}
