// run_nav_updates - top level call for the autopilot
// ensures calculations such as "distance to waypoint" are calculated before autopilot makes decisions
// To-Do - rename and move this function to make it's purpose more clear
void Copter::run_nav_updates(void)
{
// fetch position from inertial navigation
calc_position();
// calculate distance and bearing for reporting and autopilot decisions
calc_distance_and_bearing();
// run autopilot to make high level decisions about control modes
run_autopilot();
}
static void *
create_ref_vert_shader(struct vl_mc *r)
{
struct ureg_program *shader;
struct ureg_src mv_scale;
struct ureg_src vmv[2];
struct ureg_dst t_vpos;
struct ureg_dst o_vmv[2];
unsigned i;
shader = ureg_create(TGSI_PROCESSOR_VERTEX);
if (!shader)
return NULL;
vmv[0] = ureg_DECL_vs_input(shader, VS_I_MV_TOP);
vmv[1] = ureg_DECL_vs_input(shader, VS_I_MV_BOTTOM);
t_vpos = calc_position(r, shader, ureg_imm2f(shader,
(float)VL_MACROBLOCK_WIDTH / r->buffer_width,
(float)VL_MACROBLOCK_HEIGHT / r->buffer_height)
);
o_vmv[0] = ureg_DECL_output(shader, TGSI_SEMANTIC_GENERIC, VS_O_VTOP);
o_vmv[1] = ureg_DECL_output(shader, TGSI_SEMANTIC_GENERIC, VS_O_VBOTTOM);
/*
* mv_scale.xy = 0.5 / (dst.width, dst.height);
* mv_scale.z = 1.0f / 4.0f
* mv_scale.w = 1.0f / 255.0f
*
* // Apply motion vectors
* o_vmv[0..1].xy = vmv[0..1] * mv_scale + t_vpos
* o_vmv[0..1].zw = vmv[0..1] * mv_scale
*
*/
mv_scale = ureg_imm4f(shader,
0.5f / r->buffer_width,
0.5f / r->buffer_height,
1.0f / 4.0f,
1.0f / PIPE_VIDEO_MV_WEIGHT_MAX);
for (i = 0; i < 2; ++i) {
ureg_MAD(shader, ureg_writemask(o_vmv[i], TGSI_WRITEMASK_XY), mv_scale, vmv[i], ureg_src(t_vpos));
ureg_MUL(shader, ureg_writemask(o_vmv[i], TGSI_WRITEMASK_ZW), mv_scale, vmv[i]);
}
ureg_release_temporary(shader, t_vpos);
ureg_END(shader);
return ureg_create_shader_and_destroy(shader, r->pipe);
}
static void *
create_ycbcr_vert_shader(struct vl_mc *r, vl_mc_ycbcr_vert_shader vs_callback, void *callback_priv)
{
struct ureg_program *shader;
struct ureg_src vrect, vpos;
struct ureg_dst t_vpos, t_vtex;
struct ureg_dst o_vpos, o_flags;
struct vertex2f scale = {
(float)VL_BLOCK_WIDTH / r->buffer_width * VL_MACROBLOCK_WIDTH / r->macroblock_size,
(float)VL_BLOCK_HEIGHT / r->buffer_height * VL_MACROBLOCK_HEIGHT / r->macroblock_size
};
unsigned label;
shader = ureg_create(TGSI_PROCESSOR_VERTEX);
if (!shader)
return NULL;
vrect = ureg_DECL_vs_input(shader, VS_I_RECT);
vpos = ureg_DECL_vs_input(shader, VS_I_VPOS);
t_vpos = calc_position(r, shader, ureg_imm2f(shader, scale.x, scale.y));
t_vtex = ureg_DECL_temporary(shader);
o_vpos = ureg_DECL_output(shader, TGSI_SEMANTIC_POSITION, VS_O_VPOS);
o_flags = ureg_DECL_output(shader, TGSI_SEMANTIC_GENERIC, VS_O_FLAGS);
/*
* o_vtex.xy = t_vpos
* o_flags.z = intra * 0.5
*
* if(interlaced) {
* t_vtex.xy = vrect.y ? { 0, scale.y } : { -scale.y : 0 }
* t_vtex.z = vpos.y % 2
* t_vtex.y = t_vtex.z ? t_vtex.x : t_vtex.y
* o_vpos.y = t_vtex.y + t_vpos.y
*
* o_flags.w = t_vtex.z ? 0 : 1
* }
*
*/
vs_callback(callback_priv, r, shader, VS_O_VTEX, t_vpos);
ureg_MUL(shader, ureg_writemask(o_flags, TGSI_WRITEMASK_Z),
ureg_scalar(vpos, TGSI_SWIZZLE_Z), ureg_imm1f(shader, 0.5f));
ureg_MOV(shader, ureg_writemask(o_flags, TGSI_WRITEMASK_W), ureg_imm1f(shader, -1.0f));
if (r->macroblock_size == VL_MACROBLOCK_HEIGHT) { //TODO
ureg_IF(shader, ureg_scalar(vpos, TGSI_SWIZZLE_W), &label);
ureg_CMP(shader, ureg_writemask(t_vtex, TGSI_WRITEMASK_XY),
ureg_negate(ureg_scalar(vrect, TGSI_SWIZZLE_Y)),
ureg_imm2f(shader, 0.0f, scale.y),
ureg_imm2f(shader, -scale.y, 0.0f));
ureg_MUL(shader, ureg_writemask(t_vtex, TGSI_WRITEMASK_Z),
ureg_scalar(vpos, TGSI_SWIZZLE_Y), ureg_imm1f(shader, 0.5f));
ureg_FRC(shader, ureg_writemask(t_vtex, TGSI_WRITEMASK_Z), ureg_src(t_vtex));
ureg_CMP(shader, ureg_writemask(t_vtex, TGSI_WRITEMASK_Y),
ureg_negate(ureg_scalar(ureg_src(t_vtex), TGSI_SWIZZLE_Z)),
ureg_scalar(ureg_src(t_vtex), TGSI_SWIZZLE_X),
ureg_scalar(ureg_src(t_vtex), TGSI_SWIZZLE_Y));
ureg_ADD(shader, ureg_writemask(o_vpos, TGSI_WRITEMASK_Y),
ureg_src(t_vpos), ureg_src(t_vtex));
ureg_CMP(shader, ureg_writemask(o_flags, TGSI_WRITEMASK_W),
ureg_negate(ureg_scalar(ureg_src(t_vtex), TGSI_SWIZZLE_Z)),
ureg_imm1f(shader, 0.0f), ureg_imm1f(shader, 1.0f));
ureg_fixup_label(shader, label, ureg_get_instruction_number(shader));
ureg_ENDIF(shader);
}
ureg_release_temporary(shader, t_vtex);
ureg_release_temporary(shader, t_vpos);
ureg_END(shader);
return ureg_create_shader_and_destroy(shader, r->pipe);
}
static void update_all_nav(void) {
get_next_waypoint();
// Check if a new GPS coordinate was received and update the position
if (statevars.status & STATUS_GPS_FIX_AVAIL) {
// Calculate a new gps-based heading using the previous coord (current)
// and the newest coord (statevars)
gps_hdg_most_recent = calc_true_bearing(current_lat, current_long, statevars.gps_lat_ddeg, statevars.gps_long_ddeg);
current_lat = statevars.gps_lat_ddeg;
current_long = statevars.gps_long_ddeg;
}
// Check if a new GPS heading and speed were received and update
if (statevars.status & STATUS_GPS_GPRMC_RCVD) {
// Not using the gps heading because sometimes it's horrendous. Instead,
// we'll calculate our own using calc_true_bearing()
// gps_hdg_most_recent = statevars.gps_ground_course_deg;
gps_speed_most_recent = statevars.gps_ground_speed_kt * METERS_PER_SECOND_PER_KNOT;
}
// Calculate the number of ticks that occurred during the current iteration
// Since the tick count is cumulative, the new tick count will always be
// greater-than or equal to the previous tick count
uint32_t new_tick_count = statevars.odometer_ticks;
uint32_t tick_diff = new_tick_count - prev_tick_count;
current_speed = calc_speed_mps(tick_diff);
// TODO I know; we're doing another tick_diff / TICKS_PER_METER calculation.
// But we'll optimize this later
float distance_since_prev_iter_m = tick_diff / TICKS_PER_METER;
// We're using calc_speed_mps() instead to allow integer-based distance diff eval
// current_speed = calc_speed(distance_since_prev_iter_m);
// Advance the tick count now that we're done with the previous value
prev_tick_count = new_tick_count;
if (current_speed == 0.0) {
current_speed = gps_speed_most_recent;
}
nav_heading_deg = calc_nav_heading();
float old_lat = current_lat;
float old_long = current_long;
calc_position(¤t_lat, ¤t_long, old_lat, old_long, distance_since_prev_iter_m, nav_heading_deg);
float waypt_true_bearing = calc_true_bearing(current_lat, current_long, waypoint_lat, waypoint_long);
rel_bearing_deg = calc_relative_bearing(waypt_true_bearing, nav_heading_deg);
distance_to_waypoint_m = calc_dist_to_waypoint(current_lat, current_long, waypoint_lat, waypoint_long);
statevars.nav_heading_deg = nav_heading_deg;
statevars.nav_latitude = current_lat;
statevars.nav_longitude = current_long;
statevars.nav_waypt_latitude = waypoint_lat;
statevars.nav_waypt_longitude = waypoint_long;
statevars.nav_rel_bearing_deg = rel_bearing_deg;
statevars.nav_distance_to_waypt_m = distance_to_waypoint_m;
statevars.nav_speed = current_speed;
return;
}