diff --git a/ADIS_Raspi_UDP/.cproject b/ADIS_Raspi_UDP/.cproject new file mode 100644 index 0000000..07b1efb --- /dev/null +++ b/ADIS_Raspi_UDP/.cproject @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + make + + all + true + true + true + + + make + + clean + true + true + true + + + + + \ No newline at end of file diff --git a/ADIS_Raspi_UDP/.project b/ADIS_Raspi_UDP/.project new file mode 100644 index 0000000..e2759ea --- /dev/null +++ b/ADIS_Raspi_UDP/.project @@ -0,0 +1,27 @@ + + + ADIS_Raspi_UDP + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + + diff --git a/ADIS_Raspi_UDP/obj/.gitignore b/ADIS_Raspi_UDP/obj/.gitignore new file mode 100644 index 0000000..dae2c78 --- /dev/null +++ b/ADIS_Raspi_UDP/obj/.gitignore @@ -0,0 +1,3 @@ +*.o +*.d +udp_app diff --git a/ADIS_Raspi_UDP/readme.txt b/ADIS_Raspi_UDP/readme.txt new file mode 100644 index 0000000..a593c70 --- /dev/null +++ b/ADIS_Raspi_UDP/readme.txt @@ -0,0 +1,7 @@ +readme.txt +---------- +This project implements a command line client for UDP on the Raspberry Pi. + +Usage example: + ./udp_app --udp tx ESP32DEVKIT02 1234 "cmd rs status" + diff --git a/ADIS_Raspi_UDP/src/delay.c b/ADIS_Raspi_UDP/src/delay.c new file mode 100644 index 0000000..ac15c0d --- /dev/null +++ b/ADIS_Raspi_UDP/src/delay.c @@ -0,0 +1,21 @@ +/* + * delay.c + * + * Author: Erich Styger + * License: PDX-License-Identifier: BSD-3-Clause + */ + +#include "delay.h" +#include /* interface to sleep */ + +void delay_ms(unsigned int ms) { + //sleep(1); /* sleep for 1 second */ + usleep(ms*1000); /* sleep for number of microseconds */ +#if 0 + volatile long i; + for(i=0; i<50000000; i++) { + /* burning time */ + } +#endif +} + diff --git a/ADIS_Raspi_UDP/src/delay.h b/ADIS_Raspi_UDP/src/delay.h new file mode 100644 index 0000000..c7d525d --- /dev/null +++ b/ADIS_Raspi_UDP/src/delay.h @@ -0,0 +1,13 @@ +/* + * delay.h + * + * Author: Erich Styger + * License: PDX-License-Identifier: BSD-3-Clause + */ + +#ifndef SRC_DELAY_H_ +#define SRC_DELAY_H_ + +void delay_ms(unsigned int ms); + +#endif /* SRC_DELAY_H_ */ diff --git a/ADIS_Raspi_UDP/src/gpio.c b/ADIS_Raspi_UDP/src/gpio.c new file mode 100644 index 0000000..2480a81 --- /dev/null +++ b/ADIS_Raspi_UDP/src/gpio.c @@ -0,0 +1,345 @@ +/* + * Copyright (c) 2022, Erich Styger + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +/* see + * https://blog.lxsang.me/post/id/33 + * https://embeddedbits.org/new-linux-kernel-gpio-user-space-interface/ + * https://www.ics.com/blog/gpio-programming-exploring-libgpiod-library + * https://lloydrochester.com/post/hardware/libgpiod-intro-rpi/ + * */ + +#include "gpio.h" + +/* include API header for the new interface */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define GPIO_DEV_NAME "/dev/gpiochip0" + +/* HAT v7 requires pull-ups enabled. In case if using older headers, define locally the new flags */ +#ifndef GPIOHANDLE_REQUEST_BIAS_PULL_UP /* supported with latest libs: if not defined, maybe using older header files? Let's assume the library is a recent one */ + #define GPIOHANDLE_REQUEST_BIAS_PULL_UP (1UL << 5) + #define GPIOHANDLE_REQUEST_BIAS_PULL_DOWN (1UL << 6) + #define GPIOHANDLE_REQUEST_BIAS_DISABLE (1UL << 7) +#endif + +typedef struct { + struct gpiohandle_request rq; /* request data structure */ + struct gpiohandle_data data; +} Gpio_Desc_t; + +static Gpio_Desc_t yellowLED, redLED, greenLED; +#if PL_HAT_VERSION<=6 + static Gpio_Desc_t blueLED; +#endif +static Gpio_Desc_t upBTN, downBTN, leftBTN, rightBTN, centerBTN; + +static void gpio_list(const char *dev_name) { + struct gpiochip_info info; + struct gpioline_info line_info; + int fd, ret; + + fd = open(dev_name, O_RDONLY); + if (fd < 0) { + printf("Unabled to open %s: %s", dev_name, strerror(errno)); + return; + } + ret = ioctl(fd, GPIO_GET_CHIPINFO_IOCTL, &info); + if (ret == -1) { + printf("Unable to get chip info from ioctl: %s", strerror(errno)); + close(fd); + return; + } + printf("Chip name: %s\n", info.name); + printf("Chip label: %s\n", info.label); + printf("Number of lines: %d\n", info.lines); + for (int i = 0; i < info.lines; i++) { + line_info.line_offset = i; + ret = ioctl(fd, GPIO_GET_LINEINFO_IOCTL, &line_info); + if (ret == -1) { + printf("Unable to get line info from offset %d: %s", i, strerror(errno)); + } else { + printf("offset: %d, name: %s, consumer: %s. Flags:\t[%s]\t[%s]\t[%s]\t[%s]\t[%s]\t[%s]\t[%s]\t[%s]\n", + i, + line_info.name, + line_info.consumer, + (line_info.flags & GPIOLINE_FLAG_IS_OUT) ? "OUTPUT" : "INPUT", + (line_info.flags & GPIOLINE_FLAG_ACTIVE_LOW) ? "ACTIVE_LOW" : "ACTIVE_HIGHT", + (line_info.flags & GPIOLINE_FLAG_OPEN_DRAIN) ? "OPEN_DRAIN" : "...", + (line_info.flags & GPIOHANDLE_REQUEST_BIAS_PULL_UP) ? "PULL-UP" : "...", + (line_info.flags & GPIOHANDLE_REQUEST_BIAS_PULL_DOWN) ? "PULL-DOWN" : "...", + (line_info.flags & GPIOHANDLE_REQUEST_BIAS_DISABLE) ? "PULL-DISABLE" : "...", + (line_info.flags & GPIOLINE_FLAG_OPEN_SOURCE) ? "OPENSOURCE" : "...", + (line_info.flags & GPIOLINE_FLAG_KERNEL) ? "KERNEL" : ""); + } + } + close(fd); +} + +static void gpio_write(const char *dev_name, int offset, uint8_t value) { + struct gpiohandle_request rq; + struct gpiohandle_data data; + int fd, ret; + + printf("Write value %d to GPIO at offset %d (OUTPUT mode) on chip %s\n", value, offset, dev_name); + fd = open(dev_name, O_RDONLY); + if (fd < 0) { + printf("Unabled to open %s: %s", dev_name, strerror(errno)); + return; + } + rq.lineoffsets[0] = offset; + rq.flags = GPIOHANDLE_REQUEST_OUTPUT; + rq.lines = 1; + ret = ioctl(fd, GPIO_GET_LINEHANDLE_IOCTL, &rq); + close(fd); + if (ret == -1) { + printf("Unable to line handle from ioctl : %s", strerror(errno)); + return; + } + data.values[0] = value; + ret = ioctl(rq.fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &data); + if (ret == -1) { + printf("Unable to set line value using ioctl : %s", strerror(errno)); + } else { + usleep(2000000); /* wait 2 sec */ + } + close(rq.fd); +} + +static void writePin(Gpio_Desc_t *io, uint8_t value) { + int ret; + + io->data.values[0] = value; + ret = ioctl(io->rq.fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &io->data); + if (ret == -1) { + printf("Unable to set line value using ioctl : %s", strerror(errno)); + } +} + +static bool readPin(Gpio_Desc_t *io) { + int ret; + bool result = false; + + ret = ioctl(io->rq.fd, GPIOHANDLE_GET_LINE_VALUES_IOCTL, &io->data); + if (ret == -1) { + printf("Unable to get line value using ioctl : %s", strerror(errno)); + result = false; + } else { + result = io->data.values[0]!=0; + } + return result; +} + +static int gpio_read(const char *dev_name, int offset) +{ + struct gpiohandle_request rq; + struct gpiohandle_data data; + int fd, ret; + int result = -1; + + fd = open(dev_name, O_RDONLY); + if (fd < 0) + { + printf("Unabled to open %s: %s", dev_name, strerror(errno)); + return -1; + } + rq.lineoffsets[0] = offset; + rq.flags = GPIOHANDLE_REQUEST_INPUT; + rq.lines = 1; + ret = ioctl(fd, GPIO_GET_LINEHANDLE_IOCTL, &rq); + close(fd); + if (ret == -1) + { + printf("Unable to get line handle from ioctl : %s", strerror(errno)); + return -1; + } + ret = ioctl(rq.fd, GPIOHANDLE_GET_LINE_VALUES_IOCTL, &data); + if (ret == -1) + { + printf("Unable to get line value using ioctl : %s", strerror(errno)); + result = -1; + } + else + { + printf("Value of GPIO at offset %d (INPUT mode) on chip %s: %d\n", offset, dev_name, data.values[0]); + result = data.values[0]; + } + close(rq.fd); + return result; +} + +static void gpio_poll(const char *dev_name, int offset) +{ + struct gpioevent_request rq; + struct pollfd pfd; + int fd, ret; + + fd = open(dev_name, O_RDONLY); + if (fd < 0) + { + printf("Unabled to open %s: %s", dev_name, strerror(errno)); + return; + } + rq.lineoffset = offset; + rq.eventflags = GPIOEVENT_EVENT_RISING_EDGE; + ret = ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, &rq); + close(fd); + if (ret == -1) + { + printf("Unable to get line event from ioctl : %s", strerror(errno)); + close(fd); + return; + } + pfd.fd = rq.fd; + pfd.events = POLLIN; + ret = poll(&pfd, 1, -1); + if (ret == -1) + { + printf("Error while polling event from GPIO: %s", strerror(errno)); + } + else if (pfd.revents & POLLIN) + { + printf("Rising edge event on GPIO offset: %d, of %s\n", offset, dev_name); + } + close(rq.fd); +} + +bool Gpio_ReadPin(Gpio_Pins_e pin) { + bool val; + //return gpio_read(GPIO_DEV_NAME, pin); + switch(pin) { + case Gpio_Pins_BTN_UP_BCM: + val = readPin(&upBTN); + break; + case Gpio_Pins_BTN_DOWN_BCM: + val = readPin(&downBTN); + break; + case Gpio_Pins_BTN_LEFT_BCM: + val = readPin(&leftBTN); + break; + case Gpio_Pins_BTN_RIGHT_BCM: + val = readPin(&rightBTN); + break; + case Gpio_Pins_BTN_CENTER_BCM: + val = readPin(¢erBTN); + break; + default: + val = false; + break; + } + return val; +} + +void Gpio_WritePin(Gpio_Pins_e pin, bool val) { + switch(pin) { +#if PL_HAT_VERSION<=6 + case Gpio_Pins_LED_BLUE_PIN_BCM: + writePin(&blueLED, val); + break; +#endif + case Gpio_Pins_LED_RED_PIN_BCM: + writePin(&redLED, val); + break; + case Gpio_Pins_LED_GREEN_PIN_BCM: + writePin(&greenLED, val); + break; + case Gpio_Pins_LED_YELLOW_PIN_BCM: + writePin(&yellowLED, val); + break; + + default: + break; + } +} + +int Gpio_Deinit(void) { +#if PL_HAT_VERSION<=6 + close(blueLED.rq.fd); +#endif + close(redLED.rq.fd); + close(greenLED.rq.fd); + close(yellowLED.rq.fd); + + close(upBTN.rq.fd); + close(downBTN.rq.fd); + close(leftBTN.rq.fd); + close(rightBTN.rq.fd); + close(centerBTN.rq.fd); + return 0; +} + +static int RequestPin(Gpio_Desc_t *desc, Gpio_Pins_e pin, int lineRequestFlags, const char *label) { + int fd, ret; + + fd = open(GPIO_DEV_NAME, O_RDONLY); + if (fd < 0) { + printf("Unabled to open %s: %s", GPIO_DEV_NAME, strerror(errno)); + return -1; + } + desc->rq.lineoffsets[0] = pin; + desc->rq.flags = lineRequestFlags; + memcpy(desc->rq.default_values, &desc->data, sizeof(desc->rq.default_values)); + strcpy(desc->rq.consumer_label, label); + desc->rq.lines = 1; + ret = ioctl(fd, GPIO_GET_LINEHANDLE_IOCTL, &desc->rq); + close(fd); + if (ret == -1) { + printf("Unable to get line handle from ioctl : %s", strerror(errno)); + return -1; + } + return 0; +} + +int Gpio_Init(void) { +#if GPIO_HAT_VERSION<=6 + if (RequestPin(&blueLED, Gpio_Pins_LED_BLUE_PIN_BCM, GPIOHANDLE_REQUEST_OUTPUT, "led_blue")!=0) { + return -1; + } +#endif + if (RequestPin(&redLED, Gpio_Pins_LED_RED_PIN_BCM, GPIOHANDLE_REQUEST_OUTPUT, "led_red")!=0) { + return -1; + } + if (RequestPin(&greenLED, Gpio_Pins_LED_GREEN_PIN_BCM, GPIOHANDLE_REQUEST_OUTPUT, "led_green")!=0) { + return -1; + } + + if (RequestPin(&yellowLED, Gpio_Pins_LED_YELLOW_PIN_BCM, GPIOHANDLE_REQUEST_OUTPUT, "led_yellow")!=0) { + return -1; + } + + int inputPinFlags = GPIOHANDLE_REQUEST_INPUT; +#if PL_HAT_VERSION>6 + inputPinFlags |= GPIOHANDLE_REQUEST_BIAS_PULL_UP; +#endif + if (RequestPin(&upBTN, Gpio_Pins_BTN_UP_BCM, inputPinFlags, "button_up")!=0) { + return -1; + } + if (RequestPin(&downBTN, Gpio_Pins_BTN_DOWN_BCM, inputPinFlags, "button_down")!=0) { + return -1; + } + if (RequestPin(&leftBTN, Gpio_Pins_BTN_LEFT_BCM, inputPinFlags, "button_left")!=0) { + return -1; + } + if (RequestPin(&rightBTN, Gpio_Pins_BTN_RIGHT_BCM, inputPinFlags, "button_right")!=0) { + return -1; + } + if (RequestPin(¢erBTN, Gpio_Pins_BTN_CENTER_BCM, inputPinFlags, "button_center")!=0) { + return -1; + } +#if 0 /* testing only */ + gpio_list(GPIO_DEV_NAME); +#endif + return 0; +} diff --git a/ADIS_Raspi_UDP/src/gpio.h b/ADIS_Raspi_UDP/src/gpio.h new file mode 100644 index 0000000..33ccb7e --- /dev/null +++ b/ADIS_Raspi_UDP/src/gpio.h @@ -0,0 +1,39 @@ +/* + * Copyright (c) 2022, Erich Styger + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef SRC_GPIO_H_ +#define SRC_GPIO_H_ + +#include +#include "platform.h" + +/* Pins with the BCM/Broadcom numbers */ +typedef enum { +#if PL_HAT_VERSION<=6 + Gpio_Pins_LED_BLUE_PIN_BCM = 12, /* wPi 26 */ + Gpio_Pins_LED_GREEN_PIN_BCM = 16, /* wPi 27 */ + Gpio_Pins_LED_YELLOW_PIN_BCM = 20, /* wPi 28 */ + Gpio_Pins_LED_RED_PIN_BCM = 21, /* wPi 29 */ +#else + Gpio_Pins_LED_RED_PIN_BCM = 16, /* wPi 27 */ + Gpio_Pins_LED_GREEN_PIN_BCM = 21, /* wPi 29 */ + Gpio_Pins_LED_YELLOW_PIN_BCM = 20, /* wPi 28 */ +#endif + + Gpio_Pins_BTN_UP_BCM = 5, /* wPi 21 */ + Gpio_Pins_BTN_DOWN_BCM = 6, /* wPi 22 */ + Gpio_Pins_BTN_RIGHT_BCM = 13, /* wPi 23 */ + Gpio_Pins_BTN_LEFT_BCM = 19, /* wPi 24 */ + Gpio_Pins_BTN_CENTER_BCM = 26, /* wPi 25 */ +} Gpio_Pins_e; + +bool Gpio_ReadPin(Gpio_Pins_e pin); +void Gpio_WritePin(Gpio_Pins_e pin, bool val); + +int Gpio_Init(void); +int Gpio_Deinit(void); + +#endif /* SRC_GPIO_H_ */ diff --git a/ADIS_Raspi_UDP/src/hostname.c b/ADIS_Raspi_UDP/src/hostname.c new file mode 100644 index 0000000..4f9b634 --- /dev/null +++ b/ADIS_Raspi_UDP/src/hostname.c @@ -0,0 +1,36 @@ +/* + * Copyright (c) 2021, Erich Styger + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +/* taken from https://www.binarytides.com/hostname-to-ip-address-c-sockets-linux/ */ +#include "hostname.h" /* own interface */ +#include /* printf */ +#include /* memset */ +#include /* for exit(0); */ +#include +#include /* For errno - the error number */ +#include /* hostent */ +#include + +/* Get ip from domain name */ +int hostname_to_ip(const char *hostname, char *ipBuf, size_t ipBufLen) { + struct hostent *he; + struct in_addr **addr_list; + int i; + + if ((he = gethostbyname(hostname)) == NULL) { + /* get the host info */ + herror("gethostbyname"); + return -1; + } + addr_list = (struct in_addr **) he->h_addr_list; + for(i = 0; addr_list[i]!=NULL; i++) { + /* Return the first one */ + strncpy(ipBuf, inet_ntoa(*addr_list[i]), ipBufLen); + ipBuf[ipBufLen-1] = '\0'; /* terminate string */ + return 0; + } + return -1; +} diff --git a/ADIS_Raspi_UDP/src/hostname.h b/ADIS_Raspi_UDP/src/hostname.h new file mode 100644 index 0000000..9c45e02 --- /dev/null +++ b/ADIS_Raspi_UDP/src/hostname.h @@ -0,0 +1,15 @@ +/* + * Copyright (c) 2021, Erich Styger + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef SRC_HOSTNAME_H_ +#define SRC_HOSTNAME_H_ + +#include + +/* return for a given hostname the IP address */ +int hostname_to_ip(const char *hostname, char *ipBuf, size_t ipBufLen); + +#endif /* SRC_HOSTNAME_H_ */ diff --git a/ADIS_Raspi_UDP/src/main.c b/ADIS_Raspi_UDP/src/main.c new file mode 100644 index 0000000..3798abb --- /dev/null +++ b/ADIS_Raspi_UDP/src/main.c @@ -0,0 +1,67 @@ +/* + * main.c + * + * Author: Erich Styger + * License: PDX-License-Identifier: BSD-3-Clause + */ + +#include "main.h" +#include +#include +#include +#include "delay.h" +#include "udp.h" +#include "robotNav.h" +#include "platform.h" + +#define VERSION_INFO "v1.2" + +static void printHelp(void) { + printf("Usage:\n"); + printf(" --help : prints this list\n"); + printf(" --udp tx : send UDP datagram\n"); + printf(" --test : test led and navigation switch\n"); + printf(" --nav : navigation with UDP server using joystick\n"); +} + +int main(int argc, char *argv[]) { + int res; + + printf("UDP application, %s, use --help for a list of options\n", VERSION_INFO); + if (argc > 1) { +#if 0 /* debugging only */ + int count; + + printf ("This program was called with \"%s\".\n",argv[0]); + printf("nof arguments: %d\n", argc); + for (count = 1; count < argc; count++) { + printf("nof: %d, argv[%d] = %s\n", argc, count, argv[count]); + } +#endif + } else { + printHelp(); + return 0; + } + + PL_Init(); + if (argc==2 && strcmp(argv[1], "--help")==0) { + printHelp(); + } else if (argc==6 && strcmp(argv[1], "--udp")==0 && strcmp(argv[2], "tx")==0) { + udp_send(argv[3], atoi(argv[4]), argv[5]); + } else if (argc==2 && strcmp(argv[1], "--test")==0) { + res = ROBONAV_Init(); + if (res==0) { + ROBONAV_test(); + ROBONAV_Deinit(); + } + } else if (argc==4 && strcmp(argv[1], "--nav")==0) { + res = ROBONAV_Init(); + if (res==0) { + ROBONAV_navigate(argv[2], atoi(argv[3])); /* pass server and port */ + ROBONAV_Deinit(); + } + } else { /* unknown command */ + printHelp(); + } + return 0; +} diff --git a/ADIS_Raspi_UDP/src/main.h b/ADIS_Raspi_UDP/src/main.h new file mode 100644 index 0000000..ee9bbc7 --- /dev/null +++ b/ADIS_Raspi_UDP/src/main.h @@ -0,0 +1,13 @@ +/* + * main.h + * + * Author: Erich Styger + * License: PDX-License-Identifier: BSD-3-Clause + */ + +#ifndef SRC_MAIN_H_ +#define SRC_MAIN_H_ + +int main(int argc, char *argv[]); + +#endif /* SRC_MAIN_H_ */ diff --git a/ADIS_Raspi_UDP/src/platform.c b/ADIS_Raspi_UDP/src/platform.c new file mode 100644 index 0000000..d450712 --- /dev/null +++ b/ADIS_Raspi_UDP/src/platform.c @@ -0,0 +1,21 @@ +/* + * Copyright (c) 2022, Erich Styger + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include "platform.h" +#include "gpio.h" + +void PL_Deinit(void) { + Gpio_Deinit(); +} + +void PL_Init(void) { + printf("using libgpiod with HAT version %d\n", PL_HAT_VERSION); + if (Gpio_Init()<0) { + printf("failed Gpio_Init()!\n"); + } +} + diff --git a/ADIS_Raspi_UDP/src/platform.h b/ADIS_Raspi_UDP/src/platform.h new file mode 100644 index 0000000..8149a50 --- /dev/null +++ b/ADIS_Raspi_UDP/src/platform.h @@ -0,0 +1,15 @@ +/* + * Copyright (c) 2022, Erich Styger + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef SRC_PLATFORM_H_ +#define SRC_PLATFORM_H_ + +#define PL_HAT_VERSION (6) /* HAT version used, e.g. 4, 5, 6 or 7 */ + +void PL_Deinit(void); +void PL_Init(void); + +#endif /* SRC_PLATFORM_H_ */ diff --git a/ADIS_Raspi_UDP/src/robotNav.c b/ADIS_Raspi_UDP/src/robotNav.c new file mode 100644 index 0000000..3b7d27e --- /dev/null +++ b/ADIS_Raspi_UDP/src/robotNav.c @@ -0,0 +1,138 @@ +/* + * robotNav.c + * + * Author: Erich Styger + * License: PDX-License-Identifier: BSD-3-Clause + */ + +#include "robotNav.h" +#include "gpio.h" +#include "udp.h" +#include +#include /* interface to sleep */ + +static void BlinkLed(Gpio_Pins_e led) { + Gpio_WritePin(led, true); /* on */ + usleep(100*1000); + Gpio_WritePin(led, false); /* off */ +} + +int ROBONAV_test(void) { + bool buttonPressed; + + printf("Use NAV switch on HAT,
for > 1 sec to exit.\n"); + for(;;) { + buttonPressed = false; + if (Gpio_ReadPin(Gpio_Pins_BTN_LEFT_BCM)==0) { + printf("left\n"); + printf("green\n"); BlinkLed(Gpio_Pins_LED_GREEN_PIN_BCM); + } + if (Gpio_ReadPin(Gpio_Pins_BTN_RIGHT_BCM)==0) { + printf("right\n"); + printf("red\n"); BlinkLed(Gpio_Pins_LED_RED_PIN_BCM); + } + if (Gpio_ReadPin(Gpio_Pins_BTN_UP_BCM)==0) { + printf("up\n"); + printf("blue\n"); BlinkLed(Gpio_Pins_LED_BLUE_PIN_BCM); + } + if (Gpio_ReadPin(Gpio_Pins_BTN_DOWN_BCM)==0) { + printf("down\n"); + printf("yellow\n"); BlinkLed(Gpio_Pins_LED_YELLOW_PIN_BCM); + } + if (Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0) { + printf("center\n"); + + int timeMs = 0; + while(Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0 && timeMs <= 1000) { + usleep(100*1000); + timeMs += 100; + } + if (Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0) { /* still pressed? */ + printf("exit program\n"); + break; /* break for loop and return */ + } + } + usleep(100*1000); + } /* for */ + return 0; /* ok */ +} + +int ROBONAV_navigate(const char *server, int port) { + unsigned int buttons; /* mask of buttons pressed */ + #define BTN_LEFT_MASK (1<<0) + #define BTN_RIGHT_MASK (1<<1) + #define BTN_UP_MASK (1<<2) + #define BTN_DOWN_MASK (1<<3) + #define BTN_CENTER_MASK (1<<4) + #define ALL_BUTTONS_MASK (BTN_LEFT_MASK|BTN_RIGHT_MASK|BTN_UP_MASK|BTN_DOWN_MASK|BTN_CENTER_MASK) + + printf("Use NAV switch on HAT to navigate robot,
for > 1 sec to exit.\n"); + udp_send(server, port, "test"); + for(;;) { + buttons = 0; + if (Gpio_ReadPin(Gpio_Pins_BTN_LEFT_BCM)==0) { + buttons |= BTN_LEFT_MASK; + } + if (Gpio_ReadPin(Gpio_Pins_BTN_RIGHT_BCM)==0) { + buttons |= BTN_RIGHT_MASK; + } + if (Gpio_ReadPin(Gpio_Pins_BTN_UP_BCM)==0) { + buttons |= BTN_UP_MASK; + } + if (Gpio_ReadPin(Gpio_Pins_BTN_DOWN_BCM)==0) { + buttons |= BTN_DOWN_MASK; + } + if (Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0) { + buttons |= BTN_CENTER_MASK; + } + if ((buttons&ALL_BUTTONS_MASK)==(BTN_LEFT_MASK|BTN_UP_MASK)) { + printf("left-up\n"); + udp_send(server, port, "nav_left-up"); + } else if ((buttons&ALL_BUTTONS_MASK)==BTN_LEFT_MASK) { + printf("left\n"); + udp_send(server, port, "nav_left"); + } else if ((buttons&ALL_BUTTONS_MASK)==(BTN_RIGHT_MASK|BTN_UP_MASK)) { + printf("right-up\n"); + udp_send(server, port, "nav_right-up"); + } else if ((buttons&ALL_BUTTONS_MASK)==BTN_RIGHT_MASK) { + printf("right\n"); + udp_send(server, port, "nav_right"); + } else if ((buttons&ALL_BUTTONS_MASK)==BTN_UP_MASK) { + printf("up\n"); + udp_send(server, port, "nav_up"); + } else if ((buttons&ALL_BUTTONS_MASK)==(BTN_LEFT_MASK|BTN_DOWN_MASK)) { + printf("left-down\n"); + udp_send(server, port, "nav_down"); + } else if ((buttons&ALL_BUTTONS_MASK)==(BTN_RIGHT_MASK|BTN_DOWN_MASK)) { + printf("left-down\n"); + udp_send(server, port, "nav_down"); + } else if ((buttons&ALL_BUTTONS_MASK)==BTN_DOWN_MASK) { + printf("down\n"); + udp_send(server, port, "nav_down"); + } else if ((buttons&ALL_BUTTONS_MASK)==BTN_CENTER_MASK) { + printf("center\n"); + udp_send(server, port, "nav_center"); + + int timeMs = 0; + while(Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0 && timeMs <= 1000) { + usleep(100*1000); + timeMs += 100; + } + if (Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0) { /* still pressed? */ + printf("exit program\n"); + udp_send(server, port, "exit"); + break; /* break for loop and return */ + } + } + usleep(100*1000); + } /* for */ + return 0; +} + +int ROBONAV_Init(void) { + return 0; /* ok */ +} + +int ROBONAV_Deinit(void) { + return 0; /* ok */ +} diff --git a/ADIS_Raspi_UDP/src/robotNav.h b/ADIS_Raspi_UDP/src/robotNav.h new file mode 100644 index 0000000..a608cea --- /dev/null +++ b/ADIS_Raspi_UDP/src/robotNav.h @@ -0,0 +1,19 @@ +/* + * robotNav.h + * + * Author: Erich Styger + * License: PDX-License-Identifier: BSD-3-Clause + */ + +#ifndef SRC_ROBOTNAV_H_ +#define SRC_ROBOTNAV_H_ + +int ROBONAV_navigate(const char *server, int port); + +int ROBONAV_test(void); + +int ROBONAV_Deinit(void); + +int ROBONAV_Init(void); + +#endif /* SRC_ROBOTNAV_H_ */ diff --git a/ADIS_Raspi_UDP/src/udp.c b/ADIS_Raspi_UDP/src/udp.c new file mode 100644 index 0000000..6744248 --- /dev/null +++ b/ADIS_Raspi_UDP/src/udp.c @@ -0,0 +1,148 @@ +/* + * udp.c + * + * Author: Erich Styger + * License: PDX-License-Identifier: BSD-3-Clause + * + * UDP example code by Paul Krzyzanowski, see + * https://www.cs.rutgers.edu/~pxk/417/notes/sockets/udp.html + */ + +#include +#include +#include +#include +#include +#include +#include +#include "hostname.h" +#include "platform.h" + +#define BUFSIZE (64) + +int udp_receive(int port) { + struct sockaddr_in myaddr; /* our address */ + struct sockaddr_in remaddr; /* remote address */ + socklen_t addrlen = sizeof(remaddr); /* length of addresses */ + int recvlen; /* # bytes received */ + int fd; /* our socket */ + int msgcnt = 0; /* count # of messages we received */ + static unsigned char buf[BUFSIZE]; /* receive buffer */ + + /* create a UDP socket */ + if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + perror("cannot create socket\n"); + return 0; + } + + /* bind the socket to any valid IP address and a specific port */ + memset((char *)&myaddr, 0, sizeof(myaddr)); + myaddr.sin_family = AF_INET; + myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + myaddr.sin_port = htons(port); + + if (bind(fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) { + perror("bind failed"); + return 0; + } + + /* now loop, receiving data and printing what we received */ + for (;;) { + printf("waiting on port %d\n", port); + recvlen = recvfrom(fd, buf, sizeof(buf), 0, (struct sockaddr *)&remaddr, &addrlen); + if (recvlen > 0) { + buf[recvlen] = 0; + printf("received message: \"%s\" (%d bytes)\n", buf, recvlen); + } else { + printf("uh oh - something went wrong!\n"); + } + sprintf((char*)buf, "ack %d", msgcnt++); + printf("sending response \"%s\"\n", buf); + if (sendto(fd, buf, strlen((char*)buf), 0, (struct sockaddr *)&remaddr, addrlen) < 0) { + perror("sendto"); + } + } + /* never exits */ +} + + +/* send a UDP datagram. Parameters are strings, e.g. + * host IP, e.g. "10.180.254.51" + * port number, e.g. "1234" + * msg message string to send, e.g. "hello" + * \return negative value in case of failure + */ +int udp_send(const char *host, int port, const char *msg) { + struct sockaddr_in myaddr, remaddr; + int fd, i; + socklen_t slen=sizeof(remaddr); + static char buf[BUFSIZE]; /* message buffer */ + int recvlen; /* # bytes in acknowledgement message */ + int portNumber; + char ipBuf[64]; /* in case 'host' is a hostname and not an IP address, this will hold the IP address */ + + /* hostname to IP transformation */ + if (*host >='0' && *host<='9') { /* IP address */ + printf("IP: %s\n", host); + } else { + if (hostname_to_ip(host, ipBuf, sizeof(ipBuf))==0) { + printf("hostname '%s' -> IP: '%s'\n", host, ipBuf); + host = ipBuf; /* point to transformed IP address */ + } else { + perror("hostname_to_ip() failed"); + return -1; + } + } + + /* create a socket */ + if ((fd=socket(AF_INET, SOCK_DGRAM, 0))==-1) { + printf("socket created\n"); + } + + /* bind it to all local addresses and pick any port number */ + memset((char *)&myaddr, 0, sizeof(myaddr)); + myaddr.sin_family = AF_INET; + myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + myaddr.sin_port = htons(0); + + if (bind(fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) { + perror("bind failed"); + return -1; + } + + /* now define remaddr, the address to whom we want to send messages */ + /* For convenience, the host address is expressed as a numeric IP address */ + /* that we will convert to a binary format via inet_aton */ + memset((char *) &remaddr, 0, sizeof(remaddr)); + remaddr.sin_family = AF_INET; + remaddr.sin_port = htons(port); + if (inet_aton(host, &remaddr.sin_addr)==0) { + fprintf(stderr, "inet_aton() failed\n"); + exit(1); + } + + /* set socket timeout */ + struct timeval tv; + tv.tv_sec = 5; + tv.tv_usec = 0; + if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO,&tv,sizeof(tv)) < 0) { + perror("Error"); + } + /* now let's send the messages */ + printf("Sending datagram '%s' to '%s' on port %d\n", msg, host, port); + if (sendto(fd, msg, strlen(msg), 0, (struct sockaddr *)&remaddr, slen)==-1) { + perror("sendto"); + exit(1); + } + + /* now receive an acknowledgment from the server */ + recvlen = recvfrom(fd, buf, sizeof(buf), 0, (struct sockaddr *)&remaddr, &slen); + if (recvlen >= 0) { + buf[recvlen] = '\0'; /* expect a printable string - terminate it */ + printf("response: \n%s\n", buf); + } else { /* timeout */ + printf("socket receive timeout\n"); + } + close(fd); + return 0; /* ok */ +} diff --git a/ADIS_Raspi_UDP/src/udp.h b/ADIS_Raspi_UDP/src/udp.h new file mode 100644 index 0000000..4f29bbf --- /dev/null +++ b/ADIS_Raspi_UDP/src/udp.h @@ -0,0 +1,15 @@ +/* + * udp.h + * + * Author: Erich Styger + * License: PDX-License-Identifier: BSD-3-Clause + */ + +#ifndef SRC_UDP_H_ +#define SRC_UDP_H_ + +void udp_receive(void); + +int udp_send(const char *server, int port, const char *msg); + +#endif /* SRC_UDP_H_ */