예제 #1
0
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);
}
예제 #2
0
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);
}
예제 #3
0
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);
}
예제 #4
0
파일: foe.c 프로젝트: mpersano/protozoa
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;
	}
}
예제 #5
0
파일: test-pqr.c 프로젝트: esheldon/misc
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;
    }

}
예제 #6
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);
}
예제 #7
0
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;
}
예제 #8
0
void gs_texcoord(float x, float y, int unit)
{
	struct vec2 v2;

	vec2_set(&v2, x, y);
	gs_texcoord2v(&v2, unit);
}
예제 #9
0
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);
}
예제 #10
0
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);
}
예제 #11
0
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);
}
예제 #12
0
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);
}
예제 #13
0
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;
}
예제 #14
0
파일: Input.c 프로젝트: Kableado/GameLib
/////////////////////////////
// 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);
		}
	}
}
예제 #15
0
파일: gvec.c 프로젝트: esheldon/misc
/*
 
 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;
}
예제 #16
0
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);
}
예제 #17
0
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(&params, "scene", scene);
	calldata_setptr(&params, "item", item);
	signal_handler_signal(scene->source->signals, "add", &params);
	calldata_free(&params);

	return item;
}
예제 #18
0
파일: foe.c 프로젝트: mpersano/protozoa
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;
}
예제 #19
0
파일: credits.c 프로젝트: mpersano/protozoa
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;
	}
}
예제 #20
0
파일: in_game.c 프로젝트: mpersano/protozoa
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();
}
예제 #21
0
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);
}
예제 #22
0
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));
}
예제 #23
0
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);
}
예제 #24
0
	inline void SetScrollingOffset(float x, float y) {vec2_set(&scrollingOffset, x, y);}
예제 #25
0
파일: in_game.c 프로젝트: mpersano/protozoa
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);
	}
}
예제 #26
0
파일: foe.c 프로젝트: mpersano/protozoa
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);
}
예제 #27
0
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;
        //}
    }
}