Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 2105 | - | 1 | #include "main.h" |
| 2 | #include "processing.h" |
||
| 3 | #include "flightcontrol.h" |
||
| 4 | |||
| 5 | int tmpcount = 0; |
||
| 6 | |||
| 7 | //>> Pointers for converting Bytes into other Datatypes |
||
| 8 | //------------------------------------------------------------------------------------------------------ |
||
| 9 | u8 *flow_x_ptr = (u8 *) &optical_flow_values.flow_comp_m_x; |
||
| 10 | u8 *flow_y_ptr = (u8 *) &optical_flow_values.flow_comp_m_y; |
||
| 11 | u8 *distance_ptr = (u8 *) &optical_flow_values.ground_distance; |
||
| 12 | |||
| 13 | u8 *integrated_x_ptr = (u8 *) &optical_flow_rad.integrated_x; |
||
| 14 | u8 *integrated_y_ptr = (u8 *) &optical_flow_rad.integrated_y; |
||
| 15 | u8 *integrated_xgyro_ptr = (u8 *) &optical_flow_rad.integrated_xgyro; |
||
| 16 | u8 *integrated_ygyro_ptr = (u8 *) &optical_flow_rad.integrated_ygyro; |
||
| 17 | u8 *integrated_zgyro_ptr = (u8 *) &optical_flow_rad.integrated_zgyro; |
||
| 18 | u8 *distance_rad_ptr = (u8 *) &optical_flow_rad.distance; |
||
| 19 | |||
| 20 | u8 *gyroz_ptr = (u8 *) &optical_flow_values.gyroz; |
||
| 21 | |||
| 22 | //>> Convert incoming Flow Array to Datatypes |
||
| 23 | //------------------------------------------------------------------------------------------------------ |
||
| 24 | void process_flow_data(serial_data_struct* pdata_package_flow_process){ |
||
| 25 | if(pdata_package_flow_process->msg_id == 100){ |
||
| 26 | for (int i = 0; i < 8; ++i) |
||
| 27 | { |
||
| 28 | optical_flow_values.time_usec = (optical_flow_values.time_usec<<8) + pdata_package_flow_process->collecting_data[13-i]; |
||
| 29 | } |
||
| 30 | for (int i = 0; i < 4; ++i) |
||
| 31 | { |
||
| 32 | flow_x_ptr[i] = pdata_package_flow_process->collecting_data[14+i]; |
||
| 33 | flow_y_ptr[i] = pdata_package_flow_process->collecting_data[18+i]; |
||
| 34 | distance_ptr[i] = pdata_package_flow_process->collecting_data[22+i]; |
||
| 35 | } |
||
| 36 | optical_flow_values.flow_x = (optical_flow_values.flow_x<<8) + pdata_package_flow_process->collecting_data[27]; |
||
| 37 | optical_flow_values.flow_x = (optical_flow_values.flow_x<<8) + pdata_package_flow_process->collecting_data[26]; |
||
| 38 | optical_flow_values.flow_y = (optical_flow_values.flow_y<<8) + pdata_package_flow_process->collecting_data[29]; |
||
| 39 | optical_flow_values.flow_y = (optical_flow_values.flow_y<<8) + pdata_package_flow_process->collecting_data[28]; |
||
| 40 | optical_flow_values.sensor_id = pdata_package_flow_process->collecting_data[30]; |
||
| 41 | optical_flow_values.quality = pdata_package_flow_process->collecting_data[31]; |
||
| 42 | }else if(pdata_package_flow_process->msg_id == 106){ |
||
| 43 | for (int i = 0; i < 8; ++i) |
||
| 44 | { |
||
| 45 | optical_flow_rad.time_usec = (optical_flow_rad.time_usec<<8) + pdata_package_flow_process->collecting_data[13-i]; |
||
| 46 | } |
||
| 47 | for (int i = 0; i < 4; ++i) |
||
| 48 | { |
||
| 49 | optical_flow_rad.integration_time_us = (optical_flow_rad.integration_time_us<<8) + pdata_package_flow_process->collecting_data[17-i]; |
||
| 50 | integrated_x_ptr[i] = pdata_package_flow_process->collecting_data[18+i]; |
||
| 51 | integrated_y_ptr[i] = pdata_package_flow_process->collecting_data[22+i]; |
||
| 52 | integrated_xgyro_ptr[i] = pdata_package_flow_process->collecting_data[26+i]; |
||
| 53 | integrated_ygyro_ptr[i] = pdata_package_flow_process->collecting_data[30+i]; |
||
| 54 | integrated_zgyro_ptr[i] = pdata_package_flow_process->collecting_data[34+i]; |
||
| 55 | optical_flow_rad.time_delta_distance_us = (optical_flow_rad.time_delta_distance_us<<8) + pdata_package_flow_process->collecting_data[41-i]; |
||
| 56 | distance_rad_ptr[i] = pdata_package_flow_process->collecting_data[42+i]; |
||
| 57 | } |
||
| 58 | for (int i = 0; i < 2; ++i) |
||
| 59 | { |
||
| 60 | optical_flow_rad.temperature = (optical_flow_rad.temperature<<8) + pdata_package_flow_process->collecting_data[47-i]; |
||
| 61 | } |
||
| 62 | optical_flow_rad.sensor_id = pdata_package_flow_process->collecting_data[48]; |
||
| 63 | optical_flow_rad.quality = pdata_package_flow_process->collecting_data[49]; |
||
| 64 | }else if(pdata_package_flow_process->msg_id == 250){ |
||
| 65 | for (int i = 0; i < 4; ++i) |
||
| 66 | { |
||
| 67 | gyroz_ptr[i] = pdata_package_flow_process->collecting_data[18+i]; |
||
| 68 | } |
||
| 69 | } |
||
| 70 | } |
||
| 71 | |||
| 72 | //>> Process NMEA Data from GPS Modul |
||
| 73 | //------------------------------------------------------------------------------------------------------ |
||
| 74 | char nmeaMsg[5]; |
||
| 75 | char nmeaMsgValue[20]; |
||
| 76 | void process_gps_data(serial_data_struct* pdata_package_gps_process){ |
||
| 77 | if (pdata_package_gps_process->position_package > 0) |
||
| 78 | { |
||
| 79 | for (int i = 1; i < 6; ++i){nmeaMsg[i-1] = pdata_package_gps_process->gps_data[0][i];} |
||
| 80 | } |
||
| 81 | for (int i = 0; i < sizeof(pdata_package_gps_process->gps_data[pdata_package_gps_process->position_package]); ++i) |
||
| 82 | { |
||
| 83 | nmeaMsgValue[i] = pdata_package_gps_process->gps_data[pdata_package_gps_process->position_package][i]; |
||
| 84 | } |
||
| 85 | if(!strcmp(nmeaMsg, "GLGSV")) //GLONASS (elevation, azimuth, CNR) |
||
| 86 | { |
||
| 87 | |||
| 88 | }else if (!strcmp(nmeaMsg, "GNGLL")) //Position, Time, Fix |
||
| 89 | { |
||
| 90 | }else if (!strcmp(nmeaMsg, "GNRMC")) //Time, Date, Position, Course, Speed |
||
| 91 | { |
||
| 92 | gps_value_struct.latitude = strtof(nmeaMsgValue, NULL); |
||
| 93 | printf("%d\n", sizeof(pdata_package_gps_process->gps_data[0])); |
||
| 94 | }else if (!strcmp(nmeaMsg, "GNVTG")) //Course, Speed relative to ground |
||
| 95 | { |
||
| 96 | |||
| 97 | }else if (!strcmp(nmeaMsg, "GNGGA")) //Time, Position, Fix |
||
| 98 | { |
||
| 99 | }else if (!strcmp(nmeaMsg, "GNGSA")) //Satelite ID |
||
| 100 | { |
||
| 101 | |||
| 102 | }else if (!strcmp(nmeaMsg, "GPGSV")) //GPS (elevation, azimuth, CNR) |
||
| 103 | { |
||
| 104 | |||
| 105 | } |
||
| 106 | } |
||
| 107 | |||
| 108 | |||
| 109 | //>> Add Sensor Lables to the serial data stream to appear in the MikroKopter Tool |
||
| 110 | //------------------------------------------------------------------------------------------------------ |
||
| 111 | void add_sensor_lables(serial_data_struct* pdata_package_add_lables){ |
||
| 112 | u8 lable[17] = "RaspPi Sensor "; |
||
| 113 | switch(pdata_package_add_lables->data[0]) |
||
| 114 | { |
||
| 115 | case 16: |
||
| 116 | for (int u = 1; u < 17; ++u) |
||
| 117 | { |
||
| 118 | pdata_package_add_lables->data[u] = lable[u-1]; |
||
| 119 | } |
||
| 120 | pdata_package_add_lables->data[14] = 49; //Number of the Lable (RaspPi Sensor1) |
||
| 121 | break; |
||
| 122 | case 17: |
||
| 123 | for (int u = 1; u < 17; ++u) |
||
| 124 | { |
||
| 125 | pdata_package_add_lables->data[u] = lable[u-1]; |
||
| 126 | } |
||
| 127 | pdata_package_add_lables->data[14] = 50; //(RaspPi Sensor2) |
||
| 128 | break; |
||
| 129 | case 18: |
||
| 130 | for (int u = 1; u < 17; ++u) |
||
| 131 | { |
||
| 132 | pdata_package_add_lables->data[u] = lable[u-1]; |
||
| 133 | } |
||
| 134 | pdata_package_add_lables->data[14] = 51; //(RaspPi Sensor3) |
||
| 135 | break; |
||
| 136 | case 19: |
||
| 137 | for (int u = 1; u < 17; ++u) |
||
| 138 | { |
||
| 139 | pdata_package_add_lables->data[u] = lable[u-1]; |
||
| 140 | } |
||
| 141 | pdata_package_add_lables->data[14] = 52; //(RaspPi Sensor4) |
||
| 142 | break; |
||
| 143 | } |
||
| 144 | } |
||
| 145 | |||
| 146 | //>> Add Sensor Data to the serial data stream to appear in the MikroKopter Tool |
||
| 147 | //------------------------------------------------------------------------------------------------------ |
||
| 148 | void add_sensor_data(serial_data_struct* pdata_package_add_data){ |
||
| 149 | |||
| 150 | //pdata_package_add_data->data[34] = optical_flow_values.gyroz; |
||
| 151 | /* |
||
| 152 | pdata_package_add_data->data[36] = 11; |
||
| 153 | pdata_package_add_data->data[38] = Distance; |
||
| 154 | pdata_package_add_data->data[40] = 11; |
||
| 155 | */ |
||
| 156 | |||
| 157 | int tmpdata = (pdata_package_add_data->data[14] + 255*pdata_package_add_data->data[15]) -721; |
||
| 158 | flight_data.AccZ = tmpdata; |
||
| 159 | printf("%d\n", flight_data.AccZ); |
||
| 160 | } |
||
| 161 | |||
| 162 | //>> Get the distance from the ultrasonic sensor module |
||
| 163 | //------------------------------------------------------------------------------------------------------ |
||
| 164 | u16 average1 = 0; |
||
| 165 | u8 average2 = 0; |
||
| 166 | u16 distance_value; |
||
| 167 | void get_distance(){ |
||
| 168 | distance_value = calculate_distance(); |
||
| 169 | if(distance_value < 300 && distance_value > 0){ |
||
| 170 | average1 += distance_value; |
||
| 171 | average2++; |
||
| 172 | if(average2 == 4){ |
||
| 173 | average1 /= 4; |
||
| 174 | Distance = average1; |
||
| 175 | printf("DISTANZ: %d\n", Distance); |
||
| 176 | average1 = 0; |
||
| 177 | average2 = 0; |
||
| 178 | } |
||
| 179 | } |
||
| 180 | } |
||
| 181 | |||
| 182 | //>> This Function reads the String containing the Configuration |
||
| 183 | //------------------------------------------------------------------------------------------------------ |
||
| 184 | void read_configuration(serial_data_struct* pdata_package_config){ |
||
| 185 | config_data.channel_gas = pdata_package_config->data[4]; |
||
| 186 | config_data.channel_gier = pdata_package_config->data[5]; |
||
| 187 | config_data.channel_nick = pdata_package_config->data[2]; |
||
| 188 | config_data.channel_roll = pdata_package_config->data[3]; |
||
| 189 | config_data.channel_heightControl = pdata_package_config->data[17]; |
||
| 190 | config_data.channel_autostart = pdata_package_config->data[114]; |
||
| 191 | config_data.channel_gps = pdata_package_config->data[82]; |
||
| 192 | config_data.channel_carefree = pdata_package_config->data[101]; |
||
| 193 | config_data.dynamicPositionHold = pdata_package_config->data[123]; |
||
| 194 | u8 *tmpData = (unsigned char *) &config_data; |
||
| 195 | for (int i = 0; i < 9; ++i) |
||
| 196 | { |
||
| 197 | printf("%d \n", tmpData[i]); |
||
| 198 | } |
||
| 199 | } |
||
| 200 | |||
| 201 | //>> Assign FlightCtrl Values |
||
| 202 | //------------------------------------------------------------------------------------------------------ |
||
| 203 | void assign_flightcontrol_values(serial_data_struct* pdata_package_assign){ |
||
| 204 | fc_values.value_gas = pdata_package_assign->data[config_data.channel_gas*2]; |
||
| 205 | fc_values.value_gier = pdata_package_assign->data[config_data.channel_gier*2]; |
||
| 206 | fc_values.value_nick = pdata_package_assign->data[config_data.channel_nick*2]; |
||
| 207 | fc_values.value_roll = pdata_package_assign->data[config_data.channel_roll*2]; |
||
| 208 | fc_values.value_heightControl = pdata_package_assign->data[config_data.channel_heightControl*2]; |
||
| 209 | fc_values.value_autostart = pdata_package_assign->data[config_data.channel_autostart*2]; |
||
| 210 | fc_values.value_gps = pdata_package_assign->data[config_data.channel_gps*2]; |
||
| 211 | fc_values.value_carefree = pdata_package_assign->data[(config_data.channel_carefree+1)*2]; |
||
| 212 | fc_values.value_dynamicPositionHold = pdata_package_assign->data[config_data.dynamicPositionHold*2]; |
||
| 213 | |||
| 214 | u8 *tmpData = (unsigned char *) &fc_values; |
||
| 215 | for (int i = 0; i < 9; ++i) |
||
| 216 | { |
||
| 217 | printf("%d \n", tmpData[i]); |
||
| 218 | } |
||
| 219 | } |
||
| 220 | |||
| 221 | //>> Assign 3D Data from NaviCtrl |
||
| 222 | //------------------------------------------------------------------------------------------------------ |
||
| 223 | void assign_3d_data_NC(serial_data_struct* pdata_package_flight_data){ |
||
| 224 | flight_data.StickNick = pdata_package_flight_data->data[6]; |
||
| 225 | flight_data.StickRoll = pdata_package_flight_data->data[7]; |
||
| 226 | flight_data.StickYaw = pdata_package_flight_data->data[8]; |
||
| 227 | flight_data.StickGas = pdata_package_flight_data->data[9]; |
||
| 228 | |||
| 229 | printf("%d\n", flight_data.StickGas); |
||
| 230 | } |
||
| 231 | |||
| 232 | //>> Assign 3D Data from FlightCtrl |
||
| 233 | //------------------------------------------------------------------------------------------------------ |
||
| 234 | void assign_3d_data_FC(serial_data_struct* pdata_package_flight_data){ |
||
| 235 | flight_data.AccZ = pdata_package_flight_data->data[7]; |
||
| 236 | } |
||
| 237 | |||
| 238 | //>> Transmit Requests or Commands to the kopter |
||
| 239 | //------------------------------------------------------------------------------------------------------ |
||
| 240 | serial_data_struct data_package_requests; |
||
| 241 | void request_to_kopter(u8 request){ |
||
| 242 | switch(request) |
||
| 243 | { |
||
| 244 | case 1: //Get Configuration |
||
| 245 | data_package_requests.data[0] = 3; //Number of configuration-Set (Parametersatz); |
||
| 246 | data_package_requests.data[1] = 217; |
||
| 247 | data_package_requests.data[2] = 99; |
||
| 248 | create_serial_frame(1, 'q', 3, &data_package_requests); |
||
| 249 | transmit_data(TO_KOPTER, &data_package_requests); |
||
| 250 | memset(data_package_requests.txrxdata, 0, 1023); |
||
| 251 | memset(data_package_requests.data, 0, 1023); |
||
| 252 | break; |
||
| 253 | case 2: //Request Flightcontrol Values |
||
| 254 | create_serial_frame(1, 'p', 0, &data_package_requests); |
||
| 255 | transmit_data(TO_KOPTER, &data_package_requests); |
||
| 256 | memset(data_package_requests.txrxdata, 0, 1023); |
||
| 257 | memset(data_package_requests.data, 0, 1023); |
||
| 258 | break; |
||
| 259 | case 3: |
||
| 260 | |||
| 261 | data_package_requests.data[0] = 0; |
||
| 262 | data_package_requests.data[1] = 246; |
||
| 263 | data_package_requests.data[2] = 18; |
||
| 264 | create_serial_frame(0, 'a', 3, &data_package_requests); |
||
| 265 | transmit_data(TO_KOPTER, &data_package_requests); |
||
| 266 | |||
| 267 | memset(data_package_requests.txrxdata, 0, 1023); |
||
| 268 | memset(data_package_requests.data, 0, 1023); |
||
| 269 | break; |
||
| 270 | |||
| 271 | case 4: //Switch to flightcontrol |
||
| 272 | data_package_requests.data[0] = 0; |
||
| 273 | data_package_requests.data[1] = 236; |
||
| 274 | data_package_requests.data[2] = 91; |
||
| 275 | create_serial_frame(2, 'u', 3, &data_package_requests); |
||
| 276 | transmit_data(TO_KOPTER, &data_package_requests); |
||
| 277 | memset(data_package_requests.txrxdata, 0, 1023); |
||
| 278 | memset(data_package_requests.data, 0, 1023); |
||
| 279 | break; |
||
| 280 | case 5: //Request 3D Data |
||
| 281 | data_package_requests.data[0] = 1; |
||
| 282 | create_serial_frame(2, 'c', 1, &data_package_requests); |
||
| 283 | transmit_data(TO_KOPTER, &data_package_requests); |
||
| 284 | memset(data_package_requests.txrxdata, 0, 1023); |
||
| 285 | memset(data_package_requests.data, 0, 1023); |
||
| 286 | break; |
||
| 287 | case 6: //Request Analog Values (FlightControl) |
||
| 288 | data_package_requests.data[0] = 1; |
||
| 289 | create_serial_frame(1, 'd', 1, &data_package_requests); |
||
| 290 | transmit_data(TO_KOPTER, &data_package_requests); |
||
| 291 | memset(data_package_requests.txrxdata, 0, 1023); |
||
| 292 | memset(data_package_requests.data, 0, 1023); |
||
| 293 | break; |
||
| 294 | } |
||
| 295 | |||
| 296 | } |
||
| 297 | |||
| 298 | //>> Analyze the data package |
||
| 299 | //------------------------------------------------------------------------------------------------------ |
||
| 300 | void analyze_data_package(serial_data_struct* pdata_package_analyze){ |
||
| 301 | switch(pdata_package_analyze->collecting_data[1] - 'a') |
||
| 302 | { |
||
| 303 | |||
| 304 | case 2: //NaviCtrl |
||
| 305 | switch(pdata_package_analyze->collecting_data[2]) |
||
| 306 | { |
||
| 307 | case 'A': //Get Sensor lables |
||
| 308 | collect_serial_frame(pdata_package_analyze); |
||
| 309 | if(pdata_package_analyze->data[0] > 15 && pdata_package_analyze->data[0] < 20){ //Address for Sensorlables |
||
| 310 | add_sensor_lables(pdata_package_analyze); |
||
| 311 | create_serial_frame(2, 'A', 18, pdata_package_analyze); |
||
| 312 | } |
||
| 313 | break; |
||
| 314 | case 'D': //Get Analog Values |
||
| 315 | collect_serial_frame(pdata_package_analyze); |
||
| 316 | add_sensor_data(pdata_package_analyze); |
||
| 317 | create_serial_frame(2, 'D', 66, pdata_package_analyze); |
||
| 318 | break; |
||
| 319 | case 'C': //Get 3D Data |
||
| 320 | collect_serial_frame(pdata_package_analyze); |
||
| 321 | assign_3d_data_NC(pdata_package_analyze); |
||
| 322 | break; |
||
| 323 | } |
||
| 324 | break; |
||
| 325 | case 1: //FlightCtrl |
||
| 326 | switch(pdata_package_analyze->collecting_data[2]) |
||
| 327 | { |
||
| 328 | case 'A': //Get Sensor lables |
||
| 329 | |||
| 330 | collect_serial_frame(pdata_package_analyze); |
||
| 331 | if(pdata_package_analyze->data[0] > 15 && pdata_package_analyze->data[0] < 20){ //Address for Sensorlables |
||
| 332 | add_sensor_lables(pdata_package_analyze); |
||
| 333 | create_serial_frame(1, 'A', 18, pdata_package_analyze); |
||
| 334 | } |
||
| 335 | break; |
||
| 336 | case 'D': //Get Analog Values |
||
| 337 | collect_serial_frame(pdata_package_analyze); |
||
| 338 | add_sensor_data(pdata_package_analyze); |
||
| 339 | //create_serial_frame(1, 'D', 66, pdata_package_analyze); |
||
| 340 | break; |
||
| 341 | case 'Q': //Get Configuration |
||
| 342 | collect_serial_frame(pdata_package_analyze); |
||
| 343 | //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 |
||
| 344 | read_configuration(pdata_package_analyze); |
||
| 345 | break; |
||
| 346 | case 'P': //Get FlightCtrl Values |
||
| 347 | collect_serial_frame(pdata_package_analyze); |
||
| 348 | //pdata_package_analyze->cmdLength = 0; |
||
| 349 | assign_flightcontrol_values(pdata_package_analyze); |
||
| 350 | break; |
||
| 351 | case 'C': //Get 3D Data |
||
| 352 | collect_serial_frame(pdata_package_analyze); |
||
| 353 | assign_3d_data_FC(pdata_package_analyze); |
||
| 354 | break; |
||
| 355 | } |
||
| 356 | |||
| 357 | case 0: |
||
| 358 | switch(pdata_package_analyze->collecting_data[2]) |
||
| 359 | { |
||
| 360 | case 'a': |
||
| 361 | collect_serial_frame(pdata_package_analyze); |
||
| 362 | break; |
||
| 363 | case 'h': |
||
| 364 | collect_serial_frame(pdata_package_analyze); |
||
| 365 | break; |
||
| 366 | } |
||
| 367 | |||
| 368 | } |
||
| 369 | memset(pdata_package_analyze->collecting_data, 0, 1023); |
||
| 370 | } |
||
| 371 | |||
| 372 | void processing_init(){ |
||
| 373 | request_to_kopter(SWITCH_FLIGHTCONTROL); |
||
| 374 | } |