Skip to content

Commit

Permalink
Added basic ref yaw computation
Browse files Browse the repository at this point in the history
  • Loading branch information
Mattia Fussi committed Jan 15, 2020
1 parent 3b6c39b commit e2081f8
Show file tree
Hide file tree
Showing 5 changed files with 27 additions and 18 deletions.
6 changes: 3 additions & 3 deletions graphics.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -328,18 +328,18 @@ int red = makecol(255, 0, 0);

}

void update_plot(BITMAP* bmp, double* data, int coord_x, int coord_y)
void update_plot(BITMAP* bmp, double* data, int coord_x, int coord_y, int scale)
{
int x = coord_x - (PLT_STEP * PLT_DATA_SIZE);
int y = coord_y - (int)(data[0] * PLT_SCALE_Y) - PLT_FRAME_SIZE / 2;
int y = coord_y - (int)(data[0] * scale) - PLT_FRAME_SIZE / 2;
int x_prev = x;
int y_prev = y;

for(int i = 0; i < PLT_DATA_SIZE; i++)
{
x = x + PLT_STEP;

y = coord_y - (int)(data[i] * PLT_SCALE_Y) - PLT_FRAME_SIZE / 2;
y = coord_y - (int)(data[i] * scale) - PLT_FRAME_SIZE / 2;

if (y < (coord_y - PLT_FRAME_SIZE))
y = (coord_y - PLT_FRAME_SIZE);
Expand Down
2 changes: 1 addition & 1 deletion graphics.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ int gen_obstacles(Obstacle* arr_obstacles, int n_obs);

void draw_obstacles(BITMAP* bmp, Obstacle* obs, int n_obs, int col);

void update_plot(BITMAP* bmp, double* data, int coord_x, int coord_y);
void update_plot(BITMAP* bmp, double* data, int coord_x, int coord_y, int scale);

void draw_quad(BITMAP* bmp, BITMAP* quad, BITMAP* bg, double* old, double* new);

Expand Down
26 changes: 15 additions & 11 deletions main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -301,8 +301,12 @@ double dist = 0.0;

if(start_sim)
{
printf("LQR Controller task started\n");
compute_setpoint(setpoint, waypoints, altitude_sp, waypoints_num, waypoint_flags);
printf("LQR Controller task started\n");

pthread_mutex_lock (&mux_state);
state_view = gsl_vector_view_array(arr_state, SIZE_X);
compute_setpoint(setpoint, waypoints, altitude_sp, &state_view.vector, waypoints_num, waypoint_flags);
pthread_mutex_unlock (&mux_state);
}

do{
Expand All @@ -317,7 +321,7 @@ double dist = 0.0;
printf("Waypoint %d reached\n", waypoint_idx);
waypoint_flags[waypoint_idx] = 1;
waypoint_idx++;
compute_setpoint(setpoint, waypoints, altitude_sp, waypoints_num, waypoint_flags);
compute_setpoint(setpoint, waypoints, altitude_sp, &state_view.vector, waypoints_num, waypoint_flags);
}

state_view = gsl_vector_view_array(arr_state, SIZE_X);
Expand Down Expand Up @@ -416,8 +420,8 @@ double rep_force_ampli = 0.0;
//printf("---\n");

pthread_mutex_lock(&mux_gfx);
draw_laser_points(buffer_gfx, old_traces, laser_traces, old_pose, pose);
//draw_laser_traces(buffer_gfx, old_traces, laser_traces, old_pose, pose);
//draw_laser_points(buffer_gfx, old_traces, laser_traces, old_pose, pose);
draw_laser_traces(buffer_gfx, old_traces, laser_traces, old_pose, pose);
pthread_mutex_unlock(&mux_gfx);

if (deadline_miss (tp))
Expand Down Expand Up @@ -622,13 +626,13 @@ double new_pose[SIZE_Y] = {0.0};
rectfill(buffer_gfx, 580 + 1, 390 + 1, 680 - 1, 490 - 1, makecol(0,0,0));
rectfill(buffer_gfx, 580 + 1, 495 + 1, 680 - 1, 595 - 1, makecol(0,0,0));

update_plot(buffer_gfx, plt_buf_Roll, PLT_XPOS_XCOORD - 115, PLT_XPOS_YCOORD);
update_plot(buffer_gfx, plt_buf_Pitch, PLT_YPOS_XCOORD - 115, PLT_YPOS_YCOORD);
update_plot(buffer_gfx, plt_buf_Yaw, PLT_ZPOS_XCOORD - 115, PLT_ZPOS_YCOORD);
update_plot(buffer_gfx, plt_buf_Roll, PLT_XPOS_XCOORD - 115, PLT_XPOS_YCOORD, 25);
update_plot(buffer_gfx, plt_buf_Pitch, PLT_YPOS_XCOORD - 115, PLT_YPOS_YCOORD, 25);
update_plot(buffer_gfx, plt_buf_Yaw, PLT_ZPOS_XCOORD - 115, PLT_ZPOS_YCOORD, 10);

update_plot(buffer_gfx, plt_buf_X, PLT_XPOS_XCOORD, PLT_XPOS_YCOORD);
update_plot(buffer_gfx, plt_buf_Y, PLT_YPOS_XCOORD, PLT_YPOS_YCOORD);
update_plot(buffer_gfx, plt_buf_Z, PLT_ZPOS_XCOORD, PLT_ZPOS_YCOORD);
update_plot(buffer_gfx, plt_buf_X, PLT_XPOS_XCOORD, PLT_XPOS_YCOORD, PLT_SCALE_Y);
update_plot(buffer_gfx, plt_buf_Y, PLT_YPOS_XCOORD, PLT_YPOS_YCOORD, PLT_SCALE_Y);
update_plot(buffer_gfx, plt_buf_Z, PLT_ZPOS_XCOORD, PLT_ZPOS_YCOORD, PLT_SCALE_Y);

pthread_mutex_unlock(&mux_gfx);

Expand Down
9 changes: 7 additions & 2 deletions model.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,13 @@ double compute_pos_dist(Vector* v1, Vector* v2)
* ---------------------------
*
*/
void compute_setpoint(Vector* sp, WPoint* wp, double alt, int wp_size, int* wp_flags)
void compute_setpoint(Vector* sp, WPoint* wp, double alt, Vector* pose, int wp_size, int* wp_flags)
{

WPoint xy_setpoint;
double yaw_ref = 0.0;
double x = gsl_vector_get(pose, 3);
double y = gsl_vector_get(pose, 4);

if (wp_size >= MAX_WPOINTS) return;

Expand All @@ -54,12 +57,14 @@ WPoint xy_setpoint;
break;
}
}

yaw_ref = atan2(xy_setpoint.y - y, xy_setpoint.x - x);

gsl_vector_set(sp, 2, yaw_ref);
gsl_vector_set(sp, 3, xy_setpoint.x);
gsl_vector_set(sp, 4, xy_setpoint.y);
gsl_vector_set(sp, 5, alt);

//TODO compute reference yaw

}

Expand Down
2 changes: 1 addition & 1 deletion model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ typedef gsl_matrix Matrix;

void quad_linear_model(Vector *u, Matrix *A, Matrix *B, Vector *x);

void compute_setpoint(Vector* sp, WPoint* wp, double alt, int wp_size, int* wp_flags);
void compute_setpoint(Vector* sp, WPoint* wp, double alt, Vector* pose, int wp_size, int* wp_flags);

void dlqr_control(Vector* sp, Vector* x, Matrix* K, Vector* u);

Expand Down

0 comments on commit e2081f8

Please sign in to comment.