Comment on page

# Overview of the self-driving example code [PX4]

Inside the NXP Gazebo stack, there is a pre-determined function for self-driving code that gets called upon by the NXP Cup scheduled work item. That function is

`roverControl raceTrack(const pixy_vector_s &pixy)`

inside of `nxpcup_race.cpp`

.The

`pixy_vector_s`

data structure is outlined in a uORB topic definition within the PX4 firmware. Here is the contents of that uORB topic: uint64 timestamp

# Vector 0 head and tail points

uint8 m0_x0 # Tail of vector @ x

uint8 m0_y0 # Tail of vector @ y

uint8 m0_x1 # Head of vector @ x

uint8 m0_y1 # Head of vector @ y

# Vector 1 head and tail points

uint8 m1_x0 # Tail of vector @ x

uint8 m1_y0 # Tail of vector @ y

uint8 m1_x1 # Head of vector @ x

uint8 m1_y1 # Head of vector @ y

This data structure contains the head and tail points of two vectors from the simulated pixy camera as well as a timestamp. You will just need to use the

`uint8`

points from this data structure to write your self-driving code. In the example code provided, we create two

`Vector`

variables and store the data from the `pixy`

argument in the function. To do this we provide a function called `Vector copy_vectors(const pixy_vector_s &pixy, uint8_t num)`

to perform this copy. You can see this at lines 74 and 75:Vector vec1 = copy_vectors(pixy, 1);

Vector vec2 = copy_vectors(pixy, 2);

The Vector data structure is outlined in

`nxpcup_race.h`

and is as follows:struct Vector

{

void print()

{

char buf[64];

sprintf(buf, "vector: (%d %d) (%d %d)", m_x0, m_y0, m_x1, m_y1);

printf(buf);

printf("\n");

}

uint8_t m_x0;

uint8_t m_y0;

uint8_t m_x1;

uint8_t m_y1;

};

The data structure contains

`uint8_t`

variables for the head and tail points of a vector (or line segment). This is the same way that the real pixy camera presents line data, so your existing self-driving code should be compatible. The Vector struct also contains a print statement for debugging purposes.Once we have copied vector data into the

`Vector`

data structure, we initialize some variables such as `frameHeight`

, `frameWidth`

, and `window_center`

. These variables are for calculating steering angle, and we will go over that later in this guide.Next, we create a data structure with the type

`roverControl`

. This type is as follows:struct roverControl {

float steer;

float speed;

};

This structure is very simple - just speed and steer. You will use your self-driving algorithms to apply a torque and steering angle to the car using this structure. The

`nxpcup_work.cpp`

file uses this data to set an attitude setpoint within PX4. Some other variables are

`hrt_absolute_time`

, `time_diff`

, `first_call`

, and `num_vectors`

. Descriptions of these variables are below:`hrt_absolute_time`

is the absolute timer value within PX4. This is used for stopping the car when no vectors are found.`time_diff`

is also used for stopping the car when no vectors are found.`first_call`

is used when no vectors are found to start the timer.`num_vectors`

is used to determine what algorithm to use. A function called `uint8t get_num_vectors(Vector &vec1, Vector &vec2)`

returns the number of vectors that the simulated Pixy camera is publishing. Vector data defaults to 0s if no vector data is found, and the vector data is filled in sequentially as vectors are found. The switch statement within

`nxpcup_race.cpp`

performs different instructions depending on how many vectors are found. If no vectors are found (case 0), then a timer will start and tell the vehicle to stop after 10000 usec.

If two vectors are found, then the example algorithm will find the offset in the x direction of the average of the two head points of each vector. This will give us a steering value that will steer the cup car in the correct direction. This value is stored in

`control.steer`

. The speed value will get set to `SPEED_FAST`

as configured in `nxpcup_race.h`

. This value is stored in `control.speed`

.If one vector is found, the case will switch to default. The default case finds the gradient of the vector and stores that in

`control.steer`

, and the `control.speed`

value will be set to `SPEED_NORMAL`

. Finally, the

`control`

variable will be returned for use in `nxpcup_work.cpp`

.