void set_waypoint(int16_t index)
{
DPRINT("set_waypoint(%u)\r\n", index);
if (index < numPointsInCurrentSet)
{
waypointIndex = index;
#if (USE_MAVLINK == 1)
mavlink_waypoint_changed(waypointIndex);
#endif
if (waypointIndex == 0)
{
if (numPointsInCurrentSet > 1)
{
struct relWaypointDef previous_waypoint = wp_to_relative(currentWaypointSet[numPointsInCurrentSet-1]);
current_waypoint = wp_to_relative(currentWaypointSet[0]);
navigate_set_goal(previous_waypoint.loc, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
}
else
{
current_waypoint = wp_to_relative(currentWaypointSet[0]);
navigate_set_goal(GPSlocation, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
}
setBehavior(currentWaypointSet[0].flags);
}
else
{
struct relWaypointDef previous_waypoint = wp_to_relative(currentWaypointSet[waypointIndex-1]);
current_waypoint = wp_to_relative(currentWaypointSet[waypointIndex]);
navigate_set_goal(previous_waypoint.loc, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
setBehavior(current_waypoint.flags);
}
#if (DEADRECKONING == 0)
navigate_compute_bearing_to_goal();
#endif
}
}
//void run_flightplan(void)
void flightplan_waypoints_update(void)
{
// first run any injected wp from the serial port
if (wp_inject_pos == WP_INJECT_READY)
{
current_waypoint = wp_to_relative(wp_inject);
navigate_set_goal(GPSlocation, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
setBehavior(current_waypoint.flags);
navigate_compute_bearing_to_goal();
wp_inject_pos = 0;
return;
}
// steering is based on cross track error.
// waypoint arrival is detected computing distance to the "finish line".
// note: locations are measured in meters
// velocities are in centimeters per second
// locations have a range of +-32000 meters (20 miles) from origin
if (desired_behavior._.altitude)
{
if (abs(IMUheight - navigate_get_goal(NULL)) < ((int16_t)altit.HeightMargin))
{
next_waypoint();
}
}
else
{
if (tofinish_line < WAYPOINT_PROXIMITY_RADIUS) // crossed the finish line
{
if (desired_behavior._.loiter)
{
navigate_set_goal(GPSlocation, wp_to_relative(currentWaypointSet[waypointIndex]).loc);
}
else
{
next_waypoint();
}
}
}
}
// In the future, we could include more than 2 waypoint sets...
// flightplanNum is 0 for main waypoints, and 1 for RTL waypoints
//void init_flightplan(int16_t flightplanNum)
void flightplan_waypoints_begin(int16_t flightplanNum)
{
if (flightplanNum == 1) // RTL waypoint set
{
load_flightplan(rtlWaypoints, NUMBER_RTL_POINTS);
// currentWaypointSet = (struct waypointDef*)rtlWaypoints;
// numPointsInCurrentSet = NUMBER_RTL_POINTS;
}
else if (flightplanNum == 0) // Main waypoint set
{
load_flightplan(waypoints, NUMBER_POINTS);
// currentWaypointSet = (struct waypointDef*)waypoints;
// numPointsInCurrentSet = NUMBER_POINTS;
}
waypointIndex = 0;
current_waypoint = wp_to_relative(currentWaypointSet[0]);
navigate_set_goal(GPSlocation, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
setBehavior(current_waypoint.flags);
// udb_background_trigger(); // trigger navigation immediately
}
static void update_goal_from(struct relative3D old_goal)
{
struct relative3D new_goal;
#ifdef USE_EXTENDED_NAV
struct relative3D_32 old_goal_32, new_goal_32;
#endif
lastGoal.x = new_goal.x = (turtleLocations[PLANE].x._.W1);
lastGoal.y = new_goal.y = (turtleLocations[PLANE].y._.W1);
lastGoal.z = new_goal.z = turtleLocations[PLANE].z;
if (old_goal.x == new_goal.x && old_goal.y == new_goal.y)
{
old_goal.x = IMUlocationx._.W1;
old_goal.y = IMUlocationy._.W1;
old_goal.z = IMUlocationz._.W1;
}
#ifdef USE_EXTENDED_NAV
// TODO: RobD - review this change implemented to restore build, but not runtime tested
old_goal_32.x = old_goal.x;
old_goal_32.y = old_goal.y;
old_goal_32.z = old_goal.z;
new_goal_32.x = new_goal.x;
new_goal_32.y = new_goal.y;
new_goal_32.z = new_goal.z;
navigate_set_goal(old_goal_32, new_goal_32);
#else
navigate_set_goal(old_goal, new_goal);
#endif // USE_EXTENDED_NAV
new_goal.x = (turtleLocations[CAMERA].x._.W1);
new_goal.y = (turtleLocations[CAMERA].y._.W1);
new_goal.z = turtleLocations[CAMERA].z;
set_camera_view(new_goal);
}
void GL_draw_frame( void * vp )
{
static float rot;
static int t0;
float s;
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLineWidth(1.f);
set_camera_view();
if ( t0 == 0 ) {
init_VBO();
t0 = 1;
}
int now = get_milliseconds();
/*
while ( now - t0 > 20 ) {
rot += 1.0f;
t0 += 20;
if ( rot > 360.f )
rot -= 360.f;
if ( rot < 0.f )
rot += 360.f;
}
*/
// hack to get rotation timer right
const float p180 = 0.017453126f;
const float c180p = 57.295776f;
const float c2pi = 6.2831854f;
const int ms_per_frame = 20; // milliseconds per frame
const float rot_per_frame = 1.0f * p180;
/*
while ( now - t0 > ms_per_frame ) {
rot += 1.0f;
t0 += ms_per_frame;
} */
static int set = 0;
if ( t0 <= 0 )
t0 = 1;
if ( now - t0 > 0 )
{
int diff = now - t0;
/* the rotation is incremented 1 rot_per_frame each 1 ms_per_frame */
float newrot = rot + (rot_per_frame/(float)ms_per_frame) * ((float)diff);
if ( set < 20 )
core.printf( "hiccup > 2pi: before: %f, %f ", rot, sinf(rot) );
rot = newrot;
// catch it up
t0 = now;
if ( set < 20 )
core.printf( "after: %f, %f\n", rot, sinf( rot ) );
// clamp
if ( rot > c2pi ) {
rot = rot - c2pi;
set = 1; // no more print
core.printf( " --> MARK <-- \n" );
}
if ( set != 0 )
++set;
}
const float rotdeg = rot * c180p;
/// -- DRAW ---
// rotating wire spiral
glColor4ub( 255,255,255,255 );
glPushMatrix();
glRotatef( rotdeg, 0, 1, 0 );
draw_spiral( 24.0f, 10.f, 0.05f );
glPopMatrix();
// rotating circle
glEnable(GL_TEXTURE_2D);
glBindTexture( GL_TEXTURE_2D, core.materials.findByName( "jr_bob" )->img->texhandle );
glPushMatrix();
glTranslatef( 60.f, 60.f, -60.f );
glRotatef( rotdeg, 0, 1, 0 );
draw_circle( 0, 0, 10.f );
GL_TEX_SQUARE( -10.f, 30.f, 0.f, 20.f );
glRotatef( 180, 0, 1, 0 );
draw_circle( 0, 0, 10.f );
GL_TEX_SQUARE( -10.f, 30.f, 0.f, 20.f );
glBegin(GL_LINES); glVertex3f( 0, -30.f, 0 ); glVertex3f( 0, 30.f, 0 ); glEnd();
glPopMatrix();
glDisable( GL_TEXTURE_2D );
// FIXME : obviously move, later.
tessellatedPlane_t& tes = *(tessellatedPlane_t*)vp;
// heightmap triangles
glInterleavedArrays( GL_C3F_V3F, 0, tes.array );
glDrawArrays( GL_TRIANGLE_STRIP, 0, tes.num_verts );
//glDrawArrays( GL_LINE_STRIP, 0, tes->num_verts );
// gazillion boxes
core.drawer.drawLists();
// serpinski
glColor4ub( 255,255,0,255 );
glPushMatrix();
glTranslatef( 250.f, 100.f, -250.f );
glRotatef( rotdeg * 1.618f, 0, 1, 0 );
glRotatef( rotdeg , 0.707, 0, -0.707 );
glDisable( GL_CULL_FACE );
draw_serpinski( 3, 40.f );
glEnable( GL_CULL_FACE );
glPopMatrix();
glColor4ub( 255,255,255,255 );
// icosahedron
glPushMatrix();
glTranslatef( 500.f, 100.f, -250.f );
trans_spiral_f( rot, 50.f /*radius*/, 1.0f/*rot rate*/, 1.0f/*climb rate*/, 50.f /*ceiling*/ );
s = 14.f;
s = (sinf( rot * 2.718f ) + 1.1f) * 14.f;
glScalef( s, s, s );
glRotatef( rotdeg * 5.f, 0, 1, 0 );
glRotatef( rotdeg * 3.f , 0.707, 0, -0.707 );
glColor4ub( 230, 100, 0, 255 );
draw_icosahedron();
glColor4ub( 255, 255, 255, 255 );
draw_icosahedron_wire();
glPopMatrix();
// axis marker
float l = 100.f;
glColor3f( 1.f, 0.f, 1.f );
glLineWidth(2.f);
glBegin( GL_LINES );
glVertex3i( 0,0,0 );
glVertex3i( 0,l,0);
glVertex3i( 0,0,0 );
glVertex3i( l,0,0 );
glVertex3i( 0,0,0 );
glVertex3i( 0,0,-l );
glEnd();
// 4-sided
glPushMatrix();
glTranslatef( 300, 100, 0 );
s = 20.f;
glScalef( s, s, s );
glRotatef( rotdeg, 0, 1, 0 );
glColor4ub( 0, 255, 255, 128 );
draw_triangle4(0);
glColor4ub( 255, 255, 255, 255 );
draw_triangle4(1);
glPopMatrix();
// 4-sided, 2nd type
glPushMatrix();
glTranslatef( 340, 100, 0 );
s = 20.f;
glScalef( s, s, s );
glRotatef( rotdeg, 0, 1, 0 );
glColor4ub( 100, 0, 100, 128 );
draw_triangle4_2(0);
glColor4ub( 255, 255, 255, 255 );
draw_triangle4(1); // inner lines don't draw right, so use first form
glPopMatrix();
// 5-sided
glPushMatrix();
glTranslatef( 100, 100, -50 );
s = 20.f;
glScalef( s, s, s );
glRotatef( rotdeg, 1, 0, 0 );
glColor4ub( 100, 50, 0, 200 );
draw_triangle5(0);
glColor4ub( 255, 255, 255, 255 );
draw_triangle5(1);
glPopMatrix();
// unit-cube w/ tri
glPushMatrix();
glTranslatef( 150.f, 130.f, -800.f );
glTranslatef( 0.f, 0.f, sinf(rot)*1600.f );
s = 20.f;
glScalef( s, s, s );
glRotatef( 90, 1, 0, 0 );
glRotatef( sinf(rotdeg*0.03f)*180.f, 0, 1, 0 );
glColor4ub( 128,128,128,255 );
draw_unitcube(0);
glColor4ub( 255,255,255,255 );
draw_unitcube(1);
glTranslatef( 0, 1.f, 0.f );
glColor4ub( 128,128,128,255 );
draw_triangle5(0);
glColor4ub( 255,255,255,255 );
draw_triangle5(1);
glPopMatrix();
// test model
//glEnable(GL_LIGHTING);
glPushMatrix();
glTranslatef( 300.f, 75.f, 200.f );
glRotatef( 315.f, 0,1,0 );
glEnable(GL_LIGHT0);
s = 550.f;
glScalef( s, s, s );
glColor4ub( 255,255,255,255);
//draw_test_model_Vertex_Arrays();
draw_test_model_VBO();
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glColor4ub( 128,128,168,255 );
//draw_test_model_Vertex_Arrays();
draw_test_model_VBO();
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glPopMatrix();
glDisable(GL_LIGHTING);
glDisable(GL_LIGHT0);
glFlush();
SDL_GL_SwapBuffers();
}
