// 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; }