void BillboardSprite::draw(Vec3 v)
{
v = quadrant_translate_position(current_camera_position, v);
if (!point_fulstrum_test(v.x, v.y, v.z))
return;
Vec3 up = vec3_init(model_view_matrix[0]*this->scale,
model_view_matrix[4]*this->scale,
model_view_matrix[8]*this->scale);
Vec3 right = vec3_init(model_view_matrix[1]*this->scale,
model_view_matrix[5]*this->scale,
model_view_matrix[9]*this->scale);
float tx_min, tx_max, ty_min, ty_max;
tx_min = (float)(this->texture_index%16)* (1.0f/16.0f);
tx_max = tx_min + (1.0f/16.0f);
ty_min = (float)(this->texture_index/16)* (1.0f/16.0f);
ty_max = ty_min + (1.0f/16.0f);
Vec3 p = vec3_sub(v, vec3_add(right, up));
glTexCoord2f(tx_min,ty_max);
glVertex3f(p.x, p.y, p.z);
p = vec3_add(v, vec3_sub(up, right));
glTexCoord2f(tx_max,ty_max);
glVertex3f(p.x, p.y, p.z);
p = vec3_add(v, vec3_add(up, right));
glTexCoord2f(tx_max,ty_min);
glVertex3f(p.x, p.y, p.z);
p = vec3_add(v, vec3_sub(right, up));
glTexCoord2f(tx_min,ty_min);
glVertex3f(p.x, p.y, p.z);
}
bool hitscan_sprite_mobs(const Vec3& position, const Vec3& direction, float range,
SpriteMobID& id, float& distance, Vec3& collision_point)
{
SpriteMobID nearest_mob = NULL_SPRITE_MOB;
float nearest_distance = 100000.0f;
Vec3 nearest_collision_point = vec3_init(0);
const float range_sq = range * range;
const Vec3 up = vec3_init(0, 0, 1);
for (size_t i=0, j=0; i<sprite_mob_list->max && j<sprite_mob_list->count; i++)
{ // TODO
// do a line-plane intersection test against the mob, if its in frustum
SpriteMob* m = sprite_mob_list->objects[i];
if (m == NULL) continue;
j++;
Vec3 p = m->get_center();
p = quadrant_translate_position(position, p);
if (vec3_distance_squared(position, p) > range_sq)
continue;
float rad_sq = 10000.0f;
Vec3 line_point = vec3_init(0);
if (!sphere_line_distance(position, direction, p, line_point, rad_sq))
continue;
Vec3 forward = vec3_sub(position, p);
forward.z = 0.0f;
if (unlikely(vec3_length_squared(forward) == 0))
continue;
forward = vec3_normalize(forward);
const Vec3 right = vec3_normalize(vec3_cross(forward, up));
float d = 1000000.0f;
float width = get_mob_width(m->type) * 0.5f;
float height = get_mob_height(m->type) * 0.5f;
if (!line_plane_intersection(position, direction, p, width, height,
forward, right, up, d))
continue;
if (d >= nearest_distance)
continue;
nearest_distance = d;
nearest_mob = m->id;
nearest_collision_point = line_point;
}
id = nearest_mob;
distance = nearest_distance;
collision_point = translate_position(nearest_collision_point);
return (nearest_mob != NULL_SPRITE_MOB);
}
WorldHitscanResult() :
type(HITSCAN_TARGET_NONE), distance(FAR_AWAY), collision_point(vec3_init(FAR_AWAY)),
start_position(vec3_init(0)), direction(vec3_init(0, 0, 1)), range(128),
block_hit(false), block_position(vec3i_init(0)),
block_open_position(vec3i_init(0)), block_side(vec3i_init(0, 0, 1)),
cube_type(ERROR_CUBE), block_distance(FAR_AWAY), voxel_hit(false),
voxel_distance(FAR_AWAY), voxel_collision_point(vec3_init(0)),
mech_hit(false), mech_id(NULL_MECH_ID), mech_distance(FAR_AWAY),
sprite_mob_hit(false), sprite_mob_id(NULL_SPRITE_MOB),
sprite_mob_entity_id(NULL_ENTITY), sprite_mob_entity_type(NULL_ENTITY_TYPE),
sprite_mob_distance(FAR_AWAY), sprite_mob_collision_point(vec3_init(0))
{}
void start(const Vec3& position, const Vec3& direction, float range)
{
this->start_position = position;
this->direction = direction;
IF_ASSERT(vec3_length_squared(direction) == 0)
this->direction = vec3_init(0, 0, 1);
this->range = range;
}
/* find the two endpoints of a line segment that are inside a given sphere
http://stackoverflow.com/a/17499940 */
int sphere_line_segment_intersection(const union vec3 *v0, const union vec3 *v1, const union vec3 *center, double r, union vec3 *vo0, union vec3 *vo1)
{
double cx = center->v.x;
double cy = center->v.y;
double cz = center->v.z;
double px = v0->v.x;
double py = v0->v.y;
double pz = v0->v.z;
double vx = v1->v.x - px;
double vy = v1->v.y - py;
double vz = v1->v.z - pz;
double A = vx * vx + vy * vy + vz * vz;
double B = 2.0 * (px * vx + py * vy + pz * vz - vx * cx - vy * cy - vz * cz);
double C = px * px - 2 * px * cx + cx * cx + py * py - 2 * py * cy + cy * cy +
pz * pz - 2 * pz * cz + cz * cz - r * r;
double D = B * B - 4.0 * A * C;
/* outside or tanget to sphere, no segment intersection */
if (D <= 0)
return -1;
double t1 = (-B - sqrt(D)) / (2.0 * A);
double t2 = (-B + sqrt(D)) / (2.0 * A);
/* infinte line intersects but this segment doesn't */
if ((t1 < 0 && t2 < 0) || (t1 > 1 && t2 > 1))
return -1;
if (t1 < 0)
vec3_copy(vo0, v0);
else if (t1 > 1)
vec3_copy(vo1, v1);
else
vec3_init(vo0, v0->v.x * (1.0 - t1) + t1 * v1->v.x, v0->v.y * (1.0 - t1) + t1 * v1->v.y, v0->v.z * (1.0 - t1) + t1 * v1->v.z);
if (t2 < 0)
vec3_copy(vo0, v0);
else if (t2 > 1)
vec3_copy(vo1, v1);
else
vec3_init(vo1, v0->v.x * (1.0 - t2) + t2 * v1->v.x, v0->v.y * (1.0 - t2) + t2 * v1->v.y, v0->v.z * (1.0 - t2) + t2 * v1->v.z);
return 2;
}
void vec3_attach(PyObject * m)
{
vec3_init();
if (PyType_Ready(&vec3_type) < 0)
return; // TODO error?
Py_INCREF(&vec3_type);
PyModule_AddObject(m, "vec3", (PyObject *)&vec3_type);
}
Vertex * vertex_init_float(GLfloat x, GLfloat y,
GLfloat z)
{
Vertex *vertex = NULL;
vertex = malloc(sizeof(Vertex));
if (vertex)
{
vertex->size = 3;
vertex->pos = vec3_init(x, y, z);
}
return(vertex);
}
/* This function implements the mandel box fractal.
* It's quite expensive and not used anywhere yet.
*/
void vol_draw_mandel_box (double xc, double yc, double zc, double xsc, double ysc, double zsc, double s, double r, double f, int it, double cfact)
{
int x, y, z;
for (z = 0; z < DRAW_CTX.size; z++)
for (y = 0; y < DRAW_CTX.size; y++)
for (x = 0; x < DRAW_CTX.size; x++)
{
vec3_init (c, x, y, z);
vec3_s_div (c, DRAW_CTX.size);
c[0] += xsc;
c[1] += ysc;
c[2] += zsc;
vec3_s_mul (c, cfact);
c[0] += -xsc * cfact;
c[1] += -ysc * cfact;
c[2] += -zsc * cfact;
c[0] += xc;
c[1] += yc;
c[2] += zc;
int i;
int escape = 0;
vec3_init (v, 0, 0, 0);
for (i = 0; i < it; i++)
{
double d = _vol_draw_mandel_box_equation (v, s, r, f, c);
if (d > 1024)
{
escape = 1;
break;
}
}
if (!escape)
vol_draw_op (x, y, z, 0.5);
}
}
void vol_draw_fill_sphere (float x, float y, float z, float size)
{
float cntr = size / 2;
vec3_init (center, x + cntr, y + cntr, z + cntr);
float j, k, l;
for (j = 0; j < size; j++)
for (k = 0; k < size; k++)
for (l = 0; l < size; l++)
{
vec3_init (cur, x + j, y + k, z + l);
vec3_sub (cur, center);
float vlen = vec3_len (cur);
float diff = vlen - (cntr - (size / 10));
if (diff < 0)
{
double sphere_val = (-diff / cntr);
vol_draw_op (x + j, y + k, z + l, sphere_val);
}
}
}
void RailTrailEffect::draw_quad(const Vec3& p, float r, float theta, float phi) // quadratic radius
{ // with no rotation modifications, it faces upwards
static const float tx_min = 0.0f;
static const float tx_max = 1.0f;
static const float ty_min = 0.0f;
static const float ty_max = 1.0f;
Vec3 bl, tl, tr, br;
bl = tl = tr = br = vec3_init(0.0f);
// FIXME: MOVE SHIT LIKE THIS TO BE CALC'ED ONLY ONCE then applied to all the interpolated points
// rotate the y & z (pitch) .... prob need to reverse/mirror the y
float radian = phi*PI;
// first setup up-facing quad with correct PITCH
// bottom edge
bl.y = br.y = -r*sinf(radian);
bl.z = br.z = r*cosf(radian);
//printf("bl.y: %8.2f bl.z: %8.2f \n", bl.y, bl.z);
// top edge
tl.y = tr.y = r*sinf(radian);
tl.z = tr.z = -r*cosf(radian);
//printf("tl.y: %8.2f tl.z: %8.2f \n", tl.y, tl.z);
// sides
bl.x = tl.x = -r;
br.x = tr.x = r;
// do YAW
bl = vec3_euler_rotation(bl, theta-0.5f, 0, 0);
tl = vec3_euler_rotation(tl, theta-0.5f, 0, 0);
tr = vec3_euler_rotation(tr, theta-0.5f, 0, 0);
br = vec3_euler_rotation(br, theta-0.5f, 0, 0);
// translate to world space
bl = vec3_add(p, bl);
tl = vec3_add(p, tl);
tr = vec3_add(p, tr);
br = vec3_add(p, br);
// draw
glTexCoord2f(tx_max, ty_max);
glVertex3f(bl.x, bl.y, bl.z); // Bottom left
glTexCoord2f(tx_min, ty_max);
glVertex3f(tl.x, tl.y, tl.z); // Top left
glTexCoord2f(tx_min, ty_min);
glVertex3f(tr.x, tr.y, tr.z); // Top right
glTexCoord2f(tx_max, ty_min);
glVertex3f(br.x, br.y, br.z); // Bottom right
}
Transform * transform_init()
{
Transform *transform = NULL;
transform = malloc(sizeof(Transform));
if (transform)
{
// Methods
transform->get_transformation = get_transformation;
transform->set_scaling = set_scaling;
transform->set_scaling_vec3 = set_scaling_vec3;
transform->set_rotation = set_rotation;
transform->set_rotation_vec3 = set_rotation_vec3;
transform->set_translation = set_translation;
transform->set_translation_vec3 = set_translation_vec3;
// Attributes
transform->translation = NULL;
transform->translation = vec3_init(0.0f,
0.0f,
0.0f);
transform->rotation = NULL;
transform->rotation = vec3_init(0.0f,
0.0f,
0.0f);
transform->scaling = NULL;
transform->scaling = vec3_init(1.0f,
1.0f,
1.0f);
}
return(transform);
}
/* for a plane defined by n=normal, return a u and v vector that is on that plane and perpendictular */
void plane_vector_u_and_v_from_normal(union vec3 *u, union vec3 *v, const union vec3 *n)
{
union vec3 basis = { { 1, 0, 0 } };
/* find a vector we can use for our basis to define v */
float dot = vec3_dot(n, &basis);
if (fabs(dot) >= 1.0 - ZERO_TOLERANCE) {
/* if forward is parallel, we can use up */
vec3_init(&basis, 0, 1, 0);
}
/* find the right vector from our basis and the normal */
vec3_cross(v, &basis, n);
vec3_normalize_self(v);
/* now the final forward vector is perpendicular n and v */
vec3_cross(u, n, v);
vec3_normalize_self(u);
}
static union vec3 compute_triangle_normal(struct triangle *t)
{
union vec3 v1, v2, cross;
v1.v.x = t->v[1]->x - t->v[0]->x;
v1.v.y = t->v[1]->y - t->v[0]->y;
v1.v.z = t->v[1]->z - t->v[0]->z;
v2.v.x = t->v[2]->x - t->v[1]->x;
v2.v.y = t->v[2]->y - t->v[1]->y;
v2.v.z = t->v[2]->z - t->v[1]->z;
vec3_cross(&cross, &v1, &v2);
vec3_normalize_self(&cross);
/* make sure we always have a valid normal */
if (isnan(cross.v.x) || isnan(cross.v.y) || isnan(cross.v.z))
vec3_init(&cross, 0, 1, 0);
return cross;
}
void explode_landmine_damage_players(const Vec3i& position)
{
printf("Landmine damaging players at "); vec3i_print(position);
Vec3 p = vec3_add(vec3_init(position), vec3_init(0.5f));
Hitscan::against_agents(p, vec3_init(0, 0, 1), 2);
}
// check upper bounds
// we should not be calling this function out of bounds, so assert
IF_ASSERT(!is_boxed_position(position)) return;
// constant for helping walk the axes
static const int dir[2] = { -1, 1 };
static const int sides[3][3] = {
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }};
//static const struct Vec3 vsides[3] = {
//vec3_init(1, 0, 0),
//vec3_init(0, 1, 0),
//vec3_init(0, 0, 1)};
const struct Vec3 fp = vec3_add(vec3_init(position), vec3_init(0.5f));
// notify clients
Particle::plasmagen_explode_StoC msg;
msg.position = fp;
msg.broadcast();
// boundaries for the explosion, which can be contained by iron and bedrock
int bounds[3][2];
CubeType cubes[3][2];
for (int i=0; i<3; i++)
for (int j=0; j<2; j++)
{
bounds[i][j] = PLASMAGEN_BLOCK_BLAST_RADIUS;
cubes[i][j] = EMPTY_CUBE;
}
namespace Voxels
{
void VoxelModel::set_skeleton_root(const Vec3& position, float theta)
{ //set offset and rotation
IF_ASSERT(!this->skeleton_inited) return;
Vec3 angles = vec3_init(theta, 0.0f, 0.0f);
vox_skeleton_world_matrix[0] = affine_euler_rotation_and_translation(position, angles);
vox_skeleton_world_matrix[0].c = translate_position(vox_skeleton_world_matrix[0].c);
}
void VoxelModel::set_skeleton_root(float *data)
{
IF_ASSERT(!this->skeleton_inited) return;
vox_skeleton_world_matrix[0] = affine_euler_rotation_and_translation(
vec3_init(data[0], data[1], data[2]),
vec3_init(data[3], data[4], data[5]));
vox_skeleton_world_matrix[0].c = translate_position(vox_skeleton_world_matrix[0].c);
}
int VoxelModel::get_parent_node_index(int part)
{
IF_ASSERT(part < 0 || part >= this->n_parts) return -1;
VoxPart* vp = vox_dat->vox_part[part];
if (vp == NULL) return -1;
return vp->skeleton_parent_matrix;
}
void VoxelModel::set_node_rotation_by_part(int part, float theta, float phi, float rho)
{
IF_ASSERT(!this->skeleton_inited) return;
void play_animation(AnimationType animation_type, struct Vec3 position)
{
class AnimationProperty* data = get_animation_data(animation_type);
IF_ASSERT(data == NULL) return;
IF_ASSERT(data->callback == NULL || data->metadata == NULL) return;
switch (data->metadata_type)
{
case AnimDataNone:
break;
case AnimDataSimple:
((class AnimationStateMetadata*)data->metadata)->position = position;
((class AnimationStateMetadata*)data->metadata)->velocity = vec3_scalar_mult(vec3_init(1), data->momentum);
break;
default:
GS_ASSERT(false);
break;
}
data->callback(animation_type, data->metadata);
}
void main_init(int argc, char *argv[])
{
bool override_hw = false;
if (argc > 1)
{
if (strcmp(argv[1], "calibrate") == 0)
calibrate = true;
else
override_hw = true;
}
/* init data structures: */
memset(&pos_in, 0, sizeof(pos_in_t));
vec3_init(&pos_in.acc);
/* init SCL subsystem: */
syslog(LOG_INFO, "initializing signaling and communication link (SCL)");
if (scl_init("pilot") != 0)
{
syslog(LOG_CRIT, "could not init scl module");
die();
}
/* init params subsystem: */
syslog(LOG_INFO, "initializing opcd interface");
opcd_params_init("pilot.", 1);
/* initialize logger: */
syslog(LOG_INFO, "opening logger");
if (logger_open() != 0)
{
syslog(LOG_CRIT, "could not open logger");
die();
}
syslog(LOG_CRIT, "logger opened");
LOG(LL_INFO, "initializing platform");
if (arcade_quad_init(&platform, override_hw) < 0)
{
LOG(LL_ERROR, "could not initialize platform");
die();
}
acc_mag_cal_init();
cmc_init();
const size_t array_len = sizeof(float) * platform.n_motors;
setpoints = malloc(array_len);
ASSERT_NOT_NULL(setpoints);
memset(setpoints, 0, array_len);
rpm_square = malloc(array_len);
ASSERT_NOT_NULL(rpm_square);
memset(rpm_square, 0, array_len);
LOG(LL_INFO, "initializing model/controller");
pos_init();
ne_speed_ctrl_init(REALTIME_PERIOD);
att_ctrl_init();
yaw_ctrl_init();
u_ctrl_init();
u_speed_init();
navi_init();
LOG(LL_INFO, "initializing command interface");
cmd_init();
motors_state_init();
blackbox_init();
/* init flight logic: */
flight_logic_init();
/* init calibration data: */
cal_init(&gyro_cal, 3, 1000);
cal_ahrs_init();
flight_state_init(50, 150, 4.0);
piid_init(REALTIME_PERIOD);
interval_init(&gyro_move_interval);
gps_data_init(&gps_data);
mag_decl_init();
cal_init(&rc_cal, 3, 500);
tsfloat_t acc_fg;
opcd_param_t params[] =
{
{"acc_fg", &acc_fg},
OPCD_PARAMS_END
};
opcd_params_apply("main.", params);
filter1_lp_init(&lp_filter, tsfloat_get(&acc_fg), 0.06, 3);
cm_init();
mon_init();
LOG(LL_INFO, "entering main loop");
}
void main_step(const float dt,
const marg_data_t *marg_data,
const gps_data_t *gps_data,
const float ultra,
const float baro,
const float voltage,
const float current,
const float channels[MAX_CHANNELS],
const uint16_t sensor_status,
const bool override_hw)
{
vec2_t ne_pos_err, ne_speed_sp, ne_spd_err;
vec2_init(&ne_pos_err);
vec2_init(&ne_speed_sp);
vec2_init(&ne_spd_err);
vec3_t mag_normal;
vec3_init(&mag_normal);
float u_pos_err = 0.0f;
float u_spd_err = 0.0f;
int bb_rate;
if (calibrate)
{
ONCE(LOG(LL_INFO, "publishing calibration data; actuators disabled"));
bb_rate = 20;
goto out;
}
else
bb_rate = 1;
pos_in.dt = dt;
pos_in.ultra_u = ultra;
pos_in.baro_u = baro;
if (!(sensor_status & MARG_VALID))
{
marg_err += 1;
if (marg_err > 500)
{
/* we are in serious trouble */
memset(setpoints, 0, sizeof(float) * platform.n_motors);
platform_write_motors(setpoints);
die();
}
goto out;
}
marg_err = 0;
float cal_channels[MAX_CHANNELS];
memcpy(cal_channels, channels, sizeof(cal_channels));
if (sensor_status & RC_VALID)
{
/* apply calibration if remote control input is valid: */
float cal_data[3] = {channels[CH_PITCH], channels[CH_ROLL], channels[CH_YAW]};
cal_sample_apply(&rc_cal, cal_data);
cal_channels[CH_PITCH] = cal_data[0];
cal_channels[CH_ROLL] = cal_data[1];
cal_channels[CH_YAW] = cal_data[2];
}
/* perform gyro calibration: */
marg_data_t cal_marg_data;
marg_data_init(&cal_marg_data);
marg_data_copy(&cal_marg_data, marg_data);
if (cal_sample_apply(&gyro_cal, cal_marg_data.gyro.ve) == 0 && gyro_moved(&marg_data->gyro))
{
if (interval_measure(&gyro_move_interval) > 1.0)
LOG(LL_ERROR, "gyro moved during calibration, retrying");
cal_reset(&gyro_cal);
}
/* update relative gps position, if we have a fix: */
float mag_decl;
if (sensor_status & GPS_VALID)
{
gps_util_update(&gps_rel_data, gps_data);
pos_in.pos_n = gps_rel_data.dn;
pos_in.pos_e = gps_rel_data.de;
pos_in.speed_n = gps_rel_data.speed_n;
pos_in.speed_e = gps_rel_data.speed_e;
ONCE(gps_start_set(gps_data));
mag_decl = mag_decl_get();
}
else
{
pos_in.pos_n = 0.0f;
pos_in.pos_e = 0.0f;
pos_in.speed_n = 0.0f;
pos_in.speed_e = 0.0f;
mag_decl = 0.0f;
}
/* apply acc/mag calibration: */
acc_mag_cal_apply(&cal_marg_data.acc, &cal_marg_data.mag);
vec_copy(&mag_normal, &cal_marg_data.mag);
/* apply current magnetometer compensation: */
cmc_apply(&cal_marg_data.mag, current);
//printf("%f %f %f %f\n", current, cal_marg_data.mag.x, cal_marg_data.mag.y, cal_marg_data.mag.z);
/* determine flight state: */
bool flying = flight_state_update(&cal_marg_data.acc.ve[0]);
if (!flying && pos_in.ultra_u == 7.0)
pos_in.ultra_u = 0.2;
/* compute orientation estimate: */
euler_t euler;
int ahrs_state = cal_ahrs_update(&euler, &cal_marg_data, mag_decl, dt);
if (ahrs_state < 0 || !cal_complete(&gyro_cal))
goto out;
ONCE(init = 1; LOG(LL_DEBUG, "system initialized; orientation = yaw: %f pitch: %f roll: %f", euler.yaw, euler.pitch, euler.roll));
/* rotate local ACC measurements into global NEU reference frame: */
vec3_t world_acc;
vec3_init(&world_acc);
body_to_neu(&world_acc, &euler, &cal_marg_data.acc);
/* center global ACC readings: */
filter1_run(&lp_filter, &world_acc.ve[0], &acc_vec[0]);
FOR_N(i, 3)
pos_in.acc.ve[i] = world_acc.ve[i] - acc_vec[i];
/* compute next 3d position estimate using Kalman filters: */
pos_t pos_est;
vec2_init(&pos_est.ne_pos);
vec2_init(&pos_est.ne_speed);
pos_update(&pos_est, &pos_in);
/* execute flight logic (sets cm_x parameters used below): */
bool hard_off = false;
bool motors_enabled = flight_logic_run(&hard_off, sensor_status, flying, cal_channels, euler.yaw, &pos_est.ne_pos, pos_est.baro_u.pos, pos_est.ultra_u.pos, platform.max_thrust_n, platform.mass_kg, dt);
/* execute up position/speed controller(s): */
float a_u = 0.0f;
if (cm_u_is_pos())
{
if (cm_u_is_baro_pos())
a_u = u_ctrl_step(&u_pos_err, cm_u_sp(), pos_est.baro_u.pos, pos_est.baro_u.speed, dt);
else /* ultra pos */
a_u = u_ctrl_step(&u_pos_err, cm_u_sp(), pos_est.ultra_u.pos, pos_est.ultra_u.speed, dt);
}
else if (cm_u_is_spd())
a_u = u_speed_step(&u_spd_err, cm_u_sp(), pos_est.baro_u.speed, dt);
else
a_u = cm_u_sp();
/* execute north/east navigation and/or read speed vector input: */
if (cm_att_is_gps_pos())
{
vec2_t pos_sp;
vec2_init(&pos_sp);
cm_att_sp(&pos_sp);
navi_run(&ne_speed_sp, &ne_pos_err, &pos_sp, &pos_est.ne_pos, dt);
}
else if (cm_att_is_gps_spd())
cm_att_sp(&ne_speed_sp);
/* execute north/east speed controller: */
vec2_t a_ne;
vec2_init(&a_ne);
ne_speed_ctrl_run(&a_ne, &ne_spd_err, &ne_speed_sp, dt, &pos_est.ne_speed);
vec3_t a_neu;
vec3_set(&a_neu, a_ne.x, a_ne.y, a_u);
vec3_t f_neu;
vec3_init(&f_neu);
vec_scalar_mul(&f_neu, &a_neu, platform.mass_kg); /* f[i] = a[i] * m, makes ctrl device-independent */
float hover_force = platform.mass_kg * 9.81f;
f_neu.z += hover_force;
/* execute NEU forces optimizer: */
float thrust;
vec2_t pr_pos_sp;
vec2_init(&pr_pos_sp);
att_thrust_calc(&pr_pos_sp, &thrust, &f_neu, euler.yaw, platform.max_thrust_n, 0);
/* execute direct attitude angle control, if requested: */
if (cm_att_is_angles())
cm_att_sp(&pr_pos_sp);
/* execute attitude angles controller: */
vec2_t att_err;
vec2_init(&att_err);
vec2_t pr_spd;
vec2_set(&pr_spd, -cal_marg_data.gyro.y, cal_marg_data.gyro.x);
vec2_t pr_pos, pr_pos_ctrl;
vec2_set(&pr_pos, -euler.pitch, euler.roll);
vec2_init(&pr_pos_ctrl);
att_ctrl_step(&pr_pos_ctrl, &att_err, dt, &pr_pos, &pr_spd, &pr_pos_sp);
float piid_sp[3] = {0.0f, 0.0f, 0.0f};
piid_sp[PIID_PITCH] = pr_pos_ctrl.ve[0];
piid_sp[PIID_ROLL] = pr_pos_ctrl.ve[1];
/* execute direct attitude rate control, if requested:*/
if (cm_att_is_rates())
{
vec2_t rates_sp;
vec2_init(&rates_sp);
cm_att_sp(&rates_sp);
piid_sp[PIID_PITCH] = rates_sp.ve[0];
piid_sp[PIID_ROLL] = rates_sp.ve[1];
}
/* execute yaw position controller: */
float yaw_speed_sp, yaw_err;
if (cm_yaw_is_pos())
yaw_speed_sp = yaw_ctrl_step(&yaw_err, cm_yaw_sp(), euler.yaw, cal_marg_data.gyro.z, dt);
else
yaw_speed_sp = cm_yaw_sp(); /* direct yaw speed control */
//yaw_speed_sp = yaw_ctrl_step(&yaw_err, 0.0, euler.yaw, cal_marg_data.gyro.z, dt);
piid_sp[PIID_YAW] = yaw_speed_sp;
/* execute stabilizing PIID controller: */
f_local_t f_local = {{thrust, 0.0f, 0.0f, 0.0f}};
float piid_gyros[3] = {cal_marg_data.gyro.x, -cal_marg_data.gyro.y, cal_marg_data.gyro.z};
piid_run(&f_local.ve[1], piid_gyros, piid_sp, dt);
/* computate rpm ^ 2 out of the desired forces: */
inv_coupling_calc(rpm_square, f_local.ve);
/* compute motor setpoints out of rpm ^ 2: */
piid_int_enable(platform_ac_calc(setpoints, motors_spinning(), voltage, rpm_square));
/* enables motors, if flight logic requests it: */
motors_state_update(flying, dt, motors_enabled);
/* reset controllers, if motors are not controllable: */
if (!motors_controllable())
{
navi_reset();
ne_speed_ctrl_reset();
u_ctrl_reset();
att_ctrl_reset();
piid_reset();
}
/* handle special cases for motor setpoints: */
if (motors_starting())
FOR_N(i, platform.n_motors) setpoints[i] = platform.ac.min;
if (hard_off || motors_stopping())
FOR_N(i, platform.n_motors) setpoints[i] = platform.ac.off_val;
printf("%f %f %f\n", rad2deg(euler.pitch), rad2deg(euler.roll), rad2deg(euler.yaw));
/* write motors: */
if (!override_hw)
{
//platform_write_motors(setpoints);
}
/* set monitoring data: */
mon_data_set(ne_pos_err.x, ne_pos_err.y, u_pos_err, yaw_err);
out:
EVERY_N_TIMES(bb_rate, blackbox_record(dt, marg_data, gps_data, ultra, baro, voltage, current, channels, sensor_status, /* sensor inputs */
&ne_pos_err, u_pos_err, /* position errors */
&ne_spd_err, u_spd_err /* speed errors */,
&mag_normal));
}
void RailTrailEffect::tick()
{
if (Options::animation_level <= 0) return;
static const float span = 0.5f; // space between particle layers
static const float spin_span = PI / 8;
const float dist = vec3_distance(this->end, this->start);
const float step = span / (dist);
Vec3 fwd = vec3_sub(this->end, this->start);
//this->start = vec3_add(this->start, vec3_scalar_mult(vec3_normalize(this->start), 0.005f));
// RIGHT SETTINGS WOULD LOOK GOOD ON MOB IMPACTS ---> voxel_explode(this->end, /*min*/ 4, /*max*/ 14, /*size*/ 0.2f, /*force*/ 0.1f, COLOR_GREEN);
float theta, phi;
vec3_to_angles(fwd, &theta, &phi);
float curr_spin = 0.0f;
//float curr_spin = (PI/16) * this->ttl;
for (float fl=0.0f; fl<=1.0f; fl+=step)
{
Vec3 curr = vec3_interpolate(this->start, this->end, fl);
//float r = 0.17f; // quadratic radius
float r = 0.08f; // quadratic radius
Vec3 spiral = vec3_init(
r * cosf(curr_spin),
0,
r * sinf(curr_spin)
);
Vec3 spiral2 = vec3_init(
r * cosf(curr_spin + PI),
0,
r * sinf(curr_spin + PI)
);
spiral = vec3_euler_rotation(spiral, theta-0.5f, 0, phi-0.5f);
spiral2 = vec3_euler_rotation(spiral2, theta-0.5f, 0, phi-0.5f);
spiral = vec3_add(curr, spiral);
spiral2 = vec3_add(curr, spiral2);
spiral = translate_position(spiral);
spiral2 = translate_position(spiral2);
//float anim_scale = float(Options::animation_level)/3.0f;
//n = anim_scale*float(n);
Particle::Shrapnel *s;
s = Particle::create_shrapnel(spiral, /*vel*/ vec3_init(0,0,0));
if (s == NULL) return;
//s->ttl = randrange(8, 15);
//s->scale = 0.1f;
//s->texture_index = 54;
s->ttl = randrange(2, 4);
//s->ttl = 2;
s->scale = 0.06f;
s->texture_index = 22;
s = Particle::create_shrapnel(spiral2, /*vel*/ vec3_init(0,0,0));
if (s == NULL) return;
//s->ttl = randrange(8, 15);
//s->scale = 0.1f;
s->texture_index = 54;
s->ttl = randrange(2, 4);
//s->ttl = 2;
s->scale = 0.06f;
s->texture_index = 22;
//draw_quad(curr, span, theta, phi);
curr_spin += spin_span;
if (curr_spin >= PI*2)
curr_spin = 0.0f;
}
}