bool auto_logic_run(bool *hard_off, bool is_full_auto, uint16_t sensor_status, bool flying, float channels[MAX_CHANNELS], float yaw, vec2_t *ne_gps_pos, float u_baro_pos, float u_ultra_pos)
{
int rc_valid = sensor_status & RC_VALID;
if (is_full_auto || rc_valid)
{
float gas_stick = channels[CH_GAS];
if (is_full_auto || gas_stick > 0.5)
{
pthread_mutex_lock(&mutex);
if (mode_is_ground)
cm_u_set_ultra_pos(setp_u);
else
cm_u_set_baro_pos(setp_u);
pthread_mutex_unlock(&mutex);
}
else
cm_u_set_spd((gas_stick - 0.5) * 5.0);
}
else
cm_u_set_spd(-0.5);
if (u_ultra_pos > 1.0)
{
float yaw_stick = channels[CH_YAW];
if (is_full_auto || (rc_valid && fabs(yaw_stick) < 0.05))
cm_yaw_set_pos(tsfloat_get(&setp_yaw));
else
cm_yaw_set_spd(yaw_stick * 2.0f);
}
else
cm_yaw_set_spd(0.0f);
float pitch = channels[CH_PITCH];
float roll = channels[CH_ROLL];
float sw_l = channels[CH_SWITCH_L];
if (!is_full_auto && rc_valid && !is_full_auto && sqrt(pitch * pitch + roll * roll) > 0.1)
hyst_gps_override = 1.0;
hyst_gps_override -= 0.006;
if (hyst_gps_override > 0.0)
{
vec2_t angles;
vec2_set(&angles, pitch, roll);
cm_att_set_angles(&angles);
}
else
{
vec2_t ne_gps_setpoint;
vec2_set(&ne_gps_setpoint, tsfloat_get(&setp_n), tsfloat_get(&setp_e));
cm_att_set_gps_pos(&ne_gps_setpoint);
}
if (!is_full_auto && (sensor_status & RC_VALID) && sw_l > 0.5)
*hard_off = true;
return tsint_get(&motors_enabled);
}
static void build_sprite(struct vb_data *data, float fcx, float fcy,
float start_u, float end_u, float start_v, float end_v)
{
struct vec2 *tvarray = data->tvarray[0].array;
vec3_zero(data->points);
vec3_set(data->points+1, fcx, 0.0f, 0.0f);
vec3_set(data->points+2, 0.0f, fcy, 0.0f);
vec3_set(data->points+3, fcx, fcy, 0.0f);
vec2_set(tvarray, start_u, start_v);
vec2_set(tvarray+1, end_u, start_v);
vec2_set(tvarray+2, start_u, end_v);
vec2_set(tvarray+3, end_u, end_v);
}
bool auto_logic_run(bool *hard_off, bool is_full_auto, uint16_t sensor_status, bool flying, float channels[PP_MAX_CHANNELS], float yaw, vec2_t *ne_gps_pos, float u_baro_pos, float u_ultra_pos)
{
/* set u position: */
pthread_mutex_lock(&mutex);
if (mode_is_ground)
cm_u_set_ultra_pos(setp_u);
else
cm_u_set_baro_pos(setp_u);
pthread_mutex_unlock(&mutex);
/* set gps position: */
vec2_t ne_gps_setpoint;
vec2_set(&ne_gps_setpoint, tsfloat_get(&setp_n), tsfloat_get(&setp_e));
cm_att_set_gps_pos(&ne_gps_setpoint);
/* set yaw position: */
cm_yaw_set_pos(tsfloat_get(&setp_yaw));
/* safe_auto code: */
if (!is_full_auto && (sensor_status & RC_VALID))
{
printf("SAFE_AUTO\n");
float sw_l = channels[CH_SWITCH_L];
if (sw_l > 0.5)
{
*hard_off = true;
}
float gas_stick = channels[CH_GAS];
if (gas_stick < 0.8)
cm_u_set_acc((gas_stick - 0.5) * 5.0);
float pitch = channels[CH_PITCH];
float roll = channels[CH_ROLL];
if (sqrt(pitch * pitch + roll * roll) > 0.1)
hyst_gps_override = 1.0;
hyst_gps_override -= 0.006;
if (hyst_gps_override > 0.0)
{
vec2_t angles;
vec2_set(&angles, pitch, roll);
cm_att_set_angles(&angles);
}
}
return tsint_get(&motors_enabled);
}
static void
foe_add_trail(const vector2 *pos, const vector2 *dir, int palette)
{
const int nparticles = 3 + irand(7);
int i;
vector2 tp = *pos, e = *dir;
vec2_mul_scalar(&e, 1.02f);
vec2_sub_from(&tp, &e);
for (i = 0; i < nparticles; i++) {
particle *p = add_particle();
if (!p)
break;
p->ttl = 15 + irand(15);
p->t = 0;
p->palette = palette;
p->width = p->height = .2f + .1f*frand();
p->pos.x = tp.x + .2f*frand() - .1f;
p->pos.y = tp.y + .2f*frand() - .1f;
vec2_set(&p->dir, 1, 0);
p->speed = 0.f;
p->friction = .9f;
}
}
static int do_set(struct vec2 *QbyP,
struct mtx2 *Cinv,
double P, double Q1, double Q2,
double R11, double R12, double R22)
{
struct mtx2 QQ={0};
if (P > 0) {
mtx2_set(&QQ, Q1*Q1, Q1*Q2, Q2*Q2);
double Pinv = 1.0/P;
double P2inv = Pinv*Pinv;
mtx2_set(Cinv,
Q1*Q1*P2inv - R11*Pinv,
Q1*Q2*P2inv - R12*Pinv,
Q2*Q2*P2inv - R22*Pinv);
vec2_set(QbyP, Q1*Pinv, Q2*Pinv);
return 1;
} else {
return 0;
}
}
static void chroma_key_render(void *data, gs_effect_t *effect)
{
struct chroma_key_filter_data *filter = data;
obs_source_t *target = obs_filter_get_target(filter->context);
uint32_t width = obs_source_get_base_width(target);
uint32_t height = obs_source_get_base_height(target);
struct vec2 pixel_size;
obs_source_process_filter_begin(filter->context, GS_RGBA,
OBS_ALLOW_DIRECT_RENDERING);
vec2_set(&pixel_size, 1.0f / (float)width, 1.0f / (float)height);
gs_effect_set_vec4(filter->color_param, &filter->color);
gs_effect_set_float(filter->contrast_param, filter->contrast);
gs_effect_set_float(filter->brightness_param, filter->brightness);
gs_effect_set_float(filter->gamma_param, filter->gamma);
gs_effect_set_vec2(filter->chroma_param, &filter->chroma);
gs_effect_set_vec4(filter->key_rgb_param, &filter->key_rgb);
gs_effect_set_vec2(filter->pixel_size_param, &pixel_size);
gs_effect_set_float(filter->similarity_param, filter->similarity);
gs_effect_set_float(filter->smoothness_param, filter->smoothness);
gs_effect_set_float(filter->spill_param, filter->spill);
obs_source_process_filter_end(filter->context, filter->effect, 0, 0);
UNUSED_PARAMETER(effect);
}
static inline void chroma_settings_update(
struct chroma_key_filter_data *filter, obs_data_t *settings)
{
int64_t similarity = obs_data_get_int(settings, SETTING_SIMILARITY);
int64_t smoothness = obs_data_get_int(settings, SETTING_SMOOTHNESS);
int64_t spill = obs_data_get_int(settings, SETTING_SPILL);
uint32_t key_color = (uint32_t)obs_data_get_int(settings,
SETTING_KEY_COLOR);
const char *key_type = obs_data_get_string(settings,
SETTING_COLOR_TYPE);
struct vec4 key_color_v4;
struct matrix4 yuv_mat_m4;
if (strcmp(key_type, "green") == 0)
key_color = 0x00FF00;
else if (strcmp(key_type, "blue") == 0)
key_color = 0xFF9900;
else if (strcmp(key_type, "magenta") == 0)
key_color = 0xFF00FF;
vec4_from_rgba(&filter->key_rgb, key_color | 0xFF000000);
memcpy(&yuv_mat_m4, yuv_mat, sizeof(yuv_mat));
vec4_transform(&key_color_v4, &filter->key_rgb, &yuv_mat_m4);
vec2_set(&filter->chroma, key_color_v4.y, key_color_v4.z);
filter->similarity = (float)similarity / 1000.0f;
filter->smoothness = (float)smoothness / 1000.0f;
filter->spill = (float)spill / 1000.0f;
}
void gs_texcoord(float x, float y, int unit)
{
struct vec2 v2;
vec2_set(&v2, x, y);
gs_texcoord2v(&v2, unit);
}
static inline void render_output_texture(struct obs_core_video *video,
int cur_texture, int prev_texture)
{
profile_start(render_output_texture_name);
gs_texture_t *texture = video->render_textures[prev_texture];
gs_texture_t *target = video->output_textures[cur_texture];
uint32_t width = gs_texture_get_width(target);
uint32_t height = gs_texture_get_height(target);
struct vec2 base_i;
vec2_set(&base_i,
1.0f / (float)video->base_width,
1.0f / (float)video->base_height);
gs_effect_t *effect = get_scale_effect(video, width, height);
gs_technique_t *tech;
if (video->ovi.output_format == VIDEO_FORMAT_RGBA) {
tech = gs_effect_get_technique(effect, "Draw");
} else {
tech = gs_effect_get_technique(effect, "DrawMatrix");
}
gs_eparam_t *image = gs_effect_get_param_by_name(effect, "image");
gs_eparam_t *matrix = gs_effect_get_param_by_name(effect,
"color_matrix");
gs_eparam_t *bres_i = gs_effect_get_param_by_name(effect,
"base_dimension_i");
size_t passes, i;
if (!video->textures_rendered[prev_texture])
goto end;
gs_set_render_target(target, NULL);
set_render_size(width, height);
if (bres_i)
gs_effect_set_vec2(bres_i, &base_i);
gs_effect_set_val(matrix, video->color_matrix, sizeof(float) * 16);
gs_effect_set_texture(image, texture);
gs_enable_blending(false);
passes = gs_technique_begin(tech);
for (i = 0; i < passes; i++) {
gs_technique_begin_pass(tech, i);
gs_draw_sprite(texture, 0, width, height);
gs_technique_end_pass(tech);
}
gs_technique_end(tech);
gs_enable_blending(true);
video->textures_output[cur_texture] = true;
end:
profile_end(render_output_texture_name);
}
static inline void build_sprite(struct vb_data *data, float fcx, float fcy,
uint32_t flip)
{
struct vec2 *tvarray = data->tvarray[0].array;
float start_u, end_u;
float start_v, end_v;
assign_sprite_uv(&start_u, &end_u, (flip & GS_FLIP_U) != 0);
assign_sprite_uv(&start_v, &end_v, (flip & GS_FLIP_V) != 0);
vec3_zero(data->points);
vec3_set(data->points+1, fcx, 0.0f, 0.0f);
vec3_set(data->points+2, 0.0f, fcy, 0.0f);
vec3_set(data->points+3, fcx, fcy, 0.0f);
vec2_set(tvarray, start_u, start_v);
vec2_set(tvarray+1, end_u, start_v);
vec2_set(tvarray+2, start_u, end_v);
vec2_set(tvarray+3, end_u, end_v);
}
static void AddTestItems(obs_scene_t scene, obs_source_t source)
{
obs_sceneitem_t item = NULL;
struct vec2 scale;
vec2_set(&scale, 20.0f, 20.0f);
item = obs_scene_add(scene, source);
obs_sceneitem_setscale(item, &scale);
}
float measure_distance(const Stroke *a, int i, const Stroke *b, int j,
const Vec2 *offset)
/* Measure the offset Euclidean distance between two points */
{
Vec2 v;
vec2_set(&v, a->points[i].x + offset->x - b->points[j].x,
a->points[i].y + offset->y - b->points[j].y);
return vec2_square(&v);
}
vec2 OBSBasicPreview::GetMouseEventPos(QMouseEvent *event)
{
OBSBasic *main = reinterpret_cast<OBSBasic*>(App()->GetMainWindow());
vec2 pos;
vec2_set(&pos,
(float(event->x()) - main->previewX) / main->previewScale,
(float(event->y()) - main->previewY) / main->previewScale);
return pos;
}
/////////////////////////////
// Input_GetDir
//
// Reports the direction of the dpad.
int Input_GetDir(vec2 dir) {
float vlen;
Uint8 buttons;
int mx, my;
float dlen;
extern int _width, _height;
// Get mouse state
buttons = SDL_GetMouseState(&mx, &my);
if (buttons) {
// Use the mouse
vec2_set(dir, mx - (_width / 2), my - (_height / 2.0f));
dlen = 1.0f / sqrtf(vec2_dot(dir, dir));
vec2_scale(dir, dir, dlen);
return (1);
} else {
// Use the keyboar
vec2_set(dir, 0.0f, 0.0f);
if (Input_GetKey(InputKey_Up)) {
dir[1] -= 1.0f;
}
if (Input_GetKey(InputKey_Down)) {
dir[1] += 1.0f;
}
if (Input_GetKey(InputKey_Left)) {
dir[0] -= 1.0f;
}
if (Input_GetKey(InputKey_Right)) {
dir[0] += 1.0f;
}
vlen = vec2_dot(dir, dir);
if (vlen > 0.0f) {
vlen = sqrtf(vlen);
vec2_scale(dir, dir, 1.0f / vlen);
return (1);
} else {
return (0);
}
}
}
/*
Find the weighted average center and set all centers
equal to this value.
for j gaussians
munew = sum_j( C_j^-1 p_j mu_j )/sum( C_j^-1 p_j )
where the mus are mean vectors and the C are the covarance
matrices.
The following would be a lot simple if we use vec2 and mtx2
types in the gaussian! Maybe some other day.
*/
int gvec_wmean_center(const struct gvec* gvec, struct vec2* mu_new)
{
int status=1;
struct vec2 mu_Cinvp, mu_Cinvpsum;
struct mtx2 Cinvpsum, Cinvpsum_inv, C, Cinvp;
memset(&Cinvpsum,0,sizeof(struct mtx2));
memset(&mu_Cinvpsum,0,sizeof(struct vec2));
const struct gauss* gauss = gvec->data;
for (size_t i=0; i<gvec->size; i++) {
// p*C^-1
mtx2_set(&C, gauss->irr, gauss->irc, gauss->icc);
if (!mtx2_invert(&C, &Cinvp)) {
wlog("gvec_fix_centers: zero determinant found in C");
status=0;
goto _gvec_wmean_center_bail;
}
mtx2_sprodi(&Cinvp, gauss->p);
// running sum of p*C^-1
mtx2_sumi(&Cinvpsum, &Cinvp);
// set the center as a vec2
vec2_set(&mu_Cinvp, gauss->row, gauss->col);
// p*C^-1 * mu in place on mu
mtx2_vec2prodi(&Cinvp, &mu_Cinvp);
// running sum of p*C^-1 * mu
vec2_sumi(&mu_Cinvpsum, &mu_Cinvp);
gauss++;
}
if (!mtx2_invert(&Cinvpsum, &Cinvpsum_inv)) {
wlog("gvec_fix_centers: zero determinant found in Cinvpsum");
status=0;
goto _gvec_wmean_center_bail;
}
mtx2_vec2prod(&Cinvpsum_inv, &mu_Cinvpsum, mu_new);
_gvec_wmean_center_bail:
return status;
}
void OBSBasicPreview::mousePressEvent(QMouseEvent *event)
{
OBSBasic *main = reinterpret_cast<OBSBasic*>(App()->GetMainWindow());
float x = float(event->x()) - main->previewX;
float y = float(event->y()) - main->previewY;
if (event->button() != Qt::LeftButton)
return;
mouseDown = true;
vec2_set(&startPos, x, y);
GetStretchHandleData(startPos);
vec2_divf(&startPos, &startPos, main->previewScale);
startPos.x = std::round(startPos.x);
startPos.y = std::round(startPos.y);
mouseOverItems = SelectedAtPos(startPos);
vec2_zero(&lastMoveOffset);
}
obs_sceneitem_t obs_scene_add(obs_scene_t scene, obs_source_t source)
{
struct obs_scene_item *last;
struct obs_scene_item *item = bmalloc(sizeof(struct obs_scene_item));
struct calldata params = {0};
memset(item, 0, sizeof(struct obs_scene_item));
item->source = source;
item->visible = true;
item->parent = scene;
item->ref = 1;
vec2_set(&item->scale, 1.0f, 1.0f);
if (source)
obs_source_addref(source);
pthread_mutex_lock(&scene->mutex);
last = scene->first_item;
if (!last) {
scene->first_item = item;
} else {
while (last->next)
last = last->next;
last->next = item;
item->prev = last;
}
pthread_mutex_unlock(&scene->mutex);
calldata_setptr(¶ms, "scene", scene);
calldata_setptr(¶ms, "item", item);
signal_handler_signal(scene->source->signals, "add", ¶ms);
calldata_free(¶ms);
return item;
}
static int
brute_hit(struct foe_brute *f, const vector2 *hit)
{
const vector2 *pos = &f->common.pos;
const float radius = foe_data[FOE_BRUTE].radius;
if (vec2_distance_squared(hit, pos) < radius*radius) {
if (--f->hit_count == 0) {
foe_common_gen_explosion(&f->common, 1.3f);
kill_foe(&f->common);
foe_common_bump_score(&f->common);
} else {
vector2 s, os;
float t;
f->last_hit_tic = gc.level_tics;
s = f->common.pos;
vec2_sub_from(&s, &ship.pos);
vec2_normalize(&s);
vec2_set(&os, -s.y, s.x);
t = 5.f*(frand() - .5f);
s.x += t*os.x;
s.y += t*os.y;
vec2_mul_scalar(&s, .3f/vec2_length(&s));
vec2_add_to(&f->common.dir, &s);
f->common.angle_speed *= 2;
}
return 1;
}
return 0;
}
static void
gen_explosion(float x, float y)
{
int i, n, c;
particle *p;
n = 150 + irand(150);
c = irand(NUM_PALETTES);
for (i = 0; i < n; i++) {
vector2 offs;
p = add_particle();
if (!p)
break;
p->ttl = 20 + irand(20);
p->t = 0;
p->width = .3f*(.6f + .1f*frand());
p->height = .3f*(2.8f + .4f*frand());
p->pos.x = x;
p->pos.y = y;
p->palette = c;
vec2_set(&p->dir, frand() - .5f, frand() - .5f);
vec2_normalize(&p->dir);
offs = p->dir;
vec2_mul_scalar(&offs, 1.3f*frand());
vec2_add_to(&p->pos, &offs);
p->speed = PARTICLE_SPEED + .05f*frand();
p->friction = .96f;
}
}
static void
update_crosshair(void)
{
GLdouble modelview[16], projection[16];
GLdouble x0, y0, z0;
GLdouble x1, y1, z1;
float x, y, t;
GLint viewport[4];
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
set_modelview_matrix();
glGetIntegerv(GL_VIEWPORT, viewport);
glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
glGetDoublev(GL_PROJECTION_MATRIX, projection);
gluUnProject(last_mouse_x, viewport[3] - last_mouse_y - 1, 0,
modelview, projection, viewport,
&x0, &y0, &z0);
gluUnProject(last_mouse_x, viewport[3] - last_mouse_y - 1, 1,
modelview, projection, viewport,
&x1, &y1, &z1);
/* (x, y) at z = 0 */
t = -z0/(z1 - z0);
x = x0 + t*(x1 - x0);
y = y0 + t*(y1 - y0);
vec2_set(&crosshair, x, y);
glPopMatrix();
}
static void scale_filter_tick(void *data, float seconds)
{
struct scale_filter_data *filter = data;
enum obs_base_effect type;
obs_source_t *target;
bool lower_than_2x;
double cx_f;
double cy_f;
int cx;
int cy;
if (filter->base_canvas_resolution) {
struct obs_video_info ovi;
obs_get_video_info(&ovi);
filter->cx_in = ovi.base_width;
filter->cy_in = ovi.base_height;
}
target = obs_filter_get_target(filter->context);
filter->cx_out = 0;
filter->cy_out = 0;
filter->target_valid = !!target;
if (!filter->target_valid)
return;
cx = obs_source_get_base_width(target);
cy = obs_source_get_base_height(target);
if (!cx || !cy) {
filter->target_valid = false;
return;
}
filter->cx_out = cx;
filter->cy_out = cy;
if (!filter->valid)
return;
/* ------------------------- */
cx_f = (double)cx;
cy_f = (double)cy;
double old_aspect = cx_f / cy_f;
double new_aspect =
(double)filter->cx_in / (double)filter->cy_in;
if (filter->aspect_ratio_only) {
if (fabs(old_aspect - new_aspect) <= EPSILON) {
filter->target_valid = false;
return;
} else {
if (new_aspect > old_aspect) {
filter->cx_out = (int)(cy_f * new_aspect);
filter->cy_out = cy;
} else {
filter->cx_out = cx;
filter->cy_out = (int)(cx_f / new_aspect);
}
}
} else {
filter->cx_out = filter->cx_in;
filter->cy_out = filter->cy_in;
}
vec2_set(&filter->dimension_i,
1.0f / (float)cx,
1.0f / (float)cy);
if (filter->undistort) {
filter->undistort_factor = new_aspect / old_aspect;
} else {
filter->undistort_factor = 1.0;
}
/* ------------------------- */
lower_than_2x = filter->cx_out < cx / 2 || filter->cy_out < cy / 2;
if (lower_than_2x && filter->sampling != OBS_SCALE_POINT) {
type = OBS_EFFECT_BILINEAR_LOWRES;
} else {
switch (filter->sampling) {
default:
case OBS_SCALE_POINT:
case OBS_SCALE_BILINEAR: type = OBS_EFFECT_DEFAULT; break;
case OBS_SCALE_BICUBIC: type = OBS_EFFECT_BICUBIC; break;
case OBS_SCALE_LANCZOS: type = OBS_EFFECT_LANCZOS; break;
}
}
filter->effect = obs_get_base_effect(type);
filter->image_param = gs_effect_get_param_by_name(filter->effect,
"image");
if (type != OBS_EFFECT_DEFAULT) {
filter->dimension_param = gs_effect_get_param_by_name(
filter->effect, "base_dimension_i");
} else {
filter->dimension_param = NULL;
}
if (type == OBS_EFFECT_BICUBIC || type == OBS_EFFECT_LANCZOS) {
filter->undistort_factor_param = gs_effect_get_param_by_name(
filter->effect, "undistort_factor");
}
else {
filter->undistort_factor_param = NULL;
}
UNUSED_PARAMETER(seconds);
}
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 OBSBasicPreview::StretchItem(const vec2 &pos)
{
Qt::KeyboardModifiers modifiers = QGuiApplication::keyboardModifiers();
obs_bounds_type boundsType = obs_sceneitem_get_bounds_type(stretchItem);
uint32_t stretchFlags = (uint32_t)stretchHandle;
bool shiftDown = (modifiers & Qt::ShiftModifier);
vec3 tl, br, pos3;
vec3_zero(&tl);
vec3_set(&br, stretchItemSize.x, stretchItemSize.y, 0.0f);
vec3_set(&pos3, pos.x, pos.y, 0.0f);
vec3_transform(&pos3, &pos3, &screenToItem);
if (stretchFlags & ITEM_LEFT)
tl.x = pos3.x;
else if (stretchFlags & ITEM_RIGHT)
br.x = pos3.x;
if (stretchFlags & ITEM_TOP)
tl.y = pos3.y;
else if (stretchFlags & ITEM_BOTTOM)
br.y = pos3.y;
if (!(modifiers & Qt::ControlModifier))
SnapStretchingToScreen(tl, br);
obs_source_t source = obs_sceneitem_getsource(stretchItem);
vec2 baseSize;
vec2_set(&baseSize,
float(obs_source_getwidth(source)),
float(obs_source_getheight(source)));
vec2 size;
vec2_set(&size,br. x - tl.x, br.y - tl.y);
if (boundsType != OBS_BOUNDS_NONE) {
if (shiftDown)
ClampAspect(tl, br, size, baseSize);
if (tl.x > br.x) std::swap(tl.x, br.x);
if (tl.y > br.y) std::swap(tl.y, br.y);
vec2_abs(&size, &size);
obs_sceneitem_set_bounds(stretchItem, &size);
} else {
if (!shiftDown)
ClampAspect(tl, br, size, baseSize);
vec2_div(&size, &size, &baseSize);
obs_sceneitem_setscale(stretchItem, &size);
}
pos3 = CalculateStretchPos(tl, br);
vec3_transform(&pos3, &pos3, &itemToScreen);
vec2 newPos;
vec2_set(&newPos, std::round(pos3.x), std::round(pos3.y));
obs_sceneitem_setpos(stretchItem, &newPos);
}
inline void SetScrollingOffset(float x, float y) {vec2_set(&scrollingOffset, x, y);}
static void
update_inner_state(void)
{
static int prev_level_tics;
inner_state.tics++;
switch (inner_state.state) {
case IS_WAVE_TITLE:
if (inner_state.tics >= WAVE_TITLE_TICS) {
set_inner_state(IS_IN_GAME);
/* set_inner_state(IS_PRE_WAVE_CLEARED); */
} else {
if (inner_state.tics == WAVE_TITLE_TICS/2) {
if (!ship.is_alive)
spawn_new_ship();
else
reset_ship();
}
}
break;
case IS_IN_GAME:
level_stat_counters.tics++;
game_stat_counters.tics++;
if (gc.tics_remaining <= 0) {
/* boom. */
hit_ship(&ship.pos, 1.f);
trigger_game_over();
} else {
gc.tics_remaining--;
if (!ship.is_alive) {
prev_level_tics = inner_state.tics;
set_inner_state(IS_RESPAWNING_SHIP);
} else if (!gc.foes_left) {
level_stat_counters.waves++;
game_stat_counters.waves++;
if (gc.tics_remaining > TIME_BONUS_MIN_TICS)
gc.score += gc.tics_remaining*TIME_BONUS_PER_TIC;
set_inner_state(IS_PRE_WAVE_CLEARED);
} else {
spawn_new_foes();
}
}
break;
case IS_PRE_WAVE_CLEARED:
if (inner_state.tics >= PRE_WAVE_CLEARED_TICS) {
gen_ship_implosion();
reset_powerups();
reset_missiles();
reset_bombs();
reset_lasers();
vec2_set(&ship.pos, 0.f, 0.f); /* HACK */
play_fx(FX_WAVE_TRANSITION);
set_inner_state(IS_WAVE_CLEARED);
}
break;
case IS_RESPAWNING_SHIP:
if (inner_state.tics >= TICS_UNTIL_RESPAWN) {
if (gc.ships_left) {
spawn_new_ship();
set_inner_state(IS_IN_GAME);
inner_state.tics = prev_level_tics;
} else {
trigger_game_over();
}
}
break;
case IS_GAME_OVER:
break;
case IS_RANK:
if (inner_state.tics >= RANK_TOTAL_TICS) {
stop_music();
if (is_highscore(gc.score)) {
set_inner_state(IS_HIGHSCORE_INPUT);
SDL_ShowCursor(SDL_ENABLE);
start_highscore_input();
} else {
/* loser. */
SDL_ShowCursor(SDL_ENABLE);
start_main_menu();
}
}
break;
case IS_WAVE_CLEARED:
if (gc.cur_wave == levels[gc.cur_level]->num_waves - 1 &&
inner_state.tics >= WAVE_CLEARED_TICS/2) {
initialize_stats_table(&level_stat_counters);
set_inner_state(IS_LEVEL_CLEARED);
} else if (inner_state.tics >= WAVE_CLEARED_TICS) {
gc.cur_wave++;
assert(gc.cur_wave < levels[gc.cur_level]->num_waves);
reset_wave();
set_inner_state(IS_WAVE_TITLE);
}
break;
case IS_HIGHSCORE_INPUT:
break;
case IS_LEVEL_CLEARED:
break;
case IS_LEVEL_TRANSITION:
if (inner_state.tics >= LEVEL_TRANSITION_TOTAL_TICS) {
gc.cur_level = (gc.cur_level + 1)%NUM_LEVELS;
set_inner_state(IS_WAVE_TITLE);
reset_level();
}
break;
default:
assert(0);
}
}
static void
foe_common_gen_explosion(struct foe_common *f, float scale)
{
int i;
particle *p;
int ndebris, nballs;
ndebris = scale*(40 + irand(30));
/* debris */
for (i = 0; i < ndebris; i++) {
p = add_particle();
if (!p)
break;
p->ttl = 20 + irand(15);
p->t = 0;
p->width = scale*.4f*(.6f + .15f*frand());
p->height = scale*.3f*(2.8f + .4f*frand());
p->pos = f->pos;
p->palette = PAL_YELLOW;
p->friction = .9;
vec2_set(&p->dir, frand() - .5f, frand() - .5f);
vec2_normalize(&p->dir);
p->speed = scale*(PARTICLE_SPEED + .05f*frand());
}
#define RADIUS .4f
/* fireballs */
nballs = scale*(8 + irand(8));
for (i = 0; i < nballs; i++) {
vector2 pos;
float r, l;
l = 1.2*scale;
pos.x = f->pos.x + l*frand() - .5f*l;
pos.y = f->pos.y + l*frand() - .5f*l;
r = scale*(.2f + .15f*frand());
add_explosion(&pos, r, EFF_EXPLOSION, 13 + irand(4));
}
/* shockwave */
add_explosion(&f->pos, scale*.3f, EFF_RING, 18);
/*
p = add_particle();
if (p) {
p->t = 0;
p->ttl = 20;
p->width = p->height = scale*.3f;
p->pos = f->pos;
p->palette = PAL_RED;
p->type = PT_SHOCKWAVE;
}
*/
perturb_water(&f->pos, scale*1200.f);
play_fx(FX_EXPLOSION_1);
}
void controller_system_update(ControllerSystem* self) {
GET_SYSTEM_COMPONENTS(self);
for (u32 i = 0; i < components->count; ++i) {
Entity entity = GET_ENTITY(i);
ControllerComponent* controller = (ControllerComponent*)GET_SYSTEM_COMPONENT(i);
MovementComponent* movement =
(MovementComponent*)GET_COMPONENT(entity, COMPONENT_MOVEMENT);
TransformComponent* transform =
(TransformComponent*)GET_COMPONENT(entity, COMPONENT_TRANSFORM);
REQUIRED_COMPONENTS(transform, movement, controller);
f32 x = 0;
f32 y = 0;
if (input_key(SDL_SCANCODE_LEFT)) {
x -= 1.f;
}
if (input_key(SDL_SCANCODE_RIGHT)) {
x += 1.f;
}
if (input_key(SDL_SCANCODE_UP)) {
y -= 1.f;
}
if (input_key(SDL_SCANCODE_DOWN)) {
y += 1.f;
}
//printf("%f\n", controller->moveSpeed);
x *= controller->moveSpeed;
y *= controller->moveSpeed;
vec2_set(&movement->velocity, x, y);
if (input_key_down(SDL_SCANCODE_Z)) {
for (u32 i = 0; i < controller->bulletSourceCount; ++i) {
bullet_source_on(&controller->bulletSources[i]);
}
}
if (input_key_up(SDL_SCANCODE_Z)) {
for (u32 i = 0; i < controller->bulletSourceCount; ++i) {
bullet_source_off(&controller->bulletSources[i]);
}
}
for (u32 i = 0; i < controller->bulletSourceCount; ++i) {
bullet_source_update(&controller->bulletSources[i], globals.time.delta, self->super.entityManager, &transform->position);
}
//if (controller->fireTimer > 0.f) {
// controller->fireTimer -= globals.time.delta;
//}
//if (input_key(SDL_SCANCODE_Z) && controller->fireTimer <= 0.f) {
// Vec2 pos = vec2_clone(&transform->position);
// pos.x += 64;
// pos.y += 52;
// entity_create_bullet(self->super.entityManager,
// vec2_clone(&pos),
// textures_get("player_bullet_1.png"));
// controller->fireTimer = controller->fireDelay;
//}
}
}