diff --git a/.gitignore b/.gitignore index 147f38ef..68261346 100644 --- a/.gitignore +++ b/.gitignore @@ -6,6 +6,7 @@ .project .metadata bin/ +library/lib/ tmp/ *.tmp *.bak diff --git a/CHANGELOG b/CHANGELOG index 2ee23a6c..7803c9db 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -1,3 +1,6 @@ +1.0.5 + * add support for Taranis SBUS remote control. + 1.0.4 * fix missing stddef.h include in i2c.h * add kmartin36's PRU encoder robustness improvement @@ -187,4 +190,4 @@ 0.1.0 * installs on Blue as well and Black -// indentation is with spaces to allow copy/paste into debian/changelog \ No newline at end of file +// indentation is with spaces to allow copy/paste into debian/changelog diff --git a/debian/changelog b/debian/changelog index 24b41de7..1d8a63dc 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,8 @@ +librobotcontrol (1.0.5) stable; urgency=low + * add support for Taranis SBUS remote control. + -- Per Dalgas Jakobsen Mon, 4 Mar 2019 16:04:00 +0000 + + librobotcontrol (1.0.4) stable; urgency=low * fix missing stddef.h include in i2c.h * add kmartin36's PRU encoder robustness improvement diff --git a/examples/src/rc_calibrate_sbus.c b/examples/src/rc_calibrate_sbus.c new file mode 100644 index 00000000..538f25fb --- /dev/null +++ b/examples/src/rc_calibrate_sbus.c @@ -0,0 +1,29 @@ +/** + * @example rc_calibrate_sbus.c + * + * Running the rc_calibrate_sbus example will print out raw data to + * the console and record the min and max values for each + * channel. These limits will be saved to disk so future sbus reads + * will be scaled correctly. + * + * Make sure the transmitter and receiver are paired before + * testing. The satellite receiver remembers which transmitter it is + * paired to, not your BeagleBone. + */ + +#include +#include + +int main() +{ + printf("Please connect a SBUS satellite receiver and make sure\n"); + printf("your transmitter is on and paired to the receiver.\n"); + printf("\n"); + printf("Press ENTER to continue or anything else to quit\n"); + getchar(); + + // run the calibration routine + rc_sbus_calibrate_routine(); + + return 0; +} diff --git a/examples/src/rc_dsm_passthrough.c b/examples/src/rc_dsm_passthrough.c index 8d72759a..352c700b 100644 --- a/examples/src/rc_dsm_passthrough.c +++ b/examples/src/rc_dsm_passthrough.c @@ -118,6 +118,11 @@ int main(int argc, char *argv[]) return -1; } rc_adc_cleanup(); + + if(rc_servo_init ()){ + fprintf(stderr,"failed to initialize servos\n"); + return -1; + } if(rc_servo_power_rail_en(1)){ fprintf(stderr,"failed to enable power rail\n"); return -1; @@ -153,6 +158,11 @@ int main(int argc, char *argv[]) rc_usleep(25000); } printf("\n"); + + if(mode == POWERON){ + rc_servo_cleanup (); + } rc_dsm_cleanup(); + return 0; -} \ No newline at end of file +} diff --git a/examples/src/rc_sbus_passthrough.c b/examples/src/rc_sbus_passthrough.c new file mode 100644 index 00000000..137bf6dc --- /dev/null +++ b/examples/src/rc_sbus_passthrough.c @@ -0,0 +1,173 @@ +/** + * @example rc_sbus_passthrough.c + * + * This sends all sbus data straight out the servo channels as they + * come in. When running this program the BBB acts exactly like a + * normal SBUS receiver. + * + * You must specify SERVO or ESC mode with -s or -e to turn om or off + * the 6V power rail. Sending 6V into an ESC may damage it!!! + * + * Raw data is also printed to the terminal for monitoring. + */ + +#include +#include +#include +#include +#include +#include +#include + +static int running; + +typedef enum p_mode_t{ + NONE, + POWERON, + POWEROFF +} p_mode_t; + +// function to be called every time new a new SBUS packet is received. +static void __send_pulses(void) +{ + int i, val; + + // send single to working channels + for(i=1; i<=8; i++){ + val=rc_sbus_ch_raw(i); + if(val>0) rc_servo_send_pulse_us(i,val); + } + + // print all channels + printf("\r"); + for(i=1;i<=RC_MAX_SBUS_ANALOG_CHANNELS;i++){ + val = rc_sbus_ch_raw(i); + if (val != 0) { + printf("% 4d ", val); + } + } + for(i=1;i<=RC_MAX_SBUS_BINARY_CHANNELS;i++){ + printf("% 1d ", rc_sbus_ch_binary(i)); + } + fflush(stdout); + return; +} + +// interrupt handler to catch ctrl-c +static void __signal_handler(__attribute__ ((unused)) int dummy) +{ + running=0; + return; +} + + +// printed if some invalid argument was given +static void __print_usage(void) +{ + printf("\n"); + printf(" Options\n"); + printf(" -s Enable 6V power rail for servos.\n"); + printf(" -e Disable 6V power rail for ESCs.\n"); + printf(" -h Print this messege.\n\n"); + return; +} + +// main routine +int main(int argc, char *argv[]) +{ + int c; + p_mode_t mode = NONE; + + // parse arguments + opterr = 0; + while ((c = getopt(argc, argv, "seh")) != -1){ + switch (c){ + + case 's': // enable power for servos + mode = POWERON; + break; + + case 'e': // disbable power for ESCs + mode = POWEROFF; + break; + + case 'h': // show help messege + __print_usage(); + return -1; + break; + + default: + fprintf(stderr,"Invalid Argument \n"); + __print_usage(); + return -1; + } + } + + if(mode == NONE){ + fprintf(stderr,"You must select a power mode -s or -e\n"); + __print_usage(); + return -1; + } + + if(rc_sbus_init()==-1) return -1; + + // if power has been requested, make sure battery is connected! + if(mode == POWERON){ + // read adc to make sure battery is connected + if(rc_adc_init()){ + fprintf(stderr,"ERROR: failed to run rc_adc_init()\n"); + return -1; + } + if(rc_adc_batt()<6.0){ + fprintf(stderr,"ERROR: battery disconnected or insufficiently charged to drive motors\n"); + return -1; + } + rc_adc_cleanup(); + + if(rc_servo_init ()){ + fprintf(stderr,"failed to initialize servos\n"); + return -1; + } + if(rc_servo_power_rail_en(1)){ + fprintf(stderr,"failed to enable power rail\n"); + return -1; + } + } + + // print header + printf("1:Thr "); + printf("2:Roll "); + printf("3:Pitch "); + printf("4:Yaw "); + printf("5:Kill "); + printf("6:Mode "); + printf("7:Aux1 "); + printf("8:Aux2 "); + printf("\n"); + + printf("Waiting for SBUS Connection"); + fflush(stdout); + + // set signal handler so the loop can exit cleanly + signal(SIGINT, __signal_handler); + running=1; + + rc_sbus_set_callback(&__send_pulses); + while(running){ + if(rc_sbus_is_connection_active()==0){ + printf("\rSeconds since last SBUS packet: "); + printf("%0.1f ", rc_sbus_nanos_since_last_packet()/1000000000.0); + printf(" "); + fflush(stdout); + } + rc_usleep(25000); + } + printf("\n"); + + if(mode == POWERON){ + rc_servo_cleanup (); + } + rc_sbus_cleanup(); + + return 0; +} diff --git a/examples/src/rc_test_sbus.c b/examples/src/rc_test_sbus.c new file mode 100644 index 00000000..e1b2a32b --- /dev/null +++ b/examples/src/rc_test_sbus.c @@ -0,0 +1,159 @@ +/** + * @example rc_test_sbus.c + * + * Prints out the normalized sbus values. Make sure the transmitter + * and receiver are paired before testing. The satellite receiver + * remembers which transmitter it is paired to, not your BeagleBone. + * + * If the values you read are not normalized between +-1, then you + * should run the rc_calibrate_sbus example to save your particular + * transmitter's min and max channel values. + */ + +#include +#include +#include +#include +#include + +// possible printing modes, user selected with command line arguments +typedef enum p_mode_t{ + P_MODE_NONE, + P_MODE_RAW, + P_MODE_NORM +} p_mode_t; + +static int running = 0; +static p_mode_t print_mode; + +// printed if some invalid argument was given +static void __print_usage(void) +{ + printf("\n"); + printf("-r print raw channel values in microseconds\n"); + printf("-n print normalized channel values/s\n"); + printf("-h print this help message\n"); + printf("\n"); +} + +// interrupt handler to catch ctrl-c +static void __signal_handler(__attribute__ ((unused)) int dummy) +{ + running=0; + return; +} + +static void __new_sbus_data_callback(void) +{ + int i,v; + printf("\r");// keep printing on same line + // print all channels + if(print_mode==P_MODE_NORM){ + for(i=0;i + * + * @brief SBUS radio interface + * + * SBUS is the Taranis standard for remote control plane and car + * radios. Typically, a receiver outputs pulse width modulated + * signals to individual servos or ESCs over separate servo + * connectors. Commonly a satellite-receiver is mounted on the outside + * of an RC plane to provide better signal strength and sends the same + * information as a serial packet over a 3-pin wire. These receivers + * have nothing to do with satellites but instead are named due to + * their remote mounting away from the standard receiver. + * + * The Robotics Cape supports direct connection of satellite receivers + * as well as binding to transmitters without the need for a standard + * receiver and bind plug as is traditionally used. The software has + * been tested with the X4R receiver. + * + * See rc_balance, rc_test_sbus, rc_bind_sbus, rc_calibrate_sbus, and + * rc_sbus_passthroguh examples. + * + * @addtogroup SBUS + * @{ + */ + +#ifndef RC_SBUS_H +#define RC_SBUS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include // for int64_t + +#define RC_MAX_SBUS_ANALOG_CHANNELS 16 +#define RC_MAX_SBUS_BINARY_CHANNELS 2 + +/** + * @brief Starts the SBUS background service + * + * @return 0 on success, -1 on failure + */ +int rc_sbus_init(void); + + +/** + * @brief stops the SBUS background service + * + * @return 0 on success, -1 on failure. 1 if there was a timeout due to user + * callback function not returning. + */ +int rc_sbus_cleanup(void); + + +/** + * @brief Returns the pulse width in microseconds commanded by the + * transmitter for a particular channel. + * + * The user can specify channels 1 through 16 but non-zero values will only be + * returned for channels the transmitter is actually using. The raw values in + * microseconds typically range from 900-2100us for a standard radio with + * default settings. + * + * @param[in] ch channel (1-16) + * + * @return pulse width in microseconds if data is being transmitted, 0 if + * data is not being transmitted on that channel, -1 on error + */ +int rc_sbus_ch_raw(int ch); + + +/** + * @brief Returns the binary value commanded by the transmitter for a + * particular channel. + * + * The user can specify channels 1 through 2. + * + * @param[in] ch channel (1-2) + * + * @return value + */ +int rc_sbus_ch_binary(int ch); + + +/** + * @brief Returns a scaled value from -1 to 1 corresponding to the min and + * max values recorded during calibration. + * + * The user MUST run the rc_calibrate_sbus example to ensure the normalized + * values returned by this function are correct. It is possible that values + * outside of the range from -1 to 1 are returned if the calibration is not + * perfect. + * + * @param[in] ch channel (1-16) + * + * @return normalized input from -1.0 to 1.0 if that channel has data, 0 if + * that channel has no data, -1 on error. + */ +double rc_sbus_ch_normalized(int ch); + + +/** + * @brief This is a check to see if new data is available. + * + * After new data is received this will return 1. It will return 0 as soon as + * any channel has been read by either rc_sbus_ch_raw or rc_sbus_ch_normalized. + * + * @return returns 1 if new data is ready to be read by the user. otherwise + * returns 0 + */ +int rc_sbus_is_new_data(void); + + +/** + * @brief Set your own callback function to be called when new SBUS data is + * ready. + * + * @param[in] func callback function + */ +void rc_sbus_set_callback(void (*func)(void)); + + +/** + * @brief Set your own callback function to be called when SBUS loses + * connection. + * + * @param[in] func callback function + */ +void rc_sbus_set_disconnect_callback(void (*func)(void)); + + +/** + * @brief Easily check on the state of the SBUS radio packets without + * checking timeouts yourself. + * + * @return returns 1 if packets are arriving in good health without + * timeouts. returns 0 otherwise. + */ +int rc_sbus_is_connection_active(void); + + +/** + * @brief Measures time since the last SBUS packet was received. + * + * @return Returns the number of nanoseconds since the last sbus packet was + * received. Return -1 on error or if no packet has ever been received. + */ +int64_t rc_sbus_nanos_since_last_packet(void); + + +/** + * @brief Returns the total number of lost frames since rc_sbus_init. + * Some errors also increment the number of lost frames. + * + * @return total number of lost frames + */ +uint32_t rc_sbus_lost_frames (void); + + +/** + * @brief Returns the total number of receive errors since rc_sbus_init. + * Some errors also increment the number of lost frames. + * + * @return total number of errors + */ +uint32_t rc_sbus_total_errors (void); + + +/** + * @brief Estimates the current signal quality based on good/bad frames. + * + * @return signal quality (0..100) + */ +int rc_sbus_signal_quality (void); + + +/** + * @brief routine for measuring the min and max values from a transmitter + * on each channel and save to disk for future use. + * + * If a channel isn't used by the transmitter then default values are saved. if + * the user forgot to move one of the channels during the calibration process + * then defualt values are also saved. + * + * @return 0 on success, -1 on failure + */ +int rc_sbus_calibrate_routine(void); + + +#ifdef __cplusplus +} +#endif + +#endif // RC_SBUS_H + +/** @} end group SBUS */ diff --git a/library/include/rc/uart_set_custom_baudrate.h b/library/include/rc/uart_set_custom_baudrate.h new file mode 100644 index 00000000..384a1a7d --- /dev/null +++ b/library/include/rc/uart_set_custom_baudrate.h @@ -0,0 +1,33 @@ +/** + * + * + * @brief C interface for the Linux UART driver to set custom baudrates + * + * This is a general-purpose C interface to the linux UART driver device + * (/dev/ttyO*). This is developed and tested on the BeagleBone platform but + * should work on other linux systems too. + * + * @author Per Dalgas Jakobsen + * @date 3/4/2019 + * + * @addtogroup UART + * @ingroup IO + * @{ + */ + +#ifndef RC_UART_SET_CUSTOM_BAUDRATE_H +#define RC_UART_SET_CUSTOM_BAUDRATE_H + +#ifdef __cplusplus +extern "C" { +#endif + +int rc_uart_set_custom_baudrate (int fd, int baudrate); + +#ifdef __cplusplus +} +#endif + +#endif // RC_UART_SET_CUSTOM_BAUDRATE_H + +///@} end group IO diff --git a/library/include/rc/version.h b/library/include/rc/version.h index 020a7981..fc29a1b1 100644 --- a/library/include/rc/version.h +++ b/library/include/rc/version.h @@ -20,7 +20,7 @@ extern "C" { #define RC_LIB_VERSION_MAJOR 1 #define RC_LIB_VERSION_MINOR 0 -#define RC_LIB_VERSION_PATCH 4 +#define RC_LIB_VERSION_PATCH 5 #define RC_LIB_VERSION_HEX ((RC_LIB_VERSION_MAJOR << 16) | \ (RC_LIB_VERSION_MINOR << 8) | \ (RC_LIB_VERSION_PATCH)) @@ -58,4 +58,4 @@ void rc_version_print(void); #endif //RC_VERSION_H -/** @} end group version*/ \ No newline at end of file +/** @} end group version*/ diff --git a/library/include/robotcontrol.h b/library/include/robotcontrol.h index 44e3f472..da44a3d1 100644 --- a/library/include/robotcontrol.h +++ b/library/include/robotcontrol.h @@ -26,8 +26,10 @@ #include #ifdef RC_AUTOPILOT_EXT #include "rc/dsm.h" +#include "rc/sbus.h" #else #include +#include #endif #include #include diff --git a/library/src/dsm.c b/library/src/dsm.c index c8b72b66..f2fe15c9 100644 --- a/library/src/dsm.c +++ b/library/src/dsm.c @@ -45,24 +45,26 @@ #define UART_TIMEOUT_S 0.2 #define CONNECTION_LOST_TIMEOUT_NS 300000000 -static int running; -static int channels[RC_MAX_DSM_CHANNELS]; -static int maxes[RC_MAX_DSM_CHANNELS]; -static int mins[RC_MAX_DSM_CHANNELS]; -static int centers[RC_MAX_DSM_CHANNELS]; -static double range_up[RC_MAX_DSM_CHANNELS]; -static double range_down[RC_MAX_DSM_CHANNELS]; -static int num_channels; // actual number of channels being sent -static int resolution; // 10 or 11 -static int new_dsm_flag; -static int dsm_frame_rate; -static uint64_t last_time; +static volatile int running; +static volatile int channels[RC_MAX_DSM_CHANNELS]; +static volatile int maxes[RC_MAX_DSM_CHANNELS]; +static volatile int mins[RC_MAX_DSM_CHANNELS]; +static volatile int centers[RC_MAX_DSM_CHANNELS]; +static volatile double range_up[RC_MAX_DSM_CHANNELS]; +static volatile double range_down[RC_MAX_DSM_CHANNELS]; +static volatile int num_channels; // actual number of channels being sent +static volatile int resolution; // 10 or 11 +static volatile int new_dsm_flag; +static volatile int dsm_frame_rate; +static volatile uint64_t last_time; +static volatile int listening; // for calibration routine only +static volatile int active_flag=0; +static volatile int init_flag=0; + +static volatile void (*new_data_callback)(); +static volatile void (*disconnect_callback)(); + static pthread_t parse_thread; -static int listening; // for calibration routine only -static void (*new_data_callback)(); -static void (*disconnect_callback)(); -static int active_flag=0; -static int init_flag=0; /** diff --git a/library/src/io/uart.c b/library/src/io/uart.c index 204533ea..c1d00a61 100644 --- a/library/src/io/uart.c +++ b/library/src/io/uart.c @@ -19,6 +19,7 @@ #include #include +#include #define MAX_BUS 16 #define STRING_BUF 64 @@ -38,6 +39,7 @@ int rc_uart_init(int bus, int baudrate, float timeout_s, int canonical_en, int s char buf[STRING_BUF]; struct termios config; speed_t speed; //baudrate + int Custom_Baudrate = 0; // sanity checks if(bus<0 || bus>MAX_BUS){ @@ -108,8 +110,13 @@ int rc_uart_init(int bus, int baudrate, float timeout_s, int canonical_en, int s speed=B50; break; default: - fprintf(stderr,"ERROR: int rc_uart_init, invalid baudrate. Please use a standard baudrate\n"); - return -1; + /* Not quite sure how to set custom speed "the right way". + Setting a standard speed, followed by a call to the + set custom speed seems to work. Please fix if you know + how to do it right. + */ + speed=B115200; + Custom_Baudrate = 1; } // close the bus in case it was already open @@ -159,6 +166,7 @@ int rc_uart_init(int bus, int baudrate, float timeout_s, int canonical_en, int s config.c_cc[VMIN]=MAX_READ_LEN; config.c_cc[VTIME] = tenths+1; + // Standard speed // set speed in config struct if(cfsetispeed(&config, speed)==-1){ perror("ERROR: in rc_uart_init calling cfsetispeed"); @@ -193,6 +201,15 @@ int rc_uart_init(int bus, int baudrate, float timeout_s, int canonical_en, int s close(tmpfd); return -1; } + if (Custom_Baudrate) { + // Custom speed + if (rc_uart_set_custom_baudrate (tmpfd, baudrate) != 0) { + perror("ERROR in rc_sbus_init, failed to set custom baudrate"); + close(tmpfd); + return -1; + } + } + if(tcflush(tmpfd,TCIOFLUSH)==-1){ perror("ERROR: in rc_uart_init calling tcflush"); close(tmpfd); diff --git a/library/src/io/uart_set_custom_baudrate.c b/library/src/io/uart_set_custom_baudrate.c new file mode 100644 index 00000000..a22e4688 --- /dev/null +++ b/library/src/io/uart_set_custom_baudrate.c @@ -0,0 +1,28 @@ +/* + * rc_uart_set_custom_baudrate is placed in separate compilation file + * because definitions in asm/ioctls.h and/or asm/termbits.h conflicts + * with definitions in sys/ioctl.h + */ + +#include +#include +#include + +#include + +int rc_uart_set_custom_baudrate (int fd, int baudrate) +{ + struct termios2 ntio; + int res; + + res = ioctl(fd, TCGETS2, &ntio); + if (res != 0) return res; + + ntio.c_cflag &= ~CBAUD; + ntio.c_cflag |= BOTHER | CREAD; + ntio.c_ispeed = baudrate; + ntio.c_ospeed = baudrate; + + res = ioctl(fd, TCSETS2, &ntio); + return res; + } diff --git a/library/src/sbus.c b/library/src/sbus.c new file mode 100644 index 00000000..7eb9efc0 --- /dev/null +++ b/library/src/sbus.c @@ -0,0 +1,706 @@ +/** + * @file sbus.c + * + * Description of the SBUS protocol can be found here: + * https://github.com/uzh-rpg/rpg_quadrotor_control/wiki/SBUS-Protocol + * + * On the X4R-SB board, there is an extra 4-pin connector + * (P6). The DSM2 connector should be connected as follows: + * BBB.DSM2.pin.3 (SBUS_TX) -> X4R.P6.pin.3 (UART4_RX) + * BBB.DSM2.pin.4 (GND) -> X4R.P6.pin.2 (GND) + * + * Additional description of how to get non-inverted UART + * signal from a X4R-SB (X4R with SBUS) receiver can be + * found here: + * https://www.getfpv.com/learn/fpv-diy-repairs-and-mods/get-uninverted-sbus-frsky-receivers/ + * + * The X4R-SB cannot run reliabily from 3.3V but can be + * fed from 5V or 6V servo rails. Run a servo cable + * (without signal wire) from BBB servo-rails to any port + * on the X4R - NOTE: be careful to connect correctly. + * + * Based on James Strawson's DSM code. + * + * @author Per Dalgas Jakobsen + * @date 2019-03-04 + */ + +#include +#include +#include // for system() +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "common.h" + +#ifdef RC_AUTOPILOT_EXT +#include "../include/rc/sbus.h" +#else +#include +#endif + +#define SBUS_CALIBRATION_FILE "sbus.cal" + +#define PAUSE 115 // microseconds +#define TOL 0.0001 + +// don't ask me why, but this is the default range for spektrum and orange +#define DEFAULT_MIN 1142 +#define DEFAULT_MAX 1858 +#define DEFAULT_CENTER 1500 + +#define SBUS_PINMUX_ID 30 +#define SBUS_PIN 0,30 //gpio0.30 P9.11 +#define SBUS_UART_BUS 4 +#define SBUS_BAUD_RATE 100000 +#define SBUS_PACKET_SIZE 25 +#define UART_TIMEOUT_S 0.2 +#define CONNECTION_LOST_TIMEOUT_NS 300000000 + +#define SBUS_HEADER_OFS 0 +#define SBUS_CHANNEL_DATA_OFS 1 +#define SBUS_FLAGS_OFS 23 +#define SBUS_FOOTER_OFS 24 + +#define SBUS_FLAG_CH17_MASK 0x01 +#define SBUS_FLAG_CH18_MASK 0x02 +#define SBUS_FLAG_FRAME_LOST_MASK 0x04 +#define SBUS_FLAG_FAILSAFE_MASK 0x08 + +#define SBUS_HEADER_VALUE 0x0f +#define SBUS_FOOTER_VALUE 0x00 + + +static volatile int running; +static volatile int analog_channels[RC_MAX_SBUS_ANALOG_CHANNELS]; +static volatile int binary_channels[RC_MAX_SBUS_BINARY_CHANNELS]; +static volatile int frame_lost = 1; +static volatile int failsafe = 1; + +static volatile int new_sbus_flag; +static volatile uint64_t last_time; +static volatile int listening; // for calibration routine only +static volatile int active_flag=0; +static volatile int init_flag=0; +static volatile uint32_t total_errors=0; +static volatile uint32_t lost_frames=0; +static volatile uint32_t signal_quality=0; + +static volatile int maxes[RC_MAX_SBUS_ANALOG_CHANNELS]; +static volatile int mins[RC_MAX_SBUS_ANALOG_CHANNELS]; +static volatile int centers[RC_MAX_SBUS_ANALOG_CHANNELS]; +static volatile double range_up[RC_MAX_SBUS_ANALOG_CHANNELS]; +static volatile double range_down[RC_MAX_SBUS_ANALOG_CHANNELS]; + +static void (*new_data_callback)(); +static void (*disconnect_callback)(); + +static pthread_t parse_thread; + + +/** + * This returns a string (char*) of '1' and '0' representing a character. For + * example, print "00101010" with printf(__byte_to_binary(42)); + * + * @param[in] c character + * + * @return { description_of_the_return_value } + */ +#ifdef DEBUG +static char* __byte_to_binary(unsigned char c) +{ + static char b[9]; + unsigned char x = (unsigned char)c; //cast to unsigned + b[0] = '\0'; + unsigned char z; + for (z = 128; z > 0; z >>= 1){ + if(x&z) strcat(b, "1"); + else strcat(b, "0"); + } + return b; +} +#endif + +/** + * This is a blocking function which returns 1 if the user presses ENTER. it + * returns 0 on any other keypress. If ctrl-C is pressed it will additionally + * set the global state to EXITITING and return -1. This is a useful function + * for checking if the user wishes to continue with a process or quit. + * + * @return { description_of_the_return_value } + */ +static int __continue_or_quit(void) +{ + // set stdin to non-canonical raw mode to capture all button presses + fflush(stdin); + if(system("stty raw")!=0){ + fprintf(stderr,"ERROR in continue_or_quit setting stty raw\n"); + return -1; + } + int c = getchar(); + int ret; + if(c==3){ + ret = -1; + } + else if(c=='\r' || c=='\n'){ + ret = 1; + } + else{ + ret = 0; + } + // put stdin back to normal canonical mode + if(system("stty cooked")!=0){ + fprintf(stderr,"ERROR in continue_or_quit setting stty cooked\n"); + return -1; + } + printf("\n"); + return ret; +} + +/** + * This is a local function which given an SBUS-packet and channel ID, + * returns the value of that channel. Channel data is encoded as 16 x + * 11-bits. + * + * @return { channel value } + */ +static int get_channel_value (uint8_t buf[], int channel) +{ + const int bit_number = channel * 11; + const int first_byte = SBUS_CHANNEL_DATA_OFS + (bit_number / 8); + const int first_bit = bit_number % 8; + + uint32_t value; + value = buf [first_byte]; + value |= ((uint32_t)buf [first_byte + 1]) << 8; + if (first_bit + 11 > 2*8) { + /* Channel is spread over 3 data bytes. */ + value |= ((uint32_t)buf [first_byte + 2]) << 16; + } + + return (value >> first_bit) & 0x7ff; +} + + +/** + * This is a local function that is started as a background thread by + * rc_initialize_sbus(). This monitors the serial port and interprets data for + * each packet. + * + * @param[in] { parameter_description } + * + * @return { description_of_the_return_value } + */ +static void* __parser_func(__attribute__ ((unused)) void* ptr){ + uint8_t buf[SBUS_PACKET_SIZE]; + int i, ret; + int16_t value; + + new_sbus_flag=0; + init_flag=1; + + while (running) { + // check for timeouts + if (active_flag != 0 && rc_sbus_nanos_since_last_packet() > CONNECTION_LOST_TIMEOUT_NS) { + active_flag = 0; + if (disconnect_callback!=NULL) disconnect_callback(); + } + + if (signal_quality > 0) { + signal_quality--; + } + + memset (buf, 0, SBUS_PACKET_SIZE); + rc_uart_flush (SBUS_UART_BUS); // flush + ret = rc_uart_read_bytes (SBUS_UART_BUS, buf, SBUS_PACKET_SIZE); + if (ret != SBUS_PACKET_SIZE) { + #ifdef DEBUG + fprintf(stderr, "WARNING: read the wrong number of bytes: %d\n", ret); + #endif + rc_uart_flush (SBUS_UART_BUS); // flush + total_errors++; + lost_frames++; + continue; + } + + if ((buf [SBUS_HEADER_OFS] != SBUS_HEADER_VALUE) || + (buf [SBUS_FOOTER_OFS] != SBUS_FOOTER_VALUE)) { + #ifdef DEBUG + fprintf(stderr, "WARNING: Header/Footer incorrect: %x, %x\n", + buf [SBUS_HEADER_OFS], buf [SBUS_FOOTER_OFS]); + #endif + rc_uart_flush (SBUS_UART_BUS); // flush + total_errors++; + lost_frames++; + continue; + } + + // raw debug mode spits out all ones and zeros + #ifdef DEBUG + fprintf (stderr, "ret=%d : ", ret); + for (i = 0; i < (SBUS_PACKET_SIZE / 2); i++){ + fprintf (stderr, __byte_to_binary (buf[2*i])); + fprintf (stderr, " "); + fprintf (stderr, __byte_to_binary (buf[(2*i)+1])); + fprintf (stderr, " "); + } + fprintf (stderr, "\n"); + #endif + + // Get all "analog" channels + for (i = 0; i < RC_MAX_SBUS_ANALOG_CHANNELS; i++){ + value = get_channel_value (buf, i); + #ifdef DEBUG + fprintf (stderr,"%d %d ",i ,value); + #endif + + // record new value + analog_channels [i] = value; + } + + // Get all "binary" channels + binary_channels [0] = buf [SBUS_FLAGS_OFS] & SBUS_FLAG_CH17_MASK ? 1 : 0; + binary_channels [1] = buf [SBUS_FLAGS_OFS] & SBUS_FLAG_CH18_MASK ? 1 : 0; + #ifdef DEBUG + fprintf (stderr,"17 %d 18 %d\n", binary_channels [0], binary_channels [1]); + #endif + + frame_lost = buf [SBUS_FLAGS_OFS] & SBUS_FLAG_FRAME_LOST_MASK ? 1 : 0; + failsafe = buf [SBUS_FLAGS_OFS] & SBUS_FLAG_FAILSAFE_MASK ? 1 : 0; + #ifdef DEBUG + fprintf (stderr,"frame_lost %d failsafe %d\n", frame_lost, failsafe); + #endif + + if (frame_lost) { + lost_frames++; + } else { + if (signal_quality >= 99) { + signal_quality = 100; + } else { + // Increment and compensate for initial decrement. + signal_quality += 2; + } + } + + new_sbus_flag=1; + active_flag=1; + last_time = rc_nanos_since_boot(); + + // run the dsm ready function. + // this is null unless user changed it + if(new_data_callback!=NULL) new_data_callback(); + + #ifdef DEBUG + printf("\n"); + #endif + } + return NULL; +} + +/** + * this is started as a background thread by rc_sbus_calibrate_routine(). Only + * used during calibration to monitor data as it comes in. + * + * @return NULL + */ +static void* __calibration_listen_func(__attribute__ ((unused)) void *ptr) +{ + int j, raw; + //wait for data to start + printf("waiting for sbus connection"); + new_sbus_flag =0; // flush the data ready flag with a read + while(!rc_sbus_is_new_data()){ + if(listening==0) return NULL; + rc_usleep(5000); + } + + //start limits at first value + for(j=0;j 0){ + if(raw>maxes[j]){ + maxes[j] = raw; + } + else if(raw 0) centers[j]=raw; + } + + + return 0; +} + + +int rc_sbus_init(void) +{ + int i; + //if calibration file exists, load it and start spektrum thread + FILE* fd; + + // open for reading + fd = fopen(CALIBRATION_DIR SBUS_CALIBRATION_FILE, "r"); + + if(fd==NULL){ + fprintf(stderr,"\nsbus Calibration File Doesn't Exist Yet\n"); + fprintf(stderr,"Run calibrate_sbus example to create one\n"); + fprintf(stderr,"Using default values for now\n"); + for(i=0;i(mins[i]-50)) || \ + (centers[i]<(maxes[i]+50) && centers[i]>(maxes[i]-50))){ + centers[i] = mins[i]; + range_up[i] = maxes[i]-mins[i]; + range_down[i] = maxes[i]-mins[i]; + } + // otherwise normal mode + else{ + range_up[i] = maxes[i]-centers[i]; + range_down[i] = centers[i]-mins[i]; + } + #ifdef DEBUG + printf("channel %d range %f center %d\n", i, range_up[i],centers[i]); + #endif + } + + if(rc_pinmux_set(SBUS_PINMUX_ID, PINMUX_UART)){ + fprintf(stderr,"ERROR in rc_sbus_init, failed to set pinmux\n"); + return -1; + } + + running = 1; // lets uarts 4 thread know it can run + last_time = 0; + active_flag = 0; + new_data_callback=NULL; + disconnect_callback=NULL; + new_sbus_flag=0; + total_errors=0; + lost_frames=0; + signal_quality=0; + + // 0.2s timeout, disable canonical (0), 2 stop bit (1), even parity (0) + if(rc_uart_init(SBUS_UART_BUS, SBUS_BAUD_RATE, UART_TIMEOUT_S, 0, 2, 1)){ + fprintf(stderr,"ERROR in rc_sbus_init, failed to init uart bus\n"); + return -1; + } + + if(rc_pthread_create(&parse_thread, __parser_func, NULL, SCHED_OTHER, 0)){ + fprintf(stderr,"ERROR in rc_sbus_init, failed to start thread\n"); + return -1; + } + + #ifdef DEBUG + printf("sbus Thread created\n"); + #endif + + // wait for thread to start + for(i=0;i<20;i++){ + rc_usleep(20000); + if(init_flag) break; + } + return 0; +} + + +int rc_sbus_cleanup(void) +{ + int ret; + // just return if not running + if(!running){ + init_flag=0; + return 0; + } + // tell parser loop to stop + running = 0; + // allow up to 1 second for thread cleanup + ret=rc_pthread_timed_join(parse_thread,NULL,1.0); + if(ret==-1){ + fprintf(stderr,"ERORR in rc_sbus_cleanup, problem joining thread for pin\n"); + } + else if(ret==1){ + fprintf(stderr,"ERROR in rc_sbus_cleanup, thread exit timeout\n"); + fprintf(stderr,"most likely cause is your callback function is stuck and didn't return\n"); + } + init_flag=0; + return ret; +} + + +int rc_sbus_ch_raw(int ch) +{ + if(init_flag==0){ + fprintf(stderr,"ERROR in rc_sbus_ch_raw, call rc_sbus_init first\n"); + return -1; + } + if(ch<1 || ch>RC_MAX_SBUS_ANALOG_CHANNELS){ + fprintf(stderr,"ERROR in rc_sbus_ch_raw channel must be between 1 & %d",RC_MAX_SBUS_ANALOG_CHANNELS); + return -1; + } + new_sbus_flag = 0; + return analog_channels[ch-1]; +} + + +int rc_sbus_ch_binary(int ch) +{ + if(init_flag==0){ + fprintf(stderr,"ERROR in rc_sbus_ch_binary, call rc_sbus_init first\n"); + return -1; + } + if(ch<1 || ch>RC_MAX_SBUS_BINARY_CHANNELS){ + fprintf(stderr,"ERROR in rc_sbus_ch_binary channel must be between 1 & %d",RC_MAX_SBUS_BINARY_CHANNELS); + return -1; + } + new_sbus_flag = 0; + return binary_channels[ch-1]; +} + + +double rc_sbus_ch_normalized(int ch) +{ + if(init_flag==0){ + fprintf(stderr,"ERROR in rc_sbus_ch_normalized, call rc_sbus_init first\n"); + return -1.0; + } + if(ch<1 || ch>RC_MAX_SBUS_ANALOG_CHANNELS){ + fprintf(stderr,"ERROR in rc_sbus_ch_raw channel must be between 1 & %d",RC_MAX_SBUS_ANALOG_CHANNELS); + return -1.0; + } + // return 0 if there was a weird condition + if(fabs(range_up[ch-1]) < TOL || fabs(range_down[ch-1]) < TOL || analog_channels[ch-1]==0) return 0.0f; + + // mark data as read + new_sbus_flag = 0; + + if(analog_channels[ch-1]==centers[ch-1]) return 0.0; + if(analog_channels[ch-1]>centers[ch-1]) return (analog_channels[ch-1]-centers[ch-1])/range_up[ch-1]; + return (analog_channels[ch-1]-centers[ch-1])/range_down[ch-1]; +} + + +int rc_sbus_is_new_data(void) +{ + if(init_flag==0){ + fprintf(stderr,"ERROR in rc_sbus_is_new_data, call rc_sbus_init first\n"); + return 0; + } + return new_sbus_flag; +} + + +void rc_sbus_set_callback(void (*func)(void)) +{ + if(init_flag==0){ + fprintf(stderr,"ERROR in rc_sbus_set_callback, call rc_sbus_init first\n"); + } + new_data_callback = func; + return; +} + +void rc_sbus_set_disconnect_callback(void (*func)(void)) +{ + if(init_flag==0){ + fprintf(stderr,"ERROR in rc_sbus_set_disconnect_callback, call rc_sbus_init first\n"); + } + disconnect_callback = func; + return; +} + + +int rc_sbus_is_connection_active(void) +{ + if(init_flag==0){ + fprintf(stderr,"ERROR in rc_sbus_is_connection_active, call rc_sbus_init first\n"); + return 0; + } + return active_flag; +} + + +int64_t rc_sbus_nanos_since_last_packet(void){ + if(init_flag==0){ + fprintf(stderr,"ERROR in rc_sbus_nanos_since_last_packet, call rc_sbus_init first\n"); + return -1; + } + // if global variable last_time ==0 then no packet must have arrived yet + if(last_time==0) return -1; + return rc_nanos_since_boot()-last_time; +} + + +uint32_t rc_sbus_lost_frames (void) +{ + return lost_frames; +} + + +uint32_t rc_sbus_total_errors (void) +{ + return total_errors; +} + + +int rc_sbus_signal_quality (void) +{ + return signal_quality; +} + + +int rc_sbus_calibrate_routine(void) +{ + int i,ret; + FILE* fd; + + running = 1; // lets uarts 4 thread know it can run + last_time = 0; + active_flag = 0; + new_data_callback=NULL; + disconnect_callback=NULL; + + // make sure directory and calibration file exist and are writable first + ret = mkdir(CALIBRATION_DIR, 0777); + // error check, EEXIST is okay, we want directory to exist! + if(ret==-1 && errno!=EEXIST){ + perror("ERROR in rc_sbus_calibration_routine making calibration file directory"); + return -1; + } + + // 0.5s timeout disable canonical (0), 1 stop bit (1), disable parity (0) + if(rc_uart_init(SBUS_UART_BUS, SBUS_BAUD_RATE, 0.5, 0, 1, 0)){ + fprintf(stderr,"ERROR in rc_sbus_calibrate_routine, failed to init uart bus\n"); + return -1; + } + + pthread_create(&parse_thread, NULL, __parser_func, (void*) NULL); + + // wait for thread to start + i=0; + while(init_flag==0){ + rc_usleep(10000); + if(i>10){ + fprintf(stderr, "ERROR in rc_sbus_calibrate_routine, timeout waiting for parser thread to start\n"); + return -1; + } + i++; + } + + // display instructions + printf("\nRaw sbus data should display below if the transmitter and\n"); + printf("receiver are paired and working. Move all channels through\n"); + printf("their range of motion and the minimum and maximum values will\n"); + printf("be recorded. When you are finished moving all channels,\n"); + printf("return 3-position switches and sticks to their natural\n"); + printf("zero-position which will be recorded.\n\n"); + printf("Two position switches can be left in either position, and sliding\n"); + printf("throttle sticks should be left at the bottom of their travel.\n"); + printf("If there is a RATE switch, make sure it's in the HIGH position.\n\n"); + printf("If there is a DISARM switch which fixes the throttle position, leave\n"); + printf("it in the ARMED state and DO NOT TOUCH IT during calibration\n"); + printf("Press ENTER to save data or any other key to abort.\n\n"); + + // start listening + listening = 1; + pthread_t listening_thread; + pthread_create(&listening_thread, NULL, __calibration_listen_func, (void*) NULL); + + // wait for user to hit enter + ret = __continue_or_quit(); + + // stop listening + listening=0; + pthread_join(listening_thread, NULL); + + + // abort if user hit something other than enter + if(ret<0){ + fprintf(stderr,"aborting calibrate_sbus routine\n"); + return -1; + } + + // if it looks like no new data came in exit + if((mins[0]==0) || (mins[0]==maxes[0])){ + fprintf(stderr,"no new data recieved, exiting\n"); + return -1; + } + + // open for writing + fd = fopen(CALIBRATION_DIR SBUS_CALIBRATION_FILE, "w"); + if(fd == NULL){ + perror("ERROR in rc_sbus_calibration_routine opening calibration file for writing"); + return -1; + } + + // if new data was captures for a channel, write data to cal file + // otherwise fill in defaults for unused channels in case + // a higher channel radio is used in the future with this cal file + for(i=0;i