#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
);
}