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