Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 2104 → Rev 2105

/RaspberryPi/ExPlat/processing.c
0,0 → 1,374
#include "main.h"
#include "processing.h"
#include "flightcontrol.h"
 
int tmpcount = 0;
 
//>> Pointers for converting Bytes into other Datatypes
//------------------------------------------------------------------------------------------------------
u8 *flow_x_ptr = (u8 *) &optical_flow_values.flow_comp_m_x;
u8 *flow_y_ptr = (u8 *) &optical_flow_values.flow_comp_m_y;
u8 *distance_ptr = (u8 *) &optical_flow_values.ground_distance;
 
u8 *integrated_x_ptr = (u8 *) &optical_flow_rad.integrated_x;
u8 *integrated_y_ptr = (u8 *) &optical_flow_rad.integrated_y;
u8 *integrated_xgyro_ptr = (u8 *) &optical_flow_rad.integrated_xgyro;
u8 *integrated_ygyro_ptr = (u8 *) &optical_flow_rad.integrated_ygyro;
u8 *integrated_zgyro_ptr = (u8 *) &optical_flow_rad.integrated_zgyro;
u8 *distance_rad_ptr = (u8 *) &optical_flow_rad.distance;
 
u8 *gyroz_ptr = (u8 *) &optical_flow_values.gyroz;
 
//>> Convert incoming Flow Array to Datatypes
//------------------------------------------------------------------------------------------------------
void process_flow_data(serial_data_struct* pdata_package_flow_process){
if(pdata_package_flow_process->msg_id == 100){
for (int i = 0; i < 8; ++i)
{
optical_flow_values.time_usec = (optical_flow_values.time_usec<<8) + pdata_package_flow_process->collecting_data[13-i];
}
for (int i = 0; i < 4; ++i)
{
flow_x_ptr[i] = pdata_package_flow_process->collecting_data[14+i];
flow_y_ptr[i] = pdata_package_flow_process->collecting_data[18+i];
distance_ptr[i] = pdata_package_flow_process->collecting_data[22+i];
}
optical_flow_values.flow_x = (optical_flow_values.flow_x<<8) + pdata_package_flow_process->collecting_data[27];
optical_flow_values.flow_x = (optical_flow_values.flow_x<<8) + pdata_package_flow_process->collecting_data[26];
optical_flow_values.flow_y = (optical_flow_values.flow_y<<8) + pdata_package_flow_process->collecting_data[29];
optical_flow_values.flow_y = (optical_flow_values.flow_y<<8) + pdata_package_flow_process->collecting_data[28];
optical_flow_values.sensor_id = pdata_package_flow_process->collecting_data[30];
optical_flow_values.quality = pdata_package_flow_process->collecting_data[31];
}else if(pdata_package_flow_process->msg_id == 106){
for (int i = 0; i < 8; ++i)
{
optical_flow_rad.time_usec = (optical_flow_rad.time_usec<<8) + pdata_package_flow_process->collecting_data[13-i];
}
for (int i = 0; i < 4; ++i)
{
optical_flow_rad.integration_time_us = (optical_flow_rad.integration_time_us<<8) + pdata_package_flow_process->collecting_data[17-i];
integrated_x_ptr[i] = pdata_package_flow_process->collecting_data[18+i];
integrated_y_ptr[i] = pdata_package_flow_process->collecting_data[22+i];
integrated_xgyro_ptr[i] = pdata_package_flow_process->collecting_data[26+i];
integrated_ygyro_ptr[i] = pdata_package_flow_process->collecting_data[30+i];
integrated_zgyro_ptr[i] = pdata_package_flow_process->collecting_data[34+i];
optical_flow_rad.time_delta_distance_us = (optical_flow_rad.time_delta_distance_us<<8) + pdata_package_flow_process->collecting_data[41-i];
distance_rad_ptr[i] = pdata_package_flow_process->collecting_data[42+i];
}
for (int i = 0; i < 2; ++i)
{
optical_flow_rad.temperature = (optical_flow_rad.temperature<<8) + pdata_package_flow_process->collecting_data[47-i];
}
optical_flow_rad.sensor_id = pdata_package_flow_process->collecting_data[48];
optical_flow_rad.quality = pdata_package_flow_process->collecting_data[49];
}else if(pdata_package_flow_process->msg_id == 250){
for (int i = 0; i < 4; ++i)
{
gyroz_ptr[i] = pdata_package_flow_process->collecting_data[18+i];
}
}
}
 
//>> Process NMEA Data from GPS Modul
//------------------------------------------------------------------------------------------------------
char nmeaMsg[5];
char nmeaMsgValue[20];
void process_gps_data(serial_data_struct* pdata_package_gps_process){
if (pdata_package_gps_process->position_package > 0)
{
for (int i = 1; i < 6; ++i){nmeaMsg[i-1] = pdata_package_gps_process->gps_data[0][i];}
}
for (int i = 0; i < sizeof(pdata_package_gps_process->gps_data[pdata_package_gps_process->position_package]); ++i)
{
nmeaMsgValue[i] = pdata_package_gps_process->gps_data[pdata_package_gps_process->position_package][i];
}
if(!strcmp(nmeaMsg, "GLGSV")) //GLONASS (elevation, azimuth, CNR)
{
 
}else if (!strcmp(nmeaMsg, "GNGLL")) //Position, Time, Fix
{
}else if (!strcmp(nmeaMsg, "GNRMC")) //Time, Date, Position, Course, Speed
{
gps_value_struct.latitude = strtof(nmeaMsgValue, NULL);
printf("%d\n", sizeof(pdata_package_gps_process->gps_data[0]));
}else if (!strcmp(nmeaMsg, "GNVTG")) //Course, Speed relative to ground
{
}else if (!strcmp(nmeaMsg, "GNGGA")) //Time, Position, Fix
{
}else if (!strcmp(nmeaMsg, "GNGSA")) //Satelite ID
{
}else if (!strcmp(nmeaMsg, "GPGSV")) //GPS (elevation, azimuth, CNR)
{
 
}
}
 
 
//>> Add Sensor Lables to the serial data stream to appear in the MikroKopter Tool
//------------------------------------------------------------------------------------------------------
void add_sensor_lables(serial_data_struct* pdata_package_add_lables){
u8 lable[17] = "RaspPi Sensor ";
switch(pdata_package_add_lables->data[0])
{
case 16:
for (int u = 1; u < 17; ++u)
{
pdata_package_add_lables->data[u] = lable[u-1];
}
pdata_package_add_lables->data[14] = 49; //Number of the Lable (RaspPi Sensor1)
break;
case 17:
for (int u = 1; u < 17; ++u)
{
pdata_package_add_lables->data[u] = lable[u-1];
}
pdata_package_add_lables->data[14] = 50; //(RaspPi Sensor2)
break;
case 18:
for (int u = 1; u < 17; ++u)
{
pdata_package_add_lables->data[u] = lable[u-1];
}
pdata_package_add_lables->data[14] = 51; //(RaspPi Sensor3)
break;
case 19:
for (int u = 1; u < 17; ++u)
{
pdata_package_add_lables->data[u] = lable[u-1];
}
pdata_package_add_lables->data[14] = 52; //(RaspPi Sensor4)
break;
}
}
 
//>> Add Sensor Data to the serial data stream to appear in the MikroKopter Tool
//------------------------------------------------------------------------------------------------------
void add_sensor_data(serial_data_struct* pdata_package_add_data){
//pdata_package_add_data->data[34] = optical_flow_values.gyroz;
/*
pdata_package_add_data->data[36] = 11;
pdata_package_add_data->data[38] = Distance;
pdata_package_add_data->data[40] = 11;
*/
 
int tmpdata = (pdata_package_add_data->data[14] + 255*pdata_package_add_data->data[15]) -721;
flight_data.AccZ = tmpdata;
printf("%d\n", flight_data.AccZ);
}
 
//>> Get the distance from the ultrasonic sensor module
//------------------------------------------------------------------------------------------------------
u16 average1 = 0;
u8 average2 = 0;
u16 distance_value;
void get_distance(){
distance_value = calculate_distance();
if(distance_value < 300 && distance_value > 0){
average1 += distance_value;
average2++;
if(average2 == 4){
average1 /= 4;
Distance = average1;
printf("DISTANZ: %d\n", Distance);
average1 = 0;
average2 = 0;
}
}
}
 
//>> This Function reads the String containing the Configuration
//------------------------------------------------------------------------------------------------------
void read_configuration(serial_data_struct* pdata_package_config){
config_data.channel_gas = pdata_package_config->data[4];
config_data.channel_gier = pdata_package_config->data[5];
config_data.channel_nick = pdata_package_config->data[2];
config_data.channel_roll = pdata_package_config->data[3];
config_data.channel_heightControl = pdata_package_config->data[17];
config_data.channel_autostart = pdata_package_config->data[114];
config_data.channel_gps = pdata_package_config->data[82];
config_data.channel_carefree = pdata_package_config->data[101];
config_data.dynamicPositionHold = pdata_package_config->data[123];
u8 *tmpData = (unsigned char *) &config_data;
for (int i = 0; i < 9; ++i)
{
printf("%d \n", tmpData[i]);
}
}
 
//>> Assign FlightCtrl Values
//------------------------------------------------------------------------------------------------------
void assign_flightcontrol_values(serial_data_struct* pdata_package_assign){
fc_values.value_gas = pdata_package_assign->data[config_data.channel_gas*2];
fc_values.value_gier = pdata_package_assign->data[config_data.channel_gier*2];
fc_values.value_nick = pdata_package_assign->data[config_data.channel_nick*2];
fc_values.value_roll = pdata_package_assign->data[config_data.channel_roll*2];
fc_values.value_heightControl = pdata_package_assign->data[config_data.channel_heightControl*2];
fc_values.value_autostart = pdata_package_assign->data[config_data.channel_autostart*2];
fc_values.value_gps = pdata_package_assign->data[config_data.channel_gps*2];
fc_values.value_carefree = pdata_package_assign->data[(config_data.channel_carefree+1)*2];
fc_values.value_dynamicPositionHold = pdata_package_assign->data[config_data.dynamicPositionHold*2];
 
u8 *tmpData = (unsigned char *) &fc_values;
for (int i = 0; i < 9; ++i)
{
printf("%d \n", tmpData[i]);
}
}
 
//>> Assign 3D Data from NaviCtrl
//------------------------------------------------------------------------------------------------------
void assign_3d_data_NC(serial_data_struct* pdata_package_flight_data){
flight_data.StickNick = pdata_package_flight_data->data[6];
flight_data.StickRoll = pdata_package_flight_data->data[7];
flight_data.StickYaw = pdata_package_flight_data->data[8];
flight_data.StickGas = pdata_package_flight_data->data[9];
 
printf("%d\n", flight_data.StickGas);
}
 
//>> Assign 3D Data from FlightCtrl
//------------------------------------------------------------------------------------------------------
void assign_3d_data_FC(serial_data_struct* pdata_package_flight_data){
flight_data.AccZ = pdata_package_flight_data->data[7];
}
 
//>> Transmit Requests or Commands to the kopter
//------------------------------------------------------------------------------------------------------
serial_data_struct data_package_requests;
void request_to_kopter(u8 request){
switch(request)
{
case 1: //Get Configuration
data_package_requests.data[0] = 3; //Number of configuration-Set (Parametersatz);
data_package_requests.data[1] = 217;
data_package_requests.data[2] = 99;
create_serial_frame(1, 'q', 3, &data_package_requests);
transmit_data(TO_KOPTER, &data_package_requests);
memset(data_package_requests.txrxdata, 0, 1023);
memset(data_package_requests.data, 0, 1023);
break;
case 2: //Request Flightcontrol Values
create_serial_frame(1, 'p', 0, &data_package_requests);
transmit_data(TO_KOPTER, &data_package_requests);
memset(data_package_requests.txrxdata, 0, 1023);
memset(data_package_requests.data, 0, 1023);
break;
case 3:
data_package_requests.data[0] = 0;
data_package_requests.data[1] = 246;
data_package_requests.data[2] = 18;
create_serial_frame(0, 'a', 3, &data_package_requests);
transmit_data(TO_KOPTER, &data_package_requests);
memset(data_package_requests.txrxdata, 0, 1023);
memset(data_package_requests.data, 0, 1023);
break;
 
case 4: //Switch to flightcontrol
data_package_requests.data[0] = 0;
data_package_requests.data[1] = 236;
data_package_requests.data[2] = 91;
create_serial_frame(2, 'u', 3, &data_package_requests);
transmit_data(TO_KOPTER, &data_package_requests);
memset(data_package_requests.txrxdata, 0, 1023);
memset(data_package_requests.data, 0, 1023);
break;
case 5: //Request 3D Data
data_package_requests.data[0] = 1;
create_serial_frame(2, 'c', 1, &data_package_requests);
transmit_data(TO_KOPTER, &data_package_requests);
memset(data_package_requests.txrxdata, 0, 1023);
memset(data_package_requests.data, 0, 1023);
break;
case 6: //Request Analog Values (FlightControl)
data_package_requests.data[0] = 1;
create_serial_frame(1, 'd', 1, &data_package_requests);
transmit_data(TO_KOPTER, &data_package_requests);
memset(data_package_requests.txrxdata, 0, 1023);
memset(data_package_requests.data, 0, 1023);
break;
}
 
}
 
//>> Analyze the data package
//------------------------------------------------------------------------------------------------------
void analyze_data_package(serial_data_struct* pdata_package_analyze){
switch(pdata_package_analyze->collecting_data[1] - 'a')
{
case 2: //NaviCtrl
switch(pdata_package_analyze->collecting_data[2])
{
case 'A': //Get Sensor lables
collect_serial_frame(pdata_package_analyze);
if(pdata_package_analyze->data[0] > 15 && pdata_package_analyze->data[0] < 20){ //Address for Sensorlables
add_sensor_lables(pdata_package_analyze);
create_serial_frame(2, 'A', 18, pdata_package_analyze);
}
break;
case 'D': //Get Analog Values
collect_serial_frame(pdata_package_analyze);
add_sensor_data(pdata_package_analyze);
create_serial_frame(2, 'D', 66, pdata_package_analyze);
break;
case 'C': //Get 3D Data
collect_serial_frame(pdata_package_analyze);
assign_3d_data_NC(pdata_package_analyze);
break;
}
break;
case 1: //FlightCtrl
switch(pdata_package_analyze->collecting_data[2])
{
case 'A': //Get Sensor lables
collect_serial_frame(pdata_package_analyze);
if(pdata_package_analyze->data[0] > 15 && pdata_package_analyze->data[0] < 20){ //Address for Sensorlables
add_sensor_lables(pdata_package_analyze);
create_serial_frame(1, 'A', 18, pdata_package_analyze);
}
break;
case 'D': //Get Analog Values
collect_serial_frame(pdata_package_analyze);
add_sensor_data(pdata_package_analyze);
//create_serial_frame(1, 'D', 66, pdata_package_analyze);
break;
case 'Q': //Get Configuration
collect_serial_frame(pdata_package_analyze);
//pdata_package_analyze->cmdLength = 0; //Do not send Configuration back to the MikroKopter Tool, it gives an Error. It is only needed for this program
read_configuration(pdata_package_analyze);
break;
case 'P': //Get FlightCtrl Values
collect_serial_frame(pdata_package_analyze);
//pdata_package_analyze->cmdLength = 0;
assign_flightcontrol_values(pdata_package_analyze);
break;
case 'C': //Get 3D Data
collect_serial_frame(pdata_package_analyze);
assign_3d_data_FC(pdata_package_analyze);
break;
}
case 0:
switch(pdata_package_analyze->collecting_data[2])
{
case 'a':
collect_serial_frame(pdata_package_analyze);
break;
case 'h':
collect_serial_frame(pdata_package_analyze);
break;
}
 
}
memset(pdata_package_analyze->collecting_data, 0, 1023);
}
 
void processing_init(){
request_to_kopter(SWITCH_FLIGHTCONTROL);
}