diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 2a70b9fc507a3c..4711dd51266b69 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -36,6 +36,7 @@ #include "AP_RCProtocol_GHST.h" #include "AP_RCProtocol_MAVLinkRadio.h" #include "AP_RCProtocol_Joystick_SFML.h" +#include "AP_RCProtocol_UDP_Packed16BitValues.h" #include #include @@ -96,6 +97,9 @@ void AP_RCProtocol::init() #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED backend[AP_RCProtocol::JOYSTICK_SFML] = new AP_RCProtocol_Joystick_SFML(*this); #endif +#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED + backend[AP_RCProtocol::UDP_PACKED16BITVALUES] = new AP_RCProtocol_UDP_Packed16BitValues(*this); +#endif } AP_RCProtocol::~AP_RCProtocol() @@ -444,6 +448,9 @@ bool AP_RCProtocol::new_input() #endif #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED AP_RCProtocol::JOYSTICK_SFML, +#endif +#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED + AP_RCProtocol::UDP_PACKED16BITVALUES, #endif }; for (const auto protocol : pollable) { @@ -584,6 +591,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED case JOYSTICK_SFML: return "SFML"; +#endif +#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED + case UDP_PACKED16BITVALUES: + return "UDP-Packed16BitValues"; #endif case NONE: break; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 633dfd802d8211..edc5e9ab19488b 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -83,6 +83,9 @@ class AP_RCProtocol { #endif #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED JOYSTICK_SFML = 16, +#endif +#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED + UDP_PACKED16BITVALUES = 17, #endif NONE //last enum always is None }; @@ -173,6 +176,9 @@ class AP_RCProtocol { #endif #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED case JOYSTICK_SFML: +#endif +#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED + case UDP_PACKED16BITVALUES: #endif case NONE: return false; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_UDP_Packed16BitValues.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_UDP_Packed16BitValues.cpp new file mode 100644 index 00000000000000..96d8c3700751da --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_UDP_Packed16BitValues.cpp @@ -0,0 +1,169 @@ +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED + +#include "AP_RCProtocol_UDP_Packed16BitValues.h" + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +void AP_RCProtocol_UDP_Packed16BitValues::set_default_pwm_input_values() +{ + pwm_input[0] = 1500; + pwm_input[1] = 1500; + pwm_input[2] = 1000; + pwm_input[3] = 1500; + pwm_input[4] = 1800; + pwm_input[5] = 1000; + pwm_input[6] = 1000; + pwm_input[7] = 1800; + +#if APM_BUILD_TYPE(APM_BUILD_Rover) + // set correct default throttle for rover (allowing for reverse) + pwm_input[2] = 1500; +#elif APM_BUILD_TYPE(APM_BUILD_ArduSub) || APM_BUILD_TYPE(APM_BUILD_Blimp) + for(uint8_t i = 0; i < 8; i++) { + pwm_input[i] = 1500; + } +#endif + + num_channels = 8; +} + +bool AP_RCProtocol_UDP_Packed16BitValues::init() +{ + const auto sitl = AP::sitl(); + if (sitl == nullptr) { + return false; + } + if (!rc_in.reuseaddress()) { + return false; + } + if (!rc_in.bind("0.0.0.0", sitl->rcin_port)) { + return false; + } + if (!rc_in.set_blocking(false)) { + return false; + } + if (!rc_in.set_cloexec()) { + return false; + } + + set_default_pwm_input_values(); + + return true; +} + +void AP_RCProtocol_UDP_Packed16BitValues::update() +{ + if (!init_done) { + if (!init()) { + return; + } + init_done = true; + } + + read_all_socket_input(); + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + const auto sitl = AP::sitl(); + if (sitl == nullptr) { + return; + } + + if (sitl->rc_fail == SITL::SIM::SITL_RCFail_NoPulses) { + return; + } +#endif + + // simulate RC input at 50Hz + if (AP_HAL::millis() - last_input_ms < 20) { + return; + } + last_input_ms = AP_HAL::millis(); + + add_input( + num_channels, + pwm_input, + false, // failsafe + 0, // check me + 0 // link quality + ); +} + +/* + check for a SITL RC input packet + */ +void AP_RCProtocol_UDP_Packed16BitValues::read_all_socket_input(void) +{ + struct pwm_packet { + uint16_t pwm[16]; + } pwm_pkt; + uint8_t pwm_pkt_num_channels = 0; + + ssize_t receive_size = 1; // lies! + uint16_t count = 0; + while (receive_size > 0) { + receive_size = rc_in.recv(&pwm_pkt, sizeof(pwm_pkt), 0); + + switch (receive_size) { + case -1: + break; + case 8*2: + case 16*2: + pwm_pkt_num_channels = receive_size/2; + break; + default: + fprintf(stderr, "Malformed SITL RC input (%ld)", (long)receive_size); + return; + } + count++; + } + + if (count > 100) { + ::fprintf(stderr, "Read %u rc inputs\n", count); + } + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + const auto sitl = AP::sitl(); + if (sitl == nullptr) { + return; + } + + // convert last packet received into pwm values + switch (sitl->rc_fail) { + case SITL::SIM::SITL_RCFail_Throttle950: + // discard anything we just read from the "receiver" and set + // values to bind values: + for (uint8_t i=0; i +#include + +class AP_RCProtocol_UDP_Packed16BitValues : public AP_RCProtocol_Backend { +public: + + using AP_RCProtocol_Backend::AP_RCProtocol_Backend; + + void update() override; + +private: + + bool init(); + bool init_done; + + uint32_t last_input_ms; + + void read_all_socket_input(void); + SocketAPM_native rc_in{true}; // "true" means "datagram" + + // these are the values that will be fed into the autopilot. + // Packets we receive usually only contain a subset of channel + // values to insert into here: + uint16_t pwm_input[16]; + uint8_t num_channels; + + void set_default_pwm_input_values(); +}; + + +#endif // AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index 168f289f1134ef..fdc8171e5d7833 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -79,3 +79,6 @@ #define AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 && HAL_GCS_ENABLED #endif +#ifndef AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED +#define AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif