static void traj_sineX_quad_update(void)
{
const float om = RadOfDeg(10);
if (aos.time > (M_PI / om)) {
const float a = 20;
struct FloatVect3 jerk;
VECT3_ASSIGN(jerk , -a * om * om * om * cos(om * aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_accel , -a * om * om * sin(om * aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_vel , a * om * cos(om * aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_pos , a * sin(om * aos.time), 0, 0);
// this is based on differential flatness of the quad
EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.);
float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
const struct FloatEulers e_dot = {
0.,
9.81 * jerk.x / ((9.81 * 9.81) + (aos.ltp_accel.x * aos.ltp_accel.x)),
0.
};
FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
}
}
static void traj_coordinated_circle_update(void) {
const float speed = 15; // m/s
const float R = 100; // radius in m
float omega = speed / R;
// tan phi = v^2/Rg
float phi = atan2(speed*speed, R*9.81);
if ( aos.time > 2.*M_PI/omega) {
VECT3_ASSIGN(aos.ltp_pos, R*cos(omega*aos.time), R*sin(omega*aos.time), 0.);
VECT3_ASSIGN(aos.ltp_vel, -omega*R*sin(omega*aos.time), omega*R*cos(omega*aos.time), 0.);
VECT3_ASSIGN(aos.ltp_accel, -omega*omega*R*cos(omega*aos.time), -omega*omega*R*sin(omega*aos.time), 0.);
// float psi = atan2(aos.ltp_pos.y, aos.ltp_pos.x);
float psi = M_PI_2 + omega*aos.time; while (psi>M_PI) psi -= 2.*M_PI;
EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, psi);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
struct FloatEulers e_dot;
EULERS_ASSIGN(e_dot, 0., 0., omega);
FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
}
}
static void traj_stop_stop_x_update(void){
const float t0 = 5.;
const float dt_jerk = 0.75;
const float dt_nojerk = 10.;
const float val_jerk = 5.;
FLOAT_VECT3_INTEGRATE_FI(aos.ltp_pos, aos.ltp_vel, aos.dt);
FLOAT_VECT3_INTEGRATE_FI(aos.ltp_vel, aos.ltp_accel, aos.dt);
FLOAT_VECT3_INTEGRATE_FI(aos.ltp_accel, aos.ltp_jerk, aos.dt);
if (aos.time < t0) return;
else if (aos.time < t0+dt_jerk) {
VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); }
else if (aos.time < t0 + 2.*dt_jerk) {
VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); }
else if (aos.time < t0 + 2.*dt_jerk + dt_nojerk) {
VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); }
else if (aos.time < t0 + 3.*dt_jerk + dt_nojerk) {
VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); }
else if (aos.time < t0 + 4.*dt_jerk + dt_nojerk) {
VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); }
else {
VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); }
// this is based on differential flatness of the quad
EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
const struct FloatEulers e_dot = {
0.,
9.81*aos.ltp_jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)),
0. };
FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
}