static void gimbalControlTask(void *parameters)
{
// Loop forever
while (1) {
struct pios_can_gimbal_message bgc_message;
// Wait for queue message
if (PIOS_Queue_Receive(queue, &bgc_message, 5) == true) {
CameraDesiredData cameraDesired;
CameraDesiredGet(&cameraDesired);
cameraDesired.Declination = bgc_message.setpoint_pitch;
cameraDesired.Bearing = bgc_message.setpoint_yaw;
cameraDesired.Roll = bgc_message.fc_roll;
cameraDesired.Pitch = bgc_message.fc_pitch;
cameraDesired.Yaw = bgc_message.fc_yaw;
CameraDesiredSet(&cameraDesired);
}
}
}
/**
* Pump the some information over UDP to a visualization
*/
static void VisualizationTask(void *parameters)
{
int s;
struct sockaddr_in server;
s = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
memset(&server,0,sizeof(server));
server.sin_family = AF_INET;
server.sin_addr.s_addr = inet_addr("127.0.0.1");
server.sin_port = htons(3000);
inet_aton("127.0.0.1", &server.sin_addr);
struct uav_data uav_data;
AttitudeSimulatedData simData;
CameraDesiredData camera;
while (1) {
AttitudeSimulatedGet(&simData);
CameraDesiredGet(&camera);
uav_data.q[0] = simData.q1;
uav_data.q[1] = simData.q2;
uav_data.q[2] = simData.q3;
uav_data.q[3] = simData.q4;
uav_data.NED[0] = simData.Position[0];
uav_data.NED[1] = simData.Position[1];
uav_data.NED[2] = simData.Position[2];
uav_data.camera_roll = camera.Roll * 30;
uav_data.camera_pitch = camera.Pitch * 45;
sendto(s, (struct sockaddr *) &uav_data, sizeof(uav_data), 0, (struct sockaddr *) &server, sizeof(server));
usleep(100000);
vTaskDelay(100);
}
}
