| /RaspberryPi/ExPlat/ExPlat |
|---|
| Cannot display: file marked as a binary type. |
| svn:mime-type = application/octet-stream |
| Property changes: |
| Added: svn:mime-type |
| +application/octet-stream |
| \ No newline at end of property |
| /RaspberryPi/ExPlat/Makefile |
|---|
| 0,0 → 1,23 |
| OBJ = main.o gpio.o uart.o processing.o flightcontrol.o timer.o flowcam.o |
| DEPS = main.h gpio.h uart.h types.h processing.h flightcontrol.h timer.h flowcam.h |
| LIBS = |
| CFLAGS = -std=c99 -Wall -export-dynamic -L/usr/lib/python2.7/config -lpython2.7 -lm -ldl -lutil -lpthread |
| CC = gcc |
| EXTENSION = .c |
| #define a rule that applies to all files ending in the .o suffix, which says that the .o file depends upon the .c version of the file and all the .h files included in the DEPS macro. Compile each object file |
| %.o: %$(EXTENSION) $(DEPS) |
| $(CC) -c -o $@ $< $(CFLAGS) |
| #Combine them into the output file |
| #Set your desired exe output file name here |
| all: ExPlat clean |
| ExPlat: $(OBJ) |
| $(CC) -o $@ $^ $(CFLAGS) $(LIBS) |
| #Cleanup |
| .PHONY: clean |
| clean: |
| rm -f *.o *~ core *~ |
| /RaspberryPi/ExPlat/flightcontrol.c |
|---|
| 0,0 → 1,173 |
| #include "main.h" |
| #include "flightcontrol.h" |
| int reference_altitude; //Regelgröße |
| int reference_accz; //Regelgröße D-Anteil |
| int reference_flow_x; |
| int reference_flow_y; |
| int altitude_differenz; //Regelabweichung |
| int altitude_differenz_sum = 0; //I-Glied |
| int xDirection_differenz_sum = 0; |
| int yDirection_differenz_sum = 0; |
| int altitude_old; |
| int altitude_velocity; |
| int xDirection_old; |
| int xDirection_velocity; |
| int yDirection_old; |
| int yDirection_velocity; |
| float gas_correction_dpart; |
| float gas_correction_ipart = 0.0; |
| float gas_correction_ppart; |
| float gas_correction_final; |
| float nick_correction_dpart; |
| float nick_correction_ipart = 0.0; |
| float nick_correction_ppart; |
| float nick_correction_final; |
| float roll_correction_dpart; |
| float roll_correction_ipart = 0.0; |
| float roll_correction_ppart; |
| float roll_correction_final; |
| float pFactorGas = 0.3; //Verstärkungsfaktor P |
| float iFactorGas = 0.02; //Verstärkungsfaktor I |
| float dFactorGas = 0.3; //Verstärkungsfaktor D |
| float pFactorNick = 0.2; //Verstärkungsfaktor P |
| float iFactorNick = 0.02; //Verstärkungsfaktor I |
| float dFactorNick = 0.3; //Verstärkungsfaktor D |
| float pFactorRoll = 0.2; //Verstärkungsfaktor P |
| float iFactorRoll = 0.02; //Verstärkungsfaktor I |
| float dFactorRoll = 0.3; //Verstärkungsfaktor D |
| //>> Performing Flight Correction |
| //------------------------------------------------------------------------------------------------------ |
| void perform_flight_correction(){ |
| reference_altitude = (int)(optical_flow_values.ground_distance*1000); //Regelgröße in mm |
| reference_flow_x += (int)(optical_flow_values.flow_comp_m_x*1000); |
| reference_flow_y += (int)(optical_flow_values.flow_comp_m_y*1000); |
| altitude_differenz = STARTING_ALTITUDE - reference_altitude; //Regelabweichung in mm |
| if(reference_altitude != altitude_old){ |
| altitude_velocity = (reference_altitude - altitude_old) / 0.1; |
| } |
| if(reference_flow_x != xDirection_old){ |
| xDirection_velocity = (reference_flow_x - xDirection_old) / 0.1; |
| } |
| if(reference_flow_y != yDirection_old){ |
| yDirection_velocity = (reference_flow_y - yDirection_old) / 0.1; |
| } |
| if(altitude_differenz_sum < 10000 && altitude_differenz_sum > -10000){ |
| altitude_differenz_sum += altitude_differenz * 0.1; |
| if(altitude_differenz_sum > 9999){altitude_differenz_sum = 9999;}else |
| if(altitude_differenz_sum < -9999){altitude_differenz_sum = -9999;} |
| } |
| if(xDirection_differenz_sum < 1000 && xDirection_differenz_sum > -1000){ |
| xDirection_differenz_sum += reference_flow_x; |
| if(xDirection_differenz_sum > 9999){xDirection_differenz_sum = 9999;}else |
| if(xDirection_differenz_sum < -9999){xDirection_differenz_sum = -9999;} |
| } |
| if(yDirection_differenz_sum < 1000 && yDirection_differenz_sum > -1000){ |
| yDirection_differenz_sum += reference_flow_y; |
| if(yDirection_differenz_sum > 9999){yDirection_differenz_sum = 9999;}else |
| if(yDirection_differenz_sum < -9999){yDirection_differenz_sum = -9999;} |
| } |
| ////////////////////////////////////////////////////////////////////////// |
| // PID Regelung Gas |
| gas_correction_ipart = iFactorGas * altitude_differenz_sum; |
| gas_correction_dpart = dFactorGas * altitude_velocity *-1; |
| gas_correction_ppart = pFactorGas * altitude_differenz; |
| gas_correction_final = (gas_correction_ppart + gas_correction_dpart + gas_correction_ipart) / 10; |
| ////////////////////////////////////////////////////////////////////////// |
| ////////////////////////////////////////////////////////////////////////// |
| // PID Regelung Nick |
| nick_correction_ipart = iFactorNick * yDirection_differenz_sum; |
| nick_correction_dpart = dFactorNick * yDirection_velocity; |
| nick_correction_ppart = pFactorNick * reference_flow_y; |
| nick_correction_final = (nick_correction_ppart + nick_correction_dpart + nick_correction_ipart) / -10; |
| ////////////////////////////////////////////////////////////////////////// |
| ////////////////////////////////////////////////////////////////////////// |
| // PID Regelung Roll |
| roll_correction_ipart = iFactorRoll * xDirection_differenz_sum; |
| roll_correction_dpart = dFactorRoll * xDirection_velocity; |
| roll_correction_ppart = pFactorRoll * reference_flow_x; |
| roll_correction_final = (roll_correction_ppart + roll_correction_dpart + roll_correction_ipart) / -10; |
| ////////////////////////////////////////////////////////////////////////// |
| //printf("Final: %f PPART: %f DPART: %f\n", nick_correction_final, nick_correction_ppart, nick_correction_dpart); |
| ////////////////////////////////////////////////////////////////////////// |
| // Set Limits |
| if(gas_correction_final >= 20){ |
| fc_correction_data.Gas = 20; |
| }else if(gas_correction_final <= -20){ |
| fc_correction_data.Gas = -20; |
| }else{ |
| fc_correction_data.Gas = gas_correction_final; |
| } |
| if(nick_correction_final >= 20){ |
| fc_correction_data.Nick = 20; |
| }else if(nick_correction_final <= -20){ |
| fc_correction_data.Nick = -20; |
| }else{ |
| fc_correction_data.Nick = nick_correction_final; |
| } |
| if(roll_correction_final >= 20){ |
| fc_correction_data.Roll = 20; |
| }else if(roll_correction_final <= -20){ |
| fc_correction_data.Roll = -20; |
| }else{ |
| fc_correction_data.Roll = roll_correction_final; |
| } |
| //printf("%d\n", reference_altitude/10); |
| printf("Final: %f PPART: %f DPART: %f IPART: %f Gas Correction: %d\n", gas_correction_final, gas_correction_ppart, gas_correction_dpart, gas_correction_ipart, fc_correction_data.Gas ); |
| //printf("Final: %f PPART: %f DPART: %f IPART: %f Gas Correction: %d\n", nick_correction_final, nick_correction_ppart, nick_correction_dpart, nick_correction_ipart, fc_correction_data.Nick ); |
| fc_correction_data.Gas = 0; |
| //fc_correction_data.Roll = 0; |
| //fc_correction_data.Nick = 0; |
| altitude_old = reference_altitude; |
| xDirection_old = reference_flow_x; |
| yDirection_old = reference_flow_y; |
| } |
| //>> Transmitting Correction Data |
| //------------------------------------------------------------------------------------------------------ |
| u8 *flight_values_stream; |
| serial_data_struct data_package_flightcontrol; |
| void transmit_flight_data(){ |
| flight_values_stream = (unsigned char *) &fc_correction_data; |
| //printf("Correction: %d\n", fc_correction_data.Gas); |
| for (int i = 0; i < sizeof(struct str_fc_correction_data); ++i) |
| { |
| data_package_flightcontrol.data[i] = flight_values_stream[i]; |
| } |
| create_serial_frame(1, 'b', sizeof(struct str_fc_correction_data), &data_package_flightcontrol); |
| //printf("%s\n", data_package_flightcontrol.txrxdata); |
| transmit_data(TO_KOPTER, &data_package_flightcontrol); |
| } |
| //>> Resetting Flow and Altitude Values |
| //------------------------------------------------------------------------------------------------------ |
| void reset_reference_values(){ |
| reference_flow_x = 0.0; |
| reference_flow_y = 0.0; |
| reference_altitude = 0.0; |
| } |
| //>> Initializing flightcontrol |
| //------------------------------------------------------------------------------------------------------ |
| void flightcontrol_init(){ |
| fc_correction_data.Nick = 0; |
| fc_correction_data.Roll = 0; |
| fc_correction_data.Gier = 0; |
| fc_correction_data.Gas = 0; |
| fc_correction_data.Config = EC_VALID | EC_GAS_ADD | EC_IGNORE_RC; //Ignore Remote Control |
| } |
| /RaspberryPi/ExPlat/flightcontrol.h |
|---|
| 0,0 → 1,74 |
| #ifndef _FLIGHTCONTROL_H |
| #define _FLIGHTCONTROL_H |
| #define EC_VALID 0x01 //only valid if this is 1 |
| #define EC_GAS_ADD 0x02 //if 1 -> use the GAS Value not as MAX |
| #define EC_IGNORE_RC 0x80 //if 1 -> for Flying without RC-Control |
| #define STARTING_ALTITUDE 1000 |
| #define MAX_ALTITUDE 3000 |
| #define MAX_CLIMBING_SPEED 50 |
| #define STARTING_THRUST 10 |
| extern void perform_flight_correction(); |
| extern void reset_reference_values(); |
| extern void flightcontrol_init(); |
| extern void transmit_flight_data(); |
| extern void flightcontrol_timekeeper(); |
| struct str_3d_data |
| { |
| int16 AngleNick; |
| int16 AngleRoll; |
| int16 Heading; |
| u8 StickNick; |
| u8 StickRoll; |
| u8 StickYaw; |
| u8 StickGas; |
| int16 Altimeter; |
| int16 AccZ; |
| }; |
| struct str_3d_data flight_data; |
| struct str_configuration |
| { |
| u8 channel_gas; |
| u8 channel_gier; |
| u8 channel_nick; |
| u8 channel_roll; |
| u8 channel_heightControl; |
| u8 channel_autostart; |
| u8 channel_gps; |
| u8 channel_carefree; |
| u8 dynamicPositionHold; |
| }; |
| struct str_configuration config_data; |
| struct str_flightcontrol_values |
| { |
| u8 value_gas; |
| u8 value_gier; |
| u8 value_nick; |
| u8 value_roll; |
| u8 value_heightControl; |
| u8 value_autostart; |
| u8 value_gps; |
| u8 value_carefree; |
| u8 value_dynamicPositionHold; |
| }; |
| struct str_flightcontrol_values fc_values; |
| struct str_fc_correction_data |
| { |
| signed char Nick; |
| signed char Roll; |
| signed char Gier; |
| signed char Gas; |
| unsigned char Frame; |
| unsigned char Config; |
| unsigned char free; |
| }; |
| struct str_fc_correction_data fc_correction_data; |
| #endif //_FLIGHTCONTROL_H |
| /RaspberryPi/ExPlat/flowcam.c |
|---|
| 0,0 → 1,63 |
| #include "main.h" |
| #include "flowcam.h" |
| pthread_t piCamThread; |
| char* indata = NULL; |
| PyObject *pName, *pModule, *pDict, *pFunc, *pFunc2; |
| //>> Get Motion Vector Data from mmap |
| //------------------------------------------------------------------------------------------------------ |
| void get_flowcam_values(){ |
| PiCamMoVector = atoi(indata); //PiCam Motion Vectors |
| printf("MAIN: %d\n", PiCamMoVector); |
| } |
| //>> Call Function in Python Program |
| //------------------------------------------------------------------------------------------------------ |
| void *ThreadProc() |
| { |
| if(PyCallable_Check(pFunc)) |
| { |
| PyObject_CallObject(pFunc, NULL); |
| }else{ |
| PyErr_Print(); |
| } |
| //Clean up (Programm will never reach this Part) |
| Py_DECREF(pModule); |
| Py_DECREF(pName); |
| Py_Finalize(); |
| printf("piCamThread is finishing...\n"); |
| } |
| //>> Create MMAP, Python Object and start Thread |
| //------------------------------------------------------------------------------------------------------ |
| void flowcam_init(){ |
| ////////////////////////////////////////////////////////////////////////// |
| // Create a MMAP |
| int fd; |
| if((fd = open("/home/pi/ExPlat/input.dat", O_RDWR)) == -1) |
| { |
| printf("Couldn't open 'input.data'\n"); |
| } |
| indata = mmap( NULL, 1024, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); |
| if(indata != NULL) |
| { |
| printf("Wrapper has created a MMAP for file 'input.data'\n"); |
| } |
| ////////////////////////////////////////////////////////////////////////// |
| char *script = "motionVector"; |
| char *functionUse = "get_values"; |
| Py_Initialize(); |
| pName = PyString_FromString(script); |
| PyRun_SimpleString("import sys"); |
| PyRun_SimpleString("sys.path.append(\"/home/pi/ExPlat\")"); |
| pModule = PyImport_Import(pName); |
| pDict = PyModule_GetDict(pModule); |
| pFunc = PyDict_GetItemString(pDict, functionUse); |
| // POSIX code |
| pthread_create( &piCamThread, NULL, ThreadProc, NULL); |
| } |
| /RaspberryPi/ExPlat/flowcam.h |
|---|
| 0,0 → 1,10 |
| #ifndef _FLOWCAM_H |
| #define _FLOWCAM_H |
| extern void flowcam_init(); |
| extern void get_flowcam_values(); |
| volatile int PiCamMoVector; |
| #endif //_FLOWCAM_H |
| /RaspberryPi/ExPlat/gpio.c |
|---|
| 0,0 → 1,98 |
| #include "main.h" |
| #include "gpio.h" |
| struct timeval startTime; //Times needed for calculating Distance |
| struct timeval travelTime; |
| // IO Acces |
| struct bcm2835_peripheral { |
| unsigned long addr_p; |
| int mem_fd; |
| void *map; |
| volatile unsigned int *addr; |
| }; |
| struct bcm2835_peripheral gpio = {GPIO_BASE}; |
| //>> Create MMAP for GPIO |
| //------------------------------------------------------------------------------------------------------ |
| int map_peripheral(struct bcm2835_peripheral *p) |
| { |
| // Open /dev/mem |
| if ((p->mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) { |
| printf("Failed to open /dev/mem, try checking permissions.\n"); |
| return -1; |
| } |
| p->map = mmap( |
| NULL, |
| BLOCK_SIZE, |
| PROT_READ|PROT_WRITE, |
| MAP_SHARED, |
| p->mem_fd, |
| p->addr_p |
| ); |
| if (p->map == MAP_FAILED) { |
| perror("mmap"); |
| return -1; |
| } |
| p->addr = (volatile unsigned int *)p->map; |
| return 0; |
| } |
| void unmap_peripheral(struct bcm2835_peripheral *p) { |
| munmap(p->map, BLOCK_SIZE); |
| close(p->mem_fd); |
| } |
| //>> Funktion for short buzzing Sound |
| //------------------------------------------------------------------------------------------------------ |
| void buzzer_short(int seq){ |
| INP_GPIO(4); |
| OUT_GPIO(4); |
| for (int i = 0; i < seq; ++i) |
| { |
| if(seq > 1){usleep(50000);} |
| GPIO_SET = 1 << 4; |
| usleep(50000); |
| GPIO_CLR = 1 << 4; |
| } |
| } |
| //>> Calculate Distance with external sonar |
| //------------------------------------------------------------------------------------------------------ |
| int calculate_distance(){ |
| int distanceTemp; |
| int loopbreaker = 10000; //Breaks the first loop in case it gets stuck (which happens from time to time if the Objekt facing the sensor is too close) |
| GPIO_SET = 1 << ULTRA_TRIGGER; |
| usleep(10); |
| GPIO_CLR = 1 << ULTRA_TRIGGER; |
| while(!(GPIO_READ(ULTRA_ECHO))) |
| { |
| loopbreaker--; |
| if(!loopbreaker){break;} |
| } |
| gettimeofday(&startTime, NULL); |
| while((GPIO_READ(ULTRA_ECHO))) |
| gettimeofday(&travelTime, NULL); |
| loopbreaker = 0; |
| distanceTemp = (travelTime.tv_usec - startTime.tv_usec) / 58; |
| return distanceTemp; |
| } |
| //>> Initialize GPIO |
| //------------------------------------------------------------------------------------------------------ |
| void gpio_init(){ |
| if(map_peripheral(&gpio) == -1) |
| { |
| printf("Failed to map the physical GPIO registers into the virtual memory space.\n"); |
| } |
| INP_GPIO(ULTRA_ECHO); |
| OUT_GPIO(ULTRA_TRIGGER); |
| GPIO_CLR = 1 << ULTRA_TRIGGER; |
| sleep(1); |
| } |
| /RaspberryPi/ExPlat/gpio.h |
|---|
| 0,0 → 1,26 |
| #ifndef _GPIO_H |
| #define _GPIO_H |
| #define BCM2708_PERI_BASE 0x20000000 |
| #define GPIO_BASE (BCM2708_PERI_BASE + 0x200000) // GPIO controller |
| #define BLOCK_SIZE (4*1024) |
| extern struct bcm2835_peripheral gpio; |
| //>> Makros for reading and writing GPIO registers |
| //------------------------------------------------------------------------------------------------------ |
| #define INP_GPIO(g) *(gpio.addr + ((g)/10)) &= ~(7<<(((g)%10)*3)) |
| #define OUT_GPIO(g) *(gpio.addr + ((g)/10)) |= (1<<(((g)%10)*3)) |
| #define SET_GPIO_ALT(g,a) *(gpio.addr + (((g)/10))) |= (((a)<=3?(a) + 4:(a)==4?3:2)<<(((g)%10)*3)) |
| #define GPIO_SET *(gpio.addr + 7) // sets bits which are 1 ignores bits which are 0 |
| #define GPIO_CLR *(gpio.addr + 10) // clears bits which are 1 ignores bits which are 0 |
| #define GPIO_READ(g) *(gpio.addr + 13) &= (1<<(g)) |
| #define ULTRA_ECHO 24 |
| #define ULTRA_TRIGGER 23 |
| extern void gpio_init(); |
| extern void buzzer_short(int seq); |
| extern int calculate_distance(); |
| #endif //_GPIO_H |
| /RaspberryPi/ExPlat/main.c |
|---|
| 0,0 → 1,46 |
| #include "main.h" |
| serial_data_struct data_package_receive_kopter; //Data Package for Kopter Data |
| serial_data_struct data_package_receive_computer; //Data Package for Computer Data |
| serial_data_struct data_package_flow; //Data Package for Flow Data |
| int tmp = 0; |
| int main(void){ |
| gpio_init(); |
| uart_init(); |
| processing_init(); |
| timer_init(); |
| flightcontrol_init(); |
| //flowcam_init(); //Uncomment for use of Raspberry Camera Module |
| reset_reference_values(); |
| for (;;) |
| { |
| reset_timer(); |
| ////////////////////////////////////////////////////////////////////////// |
| // Request Data from Kopter, renew subscribtion every 2 Seconds |
| if(interval_in_sec(2)){ |
| request_to_kopter(GET_ANALOG_DATA); |
| //request_to_kopter(GET_3D_DATA); |
| } |
| ////////////////////////////////////////////////////////////////////////// |
| // Receive Data from Kopter and computer at 200Hz |
| if(interval_in_msec(5)){ |
| //receive_data_from_computer(&data_package_receive_computer, 0); |
| receive_data_from_kopter(&data_package_receive_kopter); |
| } |
| ////////////////////////////////////////////////////////////////////////// |
| // Receive Data from Flow Module and Perform Flight Correction at 200Hz |
| if(interval_in_msec(5)){ |
| receive_data_from_flow(&data_package_flow); |
| transmit_flight_data(); |
| } |
| //if(interval_in_sec(10)){reset_reference_values();} |
| //if(interval_in_msec(50)){get_distance();} |
| //if(interval_in_msec(5)){get_flowcam_values();} |
| } |
| } |
| /RaspberryPi/ExPlat/main.h |
|---|
| 0,0 → 1,29 |
| #ifndef _MAIN_H |
| #define _MAIN_H |
| #include </usr/include/python2.7/Python.h> |
| //#include <stdio.h> //included in python |
| //#include <string.h> //included in python |
| #include <stdint.h> |
| #include <termios.h> |
| #include <fcntl.h> |
| #include <unistd.h> |
| #include <sys/mman.h> |
| #include <sys/types.h> |
| #include <sys/time.h> |
| #include <linux/i2c-dev.h> |
| #include <pthread.h> |
| #include "types.h" |
| #include "uart.h" |
| #include "gpio.h" |
| #include "processing.h" |
| #include "timer.h" |
| #include "flightcontrol.h" |
| #include "flowcam.h" |
| //extern void usleep(int); |
| volatile int Motors_On; |
| volatile u8 global_temp; |
| #endif //_MAIN_H |
| /RaspberryPi/ExPlat/motionVector.py |
|---|
| 0,0 → 1,40 |
| import numpy as np |
| #np.set_printoptions(threshold=np.nan) |
| import picamera |
| import picamera.array |
| import sys |
| import os |
| import time |
| import mmap |
| ##>> Gain access to MMAP |
| ##------------------------------------------------------------------------------------------------------ |
| INDATAFILENAME = '/home/pi/ExPlat/input.dat' |
| LENGTHDATAMAP = 1024 |
| inDataFile = open(INDATAFILENAME, 'r+') |
| inDataNo = inDataFile.fileno() |
| inDataMap = mmap.mmap(inDataNo, LENGTHDATAMAP, access=mmap.ACCESS_WRITE) |
| s = 0; |
| ##>> Analyze Numpy Array and Print Motion Vector Data |
| ##------------------------------------------------------------------------------------------------------ |
| class AnalyzeArray(picamera.array.PiMotionAnalysis): |
| def analyse(self, a): |
| global s |
| global inDataMap |
| s += a[20,15][0] |
| #print s |
| inDataMap.seek(0) |
| inDataMap.write('%d' %s + '\n') |
| ##>> Start Capture and call AnalyzeArray after every frame |
| ##------------------------------------------------------------------------------------------------------ |
| def get_values(): |
| with picamera.PiCamera() as camera: |
| with AnalyzeArray(camera) as output: |
| camera.resolution = (640, 480) |
| camera.framerate = (20, 1) |
| camera.start_recording( |
| '/dev/null', format='h264', motion_output=output) |
| camera.wait_recording(0) |
| camera.stop_recording() |
| /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); |
| } |
| /RaspberryPi/ExPlat/processing.h |
|---|
| 0,0 → 1,63 |
| #ifndef _PROCESSING_H |
| #define _PROCESSING_H |
| #define INITIATE 3 |
| #define GET_CONFIGURATION 1 |
| #define GET_FLIGHTCONTROL_VALUES 2 |
| #define GET_3D_DATA 5 |
| #define SWITCH_FLIGHTCONTROL 4 |
| #define GET_ANALOG_DATA 6 |
| extern void processing_init(); |
| extern void analyze_data_package(serial_data_struct* pdata_package); |
| extern void get_distance(); |
| extern void request_to_kopter(u8 request); |
| extern void process_flow_data(serial_data_struct* pdata_package_flow_process); |
| extern void process_gps_data(serial_data_struct* pdata_package_gps_process); |
| volatile int Distance; |
| struct str_optical_flow |
| { |
| u64 time_usec; |
| float flow_comp_m_x; |
| float flow_comp_m_y; |
| float ground_distance; |
| int16 flow_x; |
| int16 flow_y; |
| u8 sensor_id; |
| u8 quality; |
| float gyroz; |
| }; |
| struct str_optical_flow optical_flow_values; |
| struct str_optical_flow_rad |
| { |
| u64 time_usec; |
| u32 integration_time_us; |
| float integrated_x; |
| float integrated_y; |
| float integrated_xgyro; |
| float integrated_ygyro; |
| float integrated_zgyro; |
| u32 time_delta_distance_us; |
| float distance; |
| int16 temperature; |
| u8 sensor_id; |
| u8 quality; |
| }; |
| struct str_optical_flow_rad optical_flow_rad; |
| struct str_gps |
| { |
| float latitude; |
| u8 lat_orientation; |
| float longitude; |
| u8 long_orientation; |
| float utc; |
| float utc_date; |
| u8 valid; |
| }; |
| struct str_gps gps_value_struct; |
| #endif //_PROCESSING_H |
| /RaspberryPi/ExPlat/timer.c |
|---|
| 0,0 → 1,68 |
| #include "main.h" |
| #include "timer.h" |
| struct timeval startTimeSec; //Set at every start |
| struct timeval travelTime; |
| struct timeval intermitTime; |
| u8 sec_handler; |
| u8 msec_handler; |
| u16 time_in_msec; |
| //>> Call for intervals in Seconds |
| //------------------------------------------------------------------------------------------------------ |
| u8 interval_in_sec(u8 number_of_seconds){ |
| if(sec_handler){ |
| gettimeofday(&travelTime, NULL); |
| if(!((travelTime.tv_sec - startTimeSec.tv_sec) % number_of_seconds)){ |
| return(1); |
| }else{return(0);} |
| }else{return(0);} |
| } |
| //>> Call for intervals in Milliseconds |
| //------------------------------------------------------------------------------------------------------ |
| u8 interval_in_msec(u16 number_of_milliseconds){ |
| if(msec_handler) |
| { |
| if(!(time_in_msec % number_of_milliseconds)){ |
| return(1); |
| }else{return(0);} |
| }else{return(0);} |
| } |
| //>> Reset Timer for every loop cycle |
| //------------------------------------------------------------------------------------------------------ |
| u16 msec_time_compare1; |
| u16 msec_time_compare2; |
| void reset_timer(){ |
| gettimeofday(&intermitTime, NULL); |
| msec_time_compare1 = intermitTime.tv_usec/1000; |
| if(!(msec_time_compare1 == msec_time_compare2)){ |
| time_in_msec++; |
| msec_handler = 1; |
| }else{ |
| msec_handler = 0; |
| } |
| if(time_in_msec == 10001){ |
| time_in_msec = 1; |
| } |
| msec_time_compare2 = intermitTime.tv_usec/1000; |
| if(!(intermitTime.tv_sec == travelTime.tv_sec)){ |
| sec_handler = 1; |
| }else{ |
| sec_handler = 0; |
| } |
| } |
| //>> Initialize Timer, store reference Time |
| //------------------------------------------------------------------------------------------------------ |
| void timer_init(){ |
| gettimeofday(&startTimeSec, NULL); |
| sec_handler = 1; |
| msec_handler = 1; |
| time_in_msec = 0; |
| } |
| /RaspberryPi/ExPlat/timer.h |
|---|
| 0,0 → 1,10 |
| #ifndef _TIMER_H |
| #define _TIMER_H |
| extern void timer_init(); |
| extern void reset_timer(); |
| extern u8 interval_in_sec(u8 number_of_seconds); |
| extern u8 interval_in_msec(u16 number_of_milliseconds); |
| #endif //_TIMER_H |
| /RaspberryPi/ExPlat/types.h |
|---|
| 0,0 → 1,39 |
| #ifndef _TYPES_H |
| #define _TYPES_H |
| typedef signed char int8; |
| typedef unsigned char u8; |
| typedef signed int int16; |
| typedef unsigned int u16; |
| typedef signed long int int32; |
| typedef unsigned long int u32; |
| typedef signed long long int int64; |
| typedef unsigned long long int u64; |
| #define TRUE 1 |
| #define FALSE 0 |
| //>> Main struct for Communication |
| //------------------------------------------------------------------------------------------------------ |
| typedef struct |
| { |
| u8 c; |
| u8 collecting; |
| int count; |
| u8 address; |
| u8 cmdID; |
| u8 data[1024]; |
| u8 collecting_data[1024]; |
| u8 txrxdata[1024]; |
| u16 position; |
| u16 dataLength; |
| u8 position_package; |
| u8 position_package_gps; |
| u16 cmdLength; |
| u8 readyForTransmit; |
| u8 length_payload; |
| u8 msg_id; |
| u8 gps_data[30][20]; |
| }__attribute__((packed)) serial_data_struct; |
| #endif //_TYPES_H |
| /RaspberryPi/ExPlat/uart.c |
|---|
| 0,0 → 1,429 |
| #include "main.h" |
| #include "uart.h" |
| int uart0_computer = -1; |
| int uart1_kopter = -1; |
| int uart2_flow = -1; |
| int uart3_gps = -1; |
| //>> Storing Chars one by one |
| //------------------------------------------------------------------------------------------------------ |
| u8 get_char(serial_data_struct* pdata_package){ |
| if(pdata_package->count--) |
| { |
| pdata_package->c = pdata_package->txrxdata[pdata_package->position++]; |
| return(1); |
| }else{return(0);} |
| } |
| //>> Searching for initiation-character and storing message |
| //------------------------------------------------------------------------------------------------------ |
| u8 collect_data(serial_data_struct* pdata_package){ |
| if(pdata_package->c == '#') |
| { |
| pdata_package->collecting = 1; |
| } |
| if(pdata_package->collecting) |
| { |
| pdata_package->collecting_data[pdata_package->position_package++] = pdata_package->c; |
| if(pdata_package->c == '\r'){ |
| return(0); |
| } |
| } |
| return(1); |
| } |
| //>> Searching for initiation-character and storing message (for Flow Control) |
| //------------------------------------------------------------------------------------------------------ |
| u8 collect_data_flow(serial_data_struct* pdata_package){ |
| if(pdata_package->c == 254) |
| { |
| pdata_package->collecting = 1; |
| } |
| if(pdata_package->collecting) |
| { |
| pdata_package->collecting_data[pdata_package->position_package++] = pdata_package->c; |
| if(pdata_package->position_package == pdata_package->length_payload + 8){ |
| return(0); |
| } |
| } |
| return(1); |
| } |
| //>> Searching for initiation-character and storing message (for GPS) |
| //------------------------------------------------------------------------------------------------------ |
| u8 collect_data_gps(serial_data_struct* pdata_package){ |
| if(pdata_package->c == '$') |
| { |
| pdata_package->collecting = 1; |
| } |
| if(pdata_package->collecting) |
| { |
| if(pdata_package->c == ','){ |
| pdata_package->position_package_gps++; |
| pdata_package->position_package = 0; |
| }else{ |
| pdata_package->gps_data[pdata_package->position_package_gps][pdata_package->position_package++] = pdata_package->c; |
| } |
| if(pdata_package->c == '\r'){ |
| return(0); |
| } |
| } |
| return(1); |
| } |
| float tmpflow = 0.0; |
| //>> Receiving Data from the Optical Flow Module and processing it |
| //------------------------------------------------------------------------------------------------------ |
| void receive_data_from_flow(serial_data_struct* pdata_package_flow){ |
| if (uart2_flow != -1) |
| { |
| int rx_length = read(uart2_flow, (void*)pdata_package_flow->txrxdata, 1024); |
| if (rx_length > 0) |
| { |
| pdata_package_flow->cmdLength = rx_length; |
| pdata_package_flow->count = rx_length; |
| pdata_package_flow->position = 0; |
| while(get_char(pdata_package_flow)) |
| { |
| if(!collect_data_flow(pdata_package_flow)) |
| { |
| pdata_package_flow->collecting = 0; |
| pdata_package_flow->dataLength = pdata_package_flow->position_package; |
| pdata_package_flow->position_package = 0; |
| //|----------------------------Process Flow Data----------------------------|// |
| //| |// |
| process_flow_data(pdata_package_flow); |
| //perform_flight_correction(); |
| //| |// |
| //|----------------------------Process Flow Data----------------------------|// |
| memset(pdata_package_flow->collecting_data, 0, 1023); |
| }else{ |
| if(pdata_package_flow->position_package == 6){ |
| pdata_package_flow->length_payload = pdata_package_flow->collecting_data[1]; |
| pdata_package_flow->msg_id = pdata_package_flow->collecting_data[5]; |
| } |
| if(pdata_package_flow->collecting_data[0] != 254){ |
| memset(pdata_package_flow->collecting_data, 0, 1023); |
| memset(pdata_package_flow->txrxdata, 0, 1023); |
| pdata_package_flow->collecting = 0; |
| pdata_package_flow->position_package = 0; |
| pdata_package_flow->msg_id = 0; |
| } |
| } |
| } |
| memset(pdata_package_flow->txrxdata, 0, 1023); |
| } |
| }else{ |
| printf("FLOW RX Error\n"); |
| } |
| } |
| //>> Receiving Data from the MikroKopter-tool und transmitting it to the kopter |
| //------------------------------------------------------------------------------------------------------ |
| void receive_data_from_computer(serial_data_struct* pdata_package_receive_computer, u8 psendThrough){ |
| if (uart0_computer != -1) |
| { |
| memset(pdata_package_receive_computer->txrxdata, 0, 1023); |
| int rx_length = read(uart0_computer, (void*)pdata_package_receive_computer->txrxdata, 256); //Filestream, buffer to store in, number of bytes to read (max) |
| if (rx_length > 0) |
| { |
| pdata_package_receive_computer->cmdLength = rx_length; |
| transmit_data(TO_KOPTER, pdata_package_receive_computer); |
| if(!psendThrough){ |
| pdata_package_receive_computer->count = rx_length; |
| pdata_package_receive_computer->position = 0; |
| while(get_char(pdata_package_receive_computer)) |
| { |
| if(!collect_data(pdata_package_receive_computer)) |
| { |
| pdata_package_receive_computer->collecting = 0; |
| pdata_package_receive_computer->dataLength = pdata_package_receive_computer->position_package; |
| pdata_package_receive_computer->position_package = 0; |
| u16 crc = 0; |
| u8 crc1, crc2; |
| for (int i = 0; i < pdata_package_receive_computer->dataLength - 3; i++) |
| { |
| crc += pdata_package_receive_computer->collecting_data[i]; |
| } |
| crc %= 4096; |
| crc1 = '=' + crc / 64; |
| crc2 = '=' + crc % 64; |
| if(crc1 == pdata_package_receive_computer->collecting_data[pdata_package_receive_computer->dataLength - 3] && crc2 == pdata_package_receive_computer->collecting_data[pdata_package_receive_computer->dataLength - 2]) |
| { |
| analyze_data_package(pdata_package_receive_computer); |
| //>> CRC OK |
| pdata_package_receive_computer->position_package = 0; |
| }else{ |
| //>> CRC NOT OK |
| printf("RX Checksum Error\n"); |
| pdata_package_receive_computer->position_package = 0; |
| memset(pdata_package_receive_computer->collecting_data, 0, 1023); |
| } |
| } |
| } |
| } |
| } |
| }else{ |
| printf("RX Error\n"); |
| } |
| } |
| //>> Receiving Data from the Kopter and collecting serial frame if needed |
| //------------------------------------------------------------------------------------------------------ |
| void receive_data_from_kopter(serial_data_struct* pdata_package_receive_kopter){ |
| if (uart1_kopter != -1) |
| { |
| memset(pdata_package_receive_kopter->txrxdata, 0, 1023); |
| int rx_length = read(uart1_kopter, (void*)pdata_package_receive_kopter->txrxdata, 256); //Filestream, buffer to store in, number of bytes to read (max) |
| if (rx_length > 0) |
| { |
| pdata_package_receive_kopter->cmdLength = rx_length; |
| pdata_package_receive_kopter->count = rx_length; |
| pdata_package_receive_kopter->position = 0; |
| while(get_char(pdata_package_receive_kopter)) |
| { |
| if(!collect_data(pdata_package_receive_kopter)) |
| { |
| pdata_package_receive_kopter->collecting = 0; |
| pdata_package_receive_kopter->dataLength = pdata_package_receive_kopter->position_package; |
| pdata_package_receive_kopter->position_package = 0; |
| u16 crc = 0; |
| u8 crc1, crc2; |
| for (int i = 0; i < pdata_package_receive_kopter->dataLength - 3; i++) |
| { |
| crc += pdata_package_receive_kopter->collecting_data[i]; |
| } |
| crc %= 4096; |
| crc1 = '=' + crc / 64; |
| crc2 = '=' + crc % 64; |
| if(crc1 == pdata_package_receive_kopter->collecting_data[pdata_package_receive_kopter->dataLength - 3] && crc2 == pdata_package_receive_kopter->collecting_data[pdata_package_receive_kopter->dataLength - 2]) |
| { |
| analyze_data_package(pdata_package_receive_kopter); |
| //>> CRC OK |
| pdata_package_receive_kopter->position_package = 0; |
| }else{ |
| //>> CRC NOT OK |
| printf("RX Checksum Error\n"); |
| pdata_package_receive_kopter->position_package = 0; |
| memset(pdata_package_receive_kopter->collecting_data, 0, 1023); |
| } |
| } |
| } |
| transmit_data(TO_COMPUTER, pdata_package_receive_kopter); |
| } |
| }else{ |
| printf("KOPTER RX Error\n"); |
| } |
| } |
| //>> Receiving Data from the Optical Flow Module and processing it |
| //------------------------------------------------------------------------------------------------------ |
| void receive_data_from_gps(serial_data_struct* pdata_package_gps){ |
| if (uart3_gps != -1) |
| { |
| int rx_length = read(uart3_gps, (void*)pdata_package_gps->txrxdata, 1024); |
| if (rx_length > 0) |
| { |
| pdata_package_gps->cmdLength = rx_length; |
| pdata_package_gps->count = rx_length; |
| pdata_package_gps->position = 0; |
| while(get_char(pdata_package_gps)) |
| { |
| if(!collect_data_gps(pdata_package_gps)) |
| { |
| pdata_package_gps->collecting = 0; |
| pdata_package_gps->dataLength = pdata_package_gps->position_package_gps; |
| pdata_package_gps->position_package = 0; |
| pdata_package_gps->position_package_gps = 0; |
| //|----------------------------Process Flow Data----------------------------|// |
| //| |
| for (int i = 0; i < pdata_package_gps->dataLength; ++i) |
| { |
| process_gps_data(pdata_package_gps); |
| pdata_package_gps->position_package++; |
| } |
| pdata_package_gps->position_package = 0; |
| //| |// |
| //|----------------------------Process Flow Data----------------------------|// |
| memset(pdata_package_gps->gps_data, 0, 30*20*sizeof(pdata_package_gps->gps_data[0][0])); |
| } |
| } |
| memset(pdata_package_gps->txrxdata, 0, 1023); } |
| }else{ |
| printf("GPS RX Error\n"); |
| } |
| } |
| //>> Transmitting data |
| //------------------------------------------------------------------------------------------------------ |
| void transmit_data(int uart_filestream, serial_data_struct* pdata_package_transmit){ |
| if (uart_filestream == 1) |
| { |
| uart_filestream = uart0_computer; |
| /* |
| printf("TO COMPUTER\n"); |
| printf("%s\n", pdata_package_transmit->txrxdata); |
| */ |
| }else if(uart_filestream == 2) |
| { |
| uart_filestream = uart1_kopter; |
| /* |
| printf("TO KOPTER\n"); |
| printf("%s\n", pdata_package_transmit->txrxdata); |
| for (int i = 0; i < pdata_package_transmit->cmdLength; ++i) |
| { |
| printf("%d ", pdata_package_transmit->txrxdata[i]); |
| } |
| printf("\n"); |
| */ |
| }else if(uart_filestream == 3){ |
| uart_filestream = uart2_flow; |
| } |
| if (uart_filestream != -1) |
| { |
| int count = write(uart_filestream, pdata_package_transmit->txrxdata, pdata_package_transmit->cmdLength); |
| if (count < 0) |
| { |
| //printf("UART TX error: %d\n", uart_filestream); |
| } |
| } |
| } |
| //>> Adding address and command ID, encoding and checksum |
| //------------------------------------------------------------------------------------------------------ |
| void create_serial_frame(u8 address, u8 cmdID, u16 cmdLength, serial_data_struct* pdata_package_create){ |
| pdata_package_create->txrxdata[0] = '\r'; |
| pdata_package_create->txrxdata[1] = '#'; |
| pdata_package_create->txrxdata[2] = address + 'a'; |
| pdata_package_create->txrxdata[3] = cmdID; |
| //encode data |
| u8 a,b,c; |
| int counter = 0; |
| int u = 4; |
| while(cmdLength){ |
| if(cmdLength) |
| { |
| cmdLength--; |
| a = pdata_package_create->data[counter++]; |
| }else{a = 0; counter++;}; |
| if(cmdLength) |
| { |
| cmdLength--; |
| b = pdata_package_create->data[counter++]; |
| }else{b = 0; counter++;}; |
| if(cmdLength) |
| { |
| cmdLength--; |
| c = pdata_package_create->data[counter++]; |
| }else{c = 0; counter++;}; |
| pdata_package_create->txrxdata[u++] = '=' + (a >> 2); |
| pdata_package_create->txrxdata[u++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
| pdata_package_create->txrxdata[u++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
| pdata_package_create->txrxdata[u++] = '=' + (c & 0x3f); |
| } |
| //add Checksum |
| u16 crc = 0; |
| u8 crc1, crc2; |
| for (int i = 1; i < u; i++) |
| { |
| crc += pdata_package_create->txrxdata[i]; |
| } |
| crc %= 4096; |
| crc1 = '=' + crc / 64; |
| crc2 = '=' + crc % 64; |
| pdata_package_create->txrxdata[u++] = crc1; |
| pdata_package_create->txrxdata[u++] = crc2; |
| pdata_package_create->txrxdata[u++] = '\r'; |
| pdata_package_create->cmdLength = u; |
| } |
| //>> Decoding Data and retrieving address and cmdID |
| //------------------------------------------------------------------------------------------------------ |
| void collect_serial_frame(serial_data_struct* pdata_package_collect){ |
| pdata_package_collect->position_package++; //first character: # |
| pdata_package_collect->address = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - 'a'; //address |
| pdata_package_collect->cmdID = pdata_package_collect->collecting_data[pdata_package_collect->position_package++]; //cmdID |
| u8 a, b, c, d; |
| u16 datacpy = 0; |
| while(pdata_package_collect->position_package < pdata_package_collect->dataLength-3){ |
| a = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - '='; |
| b = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - '='; |
| c = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - '='; |
| d = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - '='; |
| pdata_package_collect->data[datacpy++] = (a << 2) | (b >> 4); |
| pdata_package_collect->data[datacpy++] = ((b & 0x0f) << 4) | (c >> 2); |
| pdata_package_collect->data[datacpy++] = ((c & 0x03) << 6) | d; |
| } |
| pdata_package_collect->dataLength = datacpy; |
| pdata_package_collect->position_package = 0; |
| /* |
| printf("%s\n", pdata_package_collect->collecting_data); |
| for (int i = 0; i < datacpy; ++i) |
| { |
| printf("|%d|: %d ", i, pdata_package_collect->data[i]); |
| } |
| printf("%s\n", pdata_package_collect->data); |
| */ |
| } |
| //>> Initializing serial interface |
| //------------------------------------------------------------------------------------------------------ |
| void uart_init(){ |
| uart0_computer = open(SERIAL_COMPUTER, O_RDWR | O_NOCTTY | O_NDELAY); |
| uart1_kopter = open(SERIAL_KOPTER,O_RDWR | O_NOCTTY | O_NDELAY); |
| uart2_flow = open(SERIAL_FLOW,O_RDWR | O_NOCTTY | O_NDELAY); |
| uart3_gps = open(SERIAL_GPS,O_RDWR | O_NOCTTY | O_NDELAY); |
| if (uart0_computer == -1) |
| { |
| printf("Error - Unable to open UART0 (Computer)!\n"); |
| } |
| if (uart1_kopter == -1) |
| { |
| printf("Error - Unable to open UART1 (Kopter)!\n"); |
| } |
| if (uart2_flow == -1) |
| { |
| printf("Error - Unable to open UART2 (FLOW)!\n"); |
| } |
| if (uart3_gps == -1) |
| { |
| printf("Error - Unable to open UART2 (GPS)!\n"); |
| } |
| //UART Options |
| struct termios options; |
| tcgetattr(uart0_computer, &options); |
| options.c_cflag = B57600 | CS8 | CLOCAL | CREAD; |
| options.c_iflag = IGNPAR; |
| options.c_oflag = 0; |
| options.c_lflag = 0; |
| tcflush(uart0_computer, TCIFLUSH); |
| tcsetattr(uart0_computer, TCSANOW, &options); |
| tcflush(uart1_kopter, TCIFLUSH); |
| tcsetattr(uart1_kopter, TCSANOW, &options); |
| tcflush(uart2_flow, TCIFLUSH); |
| tcsetattr(uart2_flow, TCSANOW, &options); |
| //UART Options |
| struct termios options_flow; |
| tcgetattr(uart3_gps, &options_flow); |
| options_flow.c_cflag = B9600 | CS8 | CLOCAL | CREAD; |
| options.c_iflag = IGNPAR; |
| options_flow.c_oflag = 0; |
| options_flow.c_lflag = 0; |
| tcflush(uart3_gps, TCIFLUSH); |
| tcsetattr(uart3_gps, TCSANOW, &options_flow); |
| } |
| /RaspberryPi/ExPlat/uart.h |
|---|
| 0,0 → 1,45 |
| #ifndef _UART_H |
| #define _UART_H |
| //addresses |
| #define FC_address 1 //(b) |
| #define NC_address 2 //(c) |
| #define MK3MAG_address 3 //(d) |
| #define BL_CTRL_address 5 //(f) |
| //Command IDs |
| #define SERIAL_CHANNELS 'y' |
| #define EXTERNAL_CONTROL 'b' |
| //Serial Addresses |
| #define SERIAL_COMPUTER "/dev/ttyUSB0" |
| #define SERIAL_KOPTER "/dev/ttyUSB1" |
| #define SERIAL_FLOW "/dev/ttyACM0" |
| #define SERIAL_GPS "/dev/ttyAMA0" |
| #define ADDRESS 0x42 |
| //Filestream |
| #define TO_KOPTER 2 |
| #define TO_COMPUTER 1 |
| #define TO_FLOW 3 |
| //struct js_event e; |
| extern void create_serial_frame(u8 address, u8 cmdID, u16 cmdLength, serial_data_struct* pdata_package_create); |
| extern void collect_serial_frame(serial_data_struct* pdata_package_collect); |
| extern void receive_data_from_computer(serial_data_struct* pdata_package_receive_computer, u8 sendThrough); |
| extern void receive_data_from_kopter(serial_data_struct* pdata_package_receive_kopter); |
| extern void transmit_data(int uart_filestream, serial_data_struct* pdata_package_transmit); |
| extern void receive_data_from_flow(serial_data_struct* pdata_package_flow); |
| extern void receive_data_from_gps(serial_data_struct* pdata_package_gps); |
| extern void transmit_data_to_qt(serial_data_struct* pdata_package_transmit_flow); |
| extern void receive_data_from_qt(serial_data_struct* pdata_package_qt); |
| extern void transmit_data_to_flow(serial_data_struct* pdata_package_transmit_qt); |
| extern void i2c_init(); |
| extern void uart_init(); |
| #endif //_UART_H |