diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..4bc9dbd --- /dev/null +++ b/.clang-format @@ -0,0 +1,63 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: '-4' +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: 'true' +AlignEscapedNewlines: Left +AlignOperands: 'true' +AlignTrailingComments: 'true' +AllowShortBlocksOnASingleLine: 'false' +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: 'true' +AllowShortLoopsOnASingleLine: 'true' +AlwaysBreakAfterReturnType: None +AlwaysBreakTemplateDeclarations: 'false' +BinPackArguments: 'false' +BinPackParameters: 'true' +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: true + BeforeElse: true + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBraces: Custom +BreakBeforeInheritanceComma: 'false' +BreakConstructorInitializers: AfterColon +CompactNamespaces: 'false' +ContinuationIndentWidth: '4' +Cpp11BracedListStyle: 'true' +FixNamespaceComments: 'true' +IncludeBlocks: Merge +IndentCaseLabels: 'false' +IndentWidth: '4' +IndentWrappedFunctionNames: 'false' +KeepEmptyLinesAtTheStartOfBlocks: 'false' +Language: Cpp +NamespaceIndentation: None +PointerAlignment: Right +ReflowComments: 'true' +SpaceAfterCStyleCast: 'false' +SpaceAfterTemplateKeyword: 'true' +SpaceBeforeAssignmentOperators: 'true' +SpaceBeforeParens: Never +SpaceInEmptyParentheses: 'false' +SpacesInAngles: 'false' +SpacesInCStyleCastParentheses: 'false' +SpacesInContainerLiterals: 'false' +SpacesInParentheses: 'false' +SpacesInSquareBrackets: 'false' +Standard: Cpp11 +TabWidth: '4' +UseTab: Never + +... diff --git a/CMakeLists.txt b/CMakeLists.txt index 81320d3..88eaf3f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,11 +1,11 @@ cmake_minimum_required(VERSION 3.1) project(msp) -set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() if(UNIX) @@ -18,6 +18,9 @@ if(WIN32) include_directories(${ASIO_HEADER_PATH}) endif() +OPTION(BUILD_EXAMPLES "Build Library with examples" ON) +OPTION(BUILD_TESTS "Build Library with tests" OFF) + find_package(Threads) set(MSP_SOURCE_DIR src) @@ -28,15 +31,8 @@ include_directories(${MSP_INCLUDE_DIR}) ################################################################################ ### libraries -# low-level API -add_library(msp ${MSP_SOURCE_DIR}/MSP.cpp) -target_link_libraries(msp ${CMAKE_THREAD_LIBS_INIT}) - -# printing message content -add_library(msp_msg_print ${MSP_SOURCE_DIR}/msg_print.cpp) - # client library -add_library(mspclient ${MSP_SOURCE_DIR}/Client.cpp) +add_library(mspclient ${MSP_SOURCE_DIR}/Client.cpp ${MSP_SOURCE_DIR}/PeriodicTimer.cpp) target_link_libraries(mspclient ${CMAKE_THREAD_LIBS_INIT}) # high-level API @@ -47,54 +43,46 @@ target_link_libraries(msp_fcu mspclient) ################################################################################ ### examples / tests -# test reading speed -add_executable(msp_connection_test examples/msp_connection_test.cpp) -target_link_libraries(msp_connection_test msp) +if(BUILD_EXAMPLES) -# test and print all requests -add_executable(msp_read_test examples/msp_read_test.cpp) -target_link_libraries(msp_read_test msp msp_msg_print) + # testing publish/subscribe + add_executable(fcu_test examples/fcu_test.cpp) + target_link_libraries(fcu_test msp_fcu) -# testing publish/subscribe -add_executable(fcu_test examples/fcu_test.cpp) -target_link_libraries(fcu_test msp_fcu msp_msg_print) + # test arming and disarming + add_executable(fcu_arm examples/fcu_arm_test.cpp) + target_link_libraries(fcu_arm msp_fcu) -# test arming and disarming -add_executable(fcu_arm examples/fcu_arm_test.cpp) -target_link_libraries(fcu_arm msp_fcu) + # test setting motors directly + add_executable(fcu_motors examples/fcu_motor_test.cpp) + target_link_libraries(fcu_motors msp_fcu) -# test setting motors directly -add_executable(fcu_motors examples/fcu_motor_test.cpp) -target_link_libraries(fcu_motors msp_fcu) + # subscribing with custom type + add_executable(fcu_custom_type examples/fcu_custom_type.cpp) + target_link_libraries(fcu_custom_type msp_fcu) -# subscribing with custom type -add_executable(fcu_custom_type examples/fcu_custom_type.cpp) -target_link_libraries(fcu_custom_type msp_fcu) + # client test for asynchronous callbacks + add_executable(client_async_test examples/client_async_test.cpp) + target_link_libraries(client_async_test mspclient ) -# cleanflight requests -add_executable(cleanflight_read_test examples/cleanflight_read_test.cpp) -target_link_libraries(cleanflight_read_test msp msp_msg_print) - -# client test for asynchronous callbacks -add_executable(client_async_test examples/client_async_test.cpp) -target_link_libraries(client_async_test mspclient msp_msg_print) - -# client test for blocking read -add_executable(client_read_test examples/client_read_test.cpp) -target_link_libraries(client_read_test mspclient msp_msg_print) + # client test for blocking read + add_executable(client_read_test examples/client_read_test.cpp) + target_link_libraries(client_read_test mspclient ) +endif() ################################################################################ ### installation -install(TARGETS msp msp_msg_print msp_fcu mspclient +#install(TARGETS msp msp_msg_print msp_fcu mspclient +install(TARGETS msp_fcu mspclient LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) install(DIRECTORY ${MSP_INCLUDE_DIR} DESTINATION include/ FILES_MATCHING PATTERN "*.hpp") SET(PKG_CONFIG_LIBDIR "\${prefix}/lib" ) SET(PKG_CONFIG_INCLUDEDIR "\${prefix}/include/" ) -SET(PKG_CONFIG_LIBS "-L\${libdir} -lmsp -lmsp_fcu -lmspclient -lmsp_msg_print" ) +SET(PKG_CONFIG_LIBS "-L\${libdir} -lmsp_fcu -lmspclient" ) SET(PKG_CONFIG_CFLAGS "-I\${includedir}" ) CONFIGURE_FILE( @@ -104,3 +92,52 @@ CONFIGURE_FILE( INSTALL(FILES "${CMAKE_BINARY_DIR}/${PROJECT_NAME}.pc" DESTINATION lib/pkgconfig) + + + +############################################################################### +### testing +if(BUILD_TESTS) + enable_testing() + + # Download and unpack googletest at configure time + configure_file(CMakeLists.txt.in googletest/CMakeLists.txt) + execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . + RESULT_VARIABLE result + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/googletest ) + if(result) + message(FATAL_ERROR "CMake step for googletest failed: ${result}") + endif() + execute_process(COMMAND ${CMAKE_COMMAND} --build . + RESULT_VARIABLE result + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/googletest ) + if(result) + message(FATAL_ERROR "Build step for googletest failed: ${result}") + endif() + + # Prevent overriding the parent project's compiler/linker + # settings on Windows + set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) + + # Add googletest directly to our build. This defines + # the gtest and gtest_main targets. + add_subdirectory(/usr/src/gtest + ${CMAKE_BINARY_DIR}/googletest-build + EXCLUDE_FROM_ALL) + + # The gtest/gtest_main targets carry header search path + # dependencies automatically when using CMake 2.8.11 or + # later. Otherwise we have to add them here ourselves. + if (CMAKE_VERSION VERSION_LESS 2.8.11) + include_directories("${gtest_SOURCE_DIR}/include") + endif() + + add_executable(value_test test/value_test.cpp) + target_link_libraries(value_test gtest_main) + add_test(NAME value_test COMMAND value_test) + + add_executable(bytevector_test test/ByteVector_test.cpp) + target_link_libraries(bytevector_test gtest_main) + add_test(NAME bytevector_test COMMAND bytevector_test) + +endif() diff --git a/CMakeLists.txt.in b/CMakeLists.txt.in new file mode 100644 index 0000000..bc81dda --- /dev/null +++ b/CMakeLists.txt.in @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 2.8.2) + +project(googletest NONE) + +include(ExternalProject) +ExternalProject_Add(googletest + SOURCE_DIR "/usr/src/gtest" + BINARY_DIR "${CMAKE_BINARY_DIR}/googletest-build" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" +) diff --git a/README.md b/README.md index b213f78..d8109b2 100644 --- a/README.md +++ b/README.md @@ -39,109 +39,35 @@ You can connect to Arduino or Naze32 boards either by (1) using a built-in USB-t - change the update rate for the serial task in the range 100 ... 2000Hz - e.g. to 2000Hz: `set serial_update_rate_hz=2000`, then `save` - it might be necessary to increase the serial baudrate in the Ports configuration tab -- test communication with higher baudrate: `./get_msp_info /dev/ttyUSB0 1000000` ### Sending and Receiving RC commands - activate feature `RX_MSP` - via GUI: `Configuration` -> `Receiver` -> `Receiver Mode` -> `MSP RX input` - via CLI: execute `feature RX_MSP` - - via API: `FlightController::enableRxMSP()` and reboot Beginning with Cleanflight 2 and Betaflight 3, `MSP_SET_RAW_RC` messages are ignored on some targets with insufficient flash memory (like the naze32). You can verify this, if `MSP_RC` messages return an empty list of channels. To activate `MSP_SET_RAW_RC` messages on these targets, you need to add `#define USE_RX_MSP` to your `target.h`, e.g. `src/main/target/NAZE/target.h`. -## How to use the library (low-level API) +## FlightContoller API -You first need to instantiate the driver with the path to the device: -```C++ -msp::MSP msp(path_to_device); -``` - -### Sending and Receiving Raw Data -The library contains methods for sending data to and for receiving data from the flight controller (FC). These methods communicate once and indicate success by return values and exceptions. - -Sending raw data to the FC: -```C++ -bool sendData(const uint8_t id, const ByteVector &data) -``` - -Receiving data from the FC: -```C++ -DataID receiveData() -``` -`struct DataID` contains the id and data of the received message. - -### Communication Pattern -The communication with the FC follows the remote procedure call pattern where we can either request data from the FC or respond with data to the FC. - -This process is automated using two types of message: `Request` (receive and decode data) and `Response` (encode and send data). - -#### Request data from FC -For requesting data from the FC (FC →), a message with the id of that command is send to the FC and then we need to wait for a message with the same id containing the requested payload. - -Instantiate any message that inherits from `Request` and pass it to: -```C++ -bool request(msp::Request &request) -``` -This method returns after the first try to read and encode any MSP message. Therefore, there exist alternative implementations of this communication pattern, for example to block until a valid package with correct id has been received: -```C++ -bool request_block(msp::Request &request) -``` -or to retry sending requests until a response is received: -```C++ -bool request_wait(msp::Request &request, uint wait_ms) -``` -which can be useful to block until the FC is available and responds to messages. - -E.g. the Arduino Nano 3.0 is reseted when opening the serial device and needs some time to boot until it will respond to messages. `request_wait` will return when MultiWii is booted and able to respond to messages. From there on, `request_block` can be used to fetch data from the FC. - -#### Respond with data to FC -For sending data to the FC (→ FC) a message containing the id and the payload is send to the FC and confirmed by a acknowledge message which only contains the id with no data. - -Instantiate any message that inherits from `Response` and pass it to: -```C++ -bool respond(msp::Response &response) -``` - -Messages and the encoding/decoding methods are defined in `msp_msg.hpp`. - -### Examples - -#### Get MultiWii version and multi-copter type - -We will use the command MSP_IDENT (100) to get the version, multi-copter type and it capabilities. - -Instantiate the driver: -```C++ -msp::MSP msp(path_to_device); -``` -create request message and send the request to the FC: -```C++ -msp::Ident ident; -msp.request_block(ident); -``` -When the call to `request_block` returns, the values of structure `ident` will be populated can be accessed: -```C++ -std::cout<<"MSP version "<<(int)ident.version< -fcu::FlightController fcu("/dev/ttyUSB0", 115200); +fcu::FlightController fcu; -// wait for connection and setup -fcu.initialise(); +// do connection and setup +fcu.connect("/dev/ttyUSB0", 115200); ``` ### Periodic request for messages +Messages that need to be queried periodically can be handled automatically using a subscription. This can be done with a class method or a stand-alone function. + -Define a class that holds callback functions to process information of received message: +#### Class method pattern +Define a class that holds callback functions to process information of received message. The function signature is `void (const & )`. ```C++ class App { public: @@ -165,15 +91,44 @@ fcu.subscribe(&App::onImu, &app, 0.01); Requests are sent to and processed by the flight controller as fast as possible. It is important to note that the MultiWii FCU only processed a single message per cycle. All subscribed messages therefore share the effective bandwidth of 1/(2800 us) = 357 messages per second. -### Request and Send Messages -Additional messages that are not requested periodically can be requested by the method +#### Lambda pattern + +The `FlightController::subscribe` method is restricted to callbacks which return `void` and take an argument of `const msp::Message&`. In order to call a method that doesn't match that signature (maybe it needs additional information), it sometimes is useful to wrap the non-compliant method in a lambda that matches the expected signature. + +```C++ +auto callback = [](const msp::msg::RawImu& imu){ + // ImuSI is not a subclass of msp::Message, so it's not suitable for use as a callback argument + std::cout << msp::msg::ImuSI(imu, 512.0, 1.0/4.096, 0.92f/10.0f, 9.80665f); +} + +fcu.subscribe(callback, 0.1); +``` + + +### Manual sending of messages +Additional messages that are not sent periodically can be dispatched by the method ```C++ -bool request(msp::Request &request, const double timeout = 0) +bool sendMessage(msp::Message &message, const double timeout = 0) ``` -You need to instantiate a message of type `msp::Request` and provide it to the method with an optional timeout. +You need to instantiate an instance of the object matching the message you want to send and provide it to the method with an optional timeout. If the timeout paramter is 0, then the method will wait (possibly forever) until a response is received from the flight controller. A strictly positive value will limit the waiting to the specified interval. -Response messages are sent by ```C++ -bool respond(const msp::Response &response, const bool wait_ack=true) +msp::Status status; +if (fcu.sendMessage(status) ) { + // status will contain the values returned by the flight controller +} + +msp::SetCalibrationData calibration; +calibration.acc_zero_x = 0; +calibration.acc_zero_y = 0; +calibration.acc_zero_x = 0; +calibration.acc_gain_x = 1; +calibration.acc_gain_y = 1; +calibration.acc_gain_z = 1; +if (fcu.sendMessage(calibration) ) { + // calibration data has been sent + // since this message does not have any return data, the values are unchanged +} ``` -where the method will block until an acknowledge is received if `wait_ack=true` (default). + +If the message is of a type that has a data response from the flight controller, the instance of the message provided to the `sendMessage` call will contain the values unpacked from the flight controller's response. diff --git a/examples/cleanflight_read_test.cpp b/examples/cleanflight_read_test.cpp deleted file mode 100644 index 6418d57..0000000 --- a/examples/cleanflight_read_test.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include -#include - -#include -#include -#include - -int main(int argc, char *argv[]) { - const std::string device = (argc>1) ? std::string(argv[1]) : "/dev/ttyUSB0"; - const size_t baudrate = (argc>2) ? std::stoul(argv[2]) : 115200; - - msp::MSP msp(device, baudrate); - - // wait for established connection - { - msp::msg::Ident ident; - msp.request_wait(ident, 100); - } - - std::cout << "MSP ready..." << std::endl; - - msp::msg::ApiVersion api_version; - if(msp.request_block(api_version)) - std::cout << api_version << std::endl; - else - std::cerr << "Could not determine Cleanflight API version." << std::endl; - - msp::msg::FcVariant fc_variant; - if(msp.request_block(fc_variant)) - std::cout << fc_variant << std::endl; - - msp::msg::FcVersion fc_version; - if(msp.request_block(fc_version)) - std::cout << fc_version << std::endl; - - msp::msg::BoardInfo board_info; - if(msp.request_block(board_info)) - std::cout << board_info << std::endl; - - msp::msg::BuildInfo build_info; - if(msp.request_block(build_info)) - std::cout << build_info << std::endl; - - msp::msg::RxConfig rx_config; - msp.request_block(rx_config); - - msp::msg::Feature feature; - if(msp.request_block(feature)) - std::cout< #include - #include -#include struct SubCallbacks { - void onIdent(const msp::msg::Ident& ident) { - std::cout<1) ? std::string(argv[1]) : "/dev/ttyUSB0"; - const size_t baudrate = (argc>2) ? std::stoul(argv[2]) : 115200; +int main(int argc, char* argv[]) { + const std::string device = + (argc > 1) ? std::string(argv[1]) : "/dev/ttyUSB0"; + const size_t baudrate = (argc > 2) ? std::stoul(argv[2]) : 115200; SubCallbacks subs; msp::client::Client client; - client.connect(device, baudrate); - client.start(); - - // using class method callback -// client.subscribe(&SubCallbacks::onImu, &subs, 0.1); - - // using lambda callback with stored lambda object -// const auto imu_cb1 = [](const msp::msg::ImuRaw& imu){ -// std::cout<(imu_cb1, 0.1); + client.start(device, baudrate); // using lambda callback with stored function object - const std::function imu_cb2 = [](const msp::msg::ImuRaw& imu){ - std::cout< imu_cb2 = + [](const msp::msg::RawImu& imu) { + std::cout << imu; + std::cout << msp::msg::ImuSI( + imu, 512.0, 1.0 / 4.096, 0.92f / 10.0f, 9.80665f); + }; client.subscribe(imu_cb2, 0.1); client.subscribe(&SubCallbacks::onIdent, &subs, 10); @@ -121,8 +92,8 @@ int main(int argc, char *argv[]) { client.subscribe(&SubCallbacks::onServo, &subs, 0.1); client.subscribe(&SubCallbacks::onMotor, &subs, 0.1); client.subscribe(&SubCallbacks::onRc, &subs, 0.1); - client.subscribe(&SubCallbacks::onAttitude, &subs); - client.subscribe(&SubCallbacks::onAltitude, &subs); + client.subscribe(&SubCallbacks::onAttitude, &subs, 0.1); + client.subscribe(&SubCallbacks::onAltitude, &subs, 0.1); client.subscribe(&SubCallbacks::onAnalog, &subs, 10); client.subscribe(&SubCallbacks::onRcTuning, &subs, 20); client.subscribe(&SubCallbacks::onPID, &subs, 20); @@ -133,7 +104,7 @@ int main(int argc, char *argv[]) { client.subscribe(&SubCallbacks::onPidNames, &subs, 20); client.subscribe(&SubCallbacks::onBoxIds, &subs, 20); client.subscribe(&SubCallbacks::onServoConf, &subs, 20); - client.subscribe(&SubCallbacks::onDebugMessage, &subs,1); + client.subscribe(&SubCallbacks::onDebugMessage, &subs, 1); client.subscribe(&SubCallbacks::onDebug, &subs, 1); // Ctrl+C to quit diff --git a/examples/client_read_test.cpp b/examples/client_read_test.cpp index 0803550..9ab1152 100644 --- a/examples/client_read_test.cpp +++ b/examples/client_read_test.cpp @@ -1,141 +1,148 @@ #include -#include -#include - #include +#include int main(int argc, char *argv[]) { - const std::string device = (argc>1) ? std::string(argv[1]) : "/dev/ttyUSB0"; - const size_t baudrate = (argc>2) ? std::stoul(argv[2]) : 115200; + const std::string device = + (argc > 1) ? std::string(argv[1]) : "/dev/ttyUSB0"; + const size_t baudrate = (argc > 2) ? std::stoul(argv[2]) : 115200; msp::client::Client client; - client.setPrintWarnings(true); - client.connect(device, baudrate); - client.start(); + client.setLoggingLevel(msp::client::LoggingLevel::WARNING); + client.setVariant(msp::FirmwareVariant::INAV); + client.setVersion(2); + client.start(device, baudrate); - msp::msg::Ident ident; - if(client.request(ident)==1) - std::cout< - -#include #include - +#include int main(int argc, char *argv[]) { - const std::string device = (argc>1) ? std::string(argv[1]) : "/dev/ttyUSB0"; - const size_t baudrate = (argc>2) ? std::stoul(argv[2]) : 115200; + const std::string device = + (argc > 1) ? std::string(argv[1]) : "/dev/ttyUSB0"; + const size_t baudrate = (argc > 2) ? std::stoul(argv[2]) : 115200; std::chrono::high_resolution_clock::time_point start, end; - bool feature_changed = false; -start: - fcu::FlightController fcu(device, baudrate); + // bool feature_changed = false; + // start: + std::cout << "making FC" << std::endl; + fcu::FlightController fcu; // wait until connection is established // get unique box IDs start = std::chrono::high_resolution_clock::now(); - fcu.initialise(); + fcu.connect(device, baudrate); end = std::chrono::high_resolution_clock::now(); - std::cout<<"ready after: "<(end-start).count()<<" ms"<(end - + start) + .count() + << " ms" << std::endl; + /* + // on cleanflight, we need to enable the "RX_MSP" feature + if(fcu.isFirmwareCleanflight()) { + msp::msg::Feature feature_in; + if(fcu.request(feature_in)) { + std::cout << "features: "; + for (auto f : feature_in.features) + std::cout << f << " "; + std::cout << std::endl; + } + int rc = fcu.enableRxMSP(); + std::cout << "enableRxMSP returned " << rc <(end-start).count()<<" ms"<( + end - start) + .count() + << " ms" << std::endl; } // disarm the FC - std::cout<<"Disarming..."<(end-start).count()<<" ms"<( + end - start) + .count() + << " ms" << std::endl; } } diff --git a/examples/fcu_custom_type.cpp b/examples/fcu_custom_type.cpp index b0d4bd6..8dacb0b 100644 --- a/examples/fcu_custom_type.cpp +++ b/examples/fcu_custom_type.cpp @@ -1,35 +1,37 @@ #include - #include -struct MyIdent : public msp::Request { +struct MyIdent : public msp::Message { + MyIdent(msp::FirmwareVariant v) : Message(v) {} + msp::ID id() const { return msp::ID::MSP_IDENT; } msp::ByteVector raw_data; - void decode(const msp::ByteVector &data) { + virtual bool decode(const msp::ByteVector &data) override { raw_data = data; + return true; } - }; struct Callbacks { void onIdent(const MyIdent &ident) { std::cout << "Raw Ident data: "; for(auto d : ident.raw_data) { - std::cout << int(d) << ","; + std::cout << int(d) << " "; } std::cout << std::endl; } }; int main(int argc, char *argv[]) { - const std::string device = (argc>1) ? std::string(argv[1]) : "/dev/ttyUSB0"; - const size_t baudrate = (argc>2) ? std::stoul(argv[2]) : 115200; + const std::string device = + (argc > 1) ? std::string(argv[1]) : "/dev/ttyUSB0"; + const size_t baudrate = (argc > 2) ? std::stoul(argv[2]) : 115200; Callbacks cbs; - fcu::FlightController fcu(device, baudrate); - fcu.initialise(); + fcu::FlightController fcu; + fcu.connect(device, baudrate); // subscribe with costum type fcu.subscribe(&Callbacks::onIdent, &cbs, 1); diff --git a/examples/fcu_motor_test.cpp b/examples/fcu_motor_test.cpp index 80f80bb..43d22aa 100644 --- a/examples/fcu_motor_test.cpp +++ b/examples/fcu_motor_test.cpp @@ -15,17 +15,18 @@ #include int main(int argc, char *argv[]) { - const std::string device = (argc>1) ? std::string(argv[1]) : "/dev/ttyUSB0"; - const size_t baudrate = (argc>2) ? std::stoul(argv[2]) : 115200; + const std::string device = + (argc > 1) ? std::string(argv[1]) : "/dev/ttyUSB0"; + const size_t baudrate = (argc > 2) ? std::stoul(argv[2]) : 115200; - fcu::FlightController fcu(device, baudrate); - fcu.initialise(); + fcu::FlightController fcu; + fcu.connect(device, baudrate); // spin motors 1 to 4 - fcu.setMotors({1100,1100,1100,1100,0,0,0,0}); + fcu.setMotors({1100, 1100, 1100, 1100, 0, 0, 0, 0}); std::this_thread::sleep_for(std::chrono::seconds(1)); // stop motors - fcu.setMotors({1000,1000,1000,1000,1000,1000,1000,1000}); + fcu.setMotors({1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000}); } diff --git a/examples/fcu_test.cpp b/examples/fcu_test.cpp index 56b2016..e85b151 100644 --- a/examples/fcu_test.cpp +++ b/examples/fcu_test.cpp @@ -1,126 +1,98 @@ #include -#include -#include - #include +#include class App { public: std::string name; - App(const std::string name, const float acc_1g, const float gyro_unit, const float magn_gain, const float si_unit_1g) : acc_1g(acc_1g), gyro_unit(gyro_unit), magn_gain(magn_gain), si_unit_1g(si_unit_1g) { - this->name = name; - } + App(const std::string name) { this->name = name; } - void onIdent(const msp::msg::Ident& ident) { - std::cout<1) ? std::string(argv[1]) : "/dev/ttyUSB0"; - const size_t baudrate = (argc>2) ? std::stoul(argv[2]) : 115200; - - fcu::FlightController fcu(device, baudrate); +int main(int argc, char* argv[]) { + const std::string device = + (argc > 1) ? std::string(argv[1]) : "/dev/ttyUSB0"; + const size_t baudrate = (argc > 2) ? std::stoul(argv[2]) : 115200; + fcu::FlightController fcu; + fcu.setLoggingLevel(msp::client::LoggingLevel::INFO); // wait for connection - fcu.initialise(); + fcu.connect(device, baudrate); + + App app("MultiWii"); + + fcu.subscribe( + [](const msp::msg::RawImu& imu) { + std::cout << msp::msg::ImuSI( + imu, 512.0, 1.0 / 4.096, 0.92f / 10.0f, 9.80665f); + }, + 0.1); - App app("MultiWii", 512.0, 1.0/4.096, 0.92f/10.0f, 9.80665f); // define subscriptions with specific period fcu.subscribe(&App::onIdent, &app, 10); fcu.subscribe(&App::onStatus, &app, 1); // using class method callback - //fcu.subscribe(&App::onImu, &app, 0.1); - - // using lambda callback - fcu.subscribe([](const msp::msg::ImuRaw& imu){ - std::cout< -#include -#include - -#include - -int main(int argc, char *argv[]) { - const std::string device = (argc>1) ? std::string(argv[1]) : "/dev/ttyUSB0"; - const size_t baudrate = (argc>2) ? std::stoul(argv[2]) : 115200; - - msp::MSP msp(device, baudrate); - msp.setWait(1); - - // try connecting until first package is received - { - std::cout<<"Waiting for flight controller to become ready..."<(end-start).count()<<" ms"<(end-start).count(); - - std::cout<<"read "< -#include - -#include - -#include - -int main(int argc, char *argv[]) { - const std::string device = (argc>1) ? std::string(argv[1]) : "/dev/ttyUSB0"; - const size_t baudrate = (argc>2) ? std::stoul(argv[2]) : 115200; - - msp::MSP msp(device, baudrate); - msp.setWait(1); - - // wait for flight controller to become ready - { - std::cout<<"Connecting FCU..."< +#include +#include +#include +#include +#include "Value.hpp" + +namespace msp { + +struct Packable; + +class ByteVector : public std::vector { +public: + /** + * @brief ByteVector constructor + */ + ByteVector() : offset(0) {} + + /** + * @brief ByteVector constructor + * @param arg1 Single argument passed to constructor of parent std::vector + * class + */ + template + ByteVector(T1 arg1) : std::vector(arg1), offset(0) {} + + /** + * @brief ByteVector constructor + * @param arg1 First of two arguments passed to constructor of parent + * std::vector class + * @param arg2 Second of two arguments passed to constructor of parent + * std::vector class + */ + template + ByteVector(T1 arg1, T2 arg2) : + std::vector(arg1, arg2), + offset(0) {} + + /** + * @brief Packs integer types into the ByteVector. Ensures little endian + * packing. + * @tparam T Underlying data type to be packed. Must be an integral type. + * @param val Value to be packed + * @return true + */ + template ::value, + T>::type* = nullptr> + bool pack(const T& val) { + // little endian packing + for(size_t i(0); i < sizeof(val); ++i) { + this->push_back(val >> (i * 8) & 0xFF); + } + return true; + } + + /** + * @brief Packs floating point types into the ByteVector. Uses IEEE 754 + * format. + * @tparam T Underlying data type to be packed. Must be a floating point + * type. + * @param val Value to be packed + * @return True if successful + */ + template ::value, + T>::type* = nullptr> + bool pack(const T& val) { + ByteVector b(sizeof(val)); + reinterpret_cast(b.front()) = val; + return this->pack(b); + } + + /** + * @brief Packs scaled values (e.g. float to scaled int) as packed_val = + * (val+offset)*scale + * @tparam encoding_T Data type to use for the actual data packing (usually + * an integral type) + * @tparam T1 Type of input value (usually a floating point type) + * @tparam T2 Type of scale and offset coefficients + * @param val Value to be packed + * @param scale Value of scaling to apply to the offset value + * @param offset Value of offset to apply to the input value (optional, + * defaults to 0) + * @return True if successful + */ + template ::value, + T1>::type* = nullptr, + typename std::enable_if::value, + T2>::type* = nullptr> + bool pack(const T1 val, const T2 scale, const T2 offset = 0) { + auto tmp = (val + offset) * scale; + if(tmp <= std::numeric_limits::min()) + return pack(std::numeric_limits::min()); + else if(tmp >= std::numeric_limits::max()) + return pack(std::numeric_limits::max()); + return pack(static_cast(tmp)); + } + + /** + * @brief Packs string data into the ByteVector. + * @param val String to be packed + * @param max_len Optional max number of characters to transfer into the + * ByteVector + * @return True if successful + */ + bool pack(const std::string& val, + size_t max_len = std::numeric_limits::max()) { + size_t count = 0; + for(auto c : val) { + this->push_back(c); + if(++count == max_len) break; + } + // TODO: validate that this null termination is the right thing to do in + // all cases definitly correct in tested cases + this->push_back(0); + return true; + } + + /** + * @brief Packs another ByteVector into the ByteVector. + * @param data ByteVector to be packed + * @param max_len Optional max number of characters to transfer into the + * ByteVector + * @return True if successful + */ + bool pack(const ByteVector& data, + size_t max_len = std::numeric_limits::max()) { + size_t count = 0; + for(auto c : data) { + this->push_back(c); + if(++count == max_len) break; + } + return true; + } + + /** + * @brief Packs an an object which inherits from type Packable into the + * ByteVector + * @param val Reference to object to be packed + * @return True if successful + */ + template ::value, + T>::type* = nullptr> + bool pack(const T& val) { + return val.pack_into(*this); + } + + /** + * @brief Packs scaled value types (e.g. value to scaled int) as + * packed_val = (val+offset)*scale + * @tparam encoding_T Data type to use for the actual data packing (usually + * an integral type) + * @tparam T1 Type of input value (usually a floating point type) + * @tparam T2 Type of scale and offset coefficients + * @param val Value to be packed + * @param scale Value of scaling to apply to the offset value + * @param offset Value of offset to apply to the input value (optional, + * defaults to 0) + * @return True if successful + */ + template ::value, + T1>::type* = nullptr, + typename std::enable_if::value, + T2>::type* = nullptr> + bool pack(const Value val, const T2 scale, const T2 offset = 0) { + if(!val.set()) return false; + return pack(val(), scale, offset); + } + + /** + * @brief Packs a Value into the ByteVector. + * @param val The Value to be packed + * @param max_len Optional max number of characters to transfer into the + * ByteVector + * @return True if successful + */ + bool pack(const Value& val, + size_t max_len = std::numeric_limits::max()) { + if(!val.set()) return false; + return pack(val(), max_len); + } + + /** + * @brief Packs a Value into the ByteVector. + * @param val The Value to be packed + * @param max_len Optional max number of characters to transfer into the + * ByteVector + * @return True if successful + */ + bool pack(const Value& val, + size_t max_len = std::numeric_limits::max()) { + if(!val.set()) return false; + return pack(val(), max_len); + } + + /** + * @brief Packs the contents of a Value into the ByteVector. + * @tparam T Type of the Value being packed. May be automatically deduced + * from arguments + * @param val The Value to be packed + * @return True if successful + */ + template bool pack(const Value& val) { + if(!val.set()) return false; + return pack(val()); + } + + /** + * @brief Extracts little endian integers from the ByteVector. Consumes + * a number of bytes matching sizeof(T). Fails if not enough bytes + * are available. + * @tparam T Underlying data type to be extracted. Must be an integral type. + * @param val Destination of unpack operation. + * @return True on successful unpack + */ + template ::value, + T>::type* = nullptr> + bool unpack(T& val) const { + if(unpacking_remaining() < sizeof(val)) return false; + val = 0; + for(size_t i(0); i < sizeof(val); ++i) { + val |= (*this)[offset++] << (8 * i); + } + return true; + } + + /** + * @brief Extracts floating point numbers from the ByteVector. + * Consumes a number of bytes matching sizeof(T). Fails if not enough + * bytes are available. + * @tparam T Underlying data type to be extracted. Must be a floating point + * type. + * @param val Destination of unpack operation. + * @return True on successful unpack + */ + template ::value, + T>::type* = nullptr> + bool unpack(T& val) const { + if(unpacking_remaining() < sizeof(val)) return false; + val = reinterpret_cast((*this)[offset]); + offset += sizeof(val); + return true; + } + + /** + * @brief Extracts data from the ByteVector and stores it in a + * std::string. Consumes all remaining data unless instructed otherwise. + * @param val Destination of unpack operation. + * @param count Max number of bytes to extract. Optional, if unset, all + * remaining bytes will be consumed. + * @return True on successful unpack + */ + bool unpack(std::string& val, + size_t count = std::numeric_limits::max()) const { + if(count == std::numeric_limits::max()) + count = unpacking_remaining(); + if(count > unpacking_remaining()) return false; + bool rc = true; + val.clear(); + int8_t tmp; + for(size_t i = 0; i < count; ++i) { + rc &= unpack(tmp); + val += tmp; + } + return rc; + } + + /** + * @brief Extracts data from the ByteVector and stores it in a + * another ByteVector. Consumes all remaining data unless instructed + * otherwise. + * @param val Destination of unpack operation. + * @param count Max number of bytes to extract. Optional, if unset, all + * remaining bytes will be consumed. + * @return True on successful unpack + */ + bool unpack(ByteVector& val, + size_t count = std::numeric_limits::max()) const { + if(count == std::numeric_limits::max()) + count = unpacking_remaining(); + if(!consume(count)) return false; + val.clear(); + val.insert( + val.end(), unpacking_iterator(), unpacking_iterator() + count); + return true; + } + + /** + * @brief Unpacks scaled value types (e.g. scaled int to floating point) as + * val = (packed_val/scale)-offset + * @tparam encoding_T data type used to store the scaled value (usually an + * integral type) + * @tparam T1 type of output value (usually a floating point type) + * @tparam T2 type of scale and offset coefficients + * @param val Destination of unpack operation + * @param scale Value of scaling to apply to the offset value + * @param offset Value of offset to apply to the input value (optional, + * defaults to 0) + * @return True if successful + */ + template ::value, + T1>::type* = nullptr, + typename std::enable_if::value, + T2>::type* = nullptr> + bool unpack(T1& val, T2 scale, T2 offset = 0) const { + bool rc = true; + encoding_T tmp = 0; + rc &= unpack(tmp); + val = tmp / scale; + val -= offset; + return rc; + } + + /** + * @brief unpack Unpacks an an object which inherits from type Packable + * @param val Reference to object to be unpacked + * @return True if successful + */ + template ::value, + T>::type* = nullptr> + bool unpack(T& obj) const { + return obj.unpack_from(*this); + } + + /** + * @brief Unpacks Value types other than string and ByteVector + * specializations + * @tparam T Type of the Value being packed. May be automatically deduced + * from arguments + * @param val The destination of the unpack operation + * @return true on success + */ + template bool unpack(Value& val) const { + return val.set() = unpack(val()); + } + + /** + * @brief Extracts data from the ByteVector and stores it in a + * Value. Consumes all remaining data unless instructed + * otherwise. + * @param val Destination of unpack operation. + * @param count Max number of bytes to extract. Optional, if unset, all + * remaining bytes will be consumed. + * @return True on successful unpack + */ + bool unpack(Value& val, + size_t count = std::numeric_limits::max()) const { + return val.set() = unpack(val(), count); + } + + /** + * @brief Extracts data from the ByteVector and stores it in a + * Value. Consumes all remaining data unless instructed + * otherwise. + * @param val Destination of unpack operation. + * @param count Max number of bytes to extract. Optional, if unset, all + * remaining bytes will be consumed. + * @return True on successful unpack + */ + bool unpack(Value& val, + size_t count = std::numeric_limits::max()) const { + return val.set() = unpack(val(), count); + } + + /** + * @brief Unpacks scaled Value types (e.g. scaled int to floating point) as + * val = (packed_val/scale)-offset + * @tparam encoding_T data type used to store the scaled Value (usually an + * integral type) + * @tparam T1 type of output Value (usually a floating point type) + * @tparam T2 type of scale and offset coefficients + * @param val Destination of unpack operation + * @param scale Value of scaling to apply to the offset value + * @param offset Value of offset to apply to the input value (optional, + * defaults to 0) + * @return True if successful + */ + template ::value, + T1>::type* = nullptr, + typename std::enable_if::value, + T2>::type* = nullptr> + bool unpack(Value& val, T2 scale = 1, T2 offset = 0) const { + return val.set() = unpack(val(), scale, offset); + } + + /** + * @brief Gives the number of bytes which have already been consumed by + * unpack operations. + * @returns Number of bytes already consumed + */ + std::size_t unpacking_offset() const { return offset; } + + /** + * @brief Gives an iterator to the next element ready for unpacking + * @returns iterator to the next byte for unpacking + */ + std::vector::iterator unpacking_iterator() { + return this->begin() + offset; + } + + /** + * @brief Gives an iterator to the next element ready for unpacking + * @returns iterator to the next byte for unpacking + */ + std::vector::const_iterator unpacking_iterator() const { + return this->begin() + offset; + } + + /** + * @brief Manually consumes data, thus skipping the values. + * @param count Number of bytes to consume + * @returns True if successful + * @returns False if there were not enough bytes to satisfy the request + */ + bool consume(std::size_t count) const { + if(count < -offset) return false; + if(count > this->size() - offset) return false; + offset += count; + return true; + } + + /** + * @brief Returns the number of bytes still avialable for unpacking + * @returns Number of bytes remaining + */ + std::size_t unpacking_remaining() const { return this->size() - offset; } + +protected: + mutable std::size_t offset; +}; + +/** + * @brief Definition of a pure virtual class used to indicate that a child + * class can pack itself into a ByteVector and unpack itself from a ByteVector. + */ +struct Packable { + virtual bool pack_into(ByteVector& data) const = 0; + virtual bool unpack_from(const ByteVector& data) = 0; +}; + +typedef std::shared_ptr ByteVectorPtr; +typedef std::unique_ptr ByteVectorUptr; + +} // namespace msp + +std::ostream& operator<<(std::ostream& s, const msp::ByteVector& val) { + s << std::hex; + for(const auto& v : val) { + s << uint32_t(v) << " "; + } + s << std::dec << std::endl; + return s; +} + +#endif diff --git a/inc/msp/Client.hpp b/inc/msp/Client.hpp index 35e4bc1..9d93891 100644 --- a/inc/msp/Client.hpp +++ b/inc/msp/Client.hpp @@ -1,343 +1,368 @@ #ifndef CLIENT_HPP #define CLIENT_HPP -#include -#include -#include -#include +#include #include +#include #include -#include "types.hpp" +#include +#include +#include +#include "ByteVector.hpp" +#include "FirmwareVariants.hpp" +#include "Message.hpp" +#include "Subscription.hpp" namespace msp { +namespace client { -struct SerialPortImpl; +typedef asio::buffers_iterator iterator; -class PeriodicTimer { -public: +enum LoggingLevel { SILENT, WARNING, INFO, DEBUG }; - /** - * @brief PeriodicTimer define a periodic timer - * @param funct function that is called periodically - * @param period_seconds period in seconds - */ - PeriodicTimer(const std::function funct, const double period_seconds); +enum MessageStatus { + OK, // no errors + FAIL_ID, // message ID is unknown + FAIL_CRC // wrong CRC +}; - ~PeriodicTimer() { stop(); } +struct ReceivedMessage { + msp::ID id; + ByteVector payload; + MessageStatus status; +}; +class Client { +public: /** - * @brief start define and start background thread + * @brief Client Constructor + * @param device String describing the path to the serial device + * @param baudrate Baudrate of the connection (default 115200, + * unnecessary for direct USB connection) */ - void start(); + Client(); /** - * @brief stop tell thread to stop and wait for end + * @brief ~Client Destructor */ - void stop(); + ~Client(); /** - * @brief getPeriod get period in seconds - * @return period in seconds + * @brief Set the verbosity of the output + * @param level LoggingLevel matching the desired amount of output (default + * to WARNING) */ - double getPeriod() { - return period_us.count()/1.e6; - } + void setLoggingLevel(const LoggingLevel& level); /** - * @brief setPeriod change the update period of timer thread - * This will stop and restart the thread. - * @param period_seconds period in seconds + * @brief Change the device path on the next connect + * @param ver Version of MSP to use + * @return True if successful */ - void setPeriod(const double period_seconds); - -private: - std::shared_ptr thread_ptr; - std::function funct; - std::chrono::duration period_us; - std::timed_mutex mutex_timer; - bool running; -}; - -} // namespace msp - -namespace msp { -namespace client { - -class SubscriptionBase { -public: - SubscriptionBase(PeriodicTimer *timer=NULL) : timer(timer) { } - - virtual ~SubscriptionBase() { - if(timer!=NULL) { delete timer; } - } - - virtual void call(const msp::Request &req) = 0; - - bool hasTimer() { - // subscription with manual sending of requests - return !(timer->getPeriod()>0); - } + bool setVersion(const int& ver); /** - * @brief setTimerPeriod change the period of the timer - * @param period_seconds period in seconds + * @brief Query the cached device path + * @return Cached path to device */ - void setTimerPeriod(const double period_seconds) { - timer->setPeriod(period_seconds); - } + int getVersion(); /** - * @brief setTimerFrequency change the update rate of timer - * @param rate_hz frequency in Hz + * @brief Change the device path on the next connect + * @param device Path to device */ - void setTimerFrequency(const double rate_hz) { - timer->setPeriod(1.0/rate_hz); - } - -protected: - PeriodicTimer *timer; -}; - -template -class Subscription : public SubscriptionBase { -public: - typedef std::function Callback; - - Subscription(const Callback &callback) : callback(callback) { } + void setVariant(const FirmwareVariant& v); - Subscription(const Callback &callback, PeriodicTimer *timer) - : SubscriptionBase(timer), callback(callback) - { - this->timer->start(); - } - - void call(const msp::Request &req) { - callback( dynamic_cast(req) ); - } - -private: - Callback callback; -}; - -enum MessageStatus { - OK, // no errors - FAIL_ID, // message ID is unknown - FAIL_CRC // wrong CRC -}; - -struct ReceivedMessage { - uint8_t id; - std::vector data; - MessageStatus status; -}; - -class Client { -public: - Client(); + /** + * @brief Query the cached device path + * @return Cached path to device + */ + FirmwareVariant getVariant(); - ~Client(); + /** + * @brief Start communications with a flight controller + * @return True on success + */ + bool start(const std::string& device, const size_t baudrate = 115200); - void setPrintWarnings(const bool warnings) { - print_warnings = warnings; - } + /** + * @brief Stop communications with a flight controller + * @return True on success + */ + bool stop(); /** - * @brief connect establish connection to serial device - * @param device path or name of serial device - * @param baudrate serial baudrate (default: 115200) + * @brief Query the system to see if a connection is active * @return true on success */ - void connect(const std::string &device, const size_t baudrate=115200); + bool isConnected(); /** - * @brief start starts the receiver thread that handles incomming messages + * @brief Send a message to the connected flight controller. If + * the message sends data to the flight controller, it will be packed into + * a buffer and sent. The method will block (optionally for a finite amount + * of time) until a matching response is received from the flight + * controller. If the response includes data, it will be unpacked back into + * the same Message object. + * @param message Reference to a Message-derived object to be sent/recieved. + * @param timeout Maximum amount of time to block waiting for a response. + * A value of 0 (default) means wait forever. */ - void start(); + bool sendMessage(msp::Message& message, const double& timeout = 0); /** - * @brief stop stops the receiver thread + * @brief Send a message, but do not wait for any response + * @param message Reference to a Message-derived object to be sent */ - void stop(); + bool sendMessageNoWait(const msp::Message& message); /** - * @brief sendData send raw data and ID to flight controller, accepts any uint8 id - * @param id message ID - * @param data raw data - * @return true on success - * @return false on failure + * @brief Register callback function that is called when a message of + * matching ID is received + * @param callback Pointer to callback function (class method) + * @param context Object containing callback method + * @param tp Period of timer that will send subscribed requests (in + * seconds). + * @return pointer to subscription that is added to internal list */ - bool sendData(const uint8_t id, const ByteVector &data = ByteVector(0)); + template ::value>::type> + std::shared_ptr subscribe(void (C::*callback)(const T&), + C* context, const double& tp) { + return subscribe(std::bind(callback, context, std::placeholders::_1), + tp); + } /** - * @brief sendRequest request payload from FC - * @param id message ID - * @return true on success - * @return false on failure + * @brief Register callback function that is called when a + * message of matching ID is received + * @param recv_callback Function to be called upon receipt of message + * @param tp Period of timer that will send subscribed requests (in + * seconds). + * @return pointer to subscription that is added to internal list */ - bool sendRequest(const uint8_t id) { - return sendData(id, ByteVector()); + template ::value>::type> + std::shared_ptr subscribe( + const std::function& recv_callback, const double& tp) { + // validate the period + if(!(tp >= 0.0)) throw std::runtime_error("Period must be positive!"); + + // get the id of the message in question + const msp::ID id = T(fw_variant).id(); + if(log_level_ >= INFO) + std::cout << "SUBSCRIBING TO " << id << std::endl; + + // generate the callback for sending messages + std::function send_callback = + std::bind(&Client::sendMessageNoWait, this, std::placeholders::_1); + + // create a shared pointer to a new Subscription and set all properties + auto subscription = std::make_shared>( + recv_callback, send_callback, std::make_unique(fw_variant), tp); + + // gonna modify the subscription map, so lock the mutex + std::lock_guard lock(mutex_subscriptions); + + // move the new subscription into the subscription map + subscriptions.emplace(id, std::move(subscription)); + return subscriptions[id]; } - bool sendRequest(const msp::ID id) { - return sendData(uint8_t(id), ByteVector()); + /** + * @brief Check if message ID already has a subscription + * @param id Message ID + * @return True if there is already a matching subscription + */ + bool hasSubscription(const msp::ID& id) { + return (subscriptions.count(id) == 1); } /** - * @brief sendResponse send payload to FC - * @param response response with payload - * @return true on success - * @return false on failure + * @brief Get pointer to subscription + * @param id Message ID + * @return Shared pointer to subscription (empty if there was no match) */ - bool sendResponse(const msp::Response &response) { - return sendData(uint8_t(response.id()), response.encode()); + std::shared_ptr getSubscription(const msp::ID& id) { + return subscriptions.at(id); } /** - * @brief request requests payload from FC and block until payload has been received - * @param request request whose data will be set by the received payload - * @param timeout (optional) timeout in seconds - * @return true on success - * @return false on failure - * @return -1 on timeout + * @brief Main entry point for processing received data. It + * is called directly by the ASIO library, and as such it much match the + * function signatures expected by ASIO. + * @param ec ASIO error code + * @param bytes_transferred Number of byte available for processing */ - int request(msp::Request &request, const double timeout = 0); + void processOneMessage(const asio::error_code& ec, + const std::size_t& bytes_transferred); /** - * @brief request_raw request raw unstructured payload data - * @param id message ID - * @param data reference to data buffer at which the received data will be stores - * @param timeout (optional) timeout in seconds - * @return 1 on success - * @return 0 on failure - * @return -1 on timeout + * @brief Send an ID and payload to the flight controller + * @param id Message ID + * @param data Raw data (default zero length, meaning no data to send + * outbound) + * @return true on success */ - int request_raw(const uint8_t id, ByteVector &data, const double timeout = 0); + bool sendData(const msp::ID id, const ByteVector& data = ByteVector(0)); /** - * @brief respond send payload to FC and block until an ACK has been received - * @param response response with payload - * @param wait_ack if set, method will wait for message acknowledgement + * @brief Send an ID and payload to the flight controller + * @param id Message ID + * @param data Unique pointer to data. May be empty if there is no data to + * send * @return true on success - * @return false on failure */ - bool respond(const msp::Response &response, const bool wait_ack=true); + bool sendData(const msp::ID id, const ByteVectorUptr&& data) { + if(!data) return sendData(id); + return sendData(id, *data); + } +protected: /** - * @brief respond_raw send raw unstructured payload data - * @param id message ID - * @param data raw payload - * @param wait_ack if set, method will wait for message acknowledgement - * @return true on success - * @return false on failure + * @brief Establish connection to serial device and start read thread + * @return True on success */ - bool respond_raw(const uint8_t id, const ByteVector &data, const bool wait_ack=true); + bool connectPort(const std::string& device, const size_t baudrate = 115200); /** - * @brief subscribe register callback function that is called when type is received - * @param callback pointer to callback function (class method) - * @param context class with callback method - * @param tp period of timer that will send subscribed requests (in seconds), by default this is 0 and requests are not sent periodically - * @return pointer to subscription that is added to internal list + * @brief Break connection to serial device and stop read thread + * @return True on success */ - template - SubscriptionBase* subscribe(void (C::*callback)(const T&), C *context, const double tp = 0.0) { - return subscribe(std::bind(callback, context, std::placeholders::_1), tp); - } + bool disconnectPort(); /** - * @brief subscribe register callback function that is called when type is received - * @param callback function (e.g. lambda, class method, function pointer) - * @param tp period of timer that will send subscribed requests (in seconds), by default this is 0 and requests are not sent periodically - * @return pointer to subscription that is added to internal list + * @brief Starts the receiver thread that handles incomming messages + * @return True on success */ - template - SubscriptionBase* subscribe(const std::function &callback, const double tp = 0.0) { + bool startReadThread(); - if(!std::is_base_of::value) - throw std::runtime_error("Callback parameter needs to be of Request type!"); + /** + * @brief Stops the receiver thread + * @return True on success + */ + bool stopReadThread(); - if(!(tp>=0.0)) - throw std::runtime_error("Period must be positive!"); + /** + * @brief Starts the receiver thread that handles incomming messages + * @return True on success + */ + bool startSubscriptions(); - const msp::ID id = T().id(); + /** + * @brief Stops the receiver thread + * @return True on success + */ + bool stopSubscriptions(); - std::lock_guard lock(mutex_callbacks); + /** + * @brief Read a single byte from either the buffer or the serial device + * @return byte from buffer or device + */ + uint8_t extractChar(); - // register message - if(subscribed_requests.count(id)) { delete subscribed_requests[id]; } - subscribed_requests[id] = new T(); + /** + * @brief messageReady Method used by ASIO library to determine if a + * full message is present in receiving buffer. It must match the function + * signature expected by ASIO. + * @return std::pair indicating where the start the next + * message check operation and whether the current check was successful + */ + std::pair messageReady(iterator begin, iterator end); - // register subscription - subscriptions[id] = new Subscription(callback, - new PeriodicTimer( - std::bind(static_cast(&Client::sendRequest), this, id), - tp - ) - ); + /** + * @brief processOneMessageV1 Iterates over characters in the ASIO buffer + * to identify and unpack a MSPv1 encoded message + * @return ReceivedMessage data structure containing the results of + * unpacking + */ + ReceivedMessage processOneMessageV1(); - return subscriptions[id]; - } + /** + * @brief processOneMessageV2 Iterates over characters in the ASIO buffer + * to identify and unpack a MSPv2 encoded message + * @return ReceivedMessage data structure containing the results of + * unpacking + */ + ReceivedMessage processOneMessageV2(); /** - * @brief hasSubscription check if message ID is subscribed - * @param id message ID - * @return true if there is already a subscription - * @return false if ID is not subscribed + * @brief packMessageV1 Packs data ID and data payload into a MSPv1 + * formatted buffer ready for sending to the serial device + * @param id msp::ID of the message being packed + * @param data Optional binary payload to be packed into the outbound buffer + * @return ByteVector of full MSPv1 message ready for sending */ - bool hasSubscription(const msp::ID& id) { - return (subscriptions.count(id)==1); - } + ByteVector packMessageV1(const msp::ID id, + const ByteVector& data = ByteVector(0)); /** - * @brief getSubscription get pointer to subscription - * @param id message ID - * @return pointer to subscription + * @brief crcV1 Computes a checksum for MSPv1 messages + * @param id uint8_t MSP ID + * @param data Payload which is also part of the checksum + * @return uint8_t checksum */ - SubscriptionBase* getSubscription(const msp::ID& id) { - return subscriptions.at(id); - } + uint8_t crcV1(const uint8_t id, const ByteVector& data); -private: /** - * @brief crc compute checksum of data package - * @param id message ID - * @param data raw data vector - * @return checksum + * @brief packMessageV2 Packs data ID and data payload into a MSPv2 + * formatted buffer ready for sending to the serial device + * @param id msp::ID of the message being packed + * @param data Optional binary payload to be packed into the outbound buffer + * @return ByteVector of full MSPv2 message ready for sending */ - uint8_t crc(const uint8_t id, const ByteVector &data); + ByteVector packMessageV2(const msp::ID id, + const ByteVector& data = ByteVector(0)); /** - * @brief waitForOneMessage block until one message has been received + * @brief crcV2 Computes a checksum for MSPv2 messages + * @param crc Checksum value from which to start calculations + * @param data ByteVector of data to be wrapped into the checksum + * @return uint8_t checksum */ - void waitForOneMessage(); + uint8_t crcV2(uint8_t crc, const ByteVector& data); - void waitForOneMessageBlock(); + /** + * @brief crcV2 Computes a checksum for MSPv2 messages + * @param crc Checksum value from which to start calculations + * @param data Single byte to use in the checksum calculation + * @return uint8_t checksum + */ + uint8_t crcV2(uint8_t crc, const uint8_t& b); - void processOneMessage(); +protected: + asio::io_service io; /// pimpl; - // threading + // read thread management std::thread thread; - bool running; - std::condition_variable cv_request; - std::condition_variable cv_ack; - std::mutex mutex_cv_request; - std::mutex mutex_cv_ack; - std::mutex mutex_request; - std::mutex mutex_callbacks; + std::atomic_flag running_; + + // thread safety and synchronization + std::condition_variable cv_response; + std::mutex cv_response_mtx; + std::mutex mutex_response; + std::mutex mutex_buffer; std::mutex mutex_send; - // message for request method - ReceivedMessage request_received; - // subscriptions - std::map subscriptions; - std::map subscribed_requests; + + // holder for received data + std::unique_ptr request_received; + + // subscription management + std::mutex mutex_subscriptions; + std::map> subscriptions; + // debugging - bool print_warnings; + LoggingLevel log_level_; + + // reference values + int msp_ver_; + FirmwareVariant fw_variant; }; -} // namespace client -} // namespace msp +} // namespace client +} // namespace msp -#endif // CLIENT_HPP +#endif // CLIENT_HPP diff --git a/inc/msp/FirmwareVariants.hpp b/inc/msp/FirmwareVariants.hpp new file mode 100644 index 0000000..552b624 --- /dev/null +++ b/inc/msp/FirmwareVariants.hpp @@ -0,0 +1,64 @@ +#ifndef VARIANTS_HPP +#define VARIANTS_HPP + +#include +#include + +namespace msp { + +/** + * @brief Enum of firmware variants + * + */ +enum class FirmwareVariant : int { + NONE = 0, /**< not specified */ + MWII = 1, /**< MultiWii */ + BAFL = 2, /**< BetaFlight */ + BTFL = 3, /**< ButterFlight */ + CLFL = 4, /**< CleanFlight */ + INAV = 5, /**< INAV */ + RCFL = 6 /**< RaceFlight */ +}; + +const static std::map variant_map = { + {"MWII", FirmwareVariant::MWII}, + {"BAFL", FirmwareVariant::BAFL}, + {"BTFL", FirmwareVariant::BTFL}, + {"CLFL", FirmwareVariant::CLFL}, + {"INAV", FirmwareVariant::INAV}, + {"RCFL", FirmwareVariant::RCFL}}; + +/** + * @brief Converts a FirmwareVariant into a matching string + * @param variant Enum of FirmwareVariant + * @returns A string matching the firmware type + */ +std::string firmwareVariantToString(FirmwareVariant variant) { + std::string var; + switch(variant) { + case FirmwareVariant::MWII: + var = "MWII"; + break; + case FirmwareVariant::BAFL: + var = "BAFL"; + break; + case FirmwareVariant::BTFL: + var = "BTFL"; + break; + case FirmwareVariant::CLFL: + var = "CLFL"; + break; + case FirmwareVariant::INAV: + var = "INAV"; + break; + case FirmwareVariant::RCFL: + var = "RCFL"; + break; + default: + var = "NONE"; + } + return var; +} + +} // namespace msp +#endif diff --git a/inc/msp/FlightController.hpp b/inc/msp/FlightController.hpp index 5ef3259..749c531 100644 --- a/inc/msp/FlightController.hpp +++ b/inc/msp/FlightController.hpp @@ -2,251 +2,385 @@ #define FLIGHTCONTROLLER_HPP #include "Client.hpp" +#include "FlightMode.hpp" +#include "PeriodicTimer.hpp" #include "msp_msg.hpp" namespace fcu { -enum class FirmwareType { - MULTIWII, - CLEANFLIGHT -}; +enum class ControlSource { NONE, SBUS, MSP, OTHER }; class FlightController { public: - FlightController(const std::string &device, const size_t baudrate=115200); + /** + * @brief FlightController Constructor + * @param device Name of serial device (e.g. /dev/ttyACM0) + * @param baudrate Speed of serial connection (not necessary for direct USB + * connection) + * @return + */ + FlightController(); + /** + * @brief ~FlightController Destructor + */ ~FlightController(); - void waitForConnection(); + /** + * @brief Connects to a physical flight controller, which includes + * connecting the internal Client object and querying the flight controller + * for information necessary to configure the internal state to match the + * capabiliites of the flight controller. + * @param timeout Timeout passed to each internal operation (seconds) + * @return True on success + */ + bool connect(const std::string &device, const size_t baudrate = 115200, + const double &timeout = 0.0); - void initialise(); + /** + * @brief Stops MSP control if active, and disconnects the internal Client + * object + * @return True on success + */ + bool disconnect(); /** - * @brief isFirmware determine firmware type (e.g. to distiguish accepted messages) - * @param firmware_type type of firmware (enum FirmwareType) - * @return true if firmware is firmware_type - * @return false if firmware is not firmware_type + * @brief Tests connection to a flight controller + * @return True if connected */ - bool isFirmware(const FirmwareType firmware_type); + bool isConnected(); - bool isFirmwareMultiWii() { return isFirmware(FirmwareType::MULTIWII); } + /** + * @brief Sets internal flag to arm the flight controller + */ + void arm(); - bool isFirmwareCleanflight() { return isFirmware(FirmwareType::CLEANFLIGHT); } + /** + * @brief Sets internal flag to disarm the flight controller + */ + void disarm(); /** - * @brief subscribe register callback function that is called when type is received - * @param callback pointer to callback function (class method) - * @param context class with callback method - * @param tp period of timer that will send subscribed requests (in seconds), by default this is 0 and requests are not sent periodically - * @return pointer to subscription that is added to internal list + * @brief Queries internal flag governing flight controller arming + * @return True if arm flag is set */ - template - msp::client::SubscriptionBase* subscribe(void (C::*callback)(const T&), C *context, const double tp = 0.0) { - return client.subscribe(callback, context, tp); - } + bool armSet(); /** - * @brief subscribe register callback function that is called when type is received - * @param callback function (e.g. lambda, class method, function pointer) - * @param tp period of timer that will send subscribed requests (in seconds), by default this is 0 and requests are not sent periodically - * @return pointer to subscription that is added to internal list + * @brief Sets data structure with all flags governing flight mode + * @param mode FlightMode object */ - template - msp::client::SubscriptionBase* subscribe(const std::function &callback, const double tp = 0.0) { - return client.subscribe(callback, tp); - } + void setFlightMode(const FlightMode mode); /** - * @brief hasSubscription check if message ID is subscribed - * @param id message ID - * @return true if there is already a subscription - * @return false if ID is not subscribed + * @brief Queries data structure with all flags governing flight mode + * @return FlightMode object matchhing current flight mode flags */ - bool hasSubscription(const msp::ID& id) { - return client.hasSubscription(id); - } + FlightMode getFlightMode(); /** - * @brief getSubscription get pointer to subscription - * @param id message ID - * @return pointer to subscription + * @brief Sets which instruction source the flight controller should + * listen to. Also starts periodic MSP control message if MSP is selected + * @param mode FlightMode object */ - msp::client::SubscriptionBase* getSubscription(const msp::ID& id) { - return client.getSubscription(id); - } + void setControlSource(const ControlSource source); + + /** + * @brief Queries the currently active control source + * @return ControlSource object matching current control source + */ + ControlSource getControlSource(); + + /** + * @brief Sets roll, pitch, yaw, and throttle values. They are sent + * immediately and stored for resending if automatic MSP control is enabled. + * @param rpyt An array of doubles representing roll, pitch, yaw, and + * throttle in that order. Values are expected to be in the range -1.0 to + * +1.0, mapping to 1000us to 2000us pulse widths. + */ + void setRPYT(std::array &&rpyt); + + /** + * @brief Method used to generate the Rc message sent to the flight + * controller + */ + void generateMSP(); + + /** + * @brief Sends message to flight controller to save all settings + */ + bool saveSettings(); + + /** + * @brief Starts message to flight controller signalling that it should + * reboot. Will result in the serial device disappearing if using a direct + * USB connection to the board. + */ + bool reboot(); /** - * @brief sendRequest send request with ID - * @param id message ID of request - * @return true on success - * @return false on failure + * @brief Queries the currently set firmware variant (Cleanflight, + * Betaflight, etc.) + * @return msp::FirmwareVariant enum matching the current firmware */ - bool sendRequest(const uint8_t id) { - return client.sendRequest(id); + msp::FirmwareVariant getFwVariant(); + + /** + * @brief Queries the currently set protocol (MSPv1 or MSPv2) + * @return integer matching the protocol version + */ + int getProtocolVersion(); + + /** + * @brief Queries the currently set board name + * @return std::String of the board name + */ + std::string getBoardName(); + + /** + * @brief Set the verbosity of the output + * @param level LoggingLevel matching the desired amount of output (default + * to WARNING) + */ + void setLoggingLevel(const msp::client::LoggingLevel &level); + + /** + * @brief Set RC channels in order: roll, pitch, yaw, throttle by using + * channel mapping + * @param roll + * @param pitch + * @param yaw + * @param throttle + * @param aux1 + * @param aux2 + * @param aux3 + * @param aux4 + * @param auxs + * @return + */ + bool setRc(const uint16_t roll, const uint16_t pitch, const uint16_t yaw, + const uint16_t throttle, const uint16_t aux1 = 1000, + const uint16_t aux2 = 1000, const uint16_t aux3 = 1000, + const uint16_t aux4 = 1000, + const std::vector auxs = std::vector()); + + /** + * @brief Set RC channels in raw order as it is interpreted by the FC + * @param channels list of channel values (1000-2000) + * @return + */ + bool setRc(const std::vector channels); + + /** + * @brief Register callback function that is called when type is received + * @param callback Pointer to callback function (class method) + * @param context Object with callback method + * @param tp Period of timer that will send subscribed requests (in + * seconds), by default this is 0 and requests are not sent periodically + * @return Pointer to subscription that is added to internal list + */ + template ::value>::type> + std::shared_ptr subscribe( + void (C::*callback)(const T &), C *context, const double tp = 0.0) { + return client_.subscribe(callback, context, tp); } - bool request(msp::Request &request, const double timeout = 0) { - return client.request(request, timeout); + /** + * @brief Register callback function that is called when type is received + * @param callback Function to be called (e.g. lambda, class method, + * function pointer) + * @param tp Period of timer that will send subscribed requests (in + * seconds), by default this is 0 and requests are not sent periodically + * @return Pointer to subscription that is added to internal list + */ + template ::value>::type> + std::shared_ptr subscribe( + const std::function &callback, const double tp = 0.0) { + return client_.subscribe(callback, tp); } - bool request_raw(const uint8_t id, msp::ByteVector &data, const double timeout = 0) { - return client.request_raw(id, data, timeout); + /** + * @brief Check if message ID is subscribed + * @param id Message ID + * @return True if there is already a subscription + */ + bool hasSubscription(const msp::ID &id) { + return client_.hasSubscription(id); } - bool respond(const msp::Response &response, const bool wait_ack=true) { - return client.respond(response, wait_ack); + /** + * @brief Get pointer to subscription + * @param id Message ID + * @return Pointer to subscription + */ + std::shared_ptr getSubscription( + const msp::ID &id) { + return client_.getSubscription(id); } - bool respond_raw(const uint8_t id, msp::ByteVector &data, const bool wait_ack=true) { - return client.respond_raw(id, data, wait_ack); + /** + * @brief Sends a message to the flight controller + * @param message Reference to a Message-derived object + * @param timeout Number of seconds to wait for response. Default value of 0 + * means no timeout. + * @return True on success + */ + bool sendMessage(msp::Message &message, const double timeout = 0) { + return client_.sendMessage(message, timeout); } + /** + * @brief Queries the flight controller for Box (flight mode) information + */ void initBoxes(); - std::map &getBoxNames() { - return box_name_ids; - } + /** + * @brief Gets the information collected by the initBoxes() method + * @return Reference to the internal mapping of strings to box IDs + */ + std::map &getBoxNames() { return box_name_ids_; } + /** + * @brief Queries the cached flight controller information to see + * if a particular capability is present + * @return True if the sensor is present + */ bool hasCapability(const msp::msg::Capability &cap) const { - return ident.capabilities.count(cap); + return capabilities_.count(cap); } - bool hasBind() const { - return hasCapability(msp::msg::Capability::BIND); - } + /** + * @brief Queries for the presence of the BIND capability + * @return True if the BIND capaibility is present + */ + bool hasBind() const { return hasCapability(msp::msg::Capability::BIND); } + /** + * @brief Queries for the presence of the DYNBAL capability + * @return True if the DYNBAL capaibility is present + */ bool hasDynBal() const { return hasCapability(msp::msg::Capability::DYNBAL); } - bool hasFlap() const { - return hasCapability(msp::msg::Capability::FLAP); - } + /** + * @brief Queries for the presence of the FLAP capability + * @return True if the FLAP capaibility is present + */ + bool hasFlap() const { return hasCapability(msp::msg::Capability::FLAP); } + /** + * @brief Queries the cached flight controller information to see + * if a particular sensor is present + * @return True if the sensor is present + */ bool hasSensor(const msp::msg::Sensor &sensor) const { - return sensors.count(sensor); + return sensors_.count(sensor); } + /** + * @brief Queries for the presence of an accelerometer + * @return True if there is an accelerometer + */ bool hasAccelerometer() const { return hasSensor(msp::msg::Sensor::Accelerometer); } - bool hasBarometer() const { - return hasSensor(msp::msg::Sensor::Barometer); - } + /** + * @brief Queries for the presence of a barometer + * @return True if there is a barometer + */ + bool hasBarometer() const { return hasSensor(msp::msg::Sensor::Barometer); } + /** + * @brief Queries for the presence of a magentometer + * @return True if there is a magentometer + */ bool hasMagnetometer() const { return hasSensor(msp::msg::Sensor::Magnetometer); } - bool hasGPS() const { - return hasSensor(msp::msg::Sensor::GPS); - } - - bool hasSonar() const { - return hasSensor(msp::msg::Sensor::Sonar); - } - - bool isStatusActive(const std::string& status_name); - - bool isArmed() { return isStatusActive("ARM"); } - - bool isStatusFailsafe() { return isStatusActive("FAILSAFE"); } - /** - * @brief setRc set RC channels in order: roll, pitch, yaw, throttle by using channel mapping - * @param roll - * @param pitch - * @param yaw - * @param throttle - * @param aux1 - * @param aux2 - * @param aux3 - * @param aux4 - * @param auxs - * @return + * @brief Queries for the presence of a GPS + * @return True if there is a GPS */ - bool setRc(const uint16_t roll, const uint16_t pitch, - const uint16_t yaw, const uint16_t throttle, - const uint16_t aux1 = 1000, const uint16_t aux2 = 1000, - const uint16_t aux3 = 1000, const uint16_t aux4 = 1000, - const std::vector auxs = std::vector()); + bool hasGPS() const { return hasSensor(msp::msg::Sensor::GPS); } /** - * @brief setRc set RC channels in raw order as it is interpreted by the FC - * @param channels list of channel values (1000-2000) - * @return + * @brief Queries for the presence of a sonar + * @return True if there is a sonar */ - bool setRc(const std::vector channels); + bool hasSonar() const { return hasSensor(msp::msg::Sensor::Sonar); } - bool setMotors(const std::array &motor_values); + /** + * @brief Queries the flight controller to see if a status is active + * @return True if status if active + */ + bool isStatusActive(const std::string &status_name); /** - * @brief arm arm or disarm FC - * @param arm true: will arm FC, false: will disarm FC - * @return true on success + * @brief Queries the flight controller to see if the ARM status is active. + * Not to be confused with armSet(), which queries whether the flight + * controller has been instructued to turn on the ARM status. + * @return True if the ARM status is active */ - bool arm(const bool arm); + bool isArmed() { return isStatusActive("ARM"); } /** - * @brief arm_block attempt to arm and wait for status feedback, e.g. this method will block until the FC is able to aim - * @return + * @brief Queries the flight controller to see if the FAILSAFE status is + * active. + * @return True if the FAILSAFE status is active */ - bool arm_block(); + bool isStatusFailsafe() { return isStatusActive("FAILSAFE"); } /** - * @brief disarm_block attempt to disarm and wait for status feedback - * @return + * @brief Directly sets motor values using SetMotor message + * @return True on successful message delivery */ - bool disarm_block(); + bool setMotors(const std::array &motor_values); /** - * @brief updateFeatures enable and disable features on the FC - * To apply updates, changes will be written to the EEPROM and the FC will reboot. + * @brief Enable and disable features on the FC + * To apply updates, changes will be written to the EEPROM and the FC will + * reboot. * @param add set of features to enable * @param remove set of features to disable * @return 1 if features have been changed * @return 0 if no changes have been applied * @return -1 on failure */ - int updateFeatures(const std::set &add = std::set(), - const std::set &remove = std::set()); - - /** - * @brief enableRxMSP enable the "RX_MSP" feature - * The features "RX_MSP", "RX_PARALLEL_PWM", "RX_PPM" and "RX_SERIAL" are - * mutually exclusive. Hence one of the features "RX_PARALLEL_PWM", "RX_PPM" - * or "RX_SERIAL" will be disabled if active. - * @return true on success - */ - bool enableRxMSP() { - return updateFeatures( - {"RX_MSP"}, // add - {"RX_PARALLEL_PWM", "RX_PPM", "RX_SERIAL"} // remove - ); - } - - bool reboot(); - - bool writeEEPROM(); + int updateFeatures( + const std::set &add = std::set(), + const std::set &remove = std::set()); private: + // Client instance for managing the actual comms with the flight controller + msp::client::Client client_; - static const uint8_t MAX_MAPPABLE_RX_INPUTS = 8; - - msp::client::Client client; - - std::map box_name_ids; + // parameters updated by the connect method to cache flight controller info + std::string board_name_; + msp::FirmwareVariant fw_variant_; + int msp_version_; + std::map box_name_ids_; + std::set sensors_; + std::array channel_map_; + std::set capabilities_; - msp::msg::Ident ident; + // parameters updated by the user, and consumed by MSP control messages + std::array rpyt_; + bool armed_; + FlightMode flight_mode_; - std::set sensors; + std::mutex msp_updates_mutex; - FirmwareType firmware; + ControlSource control_source_; - std::vector channel_map; + msp::PeriodicTimer msp_timer_; }; -} // namespace msp +} // namespace fcu -#endif // FLIGHTCONTROLLER_HPP +#endif // FLIGHTCONTROLLER_HPP diff --git a/inc/msp/FlightMode.hpp b/inc/msp/FlightMode.hpp new file mode 100644 index 0000000..604cfbc --- /dev/null +++ b/inc/msp/FlightMode.hpp @@ -0,0 +1,54 @@ +#ifndef FLIGHTMODE_HPP +#define FLIGHTMODE_HPP + +namespace fcu { + +struct FlightMode { + enum class PRIMARY_MODE : uint32_t { + UNSET = 0, + ANGLE, + HORIZON, + NAV_POSHOLD, + NAV_CRUISE, + NAV_RTH, + NAV_WP, + MANUAL + }; + + enum class SECONDARY_MODE : uint32_t { + NONE = 0, + NAV_ALTHOLD = 1 << 0, + TURN_ASSIST = 1 << 1, + AIR_MODE = 1 << 2, + SURFACE = 1 << 3, + HEADING_HOLD = 1 << 4, + HEADFREE = 1 << 5, + HEADADJ = 1 << 6 + }; + + enum class MODIFIER : uint32_t { + NONE = 0, + ARM = 1 << 0, + CAMSTAB = 1 << 1, + BEEPER = 1 << 2, + LEDLOW = 1 << 3, + OSD_SW = 1 << 4, + TELEMETRY = 1 << 5, + BLACKBOX = 1 << 6, + FAILSAFE = 1 << 7, + HOME_RESET = 1 << 8, + GCS_NAV = 1 << 9, + FLAPERON = 1 << 10, + NAV_LAUNCH = 1 << 11, + SERVO_AUTOTRIM = 1 << 12, + AUTOTUNE = 1 << 13 + }; + + PRIMARY_MODE primary; + SECONDARY_MODE secondary; + MODIFIER modifier; +}; + +} // namespace fcu + +#endif diff --git a/inc/msp/MSP.hpp b/inc/msp/MSP.hpp deleted file mode 100644 index 4109d14..0000000 --- a/inc/msp/MSP.hpp +++ /dev/null @@ -1,253 +0,0 @@ -#ifndef MSP_HPP -#define MSP_HPP - -#include "types.hpp" - -#include -#include -#include -#include - -namespace msp { - -struct SerialPortImpl; - -// size of header+crc, e.g. amount of bytes in MSP message that do not belong to payload -// preamble (2) + direction (1) + size (1) + command (1) + crc (1) = 6 -static const size_t FRAME_SIZE = 6; - -// exception to throw when header contains wrong data -class MalformedHeader : public std::runtime_error { -public: - MalformedHeader(const uint8_t exp, const uint8_t rcv) - : std::runtime_error( - "Malformed header: " - "expected "+std::to_string(exp)+" ("+std::string(1,char(exp))+"), " - "received "+std::to_string(rcv)+" ("+std::string(1,char(rcv))+")") {} -}; - -class UnknownMsgId : public std::runtime_error { -private: - const uint8_t msg_id; -public: - UnknownMsgId(uint8_t id) : runtime_error( - "Unknown MSP id! FC refused to process message with id: "+std::to_string(id)), msg_id(id) - { } - - const uint8_t &getInvalidID() const { return msg_id; } -}; - -// exception to throw if reported CRC does not match with computed -class WrongCRC : public std::runtime_error { -public: - WrongCRC(const uint8_t msg_id, const uint8_t exp, const uint8_t rcv) - : std::runtime_error( - "CRC not matching: " - "Message "+std::to_string(size_t(msg_id))+", " - "expected CRC "+std::to_string(exp)+", " - "received CRC "+std::to_string(rcv)) - { } -}; - -class NoData : public std::runtime_error { -public: - NoData() : std::runtime_error("No data available!") { } -}; - -class NoConnection : public std::runtime_error { -public: - NoConnection(const std::string &device, const std::string &msg) - : runtime_error("Device not available: "+device+" ("+msg+")") - { } -}; - -/** - * @brief The DataID struct - */ -struct DataID { - ByteVector data; //!< rawdata vector - uint8_t id; //!< message ID - - /** - * @brief DataID - * @param data vector of raw data bytes - * @param id message ID - */ - DataID(const ByteVector data, const uint8_t id) : data(data), id(id) {} -}; - -/** - * @brief The MSP class - */ -class MSP { -public: - - /** - * @brief MSP construct MSP communication without establishing a connection - */ - MSP(); - - /** - * @brief MSP constructor for MSP communication - * @param device device path - * @param baudrate serial baudrate - */ - MSP(const std::string &device, const size_t baudrate=115200); - - ~MSP(); - - /** - * @brief connect establish connection to serial device - * @param device path or name of serial device - * @param baudrate serial baudrate (default: 115200) - * @return true on success - */ - bool connect(const std::string &device, const size_t baudrate=115200); - - /** - * @brief request send command and request data from FC once - * @param request request message - * @return true on success - * @return false on failure - */ - bool request(msp::Request &request); - - /** - * @brief request_block continuously send command and request data until data has been received - * @param request request message - * @return true when data has been received - */ - bool request_block(msp::Request &request); - - /** - * @brief request_wait wait for data while continuously sending command - * @param request request message - * @param wait_ms waiting time in between sending request and receiving response - * @param min_payload_size minimum amount of payload (bytes) that needs to be available before reading and decoding of a message starts - * @return true when data has been received - */ - bool request_wait(msp::Request &request, const size_t wait_ms, const size_t min_payload_size = 0); - - /** - * @brief respond send data to FC and read acknowledge - * @param response response message - * @return true on success - * @return false on failure - */ - bool respond(const msp::Response &response); - - /** - * @brief respond_block send data to FC until acknowledge has been received - * @param response response message with data - * @return true when acknowledge has been received - */ - bool respond_block(const msp::Response &response); - - /** - * @brief sendData send raw data and ID to flight controller, accepts any uint8 id - * @param id message ID - * @param data raw data - * @return true on success - * @return false on failure - */ - bool sendData(const uint8_t id, const ByteVector &data = ByteVector(0)); - - /** - * @brief sendData send raw data and ID to flight controller, only accepts registered message ID - * @param id message ID - * @param data raw data - * @return true on success - * @return false on failure - */ - bool sendData(const msp::ID id, const ByteVector &data = ByteVector(0)) { - return sendData(uint8_t(id), data); - } - - /** - * @brief send encode message and send payload - * @param response message sent to FC - * @return true on success - * @return false on failure - */ - bool send(const msp::Response &response) { - return sendData(response.id(), response.encode()); - } - - /** - * @brief receiveData receive raw data from flight controller - * @return pair of data and message ID - */ - DataID receiveData(); - - /** - * @brief setWait set time (microseconds) between sending and receiving - * After sending a request to the FC, we need to wait a small amount of time - * for the FC to process our request and to respond. - * - * @param wait_us waiting time in microseconds - */ - void setWait(unsigned int wait_us) { - wait = wait_us; - } - -private: - /** - * @brief crc compute checksum of data package - * @param id message ID - * @param data raw data vector - * @return checksum - */ - uint8_t crc(const uint8_t id, const ByteVector &data); - - /** - * @brief write write data vector to device - * @param data raw data vector - * @return true on success - * @return false on failure - */ - bool write(const std::vector &data); - - /** - * @brief read read data vector from device - * @param data raw data vector in which read data will be stored - * @return number of read bytes - */ - size_t read(std::vector &data); - - /** - * @brief read read given amount of bytes from device - * @param n_bytes number of bytes to read - * @return data vector with read bytes - */ - std::vector read(std::size_t n_bytes); - - /** - * @brief read read a single byte from device - * @return single byte - */ - uint8_t read() { - return read(1).front(); - } - - /** - * @brief hasData check if data is available - * @return >0 amount of bytes ready to read - * @return -1 on error - */ - int hasData(); - - /** - * @brief clear flush the serial buffer to remove old data - */ - void clear(); - - std::string device; - std::unique_ptr pimpl; - std::mutex lock_write; - std::mutex lock_read; - unsigned int wait; //!< time (micro seconds) to wait before waiting for response -}; - -} // namespace msp - -#endif // MSP_HPP diff --git a/inc/msp/Message.hpp b/inc/msp/Message.hpp new file mode 100644 index 0000000..5adfe21 --- /dev/null +++ b/inc/msp/Message.hpp @@ -0,0 +1,82 @@ +#ifndef MESSAGE_HPP +#define MESSAGE_HPP + +#include +#include +#include +#include +#include "ByteVector.hpp" +#include "FirmwareVariants.hpp" + +namespace msp { + +enum class ID : uint16_t; + +class Message { +public: + /** + * @brief get the ID of the message + * @returns ID + */ + virtual ID id() const = 0; + + /** + * @brief Message constructor accepting a FirmwareVariant + * @param v FirmwareVariant specifing which firmware this message should + * tailor itself to. + */ + Message(FirmwareVariant v) : fw_variant(v) {} + + /** + * @brief Message destructor + */ + virtual ~Message() {} + + /** + * @brief Set the firmware the message should work with + * @param v FirmwareVariant specifing which firmware this message should + * tailor itself to. + */ + void setFirmwareVariant(FirmwareVariant v) { fw_variant = v; } + + /** + * @brief Queries the firmware variant the message works with + * @returns FirmwareVariant for this message + */ + FirmwareVariant getFirmwareVariant() const { return fw_variant; } + + /** + * @brief Decode message contents from a ByteVector + * @param data Source of data + * @returns False. Override methods should return true on success + */ + virtual bool decode(const ByteVector& /*data*/) { return false; } + + /** + * @brief Encode all data into a ByteVector + * @returns Unique pointer to a ByteVector of data + */ + virtual ByteVectorUptr encode() const { return ByteVectorUptr(); } + + virtual std::ostream& print(std::ostream& s) const { + s << "Print method for message ID " << uint16_t(id()) + << " is not implemented" << std::endl; + return s; + } + +protected: + FirmwareVariant fw_variant; +}; + +} // namespace msp + +std::ostream& operator<<(std::ostream& s, const msp::ID& id) { + s << (int)id; + return s; +} + +std::ostream& operator<<(std::ostream& s, const msp::Message& val) { + return val.print(s); +} + +#endif // TYPES_HPP diff --git a/inc/msp/PeriodicTimer.hpp b/inc/msp/PeriodicTimer.hpp new file mode 100644 index 0000000..5ad612c --- /dev/null +++ b/inc/msp/PeriodicTimer.hpp @@ -0,0 +1,63 @@ +#ifndef PERIODIC_TIMER_HPP +#define PERIODIC_TIMER_HPP + +#include +#include +#include +#include +#include +#include + +namespace msp { + +class PeriodicTimer { +public: + /** + * @brief PeriodicTimer define a periodic timer + * @param funct function that is called periodically + * @param period_seconds period in seconds + */ + PeriodicTimer(const std::function funct, + const double period_seconds); + + /** + * @brief PeriodicTimer destructor + */ + ~PeriodicTimer() { stop(); } + + /** + * @brief start define and start background thread + */ + bool start(); + + /** + * @brief stop tell thread to stop and wait for end + */ + bool stop(); + + /** + * @brief getPeriod get period in seconds + * @return period in seconds + */ + double getPeriod() { return period_us.count() / 1.e6; } + + /** + * @brief setPeriod change the update period of timer thread + * This will stop and restart the thread. + * @param period_seconds period in seconds + */ + void setPeriod(const double& period_seconds); + +private: + std::shared_ptr thread_ptr; + std::function funct; + std::chrono::duration period_us; + std::timed_mutex mutex_timer; + std::chrono::steady_clock::time_point tstart; + + std::atomic_flag running_; +}; + +} // namespace msp + +#endif diff --git a/inc/msp/Subscription.hpp b/inc/msp/Subscription.hpp new file mode 100644 index 0000000..d921dcf --- /dev/null +++ b/inc/msp/Subscription.hpp @@ -0,0 +1,181 @@ +#ifndef SUBSCRIPTION_HPP +#define SUBSCRIPTION_HPP + +#include +#include "Client.hpp" +#include "Message.hpp" +#include "PeriodicTimer.hpp" + +namespace msp { +namespace client { + +class SubscriptionBase { +public: + SubscriptionBase() {} + + virtual ~SubscriptionBase() {} + + virtual void decode(msp::ByteVector& data) = 0; + + virtual void makeRequest() = 0; + + virtual void handleResponse() = 0; + + virtual msp::Message& getMsgObject() = 0; + + /** + * @brief Checks to see if the subscription fires automatically + * @returns True if the request happens automatically + */ + bool isAutomatic() { return hasTimer() && (timer_->getPeriod() > 0.0); } + + /** + * @brief Checks to see if the timer has been created + * @returns True if there is a timer + */ + bool hasTimer() { return timer_ ? true : false; } + + /** + * @brief Start the timer for automatic execution + * @returns True if the timer starts successfully + */ + bool start() { return this->timer_->start(); } + + /** + * @brief Stop the timer's automatic execution + * @returns True if the timer stops successfully + */ + bool stop() { return this->timer_->stop(); } + + /** + * @brief setTimerPeriod change the period of the timer + * @param period_seconds period in seconds + */ + void setTimerPeriod(const double& period_seconds) { + if(timer_) { + timer_->setPeriod(period_seconds); + } + else if(period_seconds > 0.0) { + timer_ = std::unique_ptr(new PeriodicTimer( + std::bind(&SubscriptionBase::makeRequest, this), + period_seconds)); + this->timer_->start(); + } + } + + /** + * @brief setTimerFrequency change the update rate of timer + * @param rate_hz frequency in Hz + */ + void setTimerFrequency(const double& rate_hz) { + if(timer_) { + timer_->setPeriod(1.0 / rate_hz); + } + else if(rate_hz > 0.0) { + timer_ = std::unique_ptr(new PeriodicTimer( + std::bind(&SubscriptionBase::makeRequest, this), + 1.0 / rate_hz)); + this->timer_->start(); + } + } + +protected: + std::unique_ptr timer_; +}; + +template class Subscription : public SubscriptionBase { +public: + typedef std::function CallbackT; + typedef std::function CallbackM; + + /** + * @brief Subscription constructor + */ + Subscription() {} + + /** + * @brief Subscription constructor setting all parameters + * @param recv_callback Callback to execute upon receipt of message + * @param send_callback Callback to execute periodically to send message + * @param io_object Object which is used for encoding/decoding data + * @param period Repition rate of the request + */ + Subscription(const CallbackT& recv_callback, const CallbackM& send_callback, + std::unique_ptr&& io_object, const double& period = 0.0) : + recv_callback_(recv_callback), + send_callback_(send_callback), + io_object_(std::move(io_object)) { + if(period > 0.0) { + timer_ = std::unique_ptr(new PeriodicTimer( + std::bind(&Subscription::makeRequest, this), period)); + this->timer_->start(); + } + } + + /** + * @brief Virtual method for decoding received data + * @param data Data to be unpacked + */ + virtual void decode(msp::ByteVector& data) override { + io_object_->decode(data); + recv_callback_(*io_object_); + } + + /** + * @brief Sets the object used for packing and unpacking data + * @param obj unique_ptr to a Message-derived object + */ + void setIoObject(std::unique_ptr&& obj) { io_object_ = std::move(obj); } + + /** + * @brief Gets a reference to the IO object + * @returns + */ + T& getIoObject() { return *io_object_; } + + /** + * @brief Gets a reference to the internal IO object as a Message + * @returns reference to a Message + */ + virtual msp::Message& getMsgObject() override { return *io_object_; } + + /** + * @brief Sets the callback to be executed on success + * @param recv_callback the callback to be executed + */ + void setReceiveCallback(const CallbackT& recv_callback) { + recv_callback_ = recv_callback; + } + + /** + * @brief Calls the receive callback if it exists + */ + virtual void handleResponse() override { + if(recv_callback_) recv_callback_(*io_object_); + } + + /** + * @brief Sets the callback used to send the request + * @param send_callback the callback to be executed + */ + void setSendCallback(const CallbackM& send_callback) { + send_callback_ = send_callback; + } + + /** + * @brief Calls the send callback if it exists + */ + virtual void makeRequest() override { + if(send_callback_) send_callback_(*io_object_); + } + +protected: + CallbackT recv_callback_; + CallbackM send_callback_; + std::unique_ptr io_object_; +}; + +} // namespace client +} // namespace msp + +#endif diff --git a/inc/msp/Value.hpp b/inc/msp/Value.hpp new file mode 100644 index 0000000..b0697c9 --- /dev/null +++ b/inc/msp/Value.hpp @@ -0,0 +1,94 @@ +#ifndef VALUE_HPP +#define VALUE_HPP + +#include +#include + +namespace msp { + +template class Value { +public: + /** + * @brief Copy assignment operator + * @returns Reference to this object + */ + Value& operator=(Value& rhs) { + data.first = rhs(); + data.second = rhs.set(); + return *this; + } + + /** + * @brief Assignment operator for non-Value objects + * @returns Reference to this object + */ + Value& operator=(T rhs) { + data.first = rhs; + data.second = true; + return *this; + } + + /** + * @brief Gets a reference to the internal data + * @returns Reference to the data + */ + T& operator()() { return data.first; } + + /** + * @brief Gets a copy of the data + * @returns + */ + T operator()() const { return data.first; } + + /** + * @brief Queries if the data has been set + * @returns True if the internal data has been assigned + */ + bool set() const { return data.second; } + + /** + * @brief Gets a reference to the data valid flag + * @returns Reference to the data valid flag + */ + bool& set() { return data.second; } + + /** + * @brief Checks if the data has been set + * @returns True if the data has been set + */ + operator bool() const { return data.second; } + +private: + std::pair data; +}; + +} // namespace msp + +template +std::ostream& operator<<(std::ostream& s, const msp::Value& val) { + if(val.set()) + s << val(); + else + s << ""; + return s; +} + +template <> +std::ostream& operator<<(std::ostream& s, const msp::Value& val) { + if(val.set()) + s << uint32_t(val()); + else + s << ""; + return s; +} + +template <> +std::ostream& operator<<(std::ostream& s, const msp::Value& val) { + if(val.set()) + s << int32_t(val()); + else + s << ""; + return s; +} + +#endif diff --git a/inc/msp/deserialise.hpp b/inc/msp/deserialise.hpp deleted file mode 100644 index 82a7546..0000000 --- a/inc/msp/deserialise.hpp +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef DESERIALISE_HPP -#define DESERIALISE_HPP - -#include "types.hpp" -#include - -namespace msp { - -///////////////////////////////////////////////////////////////////// -/// de-/serialization for 16 and 32 bit unsigned integer - -static void serialise_uint16(const uint16_t val, ByteVector &data) { - data.push_back(val>>0); - data.push_back(val>>8); -} - -static uint16_t deserialise_uint16(const ByteVector &data, const size_t start) { - return (data[start]<<0) | (data[start+1]<<8); -} - -static void serialise_int16(const int16_t val, ByteVector &data) { - data.push_back(val>>0); - data.push_back(val>>8); -} - -static int16_t deserialise_int16(const ByteVector &data, const size_t start) { - return (data[start]<<0) | (data[start+1]<<8); -} - -static int32_t deserialise_int32(const ByteVector &data, const size_t start) { - return (data[start]<<0) | (data[start+1]<<8) | (data[start+2]<<16) | (data[start+3]<<24); -} - -static void serialise_uint32(const uint32_t val, ByteVector &data) { - data.push_back(val>>0); - data.push_back(val>>8); - data.push_back(val>>16); - data.push_back(val>>24); -} - -static uint32_t deserialise_uint32(const ByteVector &data, const size_t start) { - return (data[start]<<0) | (data[start+1]<<8) | (data[start+2]<<16) | (data[start+3]<<24); -} - -} - -#endif // DESERIALISE_HPP diff --git a/inc/msp/msg_print.hpp b/inc/msp/msg_print.hpp deleted file mode 100644 index f1a52d3..0000000 --- a/inc/msp/msg_print.hpp +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef MSG_PRINT_HPP -#define MSG_PRINT_HPP - -#include "msp_msg.hpp" -#include - -std::ostream& operator<<(std::ostream& s, const msp::msg::ApiVersion& api_version); - -std::ostream& operator<<(std::ostream& s, const msp::msg::FcVariant& fc_variant); - -std::ostream& operator<<(std::ostream& s, const msp::msg::FcVersion& fc_version); - -std::ostream& operator<<(std::ostream& s, const msp::msg::BoardInfo& board_info); - -std::ostream& operator<<(std::ostream& s, const msp::msg::BuildInfo& build_info); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Feature& feature); - -std::ostream& operator<<(std::ostream& s, const msp::msg::RxMap& rx_map); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Ident& ident); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Status& status); - -std::ostream& operator<<(std::ostream& s, const msp::msg::ImuRaw& imu); - -std::ostream& operator<<(std::ostream& s, const msp::msg::ImuSI& imu); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Servo& servo); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Motor& motor); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Rc& rc); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Attitude& attitude); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Altitude& altitude); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Analog& analog); - -std::ostream& operator<<(std::ostream& s, const msp::msg::RcTuning& rc_tuning); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Pid& pid); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Box& box); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Misc& misc); - -std::ostream& operator<<(std::ostream& s, const msp::msg::MotorPins& pin); - -std::ostream& operator<<(std::ostream& s, const msp::msg::BoxNames& box_names); - -std::ostream& operator<<(std::ostream& s, const msp::msg::PidNames& pid_names); - -std::ostream& operator<<(std::ostream& s, const msp::msg::BoxIds& box_ids); - -std::ostream& operator<<(std::ostream& s, const msp::msg::ServoConf& servo_conf); - -std::ostream& operator<<(std::ostream& s, const msp::msg::Debug& debug); - -#endif // MSG_PRINT_HPP diff --git a/inc/msp/msp_id.hpp b/inc/msp/msp_id.hpp deleted file mode 100644 index f8dccb9..0000000 --- a/inc/msp/msp_id.hpp +++ /dev/null @@ -1,166 +0,0 @@ -#ifndef MSP_ID_HPP -#define MSP_ID_HPP - -namespace msp { - -enum class ID : uint8_t { - // Cleanflight - MSP_API_VERSION = 1, - MSP_FC_VARIANT = 2, - MSP_FC_VERSION = 3, - MSP_BOARD_INFO = 4, - MSP_BUILD_INFO = 5, - - MSP_BATTERY_CONFIG = 32, - MSP_SET_BATTERY_CONFIG = 33, - MSP_MODE_RANGES = 34, - MSP_SET_MODE_RANGE = 35, - MSP_FEATURE = 36, - MSP_SET_FEATURE = 37, - MSP_BOARD_ALIGNMENT = 38, - MSP_SET_BOARD_ALIGNMENT = 39, - MSP_AMPERAGE_METER_CONFIG = 40, - MSP_SET_AMPERAGE_METER_CONFIG = 41, - MSP_MIXER = 42, - MSP_SET_MIXER = 43, - MSP_RX_CONFIG = 44, - MSP_SET_RX_CONFIG = 45, - MSP_LED_COLORS = 46, - MSP_SET_LED_COLORS = 47, - MSP_LED_STRIP_CONFIG = 48, - MSP_SET_LED_STRIP_CONFIG = 49, - MSP_RSSI_CONFIG = 50, - MSP_SET_RSSI_CONFIG = 51, - MSP_ADJUSTMENT_RANGES = 52, - MSP_SET_ADJUSTMENT_RANGE = 53, - - MSP_CF_SERIAL_CONFIG = 54, - MSP_SET_CF_SERIAL_CONFIG = 55, - - MSP_VOLTAGE_METER_CONFIG = 56, - MSP_SET_VOLTAGE_METER_CONFIG = 57, - - MSP_SONAR_ALTITUDE = 58, - - MSP_ARMING_CONFIG = 61, - MSP_SET_ARMING_CONFIG = 62, - - MSP_RX_MAP = 64, - MSP_SET_RX_MAP = 65, - - MSP_REBOOT = 68, - - MSP_BF_BUILD_INFO = 69, - - MSP_DATAFLASH_SUMMARY = 70, - MSP_DATAFLASH_READ = 71, - MSP_DATAFLASH_ERASE = 72, - - MSP_LOOP_TIME = 73, - MSP_SET_LOOP_TIME = 74, - - MSP_FAILSAFE_CONFIG = 75, - MSP_SET_FAILSAFE_CONFIG = 76, - - MSP_RXFAIL_CONFIG = 77, - MSP_SET_RXFAIL_CONFIG = 78, - - MSP_SDCARD_SUMMARY = 79, - - MSP_BLACKBOX_CONFIG = 80, - MSP_SET_BLACKBOX_CONFIG = 81, - - MSP_TRANSPONDER_CONFIG = 82, - MSP_SET_TRANSPONDER_CONFIG = 83, - - MSP_OSD_CHAR_WRITE = 87, - - MSP_VTX = 88, - - MSP_OSD_VIDEO_CONFIG = 180, - MSP_SET_OSD_VIDEO_CONFIG = 181, - MSP_OSD_VIDEO_STATUS = 182, - MSP_OSD_ELEMENT_SUMMARY = 183, - MSP_OSD_LAYOUT_CONFIG = 184, - MSP_SET_OSD_LAYOUT_CONFIG = 185, - - MSP_3D = 124, - MSP_RC_DEADBAND = 125, - MSP_SENSOR_ALIGNMENT = 126, - MSP_LED_STRIP_MODECOLOR = 127, - MSP_VOLTAGE_METERS = 128, - MSP_AMPERAGE_METERS = 129, - MSP_BATTERY_STATE = 130, - MSP_PILOT = 131, - - MSP_SET_3D = 217, - MSP_SET_RC_DEADBAND = 218, - MSP_SET_RESET_CURR_PID = 219, - MSP_SET_SENSOR_ALIGNMENT = 220, - MSP_SET_LED_STRIP_MODECOLOR = 221, - MSP_SET_PILOT = 222, - MSP_PASSTHROUGH_SERIAL = 244, - - MSP_UID = 160, - MSP_GPSSVINFO = 164, - MSP_SERVO_MIX_RULES = 241, - MSP_SET_SERVO_MIX_RULE = 242, - MSP_SET_4WAY_IF = 245, - - // MultiWii - MSP_IDENT = 100, - MSP_STATUS = 101, - MSP_RAW_IMU = 102, - MSP_SERVO = 103, - MSP_MOTOR = 104, - MSP_RC = 105, - MSP_RAW_GPS = 106, - MSP_COMP_GPS = 107, - MSP_ATTITUDE = 108, - MSP_ALTITUDE = 109, - MSP_ANALOG = 110, - MSP_RC_TUNING = 111, - MSP_PID = 112, - MSP_BOX = 113, - MSP_MISC = 114, - MSP_MOTOR_PINS = 115, - MSP_BOXNAMES = 116, - MSP_PIDNAMES = 117, - MSP_WP = 118, - MSP_BOXIDS = 119, - MSP_SERVO_CONF = 120, - - MSP_NAV_STATUS = 121, - MSP_NAV_CONFIG = 122, - - MSP_CELLS = 130, - - MSP_SET_RAW_RC = 200, - MSP_SET_RAW_GPS = 201, - MSP_SET_PID = 202, - MSP_SET_BOX = 203, - MSP_SET_RC_TUNING = 204, - MSP_ACC_CALIBRATION = 205, - MSP_MAG_CALIBRATION = 206, - MSP_SET_MISC = 207, - MSP_RESET_CONF = 208, - MSP_SET_WP = 209, - MSP_SELECT_SETTING = 210, - MSP_SET_HEAD = 211, - MSP_SET_SERVO_CONF = 212, - MSP_SET_MOTOR = 214, - MSP_SET_NAV_CONFIG = 215, - - MSP_SET_ACC_TRIM = 239, - MSP_ACC_TRIM = 240, - MSP_BIND = 241, - - MSP_EEPROM_WRITE = 250, - - MSP_DEBUGMSG = 253, - MSP_DEBUG = 254, -}; - -} // namespace msp - -#endif // MSP_ID_HPP diff --git a/inc/msp/msp_msg.hpp b/inc/msp/msp_msg.hpp index 0bda8e4..d412a12 100644 --- a/inc/msp/msp_msg.hpp +++ b/inc/msp/msp_msg.hpp @@ -4,18 +4,290 @@ #ifndef MSP_MSG_HPP #define MSP_MSG_HPP -#include #include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include "Message.hpp" -#include "types.hpp" +/*================================================================ + * actual messages have id and the relevant encode decode methods + * the logic for encoding and decoding must be within a message-derived class + * non message-derived structs must have pack/unpack subroutines + * + */ -#include "deserialise.hpp" +// undefine macros defined by GNU C std library +#undef major +#undef minor namespace msp { + +enum class ID : uint16_t { + MSP_API_VERSION = 1, + MSP_FC_VARIANT = 2, + MSP_FC_VERSION = 3, + MSP_BOARD_INFO = 4, + MSP_BUILD_INFO = 5, + MSP_INAV_PID = 6, + MSP_SET_INAV_PID = 7, + MSP_NAME = 10, // out message + MSP_SET_NAME = 11, // in message + MSP_NAV_POSHOLD = 12, // only in iNav + MSP_SET_NAV_POSHOLD = 13, // only in iNav + MSP_CALIBRATION_DATA = 14, + MSP_SET_CALIBRATION_DATA = 15, + MSP_POSITION_ESTIMATION_CONFIG = 16, + MSP_SET_POSITION_ESTIMATION_CONFIG = 17, + MSP_WP_MISSION_LOAD = 18, // Load mission from NVRAM + MSP_WP_MISSION_SAVE = 19, // Save mission to NVRAM + MSP_WP_GETINFO = 20, + MSP_RTH_AND_LAND_CONFIG = 21, + MSP_SET_RTH_AND_LAND_CONFIG = 22, + MSP_FW_CONFIG = 23, + MSP_SET_FW_CONFIG = 24, + MSP_BATTERY_CONFIG = 32, // not avaialable in iNav + MSP_SET_BATTERY_CONFIG = 33, // not avaialable in iNav + MSP_MODE_RANGES = 34, + MSP_SET_MODE_RANGE = 35, + MSP_FEATURE = 36, + MSP_SET_FEATURE = 37, + MSP_BOARD_ALIGNMENT = 38, + MSP_SET_BOARD_ALIGNMENT = 39, + MSP_CURRENT_METER_CONFIG = 40, + MSP_SET_CURRENT_METER_CONFIG = 41, + MSP_MIXER = 42, + MSP_SET_MIXER = 43, + MSP_RX_CONFIG = 44, + MSP_SET_RX_CONFIG = 45, + MSP_LED_COLORS = 46, + MSP_SET_LED_COLORS = 47, + MSP_LED_STRIP_CONFIG = 48, + MSP_SET_LED_STRIP_CONFIG = 49, + MSP_RSSI_CONFIG = 50, + MSP_SET_RSSI_CONFIG = 51, + MSP_ADJUSTMENT_RANGES = 52, + MSP_SET_ADJUSTMENT_RANGE = 53, + MSP_CF_SERIAL_CONFIG = 54, + MSP_SET_CF_SERIAL_CONFIG = 55, + MSP_VOLTAGE_METER_CONFIG = 56, + MSP_SET_VOLTAGE_METER_CONFIG = 57, + MSP_SONAR_ALTITUDE = 58, + MSP_PID_CONTROLLER = 59, + MSP_SET_PID_CONTROLLER = 60, + MSP_ARMING_CONFIG = 61, + MSP_SET_ARMING_CONFIG = 62, + MSP_RX_MAP = 64, + MSP_SET_RX_MAP = 65, + MSP_BF_CONFIG = 66, // depricated, out message + MSP_SET_BF_CONFIG = 67, // depricated, in message + MSP_REBOOT = 68, + MSP_BF_BUILD_INFO = 69, // depricated, iNav + MSP_DATAFLASH_SUMMARY = 70, + MSP_DATAFLASH_READ = 71, + MSP_DATAFLASH_ERASE = 72, + MSP_LOOP_TIME = 73, // depricated, iNav + MSP_SET_LOOP_TIME = 74, // depricated, iNav + MSP_FAILSAFE_CONFIG = 75, + MSP_SET_FAILSAFE_CONFIG = 76, + MSP_RXFAIL_CONFIG = 77, // depricated, iNav + MSP_SET_RXFAIL_CONFIG = 78, // depricated, iNav + MSP_SDCARD_SUMMARY = 79, + MSP_BLACKBOX_CONFIG = 80, + MSP_SET_BLACKBOX_CONFIG = 81, + MSP_TRANSPONDER_CONFIG = 82, + MSP_SET_TRANSPONDER_CONFIG = 83, + MSP_OSD_CONFIG = 84, // out message, betaflight + MSP_SET_OSD_CONFIG = 85, // in message, betaflight + MSP_OSD_CHAR_READ = 86, // out message, betaflight + MSP_OSD_CHAR_WRITE = 87, + MSP_VTX_CONFIG = 88, + MSP_SET_VTX_CONFIG = 89, + MSP_ADVANCED_CONFIG = 90, + MSP_SET_ADVANCED_CONFIG = 91, + MSP_FILTER_CONFIG = 92, + MSP_SET_FILTER_CONFIG = 93, + MSP_PID_ADVANCED = 94, + MSP_SET_PID_ADVANCED = 95, + MSP_SENSOR_CONFIG = 96, + MSP_SET_SENSOR_CONFIG = 97, + MSP_CAMERA_CONTROL = 98, // MSP_SPECIAL_PARAMETERS + MSP_SET_ARMING_DISABLED = 99, // MSP_SET_SPECIAL_PARAMETERS + MSP_IDENT = 100, // depricated + MSP_STATUS = 101, + MSP_RAW_IMU = 102, + MSP_SERVO = 103, + MSP_MOTOR = 104, + MSP_RC = 105, + MSP_RAW_GPS = 106, + MSP_COMP_GPS = 107, + MSP_ATTITUDE = 108, + MSP_ALTITUDE = 109, + MSP_ANALOG = 110, + MSP_RC_TUNING = 111, + MSP_PID = 112, + MSP_ACTIVEBOXES = 113, // depricated, iNav + MSP_MISC = 114, // depricated, iNav + MSP_MOTOR_PINS = 115, // depricated, iNav + MSP_BOXNAMES = 116, + MSP_PIDNAMES = 117, + MSP_WP = 118, + MSP_BOXIDS = 119, + MSP_SERVO_CONF = 120, + MSP_NAV_STATUS = 121, + MSP_NAV_CONFIG = 122, + MSP_MOTOR_3D_CONFIG = 124, + MSP_RC_DEADBAND = 125, + MSP_SENSOR_ALIGNMENT = 126, + MSP_LED_STRIP_MODECOLOR = 127, + MSP_VOLTAGE_METERS = 128, // not present in iNav + MSP_CURRENT_METERS = 129, // not present in iNav + MSP_BATTERY_STATE = 130, // not present in iNav + MSP_MOTOR_CONFIG = 131, // out message + MSP_GPS_CONFIG = 132, // out message + MSP_COMPASS_CONFIG = 133, // out message + MSP_ESC_SENSOR_DATA = 134, // out message + MSP_STATUS_EX = 150, + MSP_SENSOR_STATUS = 151, // only iNav + MSP_UID = 160, + MSP_GPSSVINFO = 164, + MSP_GPSSTATISTICS = 166, + MSP_OSD_VIDEO_CONFIG = 180, + MSP_SET_OSD_VIDEO_CONFIG = 181, + MSP_DISPLAYPORT = 182, + MSP_COPY_PROFILE = 183, // not in iNav + MSP_BEEPER_CONFIG = 184, // not in iNav + MSP_SET_BEEPER_CONFIG = 185, // not in iNav + MSP_SET_TX_INFO = 186, // in message + MSP_TX_INFO = 187, // out message + MSP_SET_RAW_RC = 200, + MSP_SET_RAW_GPS = 201, + MSP_SET_PID = 202, + MSP_SET_BOX = 203, // depricated + MSP_SET_RC_TUNING = 204, + MSP_ACC_CALIBRATION = 205, + MSP_MAG_CALIBRATION = 206, + MSP_SET_MISC = 207, // depricated + MSP_RESET_CONF = 208, + MSP_SET_WP = 209, + MSP_SELECT_SETTING = 210, + MSP_SET_HEADING = 211, + MSP_SET_SERVO_CONF = 212, + MSP_SET_MOTOR = 214, + MSP_SET_NAV_CONFIG = 215, + MSP_SET_MOTOR_3D_CONF = 217, + MSP_SET_RC_DEADBAND = 218, + MSP_SET_RESET_CURR_PID = 219, + MSP_SET_SENSOR_ALIGNMENT = 220, + MSP_SET_LED_STRIP_MODECOLOR = 221, + MSP_SET_MOTOR_CONFIG = 222, // out message + MSP_SET_GPS_CONFIG = 223, // out message + MSP_SET_COMPASS_CONFIG = 224, // out message + MSP_SET_ACC_TRIM = 239, // in message + MSP_ACC_TRIM = 240, // out message + MSP_SERVO_MIX_RULES = 241, // out message + MSP_SET_SERVO_MIX_RULE = 242, // in message + MSP_PASSTHROUGH_SERIAL = 244, // not used in CF, BF, iNav + MSP_SET_4WAY_IF = 245, // in message + MSP_SET_RTC = 246, // in message + MSP_RTC = 247, // out message + MSP_EEPROM_WRITE = 250, // in message + MSP_RESERVE_1 = 251, // reserved for system usage + MSP_RESERVE_2 = 252, // reserved for system usage + MSP_DEBUGMSG = 253, // out message + MSP_DEBUG = 254, // out message + MSP_V2_FRAME = 255, // MSPv2 over MSPv1 + + MSP2_COMMON_TZ = 0x1001, // out message, TZ offset + MSP2_COMMON_SET_TZ = 0x1002, // in message, sets the TZ offset + MSP2_COMMON_SETTING = 0x1003, // in/out message, returns setting + MSP2_COMMON_SET_SETTING = 0x1004, // in message, sets a setting value + MSP2_COMMON_MOTOR_MIXER = 0x1005, + MSP2_COMMON_SET_MOTOR_MIXER = 0x1006, + MSP2_INAV_STATUS = 0x2000, + MSP2_INAV_OPTICAL_FLOW = 0x2001, + MSP2_INAV_ANALOG = 0x2002, + MSP2_INAV_MISC = 0x2003, + MSP2_INAV_SET_MISC = 0x2004, + MSP2_INAV_BATTERY_CONFIG = 0x2005, + MSP2_INAV_SET_BATTERY_CONFIG = 0x2006, + MSP2_INAV_RATE_PROFILE = 0x2007, + MSP2_INAV_SET_RATE_PROFILE = 0x2008, + MSP2_INAV_AIR_SPEED = 0x2009 +}; + +enum class ArmingFlags : uint32_t { + ARMED = (1 << 2), + WAS_EVER_ARMED = (1 << 3), + + ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7), + + ARMING_DISABLED_NOT_LEVEL = (1 << 8), + ARMING_DISABLED_SENSORS_CALIBRATING = (1 << 9), + ARMING_DISABLED_SYSTEM_OVERLOADED = (1 << 10), + ARMING_DISABLED_NAVIGATION_UNSAFE = (1 << 11), + ARMING_DISABLED_COMPASS_NOT_CALIBRATED = (1 << 12), + ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED = (1 << 13), + ARMING_DISABLED_ARM_SWITCH = (1 << 14), + ARMING_DISABLED_HARDWARE_FAILURE = (1 << 15), + ARMING_DISABLED_BOXFAILSAFE = (1 << 16), + ARMING_DISABLED_BOXKILLSWITCH = (1 << 17), + ARMING_DISABLED_RC_LINK = (1 << 18), + ARMING_DISABLED_THROTTLE = (1 << 19), + ARMING_DISABLED_CLI = (1 << 20), + ARMING_DISABLED_CMS_MENU = (1 << 21), + ARMING_DISABLED_OSD_MENU = (1 << 22), + ARMING_DISABLED_ROLLPITCH_NOT_CENTERED = (1 << 23), + ARMING_DISABLED_SERVO_AUTOTRIM = (1 << 24), + ARMING_DISABLED_OOM = (1 << 25), + ARMING_DISABLED_INVALID_SETTING = (1 << 26), + + ARMING_DISABLED_ALL_FLAGS = + (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | + ARMING_DISABLED_SENSORS_CALIBRATING | + ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | + ARMING_DISABLED_COMPASS_NOT_CALIBRATED | + ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED | + ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | + ARMING_DISABLED_BOXFAILSAFE | ARMING_DISABLED_BOXKILLSWITCH | + ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | + ARMING_DISABLED_CLI | ARMING_DISABLED_CMS_MENU | + ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED | + ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | + ARMING_DISABLED_INVALID_SETTING) +}; + +std::string armingFlagToString(uint32_t flag) { + std::string val; + if(flag & (1 << 2)) val += "ARMED "; + if(flag & (1 << 3)) val += "WAS_EVER_ARMED "; + if(flag & (1 << 7)) val += "ARMING_DISABLED_FAILSAFE_SYSTEM "; + if(flag & (1 << 8)) val += "ARMING_DISABLED_NOT_LEVEL "; + if(flag & (1 << 9)) val += "ARMING_DISABLED_SENSORS_CALIBRATING "; + if(flag & (1 << 10)) val += "ARMING_DISABLED_SYSTEM_OVERLOADED "; + if(flag & (1 << 11)) val += "ARMING_DISABLED_NAVIGATION_UNSAFE "; + if(flag & (1 << 12)) val += "ARMING_DISABLED_COMPASS_NOT_CALIBRATED "; + if(flag & (1 << 13)) val += "ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED "; + if(flag & (1 << 14)) val += "ARMING_DISABLED_ARM_SWITCH "; + if(flag & (1 << 15)) val += "ARMING_DISABLED_HARDWARE_FAILURE "; + if(flag & (1 << 16)) val += "ARMING_DISABLED_BOXFAILSAFE "; + if(flag & (1 << 17)) val += "ARMING_DISABLED_BOXKILLSWITCH "; + if(flag & (1 << 18)) val += "ARMING_DISABLED_RC_LINK "; + if(flag & (1 << 19)) val += "ARMING_DISABLED_THROTTLE "; + if(flag & (1 << 20)) val += "ARMING_DISABLED_CLI "; + if(flag & (1 << 21)) val += "ARMING_DISABLED_CMS_MENU "; + if(flag & (1 << 22)) val += "ARMING_DISABLED_OSD_MENU "; + if(flag & (1 << 23)) val += "ARMING_DISABLED_ROLLPITCH_NOT_CENTERED "; + if(flag & (1 << 24)) val += "ARMING_DISABLED_SERVO_AUTOTRIM "; + if(flag & (1 << 25)) val += "ARMING_DISABLED_OOM "; + if(flag & (1 << 26)) val += "ARMING_DISABLED_INVALID_SETTING "; + return val; +} + namespace msg { const static size_t N_SERVO = 8; @@ -23,47 +295,61 @@ const static size_t N_MOTOR = 8; const static size_t BOARD_IDENTIFIER_LENGTH = 4; -const static size_t BUILD_DATE_LENGTH = 11; -const static size_t BUILD_TIME_LENGTH = 8; +const static size_t BUILD_DATE_LENGTH = 11; +const static size_t BUILD_TIME_LENGTH = 8; const static size_t GIT_SHORT_REVISION_LENGTH = 7; +const static size_t MAX_NAME_LENGTH = 16; +const static size_t MAX_MODE_ACTIVATION_CONDITION_COUNT = 20; + +const static size_t LED_CONFIGURABLE_COLOR_COUNT = 16; +const static size_t LED_MAX_STRIP_LENGTH = 32; + +const static size_t MAX_ADJUSTMENT_RANGE_COUNT = 12; +const static size_t MAX_SIMULTANEOUS_ADJUSTMENT_COUNT = 6; + +const static size_t OSD_ITEM_COUNT = 41; // manual count from iNav io/osd.h + +const static size_t MAX_MAPPABLE_RX_INPUTS = 4; // unique to REVO? + +const static size_t LED_MODE_COUNT = 6; +const static size_t LED_DIRECTION_COUNT = 6; +const static size_t LED_SPECIAL_COLOR_COUNT = 11; + enum class MultiType : uint8_t { - TRI = 1, - QUADP, // 2 - QUADX, // 3 - BI, // 4 - GIMBAL, // 5 - Y6, // 6 - HEX6, // 7 - FLYING_WING, // 8 - Y4, // 9 - HEX6X, // 10 - OCTOX8, // 11 - OCTOFLATP, // 12 - OCTOFLATX, // 13 - AIRPLANE, // 14 + TRI = 1, + QUADP, // 2 + QUADX, // 3 + BI, // 4 + GIMBAL, // 5 + Y6, // 6 + HEX6, // 7 + FLYING_WING, // 8 + Y4, // 9 + HEX6X, // 10 + OCTOX8, // 11 + OCTOFLATP, // 12 + OCTOFLATX, // 13 + AIRPLANE, // 14 HELI_120_CCPM, // 15 HELI_90_DEG, // 16 - VTAIL4, // 17 - HEX6H, // 18 - DUALCOPTER = 20, - SINGLECOPTER, // 21 + VTAIL4, // 17 + HEX6H, // 18 + DUALCOPTER = 20, + SINGLECOPTER, // 21 }; -enum class Capability { - BIND, - DYNBAL, - FLAP, - NAVCAP, - EXTAUX -}; +enum class Capability { BIND, DYNBAL, FLAP, NAVCAP, EXTAUX }; enum class Sensor { Accelerometer, Barometer, Magnetometer, GPS, - Sonar + Sonar, + OpticalFlow, + Pitot, + GeneralHealth }; const static size_t NAUX = 4; @@ -74,941 +360,5144 @@ enum class SwitchPosition : size_t { HIGH = 2, }; -static const std::vector FEATURES = { - "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP", - "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", - "SONAR", "TELEMETRY", "AMPERAGE_METER", "3D", "RX_PARALLEL_PWM", - "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", - "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "OSD" +static const std::vector FEATURES = {"RX_PPM", + "VBAT", + "INFLIGHT_ACC_CAL", + "RX_SERIAL", + "MOTOR_STOP", + "SERVO_TILT", + "SOFTSERIAL", + "GPS", + "FAILSAFE", + "SONAR", + "TELEMETRY", + "AMPERAGE_METER", + "3D", + "RX_PARALLEL_PWM", + "RX_MSP", + "RSSI_ADC", + "LED_STRIP", + "DISPLAY", + "ONESHOT125", + "BLACKBOX", + "CHANNEL_FORWARDING", + "TRANSPONDER", + "OSD"}; + +enum class PID_Element : uint8_t { + PID_ROLL = 0, + PID_PITCH, + PID_YAW, + PID_POS_Z, + PID_POS_XY, + PID_VEL_XY, + PID_SURFACE, + PID_LEVEL, + PID_HEADING, + PID_VEL_Z, + PID_ITEM_COUNT }; ///////////////////////////////////////////////////////////////////// /// Cleanflight // MSP_API_VERSION: 1 -struct ApiVersion : public Request { - ID id() const { return ID::MSP_API_VERSION; } +struct ApiVersion : public Message { + ApiVersion(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_API_VERSION; } - size_t protocol; - size_t major; - size_t minor; + Value protocol; + Value major; + Value minor; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(protocol); + rc &= data.unpack(major); + rc &= data.unpack(minor); + return rc; + } - void decode(const std::vector &data) { - protocol = data[0]; - major = data[1]; - minor = data[2]; + virtual std::ostream& print(std::ostream& s) const override { + s << "#Api Version:" << std::endl; + s << " API: " << major << "." << minor << std::endl; + s << " Protocol: " << protocol << std::endl; + return s; } }; // MSP_FC_VARIANT: 2 -struct FcVariant : public Request { - ID id() const { return ID::MSP_FC_VARIANT; } +struct FcVariant : public Message { + FcVariant(FirmwareVariant v) : Message(v) {} - std::string identifier; + virtual ID id() const override { return ID::MSP_FC_VARIANT; } - void decode(const std::vector &data) { - identifier = std::string(data.begin(), data.end()); + Value identifier; + + virtual bool decode(const ByteVector& data) override { + return data.unpack(identifier, data.size()); + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#FC variant:" << std::endl; + s << " Identifier: " << identifier << std::endl; + return s; } }; // MSP_FC_VERSION: 3 -struct FcVersion : public Request { - ID id() const { return ID::MSP_FC_VERSION; } +struct FcVersion : public Message { + FcVersion(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_FC_VERSION; } - size_t major; - size_t minor; - size_t patch_level; + Value major; + Value minor; + Value patch_level; - void decode(const std::vector &data) { - major = data[0]; - minor = data[1]; - patch_level = data[2]; + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(major); + rc &= data.unpack(minor); + rc &= data.unpack(patch_level); + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#FC version:" << std::endl; + s << " Version: " << major << "." << minor << "." << patch_level + << std::endl; + return s; } }; // MSP_BOARD_INFO: 4 -struct BoardInfo : public Request { - ID id() const { return ID::MSP_BOARD_INFO; } - - std::string identifier; - uint16_t version; - uint8_t type; +struct BoardInfo : public Message { + BoardInfo(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_BOARD_INFO; } + + Value identifier; + Value version; + Value osd_support; + Value comms_capabilites; + Value name; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(identifier, BOARD_IDENTIFIER_LENGTH); + rc &= data.unpack(version); + rc &= data.unpack(osd_support); + rc &= data.unpack(comms_capabilites); + uint8_t name_len = 0; + rc &= data.unpack(name_len); + rc &= data.unpack(name, name_len); + return rc; + } - void decode(const std::vector &data) { - identifier = std::string(data.begin(), data.begin()+BOARD_IDENTIFIER_LENGTH); - version = deserialise_uint16(data,BOARD_IDENTIFIER_LENGTH); - type = data[BOARD_IDENTIFIER_LENGTH+2]; + virtual std::ostream& print(std::ostream& s) const override { + s << "#Board Info:" << std::endl; + s << " Identifier: " << identifier << std::endl; + s << " Version: " << version << std::endl; + s << " OSD support: " << osd_support << std::endl; + s << " Comms bitmask: " << comms_capabilites << std::endl; + s << " Board Name: " << name << std::endl; + return s; } }; // MSP_BUILD_INFO: 5 -struct BuildInfo : public Request { - ID id() const { return ID::MSP_BUILD_INFO; } +struct BuildInfo : public Message { + BuildInfo(FirmwareVariant v) : Message(v) {} - std::string buildDate; - std::string buildTime; - std::string shortGitRevision; + virtual ID id() const override { return ID::MSP_BUILD_INFO; } - void decode(const std::vector &data) { - buildDate = std::string((const char*)&data[0], BUILD_DATE_LENGTH); - buildTime = std::string((const char*)&data[BUILD_DATE_LENGTH], BUILD_TIME_LENGTH); - shortGitRevision = std::string((const char*)&data[BUILD_DATE_LENGTH+BUILD_TIME_LENGTH], GIT_SHORT_REVISION_LENGTH); - } -}; - -// MSP_FEATURE: 36 -struct Feature : public Request { - ID id() const { return ID::MSP_FEATURE; } + Value buildDate; + Value buildTime; + Value shortGitRevision; - std::set features; + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(buildDate, BUILD_DATE_LENGTH); + rc &= data.unpack(buildTime, BUILD_TIME_LENGTH); + rc &= data.unpack(shortGitRevision, GIT_SHORT_REVISION_LENGTH); + return rc; + } - void decode(const std::vector &data) { - const uint32_t mask = deserialise_uint32(data,0); - for(size_t ifeat(0); ifeat async_mode; + Value acc_task_frequency; + Value attitude_task_frequency; + Value heading_hold_rate_limit; + Value heading_hold_error_lpf_freq; + Value yaw_jump_prevention_limit; + Value gyro_lpf; + Value acc_soft_lpf_hz; +}; - std::set features; +// MSP_INAV_PID: 6 +struct InavPid : public InavPidSettings, public Message { + InavPid(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_INAV_PID; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(async_mode); + rc &= data.unpack(acc_task_frequency); + rc &= data.unpack(attitude_task_frequency); + rc &= data.unpack(heading_hold_rate_limit); + rc &= data.unpack(heading_hold_error_lpf_freq); + rc &= data.unpack(yaw_jump_prevention_limit); + rc &= data.unpack(gyro_lpf); + rc &= data.unpack(acc_soft_lpf_hz); + // read the reserved bytes + rc &= data.consume(4); + return rc; + } +}; - std::vector encode() const { - std::vector data; - uint32_t mask = 0; - for(size_t ifeat(0); ifeat(); + bool rc = true; + rc &= data->pack(async_mode); + rc &= data->pack(acc_task_frequency); + rc &= data->pack(attitude_task_frequency); + rc &= data->pack(heading_hold_rate_limit); + rc &= data->pack(heading_hold_error_lpf_freq); + rc &= data->pack(yaw_jump_prevention_limit); + rc &= data->pack(gyro_lpf); + rc &= data->pack(acc_soft_lpf_hz); + // write the reserved bytes + rc &= data->pack(uint32_t(0)); + if(!rc) data.reset(); return data; } }; -// MSP_RX_CONFIG: 44 -struct RxConfig : public Request { - ID id() const { return ID::MSP_RX_CONFIG; } +// MSP_NAME: 10 +struct BoardName : public Message { + BoardName(FirmwareVariant v) : Message(v) {} - uint8_t serialrx_provider; - uint16_t maxcheck; - uint16_t midrc; - uint16_t mincheck; - uint8_t spektrum_sat_bind; - uint16_t rx_min_usec; - uint16_t rx_max_usec; + virtual ID id() const override { return ID::MSP_NAME; } - void decode(const std::vector &data) { - serialrx_provider = data[0]; - maxcheck = deserialise_uint16(data, 1); - midrc = deserialise_uint16(data, 3); - mincheck = deserialise_uint16(data, 5); - spektrum_sat_bind = data[7]; - rx_min_usec = deserialise_uint16(data, 8); - rx_max_usec = deserialise_uint16(data, 10); - } + Value name; + + bool decode(ByteVector& data) { return data.unpack(name); } }; -// MSP_SONAR_ALTITUDE: 58 -struct SonarAltitude : public Request { - ID id() const { return ID::MSP_SONAR_ALTITUDE; } +// MSP_SET_NAME: 11 +struct SetBoardName : public Message { + SetBoardName(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_NAME; } - float altitude; // meters + Value name; - void decode(const std::vector &data) { - altitude = deserialise_int32(data, 0)/100.0f; + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(name, MAX_NAME_LENGTH)) data.reset(); + return data; } }; -// MSP_RX_MAP: 64 -struct RxMap : public Request { - ID id() const { return ID::MSP_RX_MAP; } +struct NavPosHoldSettings { + Value user_control_mode; + Value max_auto_speed; + Value max_auto_climb_rate; + Value max_manual_speed; + Value max_manual_climb_rate; + Value max_bank_angle; + Value use_thr_mid_for_althold; + Value hover_throttle; +}; - std::vector map; +// MSP_NAV_POSHOLD: 12 +struct NavPosHold : public NavPosHoldSettings, public Message { + NavPosHold(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_NAV_POSHOLD; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(user_control_mode); + rc &= data.unpack(max_auto_speed); + rc &= data.unpack(max_auto_climb_rate); + rc &= data.unpack(max_manual_speed); + rc &= data.unpack(max_manual_climb_rate); + rc &= data.unpack(max_bank_angle); + rc &= data.unpack(use_thr_mid_for_althold); + rc &= data.unpack(hover_throttle); + return rc; + } +}; - void decode(const std::vector &data) { - map = data; +// MSP_SET_NAV_POSHOLD: 13 +struct SetNavPosHold : public NavPosHoldSettings, public Message { + SetNavPosHold(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_NAV_POSHOLD; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(user_control_mode); + rc &= data->pack(max_auto_speed); + rc &= data->pack(max_auto_climb_rate); + rc &= data->pack(max_manual_speed); + rc &= data->pack(max_manual_climb_rate); + rc &= data->pack(max_bank_angle); + rc &= data->pack(use_thr_mid_for_althold); + rc &= data->pack(hover_throttle); + if(!rc) data.reset(); + return data; } }; -// MSP_SET_RX_MAP: 65 -struct SetRxMap : public Response { - ID id() const { return ID::MSP_SET_RX_MAP; } +struct CalibrationDataSettings { + Value acc_zero_x; + Value acc_zero_y; + Value acc_zero_z; + Value acc_gain_x; + Value acc_gain_y; + Value acc_gain_z; +}; + +// MSP_CALIBRATION_DATA: 14 +struct CalibrationData : public CalibrationDataSettings, public Message { + CalibrationData(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_CALIBRATION_DATA; } - std::vector map; + Value axis_calibration_flags; - std::vector encode() const { - return map; + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(axis_calibration_flags); + rc &= data.unpack(acc_zero_x); + rc &= data.unpack(acc_zero_y); + rc &= data.unpack(acc_zero_z); + rc &= data.unpack(acc_gain_x); + rc &= data.unpack(acc_gain_y); + rc &= data.unpack(acc_gain_z); + return rc; } }; -// MSP_REBOOT: 68 -struct Reboot : public Response { - ID id() const { return ID::MSP_REBOOT; } - std::vector encode() const { - return std::vector(); +// MSP_SET_CALIBRATION_DATA: 15 +struct SetCalibrationData : public CalibrationDataSettings, public Message { + SetCalibrationData(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_CALIBRATION_DATA; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(acc_zero_x); + rc &= data->pack(acc_zero_y); + rc &= data->pack(acc_zero_z); + rc &= data->pack(acc_gain_x); + rc &= data->pack(acc_gain_y); + rc &= data->pack(acc_gain_z); + if(!rc) data.reset(); + return data; } }; +struct PositionEstimationConfigSettings { + Value w_z_baro_p; + Value w_z_gps_p; + Value w_z_gps_v; + Value w_xy_gps_p; + Value w_xy_gps_v; + Value gps_min_sats; + Value use_gps_vel_NED; +}; -///////////////////////////////////////////////////////////////////// -/// Requests (1xx) - -// MSP_IDENT: 100 -struct Ident : public Request { - ID id() const { return ID::MSP_IDENT; } - - size_t version; - MultiType type; - size_t msp_version; - std::set capabilities; +// MSP_POSITION_ESTIMATION_CONFIG: 16 +struct PositionEstimationConfig : public PositionEstimationConfigSettings, + public Message { + PositionEstimationConfig(FirmwareVariant v) : Message(v) {} - void decode(const std::vector &data) { - version = data[0]; + virtual ID id() const override { + return ID::MSP_POSITION_ESTIMATION_CONFIG; + } - // determine multicopter type - type = MultiType(data[1]); + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(w_z_baro_p, 0.01); + rc &= data.unpack(w_z_gps_p, 0.01); + rc &= data.unpack(w_z_gps_v, 0.01); + rc &= data.unpack(w_xy_gps_p, 0.01); + rc &= data.unpack(w_xy_gps_v, 0.01); + rc &= data.unpack(gps_min_sats); + rc &= data.unpack(use_gps_vel_NED); + return rc; + } +}; - msp_version = data[2]; +// MSP_SET_POSITION_ESTIMATION_CONFIG: 17 +struct SetPositionEstimationConfig : public PositionEstimationConfigSettings, + public Message { + SetPositionEstimationConfig(FirmwareVariant v) : Message(v) {} - const uint32_t capability = deserialise_uint32(data, 3); - if(capability & (1 << 0)) - capabilities.insert(Capability::BIND); - if(capability & (1 << 2)) - capabilities.insert(Capability::DYNBAL); - if(capability & (1 << 3)) - capabilities.insert(Capability::FLAP); - if(capability & (1 << 4)) - capabilities.insert(Capability::NAVCAP); - if(capability & (1 << 5)) - capabilities.insert(Capability::EXTAUX); + virtual ID id() const override { + return ID::MSP_SET_POSITION_ESTIMATION_CONFIG; } - bool has(const Capability &cap) const { return capabilities.count(cap); } + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(static_cast(w_z_baro_p() * 100)); + rc &= data->pack(static_cast(w_z_gps_p() * 100)); + rc &= data->pack(static_cast(w_z_gps_v() * 100)); + rc &= data->pack(static_cast(w_xy_gps_p() * 100)); + rc &= data->pack(static_cast(w_xy_gps_v() * 100)); + rc &= data->pack(gps_min_sats); + rc &= data->pack(use_gps_vel_NED); + if(!rc) data.reset(); + return data; + } +}; - bool hasBind() const { return has(Capability::BIND); } +// MSP_WP_MISSION_LOAD: 18 +struct WpMissionLoad : public Message { + WpMissionLoad(FirmwareVariant v) : Message(v) {} - bool hasDynBal() const { return has(Capability::DYNBAL); } + virtual ID id() const override { return ID::MSP_WP_MISSION_LOAD; } - bool hasFlap() const { return has(Capability::FLAP); } + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(uint8_t(0))) data.reset(); + return data; + } }; -// MSP_STATUS: 101 -struct Status : public Request { - ID id() const { return ID::MSP_STATUS; } +// MSP_WP_MISSION_SAVE: 19 +struct WpMissionSave : public Message { + WpMissionSave(FirmwareVariant v) : Message(v) {} - uint16_t time; // in us - uint16_t errors; - std::set sensors; - size_t current_setting; - std::set active_box_id; + virtual ID id() const override { return ID::MSP_WP_MISSION_SAVE; } - void decode(const std::vector &data) { - time = deserialise_uint16(data, 0); + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(uint8_t(0))) data.reset(); + return data; + } +}; - errors = deserialise_uint16(data, 2); +// MSP_WP_GETINFO: 20 +struct WpGetInfo : public Message { + WpGetInfo(FirmwareVariant v) : Message(v) {} - // get sensors - sensors.clear(); - const uint16_t sensor = deserialise_uint16(data, 4); - if(sensor & (1 << 0)) - sensors.insert(Sensor::Accelerometer); - if(sensor & (1 << 1)) - sensors.insert(Sensor::Barometer); - if(sensor & (1 << 2)) - sensors.insert(Sensor::Magnetometer); - if(sensor & (1 << 3)) - sensors.insert(Sensor::GPS); - if(sensor & (1 << 4)) - sensors.insert(Sensor::Sonar); + virtual ID id() const override { return ID::MSP_WP_GETINFO; } - // check active boxes - active_box_id.clear(); - const uint32_t flag = deserialise_uint32(data, 6); - for(size_t ibox(0); ibox wp_capabilites; + Value max_waypoints; + Value wp_list_valid; + Value wp_count; - current_setting = data[10]; + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(wp_capabilites); + rc &= data.unpack(max_waypoints); + rc &= data.unpack(wp_list_valid); + rc &= data.unpack(wp_count); + return rc; } +}; - bool hasAccelerometer() const { return sensors.count(Sensor::Accelerometer); } +struct RthAndLandConfigSettings { + Value min_rth_distance; + Value rth_climb_first; + Value rth_climb_ignore_emerg; + Value rth_tail_first; + Value rth_allow_landing; + Value rth_alt_control_mode; + Value rth_abort_threshold; + Value rth_altitude; + Value land_descent_rate; + Value land_slowdown_minalt; + Value land_slowdown_maxalt; + Value emerg_descent_rate; +}; - bool hasBarometer() const { return sensors.count(Sensor::Barometer); } +// MSP_RTH_AND_LAND_CONFIG: 21 +struct RthAndLandConfig : public RthAndLandConfigSettings, public Message { + RthAndLandConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_RTH_AND_LAND_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(min_rth_distance); + rc &= data.unpack(rth_climb_first); + rc &= data.unpack(rth_climb_ignore_emerg); + rc &= data.unpack(rth_tail_first); + rc &= data.unpack(rth_allow_landing); + rc &= data.unpack(rth_alt_control_mode); + rc &= data.unpack(rth_abort_threshold); + rc &= data.unpack(rth_altitude); + rc &= data.unpack(land_descent_rate); + rc &= data.unpack(land_slowdown_minalt); + rc &= data.unpack(land_slowdown_maxalt); + rc &= data.unpack(emerg_descent_rate); + return rc; + } +}; - bool hasMagnetometer() const { return sensors.count(Sensor::Magnetometer); } +// MSP_SET_RTH_AND_LAND_CONFIG: 22 +struct SetRthAndLandConfig : public RthAndLandConfigSettings, public Message { + SetRthAndLandConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_RTH_AND_LAND_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(min_rth_distance); + rc &= data->pack(rth_climb_first); + rc &= data->pack(rth_climb_ignore_emerg); + rc &= data->pack(rth_tail_first); + rc &= data->pack(rth_allow_landing); + rc &= data->pack(rth_alt_control_mode); + rc &= data->pack(rth_abort_threshold); + rc &= data->pack(rth_altitude); + rc &= data->pack(land_descent_rate); + rc &= data->pack(land_slowdown_minalt); + rc &= data->pack(land_slowdown_maxalt); + rc &= data->pack(emerg_descent_rate); + if(!rc) data.reset(); + return data; + } +}; - bool hasGPS() const { return sensors.count(Sensor::GPS); } +struct FwConfigSettings { + Value cruise_throttle; + Value min_throttle; + Value max_throttle; + Value max_bank_angle; + Value max_climb_angle; + Value max_dive_angle; + Value pitch_to_throttle; + Value loiter_radius; +}; - bool hasSonar() const { return sensors.count(Sensor::Sonar); } +// MSP_FW_CONFIG: 23 +struct FwConfig : public FwConfigSettings, public Message { + FwConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_FW_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(cruise_throttle); + rc &= data.unpack(min_throttle); + rc &= data.unpack(max_throttle); + rc &= data.unpack(max_bank_angle); + rc &= data.unpack(max_climb_angle); + rc &= data.unpack(max_dive_angle); + rc &= data.unpack(pitch_to_throttle); + rc &= data.unpack(loiter_radius); + return rc; + } }; -// MSP_RAW_IMU: 102 -struct ImuRaw : public Request { - ID id() const { return ID::MSP_RAW_IMU; } +// MSP_SET_FW_CONFIG: 24 +struct SetFwConfig : public FwConfigSettings, public Message { + SetFwConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_FW_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(cruise_throttle); + rc &= data->pack(min_throttle); + rc &= data->pack(max_throttle); + rc &= data->pack(max_bank_angle); + rc &= data->pack(max_climb_angle); + rc &= data->pack(max_dive_angle); + rc &= data->pack(pitch_to_throttle); + rc &= data->pack(loiter_radius); + if(!rc) data.reset(); + return data; + } +}; - std::array acc; - std::array gyro; - std::array magn; +struct BatteryConfigSettings { + Value vbatmincellvoltage; + Value vbatmaxcellvoltage; + Value vbatwarningcellvoltage; + Value batteryCapacity; + Value voltageMeterSource; + Value currentMeterSource; +}; - void decode(const std::vector &data) { - acc = {{deserialise_int16(data, 0), deserialise_int16(data, 2), deserialise_int16(data, 4)}}; - gyro = {{deserialise_int16(data, 6), deserialise_int16(data, 8), deserialise_int16(data, 10)}}; - magn = {{deserialise_int16(data, 12), deserialise_int16(data, 14), deserialise_int16(data, 16)}}; +// MSP_BATTERY_CONFIG: 32 +struct BatteryConfig : public BatteryConfigSettings, public Message { + BatteryConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_BATTERY_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(vbatmincellvoltage); + rc &= data.unpack(vbatmaxcellvoltage); + rc &= data.unpack(vbatwarningcellvoltage); + rc &= data.unpack(batteryCapacity); + rc &= data.unpack(voltageMeterSource); + rc &= data.unpack(currentMeterSource); + return rc; } }; -// Imu in SI units -struct ImuSI { - std::array acc; // m/s^2 - std::array gyro; // deg/s - std::array magn; // uT +// MSP_SET_BATTERY_CONFIG: 33 +struct SetBatteryConfig : public BatteryConfigSettings, public Message { + SetBatteryConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_BATTERY_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(vbatmincellvoltage); + rc &= data->pack(vbatmaxcellvoltage); + rc &= data->pack(vbatwarningcellvoltage); + rc &= data->pack(batteryCapacity); + rc &= data->pack(voltageMeterSource); + rc &= data->pack(currentMeterSource); + if(!rc) data.reset(); + return data; + } +}; - ImuSI(const ImuRaw &imu_raw, - const float acc_1g, // sensor value at 1g - const float gyro_unit, // resolution in 1/(deg/s) - const float magn_gain, // scale magnetic value to uT (micro Tesla) - const float si_unit_1g // acceleration at 1g (in m/s^2) - ) - { - acc = {{imu_raw.acc[0]/acc_1g*si_unit_1g, - imu_raw.acc[1]/acc_1g*si_unit_1g, - imu_raw.acc[2]/acc_1g*si_unit_1g}}; +struct box_description { + Value id; + Value aux_channel_index; + Value startStep; + Value endStep; +}; + +// MSP_MODE_RANGES: 34 +struct ModeRanges : public Message { + ModeRanges(FirmwareVariant v) : Message(v) {} - gyro = {{imu_raw.gyro[0]*gyro_unit, - imu_raw.gyro[1]*gyro_unit, - imu_raw.gyro[2]*gyro_unit}}; + virtual ID id() const override { return ID::MSP_MODE_RANGES; } - magn = {{imu_raw.magn[0]*magn_gain, - imu_raw.magn[1]*magn_gain, - imu_raw.magn[2]*magn_gain}}; + std::array boxes; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(uint i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + rc &= data.unpack(boxes[i].id); + rc &= data.unpack(boxes[i].aux_channel_index); + rc &= data.unpack(boxes[i].startStep); + rc &= data.unpack(boxes[i].endStep); + } + return rc; } }; -// MSP_SERVO: 103 -struct Servo : public Request { - ID id() const { return ID::MSP_SERVO; } +// MSP_SET_MODE_RANGE: 35 +struct SetModeRange : public Message { + SetModeRange(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_MODE_RANGE; } - uint16_t servo[N_SERVO]; + Value mode_activation_condition_idx; + box_description box; - void decode(const std::vector &data) { - for(unsigned int i=0; i(); + bool rc = true; + rc &= data->pack(mode_activation_condition_idx); + rc &= data->pack(box.id); + rc &= data->pack(box.aux_channel_index); + rc &= data->pack(box.startStep); + rc &= data->pack(box.endStep); + if(!rc) data.reset(); + return data; } }; -// MSP_MOTOR: 104 -struct Motor : public Request { - ID id() const { return ID::MSP_MOTOR; } +// MSP_FEATURE: 36 +struct Feature : public Message { + Feature(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_FEATURE; } + + std::set features; - uint16_t motor[N_MOTOR]; + virtual bool decode(const ByteVector& data) override { + uint32_t mask; + bool rc = data.unpack(mask); + if(!rc) return rc; + features.clear(); + for(size_t ifeat(0); ifeat < FEATURES.size(); ifeat++) { + if(mask & (1 << ifeat)) features.insert(FEATURES[ifeat]); + } + return rc; + } - void decode(const std::vector &data) { - for(unsigned int i=0; i channels; + virtual ID id() const override { return ID::MSP_SET_FEATURE; } - void decode(const std::vector &data) { - channels.clear(); - // If feature 'RX_MSP' is active but "USE_RX_MSP" is undefined for the target, - // no RC data is provided as feedback. See also description at 'MSP_SET_RAW_RC'. - // In this case, return 0 for all RC channels. - for(size_t i(0); i features; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + uint32_t mask = 0; + for(size_t ifeat(0); ifeat < FEATURES.size(); ifeat++) { + if(features.count(FEATURES[ifeat])) mask |= 1 << ifeat; } + if(!data->pack(mask)) data.reset(); + return data; } }; -// MSP_RAW_GPS: 106 -struct RawGPS : public Request { - ID id() const { return ID::MSP_RAW_GPS; } +// iNav uses decidegrees, BF/CF use degrees +struct BoardAlignmentSettings { + Value roll; + Value pitch; + Value yaw; +}; - uint8_t fix; - uint8_t numSat; - uint32_t lat; - uint32_t lon; - uint16_t altitude; - uint16_t speed; - uint16_t ground_course; +// MSP_BOARD_ALIGNMENT: 38 +struct BoardAlignment : public BoardAlignmentSettings, public Message { + BoardAlignment(FirmwareVariant v) : Message(v) {} - void decode(const std::vector &data) { - fix = data[0]; - numSat = data[1]; - lat = deserialise_uint32(data, 2); - lon = deserialise_uint32(data, 6); - altitude = deserialise_uint16(data, 10); - speed = deserialise_uint16(data, 12); - ground_course = deserialise_uint16(data, 14); + virtual ID id() const override { return ID::MSP_BOARD_ALIGNMENT; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(roll); + rc &= data.unpack(pitch); + rc &= data.unpack(yaw); + return rc; } }; -// MSP_COMP_GPS: 107 -struct CompGPS : public Request { - ID id() const { return ID::MSP_COMP_GPS; } +// MSP_SET_BOARD_ALIGNMENT: 39 +struct SetBoardAlignment : public BoardAlignmentSettings, public Message { + SetBoardAlignment(FirmwareVariant v) : Message(v) {} - uint16_t distanceToHome; // meter - uint16_t directionToHome; // degree - uint8_t update; + virtual ID id() const override { return ID::MSP_BOARD_ALIGNMENT; } - void decode(const std::vector &data) { - distanceToHome = deserialise_uint16(data, 0); - directionToHome = deserialise_uint16(data, 2); - update = data[4]; + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(roll); + rc &= data->pack(pitch); + rc &= data->pack(yaw); + if(!rc) data.reset(); + return data; } }; -// MSP_ATTITUDE: 108 -struct Attitude : public Request { - ID id() const { return ID::MSP_ATTITUDE; } +struct CurrentMeterConfigSettings { + Value currnet_scale; + Value current_offset; + Value current_type; + Value capacity; +}; + +// MSP_CURRENT_METER_CONFIG: 40 (differs from Cleanflight/BetaFlight) +struct CurrentMeterConfig : public CurrentMeterConfigSettings, public Message { + CurrentMeterConfig(FirmwareVariant v) : Message(v) {} - float ang_x; // degree - float ang_y; // degree - int16_t heading; // degree + virtual ID id() const override { return ID::MSP_CURRENT_METER_CONFIG; } - void decode(const std::vector &data) { - ang_x = deserialise_int16(data, 0)/10.0f; - ang_y = deserialise_int16(data, 2)/10.0f; - heading = deserialise_int16(data, 4); + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(currnet_scale); + rc &= data.unpack(current_offset); + rc &= data.unpack(current_type); + rc &= data.unpack(capacity); + return rc; } }; -// MSP_ALTITUDE: 109 -struct Altitude : public Request { - ID id() const { return ID::MSP_ALTITUDE; } - - float altitude; // m - float vario; // m/s - - void decode(const std::vector &data) { - altitude = deserialise_int32(data, 0)/100.0f; - vario = deserialise_int16(data, 4)/100.0f; +// MSP_SET_CURRENT_METER_CONFIG: 41 (differs from Cleanflight/BetaFlight) +struct SetCurrentMeterConfig : public CurrentMeterConfigSettings, + public Message { + SetCurrentMeterConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_CURRENT_METER_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(currnet_scale); + rc &= data->pack(current_offset); + rc &= data->pack(current_type); + rc &= data->pack(capacity); + if(!rc) data.reset(); + return data; } }; -// MSP_ANALOG: 110 -struct Analog : public Request { - ID id() const { return ID::MSP_ANALOG; } +// MSP_MIXER: 42 +struct Mixer : public Message { + Mixer(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_MIXER; } - float vbat; // Volt - float powerMeterSum; // Ah - size_t rssi; // Received Signal Strength Indication [0; 1023] - float amperage; // Ampere + Value mode; - void decode(const std::vector &data) { - vbat = data[0]/10.0f; - powerMeterSum = deserialise_uint16(data, 1)/1000.0f; - rssi = deserialise_uint16(data, 3); - amperage = deserialise_uint16(data, 5)/10.0f; + virtual bool decode(const ByteVector& data) override { + return data.unpack(mode); } }; -// MSP_RC_TUNING: 111 -struct RcTuning : Request { - ID id() const { return ID::MSP_RC_TUNING; } +// MSP_SET_MIXER: 43 +struct SetMixer : public Message { + SetMixer(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_MIXER; } - double RC_RATE; - double RC_EXPO; - double RollPitchRate; - double YawRate; - double DynThrPID; - double Throttle_MID; - double Throttle_EXPO; + Value mode; - void decode(const std::vector &data) { - RC_RATE = data[0] / 100.0; - RC_EXPO = data[1] / 100.0; - RollPitchRate = data[2] / 100.0; - YawRate = data[3] / 100.0; - DynThrPID = data[4] / 100.0; - Throttle_MID = data[5] / 100.0; - Throttle_EXPO = data[6] / 100.0; + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(mode)) data.reset(); + return data; } }; -// PID struct for messages 112 and 204 -struct PidTerms { - float P; - float I; - float D; +struct RxConfigSettings { + size_t valid_data_groups; + // group1 + Value serialrx_provider; + Value maxcheck; + Value midrc; + Value mincheck; + Value spektrum_sat_bind; + // group 2 + Value rx_min_usec; + Value rx_max_usec; + // group 3 + Value rcInterpolation; + Value rcInterpolationInterval; + Value airModeActivateThreshold; + // group 4 + Value rx_spi_protocol; + Value rx_spi_id; + Value rx_spi_rf_channel_count; + // group 5 + Value fpvCamAngleDegrees; + // group 6 - iNav only + Value receiverType; + + std::ostream& rxConfigPrint(std::ostream& s) const { + s << "#RX configuration:" << std::endl; + s << " serialrx_provider: " << serialrx_provider << std::endl; + s << " maxcheck: " << maxcheck << std::endl; + s << " midrc: " << midrc << std::endl; + s << " mincheck: " << mincheck << std::endl; + s << " spektrum_sat_bind: " << spektrum_sat_bind << std::endl; + s << " rx_min_usec: " << rx_min_usec << std::endl; + s << " rx_max_usec: " << rx_max_usec << std::endl; + s << " rcInterpolation: " << rcInterpolation << std::endl; + s << " rcInterpolationInterval: " << rcInterpolationInterval + << std::endl; + s << " airModeActivateThreshold: " << airModeActivateThreshold + << std::endl; + s << " rx_spi_protocol: " << rx_spi_protocol << std::endl; + s << " rx_spi_id: " << rx_spi_id << std::endl; + s << " rx_spi_rf_channel_count: " << rx_spi_rf_channel_count + << std::endl; + s << " fpvCamAngleDegrees: " << fpvCamAngleDegrees << std::endl; + s << " receiverType: " << receiverType << std::endl; + return s; + } +}; - PidTerms() { } +// MSP_RX_CONFIG: 44 +struct RxConfig : public RxConfigSettings, public Message { + RxConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_RX_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + valid_data_groups = 1; + rc &= data.unpack(serialrx_provider); + rc &= data.unpack(maxcheck); + rc &= data.unpack(midrc); + rc &= data.unpack(mincheck); + rc &= data.unpack(spektrum_sat_bind); + if(data.unpacking_remaining() == 0) return rc; + + valid_data_groups += 1; + rc &= data.unpack(rx_min_usec); + rc &= data.unpack(rx_max_usec); + if(data.unpacking_remaining() == 0) return rc; + + valid_data_groups += 1; + rc &= data.unpack(rcInterpolation); + rc &= data.unpack(rcInterpolationInterval); + rc &= data.unpack(airModeActivateThreshold); + if(data.unpacking_remaining() == 0) return rc; + + valid_data_groups += 1; + rc &= data.unpack(rx_spi_protocol); + rc &= data.unpack(rx_spi_id); + rc &= data.unpack(rx_spi_rf_channel_count); + if(data.unpacking_remaining() == 0) return rc; + + valid_data_groups += 1; + rc &= data.unpack(fpvCamAngleDegrees); + if(data.unpacking_remaining() == 0) return rc; + + valid_data_groups += 1; + rc &= data.unpack(receiverType); + return rc; + } - PidTerms(const uint8_t P, const uint8_t I, const uint8_t D) - : P(P/10.0f), I(I/10.0f), D(D/10.0f) - { } + virtual std::ostream& print(std::ostream& s) const override { + return rxConfigPrint(s); + } }; -// MSP_PID: 112 -struct Pid : public Request { - ID id() const { return ID::MSP_PID; } - - PidTerms roll, pitch, yaw, alt; - PidTerms pos, posr, navr, level, mag, vel; +// MSP_SET_RX_CONFIG: 45 +struct SetRxConfig : public RxConfigSettings, public Message { + SetRxConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_RX_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(serialrx_provider); + rc &= data->pack(maxcheck); + rc &= data->pack(midrc); + rc &= data->pack(mincheck); + rc &= data->pack(spektrum_sat_bind); + if(valid_data_groups == 1) goto packing_finished; + rc &= data->pack(rx_min_usec); + rc &= data->pack(rx_max_usec); + if(valid_data_groups == 2) goto packing_finished; + rc &= data->pack(rcInterpolation); + rc &= data->pack(rcInterpolationInterval); + rc &= data->pack(airModeActivateThreshold); + if(valid_data_groups == 3) goto packing_finished; + rc &= data->pack(rx_spi_protocol); + rc &= data->pack(rx_spi_id); + rc &= data->pack(rx_spi_rf_channel_count); + if(valid_data_groups == 4) goto packing_finished; + rc &= data->pack(fpvCamAngleDegrees); + if(valid_data_groups == 5) goto packing_finished; + rc &= data->pack(receiverType); + packing_finished: + if(!rc) data.reset(); + return data; + } - void decode(const std::vector &data) { - roll = PidTerms(data[0], data[1], data[2]); - pitch = PidTerms(data[3], data[4], data[5]); - yaw = PidTerms(data[6], data[7], data[8]); - alt = PidTerms(data[9], data[10], data[11]); - pos = PidTerms(data[12], data[13], data[14]); - posr = PidTerms(data[15], data[16], data[17]); - navr = PidTerms(data[18], data[19], data[20]); - level = PidTerms(data[21], data[22], data[23]); - mag = PidTerms(data[24], data[25], data[26]); - vel = PidTerms(data[27], data[28], data[29]); + virtual std::ostream& print(std::ostream& s) const override { + return rxConfigPrint(s); } }; -// MSP_BOX: 113 -struct Box : public Request { - ID id() const { return ID::MSP_BOX; } - - // box activation pattern - std::vector,NAUX>> box_pattern; +struct HsvColor : public Packable { + Value h; + Value s; + Value v; + + bool unpack_from(const ByteVector& data) { + bool rc = true; + rc &= data.unpack(h); + rc &= data.unpack(s); + rc &= data.unpack(v); + return rc; + } - void decode(const std::vector &data) { - box_pattern.clear(); - for(size_t i(0); i,NAUX> aux_sp; - for(size_t iaux(0); iaux &data) { - powerTrigger = deserialise_uint16(data, 0); - minThrottle = deserialise_uint16(data, 2); - maxThrottle = deserialise_uint16(data, 4); - minCommand = deserialise_uint16(data, 6); + virtual ID id() const override { return ID::MSP_LED_COLORS; } - failsafeThrottle = deserialise_uint16(data, 8); - arm = deserialise_uint16(data, 10); - lifetime = deserialise_uint32(data, 12); - mag_declination = deserialise_uint16(data, 16) / 10.0f; + std::array colors; - vbatScale = data[18] / 10.0f; - vbatLevelWarn1 = data[19] / 10.0f; - vbatLevelWarn2 = data[20] / 10.0f; - vbatLevelCrit = data[21] / 10.0f; + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(auto& c : colors) { + rc &= data.unpack(c); + } + return rc; } }; -// MSP_MOTOR_PINS: 115 -struct MotorPins : public Request { - ID id() const { return ID::MSP_MOTOR_PINS; } +// MSP_SET_LED_COLORS: 47 +struct SetLedColors : public Message { + SetLedColors(FirmwareVariant v) : Message(v) {} - uint8_t pwm_pin[N_MOTOR]; + virtual ID id() const override { return ID::MSP_SET_LED_COLORS; } - void decode(const std::vector &data) { - for(size_t i(0); i colors; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + for(size_t i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; ++i) { + rc &= data->pack(colors[i].h); + rc &= data->pack(colors[i].s); + rc &= data->pack(colors[i].v); + } + if(!rc) data.reset(); + return data; } }; -// MSP_BOXNAMES: 116 -struct BoxNames : public Request { - ID id() const { return ID::MSP_BOXNAMES; } +// MSP_LED_STRIP_CONFIG: 48 +struct LedStripConfigs : public Message { + LedStripConfigs(FirmwareVariant v) : Message(v) {} - std::vector box_names; + virtual ID id() const override { return ID::MSP_LED_STRIP_CONFIG; } - void decode(const std::vector &data) { - box_names.clear(); + std::array configs; - std::stringstream ss(std::string(data.begin(), data.end())); - std::string bname; - while(getline(ss, bname, ';')) { - box_names.push_back(bname); + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(size_t i = 0; i < LED_MAX_STRIP_LENGTH; ++i) { + rc &= data.unpack(configs[i]); } + return rc; } }; -// MSP_PIDNAMES: 117 -struct PidNames : public Request { - ID id() const { return ID::MSP_PIDNAMES; } +// MSP_SET_LED_STRIP_CONFIG: 49 +struct SetLedStripConfig : public Message { + SetLedStripConfig(FirmwareVariant v) : Message(v) {} - std::vector pid_names; + virtual ID id() const override { return ID::MSP_SET_LED_STRIP_CONFIG; } - void decode(const std::vector &data) { - pid_names.clear(); + Value cfg_index; + Value config; - std::stringstream ss(std::string(data.begin(), data.end())); - std::string pname; - while(getline(ss, pname, ';')) { - pid_names.push_back(pname); - } + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(cfg_index); + rc &= data->pack(config); + if(!rc) data.reset(); + return data; } }; -// MSP_WP: 118 -struct WayPoint : public Request { - ID id() const { return ID::MSP_WP; } +// MSP_RSSI_CONFIG: 50 +struct RssiConfig : public Message { + RssiConfig(FirmwareVariant v) : Message(v) {} - uint8_t wp_no; - uint32_t lat; - uint32_t lon; - uint32_t altHold; - uint16_t heading; - uint16_t staytime; - uint8_t navflag; + virtual ID id() const override { return ID::MSP_RSSI_CONFIG; } - void decode(const std::vector &data) { - wp_no = data[0]; - lat = deserialise_uint32(data, 1); - lon = deserialise_uint32(data, 5); - altHold = deserialise_uint32(data, 9); - heading = deserialise_uint16(data, 13); - staytime = deserialise_uint16(data, 15); - navflag = data[18]; + Value rssi_channel; + + virtual bool decode(const ByteVector& data) override { + return data.unpack(rssi_channel); } }; -// MSP_BOXIDS: 119 -struct BoxIds : public Request { - ID id() const { return ID::MSP_BOXIDS; } +// MSP_SET_RSSI_CONFIG: 51 +struct SetRssiConfig : public Message { + SetRssiConfig(FirmwareVariant v) : Message(v) {} - std::vector box_ids; + virtual ID id() const override { return ID::MSP_SET_RSSI_CONFIG; } - void decode(const std::vector &data) { - box_ids.clear(); + Value rssi_channel; - for(uint8_t bi : data) - box_ids.push_back(bi);; + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(rssi_channel)) data.reset(); + return data; } }; -struct ServoConfRange { - uint16_t min; - uint16_t max; - uint16_t middle; - uint8_t rate; +struct adjustmentRange { + Value adjustmentIndex; + Value auxChannelIndex; + Value range_startStep; + Value range_endStep; + Value adjustmentFunction; + Value auxSwitchChannelIndex; }; -// MSP_SERVO_CONF: 120 -struct ServoConf : public Request { - ID id() const { return ID::MSP_SERVO_CONF; } +// MSP_ADJUSTMENT_RANGES: 52 +struct AdjustmentRanges : public Message { + AdjustmentRanges(FirmwareVariant v) : Message(v) {} - ServoConfRange servo_conf[N_SERVO]; + virtual ID id() const override { return ID::MSP_ADJUSTMENT_RANGES; } - void decode(const std::vector &data) { - for(size_t i(0); i ranges; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(size_t i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; ++i) { + rc &= data.unpack(ranges[i].adjustmentIndex); + rc &= data.unpack(ranges[i].auxChannelIndex); + rc &= data.unpack(ranges[i].range_startStep); + rc &= data.unpack(ranges[i].range_endStep); + rc &= data.unpack(ranges[i].adjustmentFunction); + rc &= data.unpack(ranges[i].auxSwitchChannelIndex); } + return rc; } }; -// MSP_NAV_STATUS: 121 -struct NavStatus: public Request { - ID id() const { return ID::MSP_NAV_STATUS; } +// MSP_SET_ADJUSTMENT_RANGE: 53 +struct SetAdjustmentRange : public Message { + SetAdjustmentRange(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_ADJUSTMENT_RANGE; } + + Value range_index; + adjustmentRange range; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(range_index); + rc &= data->pack(range.adjustmentIndex); + rc &= data->pack(range.auxChannelIndex); + rc &= data->pack(range.range_startStep); + rc &= data->pack(range.range_endStep); + rc &= data->pack(range.adjustmentFunction); + rc &= data->pack(range.auxSwitchChannelIndex); + if(!rc) data.reset(); + return data; + } +}; - uint8_t GPS_mode; - uint8_t NAV_state; - uint8_t mission_action; - uint8_t mission_number; - uint8_t NAV_error; - int16_t target_bearing; // degrees +struct CfSerialConfigSettings { + Value identifier; + Value functionMask; + Value mspBaudrateIndx; + Value gpsBaudrateIndx; + Value telemetryBaudrateIndx; + Value peripheralBaudrateIndx; +}; - void decode(const std::vector &data) { - GPS_mode = data[0]; - NAV_state = data[1]; - mission_action = data[2]; - mission_number = data[3]; - NAV_error = data[4]; - target_bearing = deserialise_int16(data, 5); +// MSP_CF_SERIAL_CONFIG: 54 +struct CfSerialConfig : public Message { + CfSerialConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_CF_SERIAL_CONFIG; } + + std::vector configs; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + do { + CfSerialConfigSettings tmp; + rc &= data.unpack(tmp.identifier); + rc &= data.unpack(tmp.functionMask); + rc &= data.unpack(tmp.mspBaudrateIndx); + rc &= data.unpack(tmp.gpsBaudrateIndx); + rc &= data.unpack(tmp.telemetryBaudrateIndx); + rc &= data.unpack(tmp.peripheralBaudrateIndx); + if(rc) configs.push_back(tmp); + } while(rc); + return configs.size(); } }; +// MSP_SET_CF_SERIAL_CONFIG: 55 +struct SetCfSerialConfig : public Message { + SetCfSerialConfig(FirmwareVariant v) : Message(v) {} -struct GpsConf { - uint8_t filtering; - uint8_t lead_filter; - uint8_t dont_reset_home_at_arm; - uint8_t nav_controls_heading; + virtual ID id() const override { return ID::MSP_SET_CF_SERIAL_CONFIG; } - uint8_t nav_tail_first; - uint8_t nav_rth_takeoff_heading; - uint8_t slow_nav; - uint8_t wait_for_rth_alt; + std::vector configs; - uint8_t ignore_throttle; - uint8_t takeover_baro; + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + for(const auto& config : configs) { + rc &= data->pack(config.identifier); + rc &= data->pack(config.functionMask); + rc &= data->pack(config.mspBaudrateIndx); + rc &= data->pack(config.gpsBaudrateIndx); + rc &= data->pack(config.telemetryBaudrateIndx); + rc &= data->pack(config.peripheralBaudrateIndx); + } + if(!rc) data.reset(); + return data; + } +}; - uint16_t wp_radius; // in cm - uint16_t safe_wp_distance; // in meter - uint16_t nav_max_altitude; // in meter - uint16_t nav_speed_max; // in cm/s - uint16_t nav_speed_min; // in cm/s +struct VoltageMeterConfigSettings { + Value scale_dV; + Value cell_min_dV; + Value cell_max_dV; + Value cell_warning_dV; +}; - uint8_t crosstrack_gain; // * 100 (0-2.56) - uint16_t nav_bank_max; // degree * 100; (3000 default) - uint16_t rth_altitude; // in meter - uint8_t land_speed; // between 50 and 255 (100 approx = 50cm/sec) - uint16_t fence; // fence control in meters +// MSP_VOLTAGE_METER_CONFIG: 56 (differs from Cleanflight/BetaFlight) +struct VoltageMeterConfig : public VoltageMeterConfigSettings, public Message { + VoltageMeterConfig(FirmwareVariant v) : Message(v) {} - uint8_t max_wp_number; + virtual ID id() const override { return ID::MSP_VOLTAGE_METER_CONFIG; } - uint8_t checksum; + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(scale_dV); + rc &= data.unpack(cell_min_dV); + rc &= data.unpack(cell_max_dV); + rc &= data.unpack(cell_warning_dV); + return rc; + } }; -// MSP_NAV_CONFIG: 122 -struct NavConfig: public Request { - ID id() const { return ID::MSP_NAV_CONFIG; } +// MSP_SET_VOLTAGE_METER_CONFIG: 57 (differs from Cleanflight/BetaFlight) +struct SetVoltageMeterConfig : public VoltageMeterConfigSettings, + public Message { + SetVoltageMeterConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_VOLTAGE_METER_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(scale_dV); + rc &= data->pack(cell_min_dV); + rc &= data->pack(cell_max_dV); + rc &= data->pack(cell_warning_dV); + if(!rc) data.reset(); + return data; + } +}; - GpsConf gps_conf; +// MSP_SONAR_ALTITUDE: 58 +struct SonarAltitude : public Message { + SonarAltitude(FirmwareVariant v) : Message(v) {} - void decode(const std::vector &data) { - gps_conf.filtering = data[0]; - gps_conf.lead_filter = data[1]; - gps_conf.dont_reset_home_at_arm = data[2]; - gps_conf.nav_controls_heading = data[3]; + virtual ID id() const override { return ID::MSP_SONAR_ALTITUDE; } - gps_conf.nav_tail_first = data[4]; - gps_conf.nav_rth_takeoff_heading = data[5]; - gps_conf.slow_nav = data[6]; - gps_conf.wait_for_rth_alt = data[7]; + Value altitude_cm; // meters - gps_conf.ignore_throttle = data[8]; - gps_conf.takeover_baro = data[9]; + virtual bool decode(const ByteVector& data) override { + return data.unpack(altitude_cm); + } +}; - gps_conf.wp_radius = deserialise_uint16(data, 10); - gps_conf.safe_wp_distance = deserialise_uint16(data, 12); - gps_conf.nav_max_altitude = deserialise_uint16(data, 14); - gps_conf.nav_speed_max = deserialise_uint16(data, 16); - gps_conf.nav_speed_min = deserialise_uint16(data, 18); +// MSP_PID_CONTROLLER: 59 +struct PidController : public Message { + PidController(FirmwareVariant v) : Message(v) {} - gps_conf.crosstrack_gain = data[20]; - gps_conf.nav_bank_max = deserialise_uint16(data, 21); - gps_conf.rth_altitude = deserialise_uint16(data, 23); - gps_conf.land_speed = data[25]; - gps_conf.fence = deserialise_uint16(data, 26); + virtual ID id() const override { return ID::MSP_PID_CONTROLLER; } - gps_conf.max_wp_number = data[28]; + Value controller_id; - gps_conf.checksum = data[29]; + virtual bool decode(const ByteVector& data) override { + return data.unpack(controller_id); } }; -// MSP_DEBUGMSG: 253 -struct DebugMessage : public Request { - ID id() const { return ID::MSP_DEBUGMSG; } +// MSP_SET_PID_CONTROLLER: 60 +struct SetPidController : public Message { + SetPidController(FirmwareVariant v) : Message(v) {} - std::string msg; + virtual ID id() const override { return ID::MSP_SET_PID_CONTROLLER; } +}; - void decode(const std::vector &data) { - msg = std::string(data.begin(), data.end()); - } +struct ArmingConfigSettings { + Value auto_disarm_delay; + Value disarm_kill_switch; + bool imu_small_angle_valid; + Value imu_small_angle; }; -// MSP_DEBUG: 254 -struct Debug : public Request { - ID id() const { return ID::MSP_DEBUG; } +// MSP_ARMING_CONFIG: 61 +struct ArmingConfig : public ArmingConfigSettings, public Message { + ArmingConfig(FirmwareVariant v) : Message(v) {} - uint16_t debug1; - uint16_t debug2; - uint16_t debug3; - uint16_t debug4; + virtual ID id() const override { return ID::MSP_ARMING_CONFIG; } - void decode(const std::vector &data) { - debug1 = deserialise_uint16(data, 0); - debug2 = deserialise_uint16(data, 2); - debug3 = deserialise_uint16(data, 4); - debug4 = deserialise_uint16(data, 6); + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(auto_disarm_delay); + rc &= data.unpack(disarm_kill_switch); + if(data.unpack(imu_small_angle)) imu_small_angle_valid = true; + return rc; } }; +// MSP_SET_ARMING_CONFIG: 62 +struct SetArmingConfig : public ArmingConfigSettings, public Message { + SetArmingConfig(FirmwareVariant v) : Message(v) {} -///////////////////////////////////////////////////////////////////// -/// Response (2xx) + virtual ID id() const override { return ID::MSP_SET_ARMING_CONFIG; } -// MSP_SET_RAW_RC: 200 -// This message is accepted but ignored on betaflight 3.0.1 onwards -// if "USE_RX_MSP" is not defined for the target. In this case, you can manually -// add "#define USE_RX_MSP" to your 'target.h'. -struct SetRc : public Response { - ID id() const { return ID::MSP_SET_RAW_RC; } + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(auto_disarm_delay); + rc &= data->pack(disarm_kill_switch); + if(imu_small_angle_valid) rc &= data->pack(imu_small_angle); + if(!rc) data.reset(); + return data; + } +}; - std::vector channels; +struct RxMapSettings { + std::array map; - std::vector encode() const { - std::vector data; - for(const uint16_t c : channels) { - serialise_uint16(c, data); + std::ostream& printRxMapSettings(std::ostream& s) const { + s << "#Channel mapping:" << std::endl; + for(size_t i(0); i < map.size(); i++) { + s << " " << i << ": " << size_t(map[i]) << std::endl; } - return data; + return s; } }; -// MSP_SET_RAW_GPS: 201 -struct SetRawGPS : public Request { - ID id() const { return ID::MSP_SET_RAW_GPS; } +// MSP_RX_MAP: 64 +struct RxMap : public RxMapSettings, public Message { + RxMap(FirmwareVariant v) : Message(v) {} - uint8_t fix; - uint8_t numSat; - uint32_t lat; - uint32_t lon; - uint16_t altitude; - uint16_t speed; + virtual ID id() const override { return ID::MSP_RX_MAP; } - std::vector encode() const { - std::vector data; - data.push_back(fix); - data.push_back(numSat); - serialise_uint32(lat, data); - serialise_uint32(lon, data); - serialise_uint16(altitude, data); - serialise_uint16(speed, data); - assert(data.size()==14); - return data; + virtual bool decode(const ByteVector& data) override { + if(data.size() < MAX_MAPPABLE_RX_INPUTS) return false; + bool rc = true; + for(size_t i = 0; i < MAX_MAPPABLE_RX_INPUTS; ++i) { + rc &= data.unpack(map[i]); + } + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + return printRxMapSettings(s); } }; -// MSP_SET_RC_TUNING: 204 -struct SetRcTuning : public Response { - ID id() const { return ID::MSP_SET_RC_TUNING; } - - double RC_RATE; - double RC_EXPO; - double RollPitchRate; - double YawRate; - double DynThrPID; - double Throttle_MID; - double Throttle_EXPO; - - /** - * @brief SetRcTuning construct a RC tuning response from a request - * @param rc_tuning RcTuning response - */ - SetRcTuning(const RcTuning &rc_tuning) { - RC_RATE = rc_tuning.RC_RATE; - RC_EXPO = rc_tuning.RC_EXPO; - RollPitchRate = rc_tuning.RollPitchRate; - YawRate = rc_tuning.YawRate; - DynThrPID = rc_tuning.DynThrPID; - Throttle_MID = rc_tuning.Throttle_MID; - Throttle_EXPO = rc_tuning.Throttle_EXPO; - } - - std::vector encode() const { - std::vector data(7); - data[0] = uint8_t(RC_RATE * 100); - data[1] = uint8_t(RC_EXPO * 100); - data[2] = uint8_t(RollPitchRate * 100); - data[3] = uint8_t(YawRate * 100); - data[4] = uint8_t(DynThrPID * 100); - data[5] = uint8_t(Throttle_MID * 100); - data[6] = uint8_t(Throttle_EXPO * 100); +// MSP_SET_RX_MAP: 65 +struct SetRxMap : public RxMapSettings, public Message { + SetRxMap(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_RX_MAP; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + for(const auto& channel : map) { + rc &= data->pack(channel); + } + if(!rc) data.reset(); return data; } -}; -// MSP_ACC_CALIBRATION: 205 -struct AccCalibration : public Response { - ID id() const { return ID::MSP_ACC_CALIBRATION; } - std::vector encode() const { - return std::vector(); + virtual std::ostream& print(std::ostream& s) const override { + return printRxMapSettings(s); } }; -// MSP_MAG_CALIBRATION: 206 -struct MagCalibration : public Response { - ID id() const { return ID::MSP_MAG_CALIBRATION; } - std::vector encode() const { - return std::vector(); - } +// iNav uses decidegrees, BF/CF use degrees +struct BfConfigSettings { + Value mixer_mode; + Value feature_mask; + Value serialrx_provider; + Value roll; + Value pitch; + Value yaw; + Value current_meter_scale; + Value current_meter_offset; }; -// MSP_RESET_CONF: 208 -struct ResetConfig : public Response { - ID id() const { return ID::MSP_RESET_CONF; } - std::vector encode() const { - return std::vector(); +// MSP_BF_CONFIG: 66, //out message baseflight-specific settings that aren't +// covered elsewhere +struct BfConfig : public BfConfigSettings, public Message { + BfConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_BF_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(mixer_mode); + rc &= data.unpack(feature_mask); + rc &= data.unpack(serialrx_provider); + rc &= data.unpack(roll); + rc &= data.unpack(pitch); + rc &= data.unpack(yaw); + rc &= data.unpack(current_meter_scale); + rc &= data.unpack(current_meter_offset); + return rc; } }; -// MSP_SELECT_SETTING: 210 -struct SelectSetting : public Response { - ID id() const { return ID::MSP_SELECT_SETTING; } - - size_t current_setting; - - std::vector encode() const { - std::vector data(1); - data[0] = uint8_t(current_setting); +// MSP_SET_BF_CONFIG: 67, //in message baseflight-specific settings save +struct SetBfConfig : public BfConfigSettings, public Message { + SetBfConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_BF_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(mixer_mode); + rc &= data->pack(feature_mask); + rc &= data->pack(serialrx_provider); + rc &= data->pack(roll); + rc &= data->pack(pitch); + rc &= data->pack(yaw); + rc &= data->pack(current_meter_scale); + rc &= data->pack(current_meter_offset); + if(!rc) data.reset(); return data; } }; -// MSP_SET_HEAD: 211 -struct SetHeading : public Response { - ID id() const { return ID::MSP_SET_HEAD; } - - int heading; +// MSP_REBOOT: 68 +struct Reboot : public Message { + Reboot(FirmwareVariant v) : Message(v) {} - std::vector encode() const { - std::vector data; - serialise_int16(int16_t(heading), data); - assert(data.size()==2); - return data; - } + virtual ID id() const override { return ID::MSP_REBOOT; } }; -// MSP_SET_MOTOR: 214 -struct SetMotor : public Response { - ID id() const { return ID::MSP_SET_MOTOR; } +// MSP_BF_BUILD_INFO: 69 +struct BfBuildInfo : public Message { + BfBuildInfo(FirmwareVariant v) : Message(v) {} - std::array motor; + virtual ID id() const override { return ID::MSP_BF_BUILD_INFO; } - std::vector encode() const { - std::vector data; - for(size_t i(0); i build_date; + Value reserved1; + Value reserved2; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(build_date, 11); + rc &= data.unpack(reserved1); + rc &= data.unpack(reserved2); + return rc; + } +}; + +// MSP_DATAFLASH_SUMMARY: 70 +struct DataflashSummary : public Message { + DataflashSummary(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_DATAFLASH_SUMMARY; } + + bool flash_is_ready; + Value sectors; + Value total_size; + Value offset; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(flash_is_ready); + rc &= data.unpack(sectors); + rc &= data.unpack(total_size); + rc &= data.unpack(offset); + return rc; + } +}; + +// message format differs between iNav and BF/CF +// MSP_DATAFLASH_READ: 71 +struct DataflashRead : public Message { + DataflashRead(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_DATAFLASH_READ; } + + Value read_address; + Value read_size; + bool allow_compression; + ByteVector flash_data; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(read_address); + rc &= data->pack(read_size); + rc &= data->pack(allow_compression); + if(!rc) data.reset(); + return data; + } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(read_address); + flash_data = ByteVector(data.unpacking_iterator(), data.end()); + rc &= data.consume(flash_data.size()); + return rc; + } +}; + +// MSP_DATAFLASH_ERASE: 72 +struct DataflashErase : public Message { + DataflashErase(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_DATAFLASH_ERASE; } + + virtual bool decode(const ByteVector& /*data*/) override { return true; } +}; + +// MSP_LOOP_TIME: 73 +struct LoopTime : public Message { + LoopTime(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_LOOP_TIME; } + + Value loop_time; + + virtual bool decode(const ByteVector& data) override { + return data.unpack(loop_time); + } +}; + +// MSP_SET_LOOP_TIME:74 +struct SetLoopTime : public Message { + SetLoopTime(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_LOOP_TIME; } + + Value loop_time; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(loop_time)) data.reset(); + return data; + } +}; + +struct FailsafeSettings { + bool extended_contents; + Value delay; + Value off_delay; + Value throttle; + Value kill_switch; + Value throttle_low_delay; + Value procedure; + Value recovery_delay; + Value fw_roll_angle; + Value fw_pitch_angle; + Value fw_yaw_rate; + Value stick_motion_threshold; + Value min_distance; + Value min_distance_procedure; +}; + +// MSP_FAILSAFE_CONFIG: 75 +struct FailsafeConfig : public FailsafeSettings, public Message { + FailsafeConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_FAILSAFE_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + extended_contents = false; + rc &= data.unpack(delay); + rc &= data.unpack(off_delay); + rc &= data.unpack(throttle); + rc &= data.unpack(kill_switch); + rc &= data.unpack(throttle_low_delay); + rc &= data.unpack(procedure); + if(data.unpacking_remaining() == 0) return rc; + extended_contents = true; + rc &= data.unpack(recovery_delay); + rc &= data.unpack(fw_roll_angle); + rc &= data.unpack(fw_pitch_angle); + rc &= data.unpack(fw_yaw_rate); + rc &= data.unpack(stick_motion_threshold); + rc &= data.unpack(min_distance); + rc &= data.unpack(min_distance_procedure); + return rc; + } +}; + +// MSP_SET_FAILSAFE_CONFIG: 76 +struct SetFailsafeConfig : public FailsafeSettings, public Message { + SetFailsafeConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_FAILSAFE_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(delay); + rc &= data->pack(off_delay); + rc &= data->pack(throttle); + rc &= data->pack(kill_switch); + rc &= data->pack(throttle_low_delay); + rc &= data->pack(procedure); + if(extended_contents) { + rc &= data->pack(recovery_delay); + rc &= data->pack(fw_roll_angle); + rc &= data->pack(fw_pitch_angle); + rc &= data->pack(fw_yaw_rate); + rc &= data->pack(stick_motion_threshold); + rc &= data->pack(min_distance); + rc &= data->pack(min_distance_procedure); + } + if(!rc) data.reset(); + return data; + } +}; + +struct RxFailChannelSettings { + Value mode; + Value val; +}; + +// MSP_RXFAIL_CONFIG: 77 +struct RxFailConfigs : public Message { + RxFailConfigs(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_RXFAIL_CONFIG; } + + std::vector channels; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + channels.clear(); + while(rc && data.unpacking_remaining()) { + RxFailChannelSettings tmp; + rc &= data.unpack(tmp.mode); + rc &= data.unpack(tmp.val); + channels.push_back(tmp); + } + return rc; + } +}; + +// MSP_SET_RXFAIL_CONFIG: 78 +struct SetRxFailConfigs : public RxFailChannelSettings, public Message { + SetRxFailConfigs(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_RXFAIL_CONFIG; } + + Value channel; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(channel); + rc &= data.unpack(mode); + rc &= data.unpack(val); + return rc; + } +}; + +// MSP_SDCARD_SUMMARY: 79 +struct SdcardSummary : public Message { + SdcardSummary(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SDCARD_SUMMARY; } + + Value flags; + Value state; + Value last_error; + Value free_space_kb; + Value total_space_kb; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(flags); + rc &= data.unpack(state); + rc &= data.unpack(last_error); + rc &= data.unpack(free_space_kb); + rc &= data.unpack(total_space_kb); + return rc; + } +}; + +struct BlackboxConfigSettings { + Value device; + Value rate_num; + Value rate_denom; + bool p_ratio_set; + Value p_ratio; +}; + +// MSP_BLACKBOX_CONFIG: 80 +struct BlackboxConfig : public BlackboxConfigSettings, public Message { + BlackboxConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_BLACKBOX_CONFIG; } + + Value supported; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + p_ratio_set = false; + rc &= data.unpack(supported); + rc &= data.unpack(device); + rc &= data.unpack(rate_num); + rc &= data.unpack(rate_denom); + if(data.unpacking_remaining()) { + p_ratio_set = true; + rc &= data.unpack(p_ratio); + } + return rc; + } +}; + +// MSP_SET_BLACKBOX_CONFIG: 81 +struct SetBlackboxConfig : public BlackboxConfigSettings, public Message { + SetBlackboxConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_BLACKBOX_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(device); + rc &= data->pack(rate_num); + rc &= data->pack(rate_denom); + if(p_ratio_set) rc &= data->pack(p_ratio); + if(!rc) data.reset(); + return data; + } +}; + +struct TransponderConfigSettings { + Value provider; + Value data_length; +}; + +// MSP_TRANSPONDER_CONFIG: 82 +struct TransponderConfig : public Message { + TransponderConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_TRANSPONDER_CONFIG; } + + Value transponder_count; + std::vector transponder_data; + Value provider; + ByteVector provider_data; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(transponder_count); + if(!transponder_count()) return rc; + for(uint8_t i = 0; i < transponder_count(); ++i) { + TransponderConfigSettings tmp; + rc &= data.unpack(tmp.provider); + rc &= data.unpack(tmp.data_length); + transponder_data.push_back(tmp); + } + rc &= data.unpack(provider); + if(!provider()) return rc; + uint8_t data_len = transponder_data[provider() - 1].data_length(); + provider_data = ByteVector(data.unpacking_iterator(), + data.unpacking_iterator() + data_len); + rc &= data.consume(data_len); + return rc; + } +}; + +// MSP_SET_TRANSPONDER_CONFIG: 83 +struct SetTransponderConfig : public Message { + SetTransponderConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_TRANSPONDER_CONFIG; } + + Value provider; + ByteVector provider_data; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(provider); + rc &= data->pack(provider_data); + if(!rc) data.reset(); + return data; + } +}; + +// Differences between iNav and BF/CF +// MSP_OSD_CONFIG: 84 +struct OsdConfig : public Message { + OsdConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_OSD_CONFIG; } + + Value osd_flags; + Value video_system; + Value units; + Value rssi_alarm; + Value battery_cap_warn; + Value time_alarm; + Value alt_alarm; + Value dist_alarm; + Value neg_alt_alarm; + std::array item_pos; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(osd_flags); + if(rc && osd_flags()) { + rc &= data.unpack(video_system); + rc &= data.unpack(units); + rc &= data.unpack(rssi_alarm); + rc &= data.unpack(battery_cap_warn); + rc &= data.unpack(time_alarm); + rc &= data.unpack(alt_alarm); + rc &= data.unpack(dist_alarm); + rc &= data.unpack(neg_alt_alarm); + for(size_t i = 0; i < OSD_ITEM_COUNT; ++i) { + rc &= data.unpack(item_pos[i]); + } + } + return rc; + } +}; + +// MSP_SET_OSD_CONFIG: 85 +struct SetOsdConfig : public Message { + SetOsdConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_OSD_CONFIG; } + + int8_t param_idx; + Value item_pos; + Value video_system; + Value units; + Value rssi_alarm; + Value battery_cap_warn; + Value time_alarm; + Value alt_alarm; + Value dist_alarm; + Value neg_alt_alarm; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(param_idx); + if(param_idx == -1) { + rc &= data->pack(video_system); + rc &= data->pack(units); + rc &= data->pack(rssi_alarm); + rc &= data->pack(battery_cap_warn); + rc &= data->pack(time_alarm); + rc &= data->pack(alt_alarm); + rc &= data->pack(dist_alarm); + rc &= data->pack(neg_alt_alarm); + } + else { + rc &= data->pack(item_pos); + } + if(!rc) data.reset(); + return data; + } +}; + +// MSP_OSD_CHAR_READ: 86 No reference implementation + +// MSP_OSD_CHAR_WRITE: 87 +struct OsdCharWrite : public Message { + OsdCharWrite(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_OSD_CHAR_WRITE; } + + Value addr; + std::array font_data; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(addr); + for(const auto& c : font_data) { + rc &= data->pack(c); + } + if(!rc) data.reset(); + return data; + } +}; + +// MSP_VTX_CONFIG: 88 +struct VtxConfig : public Message { + VtxConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_VTX_CONFIG; } + + Value device_type; + Value band; + Value channel; + Value power_idx; + Value pit_mode; + bool freq_set; + Value frequency; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + freq_set = false; + rc &= data.unpack(device_type); + if(rc && (device_type() != 0xFF)) { + rc &= data.unpack(band); + rc &= data.unpack(channel); + rc &= data.unpack(power_idx); + rc &= data.unpack(pit_mode); + if(data.unpacking_remaining()) { + freq_set = true; + rc &= data.unpack(frequency); + } + } + return rc; + } +}; + +// MSP_SET_VTX_CONFIG: 89 +struct SetVtxConfig : public Message { + SetVtxConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_VTX_CONFIG; } + + Value frequency; + Value power; + Value pit_mode; + + bool set_freq(uint8_t band, uint8_t channel) { + if(band & 0xF8 || channel & 0xF8) { + return false; + } + frequency = uint16_t(band - 1) & uint16_t((channel - 1) << 3); + return true; + } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(frequency); + rc &= data->pack(power); + rc &= data->pack(pit_mode); + if(!rc) data.reset(); + return data; + } +}; + +// Differs between iNav and BF/CF +struct AdvancedConfigSettings { + Value gyro_sync_denom; + Value pid_process_denom; + Value use_unsynced_pwm; + Value motor_pwm_protocol; + Value motor_pwm_rate; + Value servo_pwm_rate; // digitalIdleOffsetValue in BF/CF + Value gyro_sync; + bool pwm_inversion_set; + Value pwm_inversion; +}; + +// Betaflight Additional Commands +// MSP_ADVANCED_CONFIG: 90 +struct AdvancedConfig : public AdvancedConfigSettings, public Message { + AdvancedConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_ADVANCED_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + pwm_inversion_set = false; + rc &= data.unpack(gyro_sync_denom); + rc &= data.unpack(pid_process_denom); + rc &= data.unpack(use_unsynced_pwm); + rc &= data.unpack(motor_pwm_protocol); + rc &= data.unpack(motor_pwm_rate); + rc &= data.unpack(servo_pwm_rate); + rc &= data.unpack(gyro_sync); + if(rc && data.unpacking_remaining()) { + pwm_inversion_set = true; + rc &= data.unpack(pwm_inversion); + } + return rc; + } +}; + +// MSP_SET_ADVANCED_CONFIG: 91 +struct SetAdvancedConfig : public AdvancedConfigSettings, public Message { + SetAdvancedConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_ADVANCED_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(gyro_sync_denom); + rc &= data->pack(pid_process_denom); + rc &= data->pack(use_unsynced_pwm); + rc &= data->pack(motor_pwm_protocol); + rc &= data->pack(motor_pwm_rate); + rc &= data->pack(servo_pwm_rate); + rc &= data->pack(gyro_sync); + if(pwm_inversion_set) rc &= data->pack(pwm_inversion); + if(!rc) data.reset(); + return data; + } +}; + +struct FilterConfigSettings { + Value gyro_soft_lpf_hz; + Value dterm_lpf_hz; + Value yaw_lpf_hz; + Value gyro_soft_notch_hz_1; + Value gyro_soft_notch_cutoff_1; + Value dterm_soft_notch_hz; + Value dterm_soft_notch_cutoff; + Value gyro_soft_notch_hz_2; + Value gyro_soft_notch_cutoff_2; + bool dterm_filter_type_set; + Value dterm_filter_type; +}; + +// MSP_FILTER_CONFIG: 92 +struct FilterConfig : public FilterConfigSettings, public Message { + FilterConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_FILTER_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + dterm_filter_type_set = false; + rc &= data.unpack(gyro_soft_lpf_hz); + rc &= data.unpack(dterm_lpf_hz); + rc &= data.unpack(yaw_lpf_hz); + rc &= data.unpack(gyro_soft_notch_hz_1); + rc &= data.unpack(gyro_soft_notch_cutoff_1); + rc &= data.unpack(dterm_soft_notch_hz); + rc &= data.unpack(dterm_soft_notch_cutoff); + rc &= data.unpack(gyro_soft_notch_hz_2); + rc &= data.unpack(gyro_soft_notch_cutoff_2); + if(rc && data.unpacking_remaining()) { + dterm_filter_type_set = true; + rc &= data.unpack(dterm_filter_type); + } + return rc; + } +}; + +// MSP_SET_FILTER_CONFIG: 93 +struct SetFilterConfig : public FilterConfigSettings, public Message { + SetFilterConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_FILTER_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(gyro_soft_lpf_hz); + rc &= data->pack(dterm_lpf_hz); + rc &= data->pack(yaw_lpf_hz); + rc &= data->pack(gyro_soft_notch_hz_1); + rc &= data->pack(gyro_soft_notch_cutoff_1); + rc &= data->pack(dterm_soft_notch_hz); + rc &= data->pack(dterm_soft_notch_cutoff); + rc &= data->pack(gyro_soft_notch_hz_2); + rc &= data->pack(gyro_soft_notch_cutoff_2); + if(dterm_filter_type_set) rc &= data->pack(dterm_filter_type); + if(!rc) data.reset(); + return data; + } +}; + +struct PidAdvancedSettings { + Value rollPitchItermIgnoreRate; + Value yawItermIgnoreRate; + Value yaw_p_limit; + Value deltaMethod; + Value vbatPidCompensation; + Value setpointRelaxRatio; + Value dterm_setpoint_weight; // TODO scaled value + Value pidSumLimit; + Value itermThrottleGain; + Value + axisAccelerationLimitRollPitch; // TODO scaled and clamped value + Value axisAccelerationLimitYaw; // TODO scaled and clamped value +}; + +// Difference between iNav and BF/CF +// MSP_PID_ADVANCED: 94 +struct PidAdvanced : public PidAdvancedSettings, public Message { + PidAdvanced(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_PID_ADVANCED; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(rollPitchItermIgnoreRate); + rc &= data.unpack(yawItermIgnoreRate); + rc &= data.unpack(yaw_p_limit); + rc &= data.unpack(deltaMethod); + rc &= data.unpack(vbatPidCompensation); + rc &= data.unpack(setpointRelaxRatio); + rc &= data.unpack(dterm_setpoint_weight, 0.01); + rc &= data.unpack(pidSumLimit); + rc &= data.unpack(itermThrottleGain); + Value tmp16; + rc &= data.unpack(tmp16); + axisAccelerationLimitRollPitch = tmp16() * 10; + rc &= data.unpack(tmp16); + axisAccelerationLimitYaw = tmp16() * 10; + return rc; + } +}; + +// MSP_SET_PID_ADVANCED: 95 +struct SetPidAdvanced : public PidAdvancedSettings, public Message { + SetPidAdvanced(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_PID_ADVANCED; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(rollPitchItermIgnoreRate); + rc &= data->pack(yawItermIgnoreRate); + rc &= data->pack(yaw_p_limit); + rc &= data->pack(deltaMethod); + rc &= data->pack(vbatPidCompensation); + rc &= data->pack(setpointRelaxRatio); + rc &= data->pack(uint8_t(dterm_setpoint_weight() * 100)); + rc &= data->pack(pidSumLimit); + rc &= data->pack(itermThrottleGain); + rc &= data->pack(axisAccelerationLimitRollPitch() / 10); + rc &= data->pack(axisAccelerationLimitYaw() / 10); + if(!rc) data.reset(); + return data; + } +}; + +struct SensorConfigSettings { + Value acc_hardware; + Value baro_hardware; + Value mag_hardware; + bool extended_contents; + Value pitot_hardware; + Value rangefinder_hardware; + Value opflow_hardware; +}; + +// MSP_SENSOR_CONFIG: 96 +struct SensorConfig : public SensorConfigSettings, public Message { + SensorConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SENSOR_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + extended_contents = false; + rc &= data.unpack(acc_hardware); + rc &= data.unpack(baro_hardware); + rc &= data.unpack(mag_hardware); + if(data.unpacking_remaining()) { + extended_contents = true; + rc &= data.unpack(pitot_hardware); + rc &= data.unpack(rangefinder_hardware); + rc &= data.unpack(opflow_hardware); + } + return rc; + } +}; + +// MSP_SET_SENSOR_CONFIG: 97 +struct SetSensorConfig : public SensorConfigSettings, public Message { + SetSensorConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_SENSOR_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(acc_hardware); + rc &= data->pack(baro_hardware); + rc &= data->pack(mag_hardware); + if(!extended_contents) { + rc &= data->pack(pitot_hardware); + rc &= data->pack(rangefinder_hardware); + rc &= data->pack(opflow_hardware); + } + if(!rc) data.reset(); + return data; + } +}; + +// MSP_CAMERA_CONTROL: 98 +struct CameraControl : public Message { + CameraControl(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_CAMERA_CONTROL; } + + Value key; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(key)) data.reset(); + return data; + } +}; + +// MSP_SET_ARMING_DISABLED: 99 +struct SetArmingDisabled : public Message { + SetArmingDisabled(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_ARMING_DISABLED; } + + Value command; + Value disableRunawayTakeoff; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(command); + rc &= data->pack(disableRunawayTakeoff); + if(!rc) data.reset(); + return data; + } +}; + +///////////////////////////////////////////////////////////////////// +/// Requests (1xx) + +// MSP_IDENT: 100 +struct Ident : public Message { + Ident(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_IDENT; } + + Value version; + MultiType type; + Value msp_version; + std::set capabilities; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + + rc &= data.unpack(version); + rc &= data.unpack((uint8_t&)type); + rc &= data.unpack(msp_version); + uint32_t capability; + rc &= data.unpack(capability); + if(!rc) return false; + capabilities.clear(); + if(capability & (1 << 0)) capabilities.insert(Capability::BIND); + if(capability & (1 << 2)) capabilities.insert(Capability::DYNBAL); + if(capability & (1 << 3)) capabilities.insert(Capability::FLAP); + if(capability & (1 << 4)) capabilities.insert(Capability::NAVCAP); + if(capability & (1 << 5)) capabilities.insert(Capability::EXTAUX); + + return true; + } + + bool has(const Capability& cap) const { return capabilities.count(cap); } + + bool hasBind() const { return has(Capability::BIND); } + + bool hasDynBal() const { return has(Capability::DYNBAL); } + + bool hasFlap() const { return has(Capability::FLAP); } + + virtual std::ostream& print(std::ostream& s) const override { + std::string s_type; + switch(type) { + case msp::msg::MultiType::TRI: + s_type = "Tricopter"; + break; + case msp::msg::MultiType::QUADP: + s_type = "Quadrocopter Plus"; + break; + case msp::msg::MultiType::QUADX: + s_type = "Quadrocopter X"; + break; + case msp::msg::MultiType::BI: + s_type = "BI-copter"; + break; + default: + s_type = "UNDEFINED"; + break; + } + + s << "#Ident:" << std::endl; + + s << " MultiWii Version: " << version << std::endl + << " MSP Version: " << msp_version << std::endl + << " Type: " << s_type << std::endl + << " Capabilities:" << std::endl; + + s << " Bind: "; + hasBind() ? s << "ON" : s << "OFF"; + s << std::endl; + + s << " DynBal: "; + hasDynBal() ? s << "ON" : s << "OFF"; + s << std::endl; + + s << " Flap: "; + hasFlap() ? s << "ON" : s << "OFF"; + s << std::endl; + + return s; + } +}; + +struct StatusBase : public Packable { + Value cycle_time; // in us + Value i2c_errors; + std::set sensors; + std::set box_mode_flags; + Value current_profile; + + bool unpack_from(const ByteVector& data) { + bool rc = true; + rc &= data.unpack(cycle_time); + rc &= data.unpack(i2c_errors); + + // get sensors + sensors.clear(); + uint16_t sensor = 0; + rc &= data.unpack(sensor); + if(sensor & (1 << 0)) sensors.insert(Sensor::Accelerometer); + if(sensor & (1 << 1)) sensors.insert(Sensor::Barometer); + if(sensor & (1 << 2)) sensors.insert(Sensor::Magnetometer); + if(sensor & (1 << 3)) sensors.insert(Sensor::GPS); + if(sensor & (1 << 4)) sensors.insert(Sensor::Sonar); + if(sensor & (1 << 5)) sensors.insert(Sensor::OpticalFlow); + if(sensor & (1 << 6)) sensors.insert(Sensor::Pitot); + if(sensor & (1 << 15)) sensors.insert(Sensor::GeneralHealth); + + // check active boxes + box_mode_flags.clear(); + uint32_t flag = 0; + rc &= data.unpack(flag); + for(size_t ibox(0); ibox < sizeof(flag) * CHAR_BIT; ibox++) { + if(flag & (1 << ibox)) box_mode_flags.insert(ibox); + } + + rc &= data.unpack(current_profile); + return rc; + } + + virtual bool pack_into(ByteVector& data) const { + bool rc = true; + rc &= data.pack(cycle_time); + rc &= data.pack(i2c_errors); + + Value sensor_pack; + sensor_pack = 0; + for(const auto& s : sensors) { + switch(s) { + case Sensor::Accelerometer: + sensor_pack() |= (1 << 0); + break; + case Sensor::Barometer: + sensor_pack() |= (1 << 1); + break; + case Sensor::Magnetometer: + sensor_pack() |= (1 << 2); + break; + case Sensor::GPS: + sensor_pack() |= (1 << 3); + break; + case Sensor::Sonar: + sensor_pack() |= (1 << 4); + break; + case Sensor::OpticalFlow: + sensor_pack() |= (1 << 5); + break; + case Sensor::Pitot: + sensor_pack() |= (1 << 6); + break; + case Sensor::GeneralHealth: + sensor_pack() |= (1 << 15); + break; + } + } + rc &= data.pack(sensor_pack); + + Value box_pack; + box_pack = 0; + for(const auto& b : box_mode_flags) { + box_pack() |= (1 << b); + } + rc &= data.pack(box_pack); + return rc; + } +}; + +// MSP_STATUS: 101 +struct Status : public StatusBase, public Message { + Status(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_STATUS; } + + Value avg_system_load_pct; + Value gyro_cycle_time; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= StatusBase::unpack_from(data); + + if(fw_variant != FirmwareVariant::INAV) { + rc &= data.unpack(avg_system_load_pct); + rc &= data.unpack(gyro_cycle_time); + } + return rc; + } + + bool hasAccelerometer() const { + return sensors.count(Sensor::Accelerometer); + } + + bool hasBarometer() const { return sensors.count(Sensor::Barometer); } + + bool hasMagnetometer() const { return sensors.count(Sensor::Magnetometer); } + + bool hasGPS() const { return sensors.count(Sensor::GPS); } + + bool hasSonar() const { return sensors.count(Sensor::Sonar); } + + bool hasOpticalFlow() const { return sensors.count(Sensor::OpticalFlow); } + + bool hasPitot() const { return sensors.count(Sensor::Pitot); } + + bool isHealthy() const { return sensors.count(Sensor::GeneralHealth); } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Status:" << std::endl; + s << " Cycle time: " << cycle_time << " us" << std::endl; + s << " I2C errors: " << i2c_errors << std::endl; + s << " Sensors:" << std::endl; + + s << " Accelerometer: "; + hasAccelerometer() ? s << "ON" : s << "OFF"; + s << std::endl; + + s << " Barometer: "; + hasBarometer() ? s << "ON" : s << "OFF"; + s << std::endl; + + s << " Magnetometer: "; + hasMagnetometer() ? s << "ON" : s << "OFF"; + s << std::endl; + + s << " GPS: "; + hasGPS() ? s << "ON" : s << "OFF"; + s << std::endl; + + s << " Sonar: "; + hasSonar() ? s << "ON" : s << "OFF"; + s << std::endl; + + s << " Active Boxes (by ID):"; + for(const size_t box_id : box_mode_flags) { + s << " " << box_id; + } + s << std::endl; + + return s; + } +}; + +// MSP_RAW_IMU: 102 +struct RawImu : public Message { + RawImu(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_RAW_IMU; } + + std::array, 3> acc; + std::array, 3> gyro; + std::array, 3> mag; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(auto& a : acc) { + rc &= data.unpack(a); + } + for(auto& g : gyro) { + rc &= data.unpack(g); + } + for(auto& m : mag) { + rc &= data.unpack(m); + } + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Imu:" << std::endl; + s << " Linear acceleration: " << acc[0] << ", " << acc[1] << ", " + << acc[2] << std::endl; + s << " Angular velocity: " << gyro[0] << ", " << gyro[1] << ", " + << gyro[2] << std::endl; + s << " Magnetometer: " << mag[0] << ", " << mag[1] << ", " << mag[2] + << std::endl; + return s; + } +}; + +// helper class to convert raw imu readigs into standard physic units +// custom scaling factors have to be derived from the sensor documentation +struct ImuSI { + std::array, 3> acc; // m/s^2 + std::array, 3> gyro; // deg/s + std::array, 3> mag; // uT + + ImuSI(RawImu raw, + const float acc_1g, // sensor value at 1g + const float gyro_unit, // resolution in 1/(deg/s) + const float magn_gain, // scale magnetic value to uT (micro Tesla) + const float si_unit_1g // acceleration at 1g (in m/s^2)) + ) { + for(int i = 0; i < 3; ++i) { + acc[i] = raw.acc[i]() / acc_1g * si_unit_1g; + } + for(int i = 0; i < 3; ++i) { + gyro[i] = raw.gyro[i]() * gyro_unit; + } + for(int i = 0; i < 3; ++i) { + mag[i] = raw.mag[i]() * magn_gain; + } + } + + std::ostream& print(std::ostream& s) const { + s << "#Imu:" << std::endl; + s << " Linear acceleration: " << acc[0] << ", " << acc[1] << ", " + << acc[2] << " m/s²" << std::endl; + s << " Angular velocity: " << gyro[0] << ", " << gyro[1] << ", " + << gyro[2] << " deg/s" << std::endl; + s << " Magnetometer: " << mag[0] << ", " << mag[1] << ", " << mag[2] + << " uT" << std::endl; + return s; + } +}; + +// MSP_SERVO: 103 +struct Servo : public Message { + Servo(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SERVO; } + + std::array servo; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(auto& s : servo) rc &= data.unpack(s); + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Servo:" << std::endl; + s << " " << servo[0] << " " << servo[1] << " " << servo[2] << " " + << servo[3] << std::endl; + s << " " << servo[4] << " " << servo[5] << " " << servo[6] << " " + << servo[7] << std::endl; + return s; + } +}; + +// MSP_MOTOR: 104 +struct Motor : public Message { + Motor(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_MOTOR; } + + std::array motor; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(auto& m : motor) rc &= data.unpack(m); + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Motor:" << std::endl; + s << " " << motor[0] << " " << motor[1] << " " << motor[2] << " " + << motor[3] << std::endl; + s << " " << motor[4] << " " << motor[5] << " " << motor[6] << " " + << motor[7] << std::endl; + return s; + } +}; + +// MSP_RC: 105 +struct Rc : public Message { + Rc(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_RC; } + + std::vector channels; + + virtual bool decode(const ByteVector& data) override { + channels.clear(); + bool rc = true; + while(rc) { + uint16_t rc_data; + rc &= data.unpack(rc_data); + if(rc) channels.push_back(rc_data); + } + return !channels.empty(); + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Rc channels (" << channels.size() << ") :" << std::endl; + for(const uint16_t c : channels) { + s << c << " "; + } + s << " " << std::endl; + return s; + } +}; + +// MSP_RAW_GPS: 106 +struct RawGPS : public Message { + RawGPS(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_RAW_GPS; } + + Value fix; + Value numSat; + Value lat; + Value lon; + Value altitude; + Value ground_speed; + Value ground_course; + bool hdop_set; + Value hdop; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + hdop_set = false; + rc &= data.unpack(fix); + rc &= data.unpack(numSat); + rc &= data.unpack(lat, 1.f / 1e-7); + rc &= data.unpack(lon, 1.f / 1e-7); + rc &= data.unpack(altitude, 1.f); + rc &= data.unpack(ground_speed, 100.f); + rc &= data.unpack(ground_course, 10.f); + if(data.unpacking_remaining()) { + hdop_set = true; + rc &= data.unpack(hdop, 100.f); + } + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#RawGPS:" << std::endl; + s << " Fix: " << fix << std::endl; + s << " Num Sat: " << numSat << std::endl; + s << " Location: " << lat << " " << lon << std::endl; + s << " Altitude: " << altitude << " m" << std::endl; + s << " Ground speed: " << ground_speed << " m/s" << std::endl; + s << " Ground course: " << ground_course << " deg" << std::endl; + if(hdop_set) s << " HDOP: " << hdop << std::endl; + return s; + } +}; + +// MSP_COMP_GPS: 107 +struct CompGPS : public Message { + CompGPS(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_COMP_GPS; } + + Value distanceToHome; // meter + Value directionToHome; // degree + Value update; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(distanceToHome); + rc &= data.unpack(directionToHome); + rc &= data.unpack(update); + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#CompGPS:" << std::endl; + s << " Distance: " << distanceToHome << std::endl; + s << " Direction: " << directionToHome << std::endl; + s << " Update: " << update << std::endl; + + return s; + } +}; + +// TODO validate units +// MSP_ATTITUDE: 108 +struct Attitude : public Message { + Attitude(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_ATTITUDE; } + + Value roll; // degree + Value pitch; // degree + Value yaw; // degree + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(roll); + rc &= data.unpack(pitch); + rc &= data.unpack(yaw); + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Attitude:" << std::endl; + s << " Roll : " << roll << " deg" << std::endl; + s << " Pitch : " << pitch << " deg" << std::endl; + s << " Heading: " << yaw << " deg" << std::endl; + return s; + } +}; + +// TODO validate units +// MSP_ALTITUDE: 109 +struct Altitude : public Message { + Altitude(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_ALTITUDE; } + + Value altitude; // m + Value vario; // m/s + bool baro_altitude_set; + Value baro_altitude; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(altitude, 100.f); + rc &= data.unpack(vario, 100.f); + if(data.unpacking_remaining()) { + baro_altitude_set = true; + rc &= data.unpack(baro_altitude, 100.f); + } + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Altitude:" << std::endl; + s << " Altitude: " << altitude << " m, var: " << vario << " m/s" + << std::endl; + s << " Barometer: "; + if(baro_altitude_set) + s << baro_altitude; + else + s << ""; + s << std::endl; + return s; + } +}; + +// TODO check amperage units +// MSP_ANALOG: 110 +struct Analog : public Message { + Analog(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_ANALOG; } + + Value vbat; // Volt + Value powerMeterSum; // Ah + Value rssi; // Received Signal Strength Indication [0; 1023] + Value amperage; // Ampere + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(vbat, 0.1); + rc &= data.unpack(powerMeterSum, 0.001); + rc &= data.unpack(rssi); + rc &= data.unpack(amperage, 0.1); + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Analog:" << std::endl; + s << " Battery Voltage: " << vbat << " V" << std::endl; + s << " Current: " << amperage << " A" << std::endl; + s << " Power consumption: " << powerMeterSum << " Ah" << std::endl; + s << " RSSI: " << rssi << std::endl; + return s; + } +}; + +struct RcTuningSettings { + // RPY sequence + std::array, 3> rates; + std::array, 3> rcRates; + std::array, 3> rcExpo; + + Value dynamic_throttle_pid; + Value throttle_mid; + Value throttle_expo; + Value tpa_breakpoint; + + std::ostream& print(std::ostream& s) const { + s << "#Rc Tuning:" << std::endl; + s << " Rc Rate: " << rcRates[0] << " " << rcRates[1] << " " + << rcRates[2] << std::endl; + s << " Rc Expo: " << rcExpo[0] << " " << rcExpo[1] << " " << rcExpo[2] + << std::endl; + + s << " Dynamic Throttle PID: " << dynamic_throttle_pid << std::endl; + s << " Throttle MID: " << throttle_mid << std::endl; + s << " Throttle Expo: " << throttle_expo << std::endl; + return s; + } +}; + +// Differences between iNav and BF/CF +// MSP_RC_TUNING: 111 +struct RcTuning : public RcTuningSettings, public Message { + RcTuning(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_RC_TUNING; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(rcRates[0]); + rc &= data.unpack(rcExpo[0]); + for(size_t i = 0; i < 3; ++i) { + rc &= data.unpack(rates[0]); + } + rc &= data.unpack(dynamic_throttle_pid); + rc &= data.unpack(throttle_mid); + rc &= data.unpack(throttle_expo); + rc &= data.unpack(tpa_breakpoint); + rc &= data.unpack(rcExpo[2]); + if(fw_variant == FirmwareVariant::INAV) return rc; + rc &= data.unpack(rcRates[2]); + rc &= data.unpack(rcRates[1]); + rc &= data.unpack(rcExpo[1]); + return rc; + } +}; + +// PID struct for messages 112 and 202 +struct PidTerms : public Packable { + uint8_t P; + uint8_t I; + uint8_t D; + + bool unpack_from(const ByteVector& data) { + bool rc = true; + rc &= data.unpack(P); + rc &= data.unpack(I); + rc &= data.unpack(D); + return rc; + } + + bool pack_into(ByteVector& data) const { + bool rc = true; + rc &= data.pack(P); + rc &= data.pack(I); + rc &= data.pack(D); + return rc; + } +}; + +struct PidSettings { + std::array, + static_cast(PID_Element::PID_ITEM_COUNT)> + entry; + + std::ostream& print(std::ostream& s) const { + uint8_t PID_ROLL = + static_cast(msp::msg::PID_Element::PID_ROLL); + uint8_t PID_PITCH = + static_cast(msp::msg::PID_Element::PID_PITCH); + uint8_t PID_YAW = static_cast(msp::msg::PID_Element::PID_YAW); + uint8_t PID_POS_Z = + static_cast(msp::msg::PID_Element::PID_POS_Z); + uint8_t PID_POS_XY = + static_cast(msp::msg::PID_Element::PID_POS_XY); + uint8_t PID_VEL_XY = + static_cast(msp::msg::PID_Element::PID_VEL_XY); + uint8_t PID_SURFACE = + static_cast(msp::msg::PID_Element::PID_SURFACE); + uint8_t PID_LEVEL = + static_cast(msp::msg::PID_Element::PID_LEVEL); + uint8_t PID_HEADING = + static_cast(msp::msg::PID_Element::PID_HEADING); + uint8_t PID_VEL_Z = + static_cast(msp::msg::PID_Element::PID_VEL_Z); + + s << std::setprecision(3); + s << "#PID:" << std::endl; + s << " Name P | I | D |" << std::endl; + s << " ----------------|-------|-------|" << std::endl; + s << " Roll: " << entry[PID_ROLL]().P << "\t| " + << entry[PID_ROLL]().I << "\t| " << entry[PID_ROLL]().D << std::endl; + s << " Pitch: " << entry[PID_PITCH]().P << "\t| " + << entry[PID_PITCH]().I << "\t| " << entry[PID_PITCH]().D + << std::endl; + s << " Yaw: " << entry[PID_YAW]().P << "\t| " + << entry[PID_YAW]().I << "\t| " << entry[PID_YAW]().D << std::endl; + s << " Altitude: " << entry[PID_POS_Z]().P << "\t| " + << entry[PID_POS_Z]().I << "\t| " << entry[PID_POS_Z]().D + << std::endl; + + s << " Position: " << entry[PID_POS_XY]().P << "\t| " + << entry[PID_POS_XY]().I << "\t| " << entry[PID_POS_XY]().D + << std::endl; + s << " PositionR: " << entry[PID_VEL_XY]().P << "\t| " + << entry[PID_VEL_XY]().I << "\t| " << entry[PID_VEL_XY]().D + << std::endl; + s << " NavR: " << entry[PID_SURFACE]().P << "\t| " + << entry[PID_SURFACE]().I << "\t| " << entry[PID_SURFACE]().D + << std::endl; + s << " Level: " << entry[PID_LEVEL]().P << "\t| " + << entry[PID_LEVEL]().I << "\t| " << entry[PID_LEVEL]().D + << std::endl; + s << " Magn: " << entry[PID_HEADING]().P << "\t| " + << entry[PID_HEADING]().I << "\t| " << entry[PID_HEADING]().D + << std::endl; + s << " Vel: " << entry[PID_VEL_Z]().P << "\t| " + << entry[PID_VEL_Z]().I << "\t| " << entry[PID_VEL_Z]().D + << std::endl; + + return s; + } +}; + +// TODO: revisit +// MSP_PID: 112 +struct Pid : public PidSettings, public Message { + Pid(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_PID; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(uint8_t i = 0; + i < static_cast(PID_Element::PID_ITEM_COUNT); + ++i) { + rc &= data.unpack(entry[i]); + } + return rc; + } +}; + +// MSP_ACTIVEBOXES: 113 +struct ActiveBoxes : public Message { + ActiveBoxes(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_ACTIVEBOXES; } + + // box activation pattern + std::vector, NAUX>> box_pattern; + + virtual bool decode(const ByteVector& data) override { + box_pattern.clear(); + bool rc = true; + while(rc && data.unpacking_remaining() > 1) { + Value box_conf; + rc &= data.unpack(box_conf); + std::array, NAUX> aux_sp; + for(size_t iaux(0); iaux < NAUX; iaux++) { + for(size_t ip(0); ip < 3; ip++) { + if(box_conf() & (1 << (iaux * 3 + ip))) + aux_sp[iaux].insert(SwitchPosition(ip)); + } // each position (L,M,H) + } // each aux switch + box_pattern.push_back(aux_sp); + } // each box + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Box:" << std::endl; + for(size_t ibox(0); ibox < box_pattern.size(); ibox++) { + s << ibox << " "; + for(size_t iaux(0); iaux < box_pattern[ibox].size(); iaux++) { + s << "aux" << iaux + 1 << ": "; + if(box_pattern[ibox][iaux].count(msp::msg::SwitchPosition::LOW)) + s << "L"; + else + s << "_"; + if(box_pattern[ibox][iaux].count(msp::msg::SwitchPosition::MID)) + s << "M"; + else + s << "_"; + if(box_pattern[ibox][iaux].count( + msp::msg::SwitchPosition::HIGH)) + s << "H"; + else + s << "_"; + s << ", "; + } + s << std::endl; + } + + return s; + } +}; + +struct MiscSettings { + Value mid_rc; + Value min_throttle; + Value max_throttle; + Value min_command; + Value failsafe_throttle; + Value gps_provider; + Value gps_baudrate; + Value gps_ubx_sbas; + Value multiwii_current_meter_output; + Value rssi_channel; + Value reserved; + Value mag_declination; // degree + Value voltage_scale, cell_min, cell_max, cell_warning; + + std::ostream& print(std::ostream& s) const { + s << "#Miscellaneous:" << std::endl; + s << " Mid rc: " << mid_rc << std::endl; + s << " Min Throttle: " << min_throttle << std::endl; + s << " Max Throttle: " << max_throttle << std::endl; + s << " Failsafe Throttle: " << failsafe_throttle << std::endl; + + s << " Magnetic Declination: " << mag_declination << " deg" + << std::endl; + s << " Battery Voltage Scale: " << voltage_scale << " V" << std::endl; + s << " Battery Warning Level 1: " << cell_min << " V" << std::endl; + s << " Battery Warning Level 2: " << cell_max << " V" << std::endl; + s << " Battery Critical Level: " << cell_warning << " V" << std::endl; + + return s; + } +}; + +// MSP_MISC: 114 +struct Misc : public MiscSettings, public Message { + Misc(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_MISC; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(mid_rc); + rc &= data.unpack(min_throttle); + rc &= data.unpack(max_throttle); + rc &= data.unpack(min_command); + rc &= data.unpack(failsafe_throttle); + rc &= data.unpack(gps_provider); + rc &= data.unpack(gps_baudrate); + rc &= data.unpack(gps_ubx_sbas); + rc &= data.unpack(multiwii_current_meter_output); + rc &= data.unpack(rssi_channel); + rc &= data.unpack(reserved); + + rc &= data.unpack(mag_declination, 0.1); + rc &= data.unpack(voltage_scale, 0.1); + rc &= data.unpack(cell_min, 0.1); + rc &= data.unpack(cell_max, 0.1); + rc &= data.unpack(cell_warning, 0.1); + return rc; + } +}; + +// MSP_MOTOR_PINS: 115 +struct MotorPins : public Message { + MotorPins(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_MOTOR_PINS; } + + Value pwm_pin[N_MOTOR]; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(auto& pin : pwm_pin) rc &= data.unpack(pin); + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Motor pins:" << std::endl; + for(size_t imotor(0); imotor < msp::msg::N_MOTOR; imotor++) { + s << " Motor " << imotor << ": pin " << size_t(pwm_pin[imotor]()) + << std::endl; + } + + return s; + } +}; + +// MSP_BOXNAMES: 116 +struct BoxNames : public Message { + BoxNames(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_BOXNAMES; } + + std::vector box_names; + + virtual bool decode(const ByteVector& data) override { + box_names.clear(); + bool rc = true; + std::string str; + rc &= data.unpack(str); + std::stringstream ss(str); + std::string bname; + while(getline(ss, bname, ';')) { + box_names.push_back(bname); + } + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "# Box names:" << std::endl; + for(size_t ibox(0); ibox < box_names.size(); ibox++) { + s << " " << ibox << ": " << box_names[ibox] << std::endl; + } + return s; + } +}; + +// MSP_PIDNAMES: 117 +struct PidNames : public Message { + PidNames(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_PIDNAMES; } + + std::vector pid_names; + + virtual bool decode(const ByteVector& data) override { + pid_names.clear(); + bool rc = true; + std::string str; + rc &= data.unpack(str); + std::stringstream ss(str); + std::string pname; + while(getline(ss, pname, ';')) { + pid_names.push_back(pname); + } + return rc; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#PID names:" << std::endl; + for(size_t ipid(0); ipid < pid_names.size(); ipid++) { + s << " " << ipid << ": " << pid_names[ipid] << std::endl; + } + return s; + } +}; + +// MSP_WP: 118 +struct WayPoint : public Message { + WayPoint(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_WP; } + + Value wp_no; + Value lat; + Value lon; + Value altHold; + Value heading; + Value staytime; + Value navflag; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(wp_no); + rc &= data.unpack(lat); + rc &= data.unpack(lon); + rc &= data.unpack(altHold); + rc &= data.unpack(heading); + rc &= data.unpack(staytime); + rc &= data.unpack(navflag); + return rc; + } +}; + +// MSP_BOXIDS: 119 +struct BoxIds : public Message { + BoxIds(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_BOXIDS; } + + ByteVector box_ids; + + virtual bool decode(const ByteVector& data) override { + box_ids.clear(); + + for(uint8_t bi : data) box_ids.push_back(bi); + ; + + return true; + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Box IDs:" << std::endl; + for(size_t ibox(0); ibox < box_ids.size(); ibox++) { + s << " " << ibox << ": " << size_t(box_ids[ibox]) << std::endl; + } + return s; + } +}; + +struct ServoConfRange { + Value min; + Value max; + Value middle; + Value rate; +}; + +// MSP_SERVO_CONF: 120 +struct ServoConf : public Message { + ServoConf(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SERVO_CONF; } + + ServoConfRange servo_conf[N_SERVO]; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(size_t i(0); i < N_SERVO; i++) { + rc &= data.unpack(servo_conf[i].min); + rc &= data.unpack(servo_conf[i].max); + rc &= data.unpack(servo_conf[i].middle); + rc &= data.unpack(servo_conf[i].rate); + } + return rc; + } +}; + +// MSP_NAV_STATUS: 121 +struct NavStatus : public Message { + NavStatus(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_NAV_STATUS; } + + Value GPS_mode; + Value NAV_state; + Value mission_action; + Value mission_number; + Value NAV_error; + int16_t target_bearing; // degrees + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(GPS_mode); + rc &= data.unpack(NAV_state); + rc &= data.unpack(mission_action); + rc &= data.unpack(mission_number); + rc &= data.unpack(NAV_error); + rc &= data.unpack(target_bearing); + return rc; + } +}; + +struct GpsConf { + uint8_t filtering; + uint8_t lead_filter; + uint8_t dont_reset_home_at_arm; + uint8_t nav_controls_heading; + + uint8_t nav_tail_first; + uint8_t nav_rth_takeoff_heading; + uint8_t slow_nav; + uint8_t wait_for_rth_alt; + + uint8_t ignore_throttle; + uint8_t takeover_baro; + + uint16_t wp_radius; // in cm + uint16_t safe_wp_distance; // in meter + uint16_t nav_max_altitude; // in meter + uint16_t nav_speed_max; // in cm/s + uint16_t nav_speed_min; // in cm/s + + uint8_t crosstrack_gain; // * 100 (0-2.56) + uint16_t nav_bank_max; // degree * 100; (3000 default) + uint16_t rth_altitude; // in meter + uint8_t land_speed; // between 50 and 255 (100 approx = 50cm/sec) + uint16_t fence; // fence control in meters + + uint8_t max_wp_number; + + uint8_t checksum; +}; + +// MSP_NAV_CONFIG: 122 +struct NavConfig : public Message, public GpsConf { + NavConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_NAV_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(filtering); + rc &= data.unpack(lead_filter); + rc &= data.unpack(dont_reset_home_at_arm); + rc &= data.unpack(nav_controls_heading); + + rc &= data.unpack(nav_tail_first); + rc &= data.unpack(nav_rth_takeoff_heading); + rc &= data.unpack(slow_nav); + rc &= data.unpack(wait_for_rth_alt); + + rc &= data.unpack(ignore_throttle); + rc &= data.unpack(takeover_baro); + + rc &= data.unpack(wp_radius); + rc &= data.unpack(safe_wp_distance); + rc &= data.unpack(nav_max_altitude); + rc &= data.unpack(nav_speed_max); + rc &= data.unpack(nav_speed_min); + + rc &= data.unpack(crosstrack_gain); + rc &= data.unpack(nav_bank_max); + rc &= data.unpack(rth_altitude); + rc &= data.unpack(land_speed); + rc &= data.unpack(fence); + + rc &= data.unpack(max_wp_number); + rc &= data.unpack(checksum); + + return rc; + } +}; + +struct Motor3dConfig : public Message { + Motor3dConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_MOTOR_3D_CONFIG; } + + Value deadband3d_low; + Value deadband3d_high; + Value neutral_3d; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(deadband3d_low); + rc &= data.unpack(deadband3d_high); + rc &= data.unpack(neutral_3d); + return rc; + } +}; + +struct RcDeadbandSettings { + Value deadband; + Value yaw_deadband; + Value alt_hold_deadband; + Value deadband3d_throttle; +}; + +struct RcDeadband : public RcDeadbandSettings, public Message { + RcDeadband(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_RC_DEADBAND; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(deadband); + rc &= data.unpack(yaw_deadband); + rc &= data.unpack(alt_hold_deadband); + rc &= data.unpack(deadband3d_throttle); + return rc; + } +}; + +struct SensorAlignmentSettings { + Value gyro_align; + Value acc_align; + Value mag_align; +}; + +struct SensorAlignment : public SensorAlignmentSettings, public Message { + SensorAlignment(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SENSOR_ALIGNMENT; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(gyro_align); + rc &= data.unpack(acc_align); + rc &= data.unpack(mag_align); + return rc; + } +}; + +struct LedStripModecolor : public Message { + LedStripModecolor(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_LED_STRIP_MODECOLOR; } + + std::array, LED_MODE_COUNT> + mode_colors; + std::array special_colors; + Value led_aux_channel; + Value reserved; + Value led_strip_aux_channel; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(auto& mode : mode_colors) { + for(auto& color : mode) { + rc &= data.unpack(color); + } + } + for(auto& special : special_colors) { + rc &= data.unpack(special); + } + + rc &= data.unpack(led_aux_channel); + rc &= data.unpack(reserved); + rc &= data.unpack(led_strip_aux_channel); + return rc; + } +}; + +struct VoltageMeter { + Value id; + Value val; +}; + +struct VoltageMeters : public Message { + VoltageMeters(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_VOLTAGE_METERS; } + + std::vector meters; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(auto& meter : meters) { + rc &= data.unpack(meter.id); + rc &= data.unpack(meter.val); + } + return rc; + } +}; + +struct CurrentMeter { + Value id; + Value mAh_drawn; + Value mA; +}; + +struct CurrentMeters : public Message { + CurrentMeters(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_CURRENT_METERS; } + + std::vector meters; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + for(auto& meter : meters) { + rc &= data.unpack(meter.id); + rc &= data.unpack(meter.mAh_drawn); + rc &= data.unpack(meter.mA); + } + return rc; + } +}; + +struct BatteryState : public Message { + BatteryState(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_BATTERY_STATE; } + + Value cell_count; + Value capacity_mAh; + Value voltage; + Value mAh_drawn; + Value current; + Value state; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(cell_count); + rc &= data.unpack(capacity_mAh); + rc &= data.unpack(voltage); + rc &= data.unpack(mAh_drawn); + rc &= data.unpack(current); + rc &= data.unpack(state); + return rc; + } +}; + +struct MotorConfigSettings { + Value min_throttle; + Value max_throttle; + Value min_command; +}; + +struct MotorConfig : public MotorConfigSettings, public Message { + MotorConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_MOTOR_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(min_throttle); + rc &= data.unpack(max_throttle); + rc &= data.unpack(min_command); + return rc; + } +}; + +struct GpsConfigSettings { + Value provider; + Value sbas_mode; + Value auto_config; + Value auto_baud; +}; + +struct GpsConfig : public GpsConfigSettings, public Message { + GpsConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_GPS_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(provider); + rc &= data.unpack(sbas_mode); + rc &= data.unpack(auto_config); + rc &= data.unpack(auto_baud); + return rc; + } +}; + +struct CompassConfig : public Message { + CompassConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_COMPASS_CONFIG; } + + Value mag_declination; + + virtual bool decode(const ByteVector& data) override { + return data.unpack(mag_declination); + } +}; + +struct EscData { + Value temperature; + Value rpm; +}; + +struct EscSensorData : public Message { + EscSensorData(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_ESC_SENSOR_DATA; } + + Value motor_count; + std::vector esc_data; + + virtual bool decode(const ByteVector& data) override { + if(data.empty()) { + motor_count = 0; + return true; + } + bool rc = true; + rc &= data.unpack(motor_count); + for(int i = 0; i < motor_count(); ++i) { + EscData esc; + rc &= data.unpack(esc.temperature); + rc &= data.unpack(esc.rpm); + esc_data.push_back(esc); + } + return rc; + } +}; + +struct StatusEx : public StatusBase, public Message { + StatusEx(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_STATUS_EX; } + + // bf/cf fields + Value max_profiles; + Value control_rate_profile; + // iNav fields + Value avg_system_load_pct; + Value arming_flags; + Value acc_calibration_axis_flags; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= StatusBase::unpack_from(data); + if(fw_variant == FirmwareVariant::INAV) { + rc &= data.unpack(avg_system_load_pct); + rc &= data.unpack(arming_flags); + rc &= data.unpack(acc_calibration_axis_flags); + } + else { + rc &= data.unpack(max_profiles); + rc &= data.unpack(control_rate_profile); + } + return rc; + } +}; + +struct SensorStatus : public Message { + SensorStatus(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SENSOR_STATUS; } + + Value hardware_healthy; + Value hw_gyro_status; + Value hw_acc_status; + Value hw_compass_status; + Value hw_baro_status; + Value hw_gps_status; + Value hw_rangefinder_status; + Value hw_pitometer_status; + Value hw_optical_flow_status; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(hardware_healthy); + rc &= data.unpack(hw_gyro_status); + rc &= data.unpack(hw_acc_status); + rc &= data.unpack(hw_compass_status); + rc &= data.unpack(hw_baro_status); + rc &= data.unpack(hw_gps_status); + rc &= data.unpack(hw_rangefinder_status); + rc &= data.unpack(hw_pitometer_status); + rc &= data.unpack(hw_optical_flow_status); + return rc; + } +}; + +// MSP_UID = 160, +struct Uid : public Message { + Uid(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_UID; } + + Value u_id_0; + Value u_id_1; + Value u_id_2; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(u_id_0); + rc &= data.unpack(u_id_1); + rc &= data.unpack(u_id_2); + return rc; + } +}; + +struct GpsSvInfoSettings { + uint8_t channel; + uint8_t sv_id; + uint8_t quality; + uint8_t cno; +}; + +// MSP_GPSSVINFO = 164, +struct GpsSvInfo : public Message { + GpsSvInfo(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_GPSSVINFO; } + + Value hdop; + + Value channel_count; + std::vector sv_info; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + if(fw_variant == FirmwareVariant::INAV) { + rc &= data.consume(4); + rc &= data.unpack(hdop); + } + else { + rc &= data.unpack(channel_count); + for(uint8_t i = 0; i < channel_count(); ++i) { + GpsSvInfoSettings tmp; + rc &= data.unpack(tmp.channel); + rc &= data.unpack(tmp.sv_id); + rc &= data.unpack(tmp.quality); + rc &= data.unpack(tmp.cno); + } + } + return rc; + } +}; + +// MSP_GPSSTATISTICS = 166, +struct GpsStatistics : public Message { + GpsStatistics(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_GPSSTATISTICS; } + + Value last_msg_dt; + Value errors; + Value timeouts; + Value packet_count; + Value hdop; + Value eph; + Value epv; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(last_msg_dt); + rc &= data.unpack(errors); + rc &= data.unpack(timeouts); + rc &= data.unpack(packet_count); + rc &= data.unpack(hdop); + rc &= data.unpack(eph); + rc &= data.unpack(epv); + return rc; + } +}; + +// no actual implementations +// MSP_OSD_VIDEO_CONFIG = 180, +struct OsdVideoConfig : public Message { + OsdVideoConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_OSD_VIDEO_CONFIG; } + + virtual bool decode(const ByteVector& /*data*/) override { return false; } +}; + +// MSP_SET_OSD_VIDEO_CONFIG = 181, +struct SetOsdVideoConfig : public Message { + SetOsdVideoConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_OSD_VIDEO_CONFIG; } +}; + +// MSP_DISPLAYPORT = 182, +struct Displayport : public Message { + Displayport(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_DISPLAYPORT; } + + Value sub_cmd; + Value row; + Value col; + Value str; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(sub_cmd); + if(sub_cmd() == 3) { + rc &= data->pack(row); + rc &= data->pack(col); + rc &= data->pack(uint8_t(0)); + rc &= data->pack(uint8_t(str().size())); + rc &= data->pack(str); + } + if(!rc) data.reset(); + return data; + } +}; + +// Not available in iNav (183-185) +// MSP_COPY_PROFILE = 183, +struct CopyProfile : public Message { + CopyProfile(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_COPY_PROFILE; } + + Value profile_type; + Value dest_profile_idx; + Value src_profile_idx; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(profile_type); + rc &= data->pack(dest_profile_idx); + rc &= data->pack(src_profile_idx); + if(!rc) data.reset(); + return data; + } +}; + +struct BeeperConfigSettings { + Value beeper_off_mask; + Value beacon_tone; +}; + +// MSP_BEEPER_CONFIG = 184, +struct BeeperConfig : public BeeperConfigSettings, public Message { + BeeperConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_BEEPER_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(beeper_off_mask); + rc &= data.unpack(beacon_tone); + return rc; + } +}; + +// MSP_SET_BEEPER_CONFIG = 185, +struct SetBeeperConfig : public BeeperConfigSettings, public Message { + SetBeeperConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_BEEPER_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(beeper_off_mask); + if(beacon_tone.set()) { + rc &= data->pack(beacon_tone); + } + if(!rc) data.reset(); + return data; + } +}; + +// MSP_SET_TX_INFO = 186, // in message Used to send +// runtime information from TX lua scripts to the firmware +struct SetTxInfo : public Message { + SetTxInfo(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_TX_INFO; } + + Value rssi; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(rssi)) data.reset(); + return data; + } +}; + +// MSP_TX_INFO = 187, // out message Used by TX lua +// scripts to read information from the firmware +struct TxInfo : public Message { + TxInfo(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_TX_INFO; } + + Value rssi_source; + Value rtc_date_time_status; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(rssi_source); + rc &= data.unpack(rtc_date_time_status); + return rc; + } +}; + +///////////////////////////////////////////////////////////////////// +/// Response (2xx) + +// MSP_SET_RAW_RC: 200 +// This message is accepted but ignored on betaflight 3.0.1 onwards +// if "USE_RX_MSP" is not defined for the target. In this case, you can manually +// add "#define USE_RX_MSP" to your 'target.h'. +struct SetRawRc : public Message { + SetRawRc(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_RAW_RC; } + + std::vector channels; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + for(const uint16_t& c : channels) { + rc &= data->pack(c); + } + if(!rc) data.reset(); + return data; + } +}; + +// MSP_SET_RAW_GPS: 201 +struct SetRawGPS : public Message { + SetRawGPS(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_RAW_GPS; } + + Value fix; + Value numSat; + Value lat; + Value lon; + Value altitude; + Value speed; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(fix); + rc &= data->pack(numSat); + rc &= data->pack(lat); + rc &= data->pack(lon); + rc &= data->pack(altitude); + rc &= data->pack(speed); + assert(data->size() == 14); + if(!rc) data.reset(); + return data; + } +}; + +// MSP_SET_PID: 202, +struct SetPid : public PidSettings, public Message { + SetPid(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_PID; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + for(uint8_t i = 0; + i < static_cast(PID_Element::PID_ITEM_COUNT); + ++i) { + rc &= data->pack(entry[i]); + } + if(!rc) data.reset(); + return data; + } +}; + +// Depricated - no examples +// MSP_SET_BOX: 203, +struct SetBox : public Message { + SetBox(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_BOX; } +}; + +// Differences between iNav and BF/CF - this is BF/CF variant +// MSP_SET_RC_TUNING: 204 +struct SetRcTuning : public RcTuningSettings, public Message { + SetRcTuning(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_RC_TUNING; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(rcRates[0]); + rc &= data->pack(rcExpo[0]); + for(const auto& r : rates) { + rc &= data->pack(r); + } + rc &= data->pack(dynamic_throttle_pid); + rc &= data->pack(throttle_mid); + rc &= data->pack(throttle_expo); + rc &= data->pack(tpa_breakpoint); + // this field is optional in all firmwares + + if(!rcExpo[2].set()) goto packing_finished; + rc &= data->pack(rcExpo[2]); + // INAV quits her + + if(fw_variant == FirmwareVariant::INAV) goto packing_finished; + // these fields are optional in BF/CF + + if(!rcRates[2].set()) goto packing_finished; + rc &= data->pack(rcRates[2]); + + if(!rcRates[1].set()) goto packing_finished; + rc &= data->pack(rcRates[1]); + + if(!rcExpo[1].set()) goto packing_finished; + rc &= data->pack(rcExpo[1]); + + packing_finished: + if(!rc) data.reset(); + return data; + } +}; + +// MSP_ACC_CALIBRATION: 205 +struct AccCalibration : public Message { + AccCalibration(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_ACC_CALIBRATION; } +}; + +// MSP_MAG_CALIBRATION: 206 +struct MagCalibration : public Message { + MagCalibration(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_MAG_CALIBRATION; } +}; + +// MSP_SET_MISC: 207 +struct SetMisc : public MiscSettings, public Message { + SetMisc(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_MISC; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(mid_rc); + rc &= data->pack(min_throttle); + rc &= data->pack(max_throttle); + rc &= data->pack(failsafe_throttle); + rc &= data->pack(gps_provider); + rc &= data->pack(gps_baudrate); + rc &= data->pack(gps_ubx_sbas); + rc &= data->pack(multiwii_current_meter_output); + rc &= data->pack(rssi_channel); + rc &= data->pack(reserved); + rc &= data->pack(mag_declination, 10.f); + rc &= data->pack(voltage_scale, 10.f); + rc &= data->pack(cell_min, 10.f); + rc &= data->pack(cell_max, 10.f); + rc &= data->pack(cell_warning, 10.f); + if(!rc) data.reset(); + return data; + } +}; + +// MSP_RESET_CONF: 208 +struct ResetConfig : public Message { + ResetConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_RESET_CONF; } +}; + +// MSP_SET_WP: 209 +struct SetWp : public Message { + SetWp(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_WP; } + + Value wp_no; + Value lat; + Value lon; + Value alt; + + Value p1; + Value p2; + Value p3; + Value nav_flag; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(wp_no); + rc &= data->pack(lat); + rc &= data->pack(lon); + rc &= data->pack(alt); + rc &= data->pack(p1); + rc &= data->pack(p2); + if(fw_variant == FirmwareVariant::INAV) { + rc &= data->pack(p3); + } + rc &= data->pack(nav_flag); + return data; + } +}; + +// MSP_SELECT_SETTING: 210 +struct SelectSetting : public Message { + SelectSetting(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SELECT_SETTING; } + + uint8_t current_setting; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(current_setting)) data.reset(); + return data; + } +}; + +// MSP_SET_HEADING: 211 +struct SetHeading : public Message { + SetHeading(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_HEADING; } + + int16_t heading; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(heading)) data.reset(); + assert(data->size() == 2); + return data; + } +}; + +// MSP_SET_SERVO_CONF: 212 +struct SetServoConf : public Message { + SetServoConf(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_SERVO_CONF; } + + Value servo_idx; + Value min; + Value max; + Value middle; + Value rate; + + Value forward_from_channel; + Value reversed_sources; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(servo_idx); + rc &= data->pack(min); + rc &= data->pack(max); + rc &= data->pack(middle); + rc &= data->pack(rate); + if(fw_variant == FirmwareVariant::INAV) { + uint8_t tmp = 0; + rc &= data->pack(tmp); + rc &= data->pack(tmp); + } + rc &= data->pack(forward_from_channel); + rc &= data->pack(reversed_sources); + if(!rc) data.reset(); + return data; + } +}; + +// MSP_SET_MOTOR: 214 +struct SetMotor : public Message { + SetMotor(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_MOTOR; } + + std::array motor; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + for(size_t i(0); i < N_MOTOR; i++) rc &= data->pack(motor[i]); + assert(data->size() == N_MOTOR * 2); + if(!rc) data.reset(); return data; } }; +// MSP_SET_NAV_CONFIG = 215 +struct SetNavConfig : public Message { + SetNavConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_NAV_CONFIG; } +}; + +// MSP_SET_MOTOR_3D_CONF = 217 +struct SetMotor3dConf : public Message { + SetMotor3dConf(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_MOTOR_3D_CONF; } + + Value deadband3d_low; + Value deadband3d_high; + Value neutral_3d; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(deadband3d_low); + rc &= data->pack(deadband3d_high); + rc &= data->pack(neutral_3d); + if(!rc) data.reset(); + return data; + } +}; + +// MSP_SET_RC_DEADBAND = 218 +struct SetRcDeadband : public RcDeadbandSettings, public Message { + SetRcDeadband(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_RC_DEADBAND; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(deadband); + rc &= data->pack(yaw_deadband); + rc &= data->pack(alt_hold_deadband); + rc &= data->pack(deadband3d_throttle); + if(!rc) data.reset(); + return data; + } +}; + +// MSP_SET_RESET_CURR_PID = 219 +struct SetResetCurrPid : public Message { + SetResetCurrPid(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_RESET_CURR_PID; } +}; + +// MSP_SET_SENSOR_ALIGNMENT = 220 +struct SetSensorAlignment : public SensorAlignmentSettings, public Message { + SetSensorAlignment(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_SENSOR_ALIGNMENT; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(gyro_align); + rc &= data->pack(acc_align); + rc &= data->pack(mag_align); + if(!rc) data.reset(); + return data; + } +}; + +// MSP_SET_LED_STRIP_MODECOLOR = 221 +struct SetLedStripModecolor : public SensorAlignmentSettings, public Message { + SetLedStripModecolor(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_LED_STRIP_MODECOLOR; } + + Value mode_idx; + Value fun_idx; + Value color; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(mode_idx); + rc &= data->pack(fun_idx); + rc &= data->pack(color); + if(!rc) data.reset(); + return data; + } +}; + +// Not available in iNav (222-224) +// MSP_SET_MOTOR_CONFIG = 222 //out message Motor +// configuration (min/max throttle, etc) +struct SetMotorConfig : public MotorConfigSettings, public Message { + SetMotorConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_MOTOR_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(min_throttle); + rc &= data->pack(max_throttle); + rc &= data->pack(min_command); + if(!rc) data.reset(); + return data; + } +}; + +// MSP_SET_GPS_CONFIG = 223 //out message GPS +// configuration +struct SetGpsConfig : public GpsConfigSettings, public Message { + SetGpsConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_GPS_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(provider); + rc &= data->pack(sbas_mode); + rc &= data->pack(auto_config); + rc &= data->pack(auto_baud); + if(!rc) data.reset(); + return data; + } +}; + +// MSP_SET_COMPASS_CONFIG = 224 //out message Compass +// configuration +struct SetCompassConfig : public Message { + SetCompassConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_GPS_CONFIG; } + + Value mag_declination; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(mag_declination, 10)) data.reset(); + return data; + } +}; + +struct AccTrimSettings { + Value pitch; + Value roll; +}; + +// MSP_SET_ACC_TRIM = 239 //in message set acc angle +// trim values +struct SetAccTrim : public AccTrimSettings, public Message { + SetAccTrim(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_ACC_TRIM; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(pitch); + rc &= data->pack(roll); + if(!rc) data.reset(); + return data; + } +}; + +// MSP_ACC_TRIM = 240 //out message get acc angle +// trim values +struct AccTrim : public AccTrimSettings, public Message { + AccTrim(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_ACC_TRIM; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(pitch); + rc &= data.unpack(roll); + return rc; + } +}; + +struct ServoMixRule : public Packable { + uint8_t target_channel; + uint8_t input_source; + uint8_t rate; + uint8_t speed; + uint8_t min; + uint8_t max; + uint8_t box; + + bool unpack_from(const ByteVector& data) { + bool rc = true; + rc &= data.unpack(target_channel); + rc &= data.unpack(input_source); + rc &= data.unpack(rate); + rc &= data.unpack(speed); + rc &= data.unpack(min); + rc &= data.unpack(max); + rc &= data.unpack(box); + return rc; + } + + bool pack_into(ByteVector& data) const { + bool rc = true; + rc &= data.pack(target_channel); + rc &= data.pack(input_source); + rc &= data.pack(rate); + rc &= data.pack(speed); + rc &= data.pack(min); + rc &= data.pack(max); + rc &= data.pack(box); + return rc; + } +}; + +// MSP_SERVO_MIX_RULES = 241 //out message Returns servo +// mixer configuration +struct ServoMixRules : public Message { + ServoMixRules(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SERVO_MIX_RULES; } + + std::vector> rules; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + while(data.unpacking_remaining()) { + Value rule; + rc &= data.unpack(rule); + if(rc) + rules.push_back(rule); + else + break; + } + return rc; + } +}; + +// MSP_SET_SERVO_MIX_RULE = 242 //in message Sets servo +// mixer configuration +struct SetServoMixRule : public Message { + SetServoMixRule(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SERVO_MIX_RULES; } + + Value rule; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(rule)) data.reset(); + return data; + } +}; + +// not used in CF, BF, iNav +// MSP_PASSTHROUGH_SERIAL = 244 +struct PassthroughSerial : public Message { + PassthroughSerial(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_PASSTHROUGH_SERIAL; } +}; + +// MSP_SET_4WAY_IF = 245 //in message Sets 4way +// interface +struct Set4WayIF : public Message { + Set4WayIF(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_4WAY_IF; } + + Value esc_mode; + Value esc_port_index; + + Value esc_count; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + if(esc_mode.set()) { + rc &= data->pack(esc_mode); + rc &= data->pack(esc_port_index); + } + if(!rc) data.reset(); + return data; + } + + virtual bool decode(const ByteVector& data) override { + return data.unpack(esc_count); + } +}; + +struct RtcVals { + Value secs; + Value millis; +}; + +// MSP_SET_RTC = 246 //in message Sets the RTC +// clock +struct SetRtc : public RtcVals, public Message { + SetRtc(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_SET_RTC; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(secs); + rc &= data->pack(millis); + if(!rc) data.reset(); + return data; + } +}; + +// MSP_RTC = 247 //out message Gets the RTC +// clock +struct Rtc : public RtcVals, public Message { + Rtc(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_RTC; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(secs); + rc &= data.unpack(millis); + return rc; + } +}; + // MSP_EEPROM_WRITE: 250 -struct WriteEEPROM : public Response { - ID id() const { return ID::MSP_EEPROM_WRITE; } - std::vector encode() const { - return std::vector(); +struct WriteEEPROM : public Message { + WriteEEPROM(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_EEPROM_WRITE; } +}; + +// MSP_RESERVE_1: 251, //reserved for system usage +struct Reserve1 : public Message { + Reserve1(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_RESERVE_1; } +}; + +// MSP_RESERVE_2: 252, //reserved for system usage +struct Reserve2 : public Message { + Reserve2(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_RESERVE_2; } +}; + +// MSP_DEBUGMSG: 253 +struct DebugMessage : public Message { + DebugMessage(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_DEBUGMSG; } + + Value debug_msg; + + virtual bool decode(const ByteVector& data) override { + return data.unpack(debug_msg); + } +}; + +// MSP_DEBUG: 254 +struct Debug : public Message { + Debug(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_DEBUG; } + + Value debug1; + Value debug2; + Value debug3; + Value debug4; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(debug1); + rc &= data.unpack(debug2); + rc &= data.unpack(debug3); + rc &= data.unpack(debug4); + return rc; + } +}; + +// MSP_V2_FRAME: 255 +struct V2Frame : public Message { + V2Frame(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP_V2_FRAME; } +}; + +// MSP2_COMMON_TZ = 0x1001, //out message Gets the TZ offset +// for the local time (returns: minutes(i16)) +struct CommonTz : public Message { + CommonTz(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_COMMON_TZ; } + + Value tz_offset; + + virtual bool decode(const ByteVector& data) override { + return data.unpack(tz_offset); + } +}; + +// MSP2_COMMON_SET_TZ = 0x1002, //in message Sets the TZ offset +// for the local time (args: minutes(i16)) +struct CommonSetTz : public Message { + CommonSetTz(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_COMMON_SET_TZ; } + + Value tz_offset; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + if(!data->pack(tz_offset)) data.reset(); + return data; + } +}; + +enum class DATA_TYPE : uint8_t { + UNSET, + UINT8, + INT8, + UINT16, + INT16, + UINT32, + FLOAT, + STRING +}; + +// MSP2_COMMON_SETTING = 0x1003, //in/out message Returns the +// value for a setting +struct CommonSetting : public Message { + CommonSetting(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_COMMON_SETTING; } + + Value setting_name; + Value uint8_val; + Value int8_val; + Value uint16_val; + Value int16_val; + Value uint32_val; + Value float_val; + Value string_val; + + DATA_TYPE expected_data_type; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(setting_name); + if(!rc) data.reset(); + return data; + } + + // TODO + virtual bool decode(const ByteVector& data) override { + std::cout << "decoding " << data; + switch(expected_data_type) { + case DATA_TYPE::UINT8: + return data.unpack(uint8_val); + case DATA_TYPE::INT8: + return data.unpack(int8_val); + case DATA_TYPE::UINT16: + return data.unpack(uint16_val); + case DATA_TYPE::INT16: + return data.unpack(int16_val); + case DATA_TYPE::UINT32: + return data.unpack(uint32_val); + case DATA_TYPE::FLOAT: + return data.unpack(float_val); + case DATA_TYPE::STRING: + return data.unpack(string_val); + default: + return false; + } + } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Setting:" << std::endl; + s << " " << setting_name << ": "; + + switch(expected_data_type) { + case DATA_TYPE::UINT8: + s << uint8_val; + break; + case DATA_TYPE::INT8: + s << int8_val; + break; + case DATA_TYPE::UINT16: + s << uint16_val; + break; + case DATA_TYPE::INT16: + s << int16_val; + break; + case DATA_TYPE::UINT32: + s << uint32_val; + break; + case DATA_TYPE::FLOAT: + s << float_val; + break; + case DATA_TYPE::STRING: + s << string_val; + break; + default: + s << ""; + } + + s << std::endl; + + return s; + } +}; + +// MSP2_COMMON_SET_SETTING = 0x1004, //in message Sets the value for +// a setting +struct CommonSetSetting : public Message { + CommonSetSetting(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_COMMON_SET_SETTING; } + + Value setting_name; + Value uint8_val; + Value int8_val; + Value uint16_val; + Value int16_val; + Value uint32_val; + Value float_val; + Value string_val; + + DATA_TYPE expected_data_type; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(setting_name); + if(uint8_val.set()) + rc &= data->pack(uint8_val); + else if(int8_val.set()) + rc &= data->pack(int8_val); + else if(uint16_val.set()) + rc &= data->pack(uint16_val); + else if(int16_val.set()) + rc &= data->pack(int16_val); + else if(uint32_val.set()) + rc &= data->pack(uint32_val); + else if(float_val.set()) + rc &= data->pack(float_val); + else if(string_val.set()) + rc &= data->pack(string_val); + if(!rc) data.reset(); + return data; + } +}; + +struct MotorMixer : public Packable { + Value throttle; + Value roll; + Value pitch; + Value yaw; + + bool unpack_from(const ByteVector& data) { + bool rc = true; + rc &= data.unpack(throttle, 1000.0); + rc &= data.unpack(roll, 1000.0, 1.0); + rc &= data.unpack(pitch, 1000.0, 1.0); + rc &= data.unpack(yaw, 1000.0, 1.0); + return rc; + } + + bool pack_into(ByteVector& data) const { + bool rc = true; + rc &= data.pack(throttle, 1000.0, 1.0); + rc &= data.pack(roll, 1000.0, 1.0); + rc &= data.pack(pitch, 1000.0, 1.0); + rc &= data.pack(yaw, 1000.0, 1.0); + return rc; + } +}; + +// MSP2_COMMON_MOTOR_MIXER = 0x1005, +struct CommonMotorMixer : public Message { + CommonMotorMixer(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_COMMON_MOTOR_MIXER; } + + std::vector mixer; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + while(data.unpacking_remaining()) { + MotorMixer m; + rc &= data.unpack(m); + mixer.push_back(m); + } + return rc; + } +}; + +// MSP2_COMMON_SET_MOTOR_MIXER = 0x1006, +struct CommonSetMotorMixer : public Message { + CommonSetMotorMixer(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_COMMON_SET_MOTOR_MIXER; } + + Value index; + MotorMixer mixer; + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(index); + rc &= data->pack(mixer); + if(!rc) data.reset(); + return data; + } +}; + +// MSP2_INAV_STATUS = 0x2000, +struct InavStatus : public StatusBase, public Message { + InavStatus(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_INAV_STATUS; } + + Value avg_system_load_pct; + Value config_profile; + Value arming_flags; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(cycle_time); + rc &= data.unpack(i2c_errors); + + // get sensors + sensors.clear(); + uint16_t sensor = 0; + rc &= data.unpack(sensor); + if(sensor & (1 << 0)) sensors.insert(Sensor::Accelerometer); + if(sensor & (1 << 1)) sensors.insert(Sensor::Barometer); + if(sensor & (1 << 2)) sensors.insert(Sensor::Magnetometer); + if(sensor & (1 << 3)) sensors.insert(Sensor::GPS); + if(sensor & (1 << 4)) sensors.insert(Sensor::Sonar); + if(sensor & (1 << 5)) sensors.insert(Sensor::OpticalFlow); + if(sensor & (1 << 6)) sensors.insert(Sensor::Pitot); + if(sensor & (1 << 15)) sensors.insert(Sensor::GeneralHealth); + + rc &= data.unpack(avg_system_load_pct); + rc &= data.unpack(config_profile); + + rc &= data.unpack(arming_flags); + + // check active boxes + box_mode_flags.clear(); + uint32_t flag = 0; + rc &= data.unpack(flag); + for(size_t ibox(0); ibox < sizeof(flag) * CHAR_BIT; ibox++) { + if(flag & (1 << ibox)) box_mode_flags.insert(ibox); + } + + return rc; + } + + bool hasAccelerometer() const { + return sensors.count(Sensor::Accelerometer); } + + bool hasBarometer() const { return sensors.count(Sensor::Barometer); } + + bool hasMagnetometer() const { return sensors.count(Sensor::Magnetometer); } + + bool hasGPS() const { return sensors.count(Sensor::GPS); } + + bool hasSonar() const { return sensors.count(Sensor::Sonar); } + + bool hasOpticalFlow() const { return sensors.count(Sensor::OpticalFlow); } + + bool hasPitot() const { return sensors.count(Sensor::Pitot); } + + bool isHealthy() const { return sensors.count(Sensor::GeneralHealth); } + + virtual std::ostream& print(std::ostream& s) const override { + s << "#Status:" << std::endl; + s << " Cycle time: " << cycle_time << " us" << std::endl; + s << " Average system load: " << avg_system_load_pct << "%" + << std::endl; + s << " Arming flags: " << armingFlagToString(arming_flags()) + << std::endl; + s << " I2C errors: " << i2c_errors << std::endl; + s << " Sensors:" << std::endl; + + s << " Accelerometer: "; + hasAccelerometer() ? s << "ON" : s << "OFF"; + s << std::endl; + + s << " Barometer: "; + hasBarometer() ? s << "ON" : s << "OFF"; + s << std::endl; + + s << " Magnetometer: "; + hasMagnetometer() ? s << "ON" : s << "OFF"; + s << std::endl; + + s << " GPS: "; + hasGPS() ? s << "ON" : s << "OFF"; + s << std::endl; + + s << " Sonar: "; + hasSonar() ? s << "ON" : s << "OFF"; + s << std::endl; + + s << " Active Boxes (by ID):"; + for(const size_t box_id : box_mode_flags) { + s << " " << box_id; + } + s << std::endl; + + return s; + } +}; + +// MSP2_INAV_OPTICAL_FLOW = 0x2001, +struct InavOpticalFlow : public Message { + InavOpticalFlow(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_INAV_OPTICAL_FLOW; } + + Value raw_quality; + Value flow_rate_x; + Value flow_rate_y; + Value body_rate_x; + Value body_rate_y; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(raw_quality); + rc &= data.unpack(flow_rate_x); + rc &= data.unpack(flow_rate_y); + rc &= data.unpack(body_rate_x); + rc &= data.unpack(body_rate_y); + return rc; + } +}; + +// MSP2_INAV_ANALOG = 0x2002, +struct InavAnalog : public Message { + InavAnalog(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_INAV_ANALOG; } + + Value battery_voltage; + Value mAh_drawn; + Value rssi; + Value amperage; + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(battery_voltage); + rc &= data.unpack(mAh_drawn); + rc &= data.unpack(rssi); + rc &= data.unpack(amperage); + return rc; + } +}; + +struct InavMiscSettings { + Value mid_rc; + Value min_throttle; + Value max_throttle; + Value min_command; + Value failsafe_throttle; + Value gps_provider; + Value gps_baudrate; + Value gps_ubx_sbas; + Value rssi_channel; + Value mag_declination; + Value voltage_scale; + Value cell_min; + Value cell_max; + Value cell_warning; + Value capacity; + Value capacity_warning; + Value capacity_critical; + Value capacity_units; }; -} // namespace msg -} // namespace msp +// MSP2_INAV_MISC = 0x2003, +struct InavMisc : public InavMiscSettings, public Message { + InavMisc(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_INAV_MISC; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(mid_rc); + rc &= data.unpack(min_throttle); + rc &= data.unpack(max_throttle); + rc &= data.unpack(min_command); + rc &= data.unpack(failsafe_throttle); + rc &= data.unpack(gps_provider); + rc &= data.unpack(gps_baudrate); + rc &= data.unpack(gps_ubx_sbas); + rc &= data.unpack(rssi_channel); + rc &= data.unpack(mag_declination); + rc &= data.unpack(voltage_scale); + rc &= data.unpack(cell_min); + rc &= data.unpack(cell_max); + rc &= data.unpack(cell_warning); + rc &= data.unpack(capacity); + rc &= data.unpack(capacity_warning); + rc &= data.unpack(capacity_critical); + rc &= data.unpack(capacity_units); + return rc; + } +}; + +// MSP2_INAV_SET_MISC = 0x2004, +struct InavSetMisc : public InavMiscSettings, public Message { + InavSetMisc(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_INAV_SET_MISC; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(mid_rc); + rc &= data->pack(min_throttle); + rc &= data->pack(mid_rc); + rc &= data->pack(max_throttle); + rc &= data->pack(min_command); + rc &= data->pack(failsafe_throttle); + rc &= data->pack(gps_provider); + rc &= data->pack(gps_baudrate); + rc &= data->pack(gps_ubx_sbas); + rc &= data->pack(rssi_channel); + rc &= data->pack(mag_declination); + rc &= data->pack(voltage_scale); + rc &= data->pack(cell_min); + rc &= data->pack(cell_max); + rc &= data->pack(cell_warning); + rc &= data->pack(capacity); + rc &= data->pack(capacity_warning); + rc &= data->pack(capacity_critical); + rc &= data->pack(capacity_units); + if(!rc) data.reset(); + return data; + } +}; + +struct InavBatteryConfigSettings { + Value voltage_scale; + Value cell_min; + Value cell_max; + Value cell_warning; + Value current_offset; + Value current_scale; + Value capacity; + Value capacity_warning; + Value capacity_critical; + Value capacity_units; +}; + +// MSP2_INAV_BATTERY_CONFIG = 0x2005, +struct InavBatteryConfig : public InavBatteryConfigSettings, public Message { + InavBatteryConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_INAV_BATTERY_CONFIG; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(voltage_scale); + rc &= data.unpack(cell_min); + rc &= data.unpack(cell_max); + rc &= data.unpack(cell_warning); + rc &= data.unpack(current_offset); + rc &= data.unpack(current_scale); + rc &= data.unpack(capacity); + rc &= data.unpack(capacity_warning); + rc &= data.unpack(capacity_critical); + rc &= data.unpack(capacity_units); + return rc; + } +}; + +// MSP2_INAV_SET_BATTERY_CONFIG = 0x2006, +struct InavSetBatteryConfig : public InavBatteryConfigSettings, public Message { + InavSetBatteryConfig(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_INAV_SET_BATTERY_CONFIG; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(voltage_scale); + rc &= data->pack(cell_min); + rc &= data->pack(cell_max); + rc &= data->pack(cell_warning); + rc &= data->pack(current_offset); + rc &= data->pack(current_scale); + rc &= data->pack(capacity); + rc &= data->pack(capacity_warning); + rc &= data->pack(capacity_critical); + rc &= data->pack(capacity_units); + if(!rc) data.reset(); + return data; + } +}; + +struct InavRateProfileSettings { + Value throttle_rc_mid; + Value throttle_rc_expo; + Value throttle_dyn_pid; + Value throttle_pa_breakpoint; + + Value stabilized_rc_expo; + Value stabilized_rc_yaw_expo; + Value stabilized_rate_r; + Value stabilized_rate_p; + Value stabilized_rate_y; + + Value manual_rc_expo; + Value manual_rc_yaw_expo; + Value manual_rate_r; + Value manual_rate_p; + Value manual_rate_y; +}; + +// MSP2_INAV_RATE_PROFILE = 0x2007, +struct InavRateProfile : public InavRateProfileSettings, public Message { + InavRateProfile(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_INAV_RATE_PROFILE; } + + virtual bool decode(const ByteVector& data) override { + bool rc = true; + rc &= data.unpack(throttle_rc_mid); + rc &= data.unpack(throttle_rc_expo); + rc &= data.unpack(throttle_dyn_pid); + rc &= data.unpack(throttle_pa_breakpoint); + + rc &= data.unpack(stabilized_rc_expo); + rc &= data.unpack(stabilized_rc_yaw_expo); + rc &= data.unpack(stabilized_rate_r); + rc &= data.unpack(stabilized_rate_p); + rc &= data.unpack(stabilized_rate_y); + + rc &= data.unpack(manual_rc_expo); + rc &= data.unpack(manual_rc_yaw_expo); + rc &= data.unpack(manual_rate_r); + rc &= data.unpack(manual_rate_p); + rc &= data.unpack(manual_rate_y); + return rc; + } +}; + +// MSP2_INAV_SET_RATE_PROFILE = 0x2008, +struct InavSetRateProfile : public InavRateProfileSettings, public Message { + InavSetRateProfile(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_INAV_SET_RATE_PROFILE; } + + virtual ByteVectorUptr encode() const override { + ByteVectorUptr data = std::make_unique(); + bool rc = true; + rc &= data->pack(throttle_rc_mid); + rc &= data->pack(throttle_rc_expo); + rc &= data->pack(throttle_dyn_pid); + rc &= data->pack(throttle_pa_breakpoint); + + rc &= data->pack(stabilized_rc_expo); + rc &= data->pack(stabilized_rc_yaw_expo); + rc &= data->pack(stabilized_rate_r); + rc &= data->pack(stabilized_rate_p); + rc &= data->pack(stabilized_rate_y); + + rc &= data->pack(manual_rc_expo); + rc &= data->pack(manual_rc_yaw_expo); + rc &= data->pack(manual_rate_r); + rc &= data->pack(manual_rate_p); + rc &= data->pack(manual_rate_y); + if(!rc) data.reset(); + return data; + } +}; + +// MSP2_INAV_AIR_SPEED = 0x2009 +struct InavAirSpeed : public InavMiscSettings, public Message { + InavAirSpeed(FirmwareVariant v) : Message(v) {} + + virtual ID id() const override { return ID::MSP2_INAV_RATE_PROFILE; } + + Value speed; + + virtual bool decode(const ByteVector& data) override { + return data.unpack(speed); + } +}; + +} // namespace msg +} // namespace msp + +std::ostream& operator<<(std::ostream& s, const msp::msg::ImuSI& val) { + return val.print(s); +} -#endif // MSP_MSG_HPP +#endif // MSP_MSG_HPP diff --git a/inc/msp/types.hpp b/inc/msp/types.hpp deleted file mode 100644 index cc48649..0000000 --- a/inc/msp/types.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef TYPES_HPP -#define TYPES_HPP - -#include -#include -#include "msp_id.hpp" - -namespace msp { - -/** - * @brief ByteVector vector of bytes - */ -typedef std::vector ByteVector; - - -///////////////////////////////////////////////////////////////////// -/// Generic message types - -struct Message { - virtual ID id() const = 0; - - virtual ~Message() { } -}; - -// send to FC -struct Request : public Message { - virtual void decode(const ByteVector &data) = 0; -}; - -// received from FC -struct Response : public Message { - virtual ByteVector encode() const = 0; -}; - -} // namespace msp - -#endif // TYPES_HPP diff --git a/src/Client.cpp b/src/Client.cpp index 991f6fb..d997311 100644 --- a/src/Client.cpp +++ b/src/Client.cpp @@ -1,245 +1,501 @@ #include -#include "SerialPortImpl.cpp" - +#include #include namespace msp { +namespace client { -PeriodicTimer::PeriodicTimer(std::function funct, const double period_seconds) - : funct(funct), running(false) -{ - period_us = std::chrono::duration(size_t(period_seconds*1e6)); -} +Client::Client() : + port(io), + running_(ATOMIC_FLAG_INIT), + log_level_(SILENT), + msp_ver_(1), + fw_variant(FirmwareVariant::INAV) {} -void PeriodicTimer::start() { - // only start thread if period is above 0 - if(!(period_us.count()>0)) - return; - mutex_timer.lock(); - thread_ptr = std::shared_ptr(new std::thread( - [this]{ - running = true; - while(running) { - // call function and wait until end of period or stop is called - const auto tstart = std::chrono::high_resolution_clock::now(); - funct(); - if (mutex_timer.try_lock_until(tstart + period_us)) { - mutex_timer.unlock(); - } - } // while running - } - )); -} +Client::~Client() {} -void PeriodicTimer::stop() { - running = false; - mutex_timer.unlock(); - if(thread_ptr!=nullptr && thread_ptr->joinable()) { - thread_ptr->join(); +void Client::setLoggingLevel(const LoggingLevel& level) { log_level_ = level; } + +bool Client::setVersion(const int& ver) { + if(ver == 1 || ver == 2) { + msp_ver_ = ver; + return true; } + return false; } -void PeriodicTimer::setPeriod(const double period_seconds) { - stop(); - period_us = std::chrono::duration(size_t(period_seconds*1e6)); - start(); -} +int Client::getVersion() { return msp_ver_; } -} // namespace msp +void Client::setVariant(const FirmwareVariant& v) { fw_variant = v; } -namespace msp { -namespace client { +FirmwareVariant Client::getVariant() { return fw_variant; } -Client::Client() : pimpl(new SerialPortImpl), running(false), print_warnings(false) { - request_received.data.reserve(256); +bool Client::start(const std::string& device, const size_t baudrate) { + return connectPort(device, baudrate) && startReadThread() && + startSubscriptions(); } -Client::~Client() { - for(const std::pair d : subscribed_requests) - delete d.second; - - for(const std::pair s : subscriptions) - delete s.second; +bool Client::stop() { + return disconnectPort() && stopReadThread() && stopSubscriptions(); } -void Client::connect(const std::string &device, const size_t baudrate) { - pimpl->port.open(device); +bool Client::connectPort(const std::string& device, const size_t baudrate) { + asio::error_code ec; + port.open(device, ec); + if(ec) return false; + port.set_option(asio::serial_port::baud_rate(baudrate)); + port.set_option(asio::serial_port::parity(asio::serial_port::parity::none)); + port.set_option(asio::serial_port::character_size( + asio::serial_port::character_size(8))); + port.set_option( + asio::serial_port::stop_bits(asio::serial_port::stop_bits::one)); + return true; +} - pimpl->port.set_option(asio::serial_port::baud_rate(baudrate)); - pimpl->port.set_option(asio::serial_port::parity(asio::serial_port::parity::none)); - pimpl->port.set_option(asio::serial_port::character_size(asio::serial_port::character_size(8))); - pimpl->port.set_option(asio::serial_port::stop_bits(asio::serial_port::stop_bits::one)); +bool Client::disconnectPort() { + asio::error_code ec; + port.close(ec); + if(ec) return false; + return true; } -void Client::start() { - thread = std::thread([this]{ - running = true; - while(running) { processOneMessage(); } +bool Client::isConnected() { return port.is_open(); } + +bool Client::startReadThread() { + // no point reading if we arent connected to anything + if(!isConnected()) return false; + // can't start if we are already running + if(running_.test_and_set()) return false; + // hit it! + thread = std::thread([this] { + asio::async_read_until(port, + buffer, + std::bind(&Client::messageReady, + this, + std::placeholders::_1, + std::placeholders::_2), + std::bind(&Client::processOneMessage, + this, + std::placeholders::_1, + std::placeholders::_2)); + io.run(); }); + return true; } -void Client::stop() { - running = false; - pimpl->io.stop(); - pimpl->port.close(); - thread.join(); +bool Client::stopReadThread() { + bool rc = false; + if(running_.test_and_set()) { + io.stop(); + thread.join(); + io.reset(); + rc = true; + } + running_.clear(); + return rc; } -bool Client::sendData(const uint8_t id, const ByteVector &data) { - std::lock_guard lock(mutex_send); +bool Client::startSubscriptions() { + bool rc = true; + for(const auto& sub : subscriptions) { + rc &= sub.second->start(); + } + return rc; +} + +bool Client::stopSubscriptions() { + bool rc = true; + for(const auto& sub : subscriptions) { + rc &= sub.second->stop(); + } + return rc; +} - try { - asio::write(pimpl->port, asio::buffer("$M<",3)); // header - asio::write(pimpl->port, asio::buffer({uint8_t(data.size())})); // data size - asio::write(pimpl->port, asio::buffer({uint8_t(id)})); // message id - asio::write(pimpl->port, asio::buffer(data)); // data - asio::write(pimpl->port, asio::buffer({crc(id, data)})); // crc - } catch (const asio::system_error &ec) { - if (ec.code() == asio::error::operation_aborted) { - //operation_aborted error probably means the client is being closed +bool Client::sendMessage(msp::Message& message, const double& timeout) { + if(log_level_ >= DEBUG) + std::cout << "sending message - ID " << message.id() << std::endl; + if(!sendData(message.id(), message.encode())) { + if(log_level_ >= WARNING) + std::cerr << "message failed to send" << std::endl; + return false; + } + // prepare the condition check + std::unique_lock lock(cv_response_mtx); + const auto predicate = [&] { + mutex_response.lock(); + const bool received = (request_received != nullptr) && + (request_received->id == message.id()); + // unlock to wait for next message + if(!received) { + mutex_response.unlock(); + } + return received; + }; + // depending on the timeout, we may wait a fixed amount of time, or + // indefinitely + if(timeout > 0) { + if(!cv_response.wait_for( + lock, + std::chrono::milliseconds(size_t(timeout * 1e3)), + predicate)) { + if(log_level_ >= INFO) + std::cout << "timed out waiting for response to message ID " + << message.id() << std::endl; return false; } } + else { + cv_response.wait(lock, predicate); + } + // check status + const bool recv_success = request_received->status == OK; + ByteVector data; + if(recv_success) { + // make local copy of the data so that the read thread can keep moving + data = request_received->payload; + } + mutex_response.unlock(); + // decode the local copy of the payload + bool decode_success = false; + if(recv_success) { + decode_success = message.decode(data); + } + if(log_level_ >= DEBUG) std::cout << "" << std::endl; + return recv_success && decode_success; +} +bool Client::sendMessageNoWait(const msp::Message& message) { + if(log_level_ >= DEBUG) + std::cout << "async sending message - ID " << message.id() << std::endl; + if(!sendData(message.id(), message.encode())) { + if(log_level_ >= WARNING) + std::cerr << "async sendData failed" << std::endl; + return false; + } return true; } -int Client::request(msp::Request &request, const double timeout) { - msp::ByteVector data; - const int success = request_raw(uint8_t(request.id()), data, timeout); - if(success==1) { request.decode(data); } - return success; +uint8_t Client::extractChar() { + if(buffer.sgetc() == EOF) { + if(log_level_ >= WARNING) + std::cerr << "buffer returned EOF; reading char directly from port" + << std::endl; + asio::read(port, buffer, asio::transfer_exactly(1)); + } + return uint8_t(buffer.sbumpc()); } -int Client::request_raw(const uint8_t id, ByteVector &data, const double timeout) { - // send request - if(!sendRequest(id)) { return false; } - - // wait for thread to received message - std::unique_lock lock(mutex_cv_request); - const auto predicate = [&]{ - mutex_request.lock(); - const bool received = (request_received.id==id); - // unlock to wait for next message - if(!received) { mutex_request.unlock(); } - return received; - }; - - if(timeout>0) { - if(!cv_request.wait_for(lock, std::chrono::milliseconds(size_t(timeout*1e3)), predicate)) - return -1; +bool Client::sendData(const msp::ID id, const ByteVector& data) { + if(log_level_ >= DEBUG) std::cout << "sending: " << id << " | " << data; + ByteVector msg; + if(msp_ver_ == 2) { + msg = packMessageV2(id, data); } else { - cv_request.wait(lock, predicate); + msg = packMessageV1(id, data); + } + if(log_level_ >= DEBUG) std::cout << "packed: " << msg; + asio::error_code ec; + std::size_t bytes_written; + { + std::lock_guard lock(mutex_send); + bytes_written = + asio::write(port, asio::buffer(msg.data(), msg.size()), ec); + } + if(ec == asio::error::operation_aborted && log_level_ >= WARNING) { + // operation_aborted error probably means the client is being closed + std::cerr << "------------------> WRITE FAILED <--------------------" + << std::endl; + return false; } + if(log_level_ >= DEBUG) + std::cout << "write complete: " << bytes_written << " vs " << msg.size() + << std::endl; + return (bytes_written == msg.size()); +} - // check message status and decode - const bool success = request_received.status==OK; - if(success) { data = request_received.data; } - mutex_request.unlock(); - return success; +ByteVector Client::packMessageV1(const msp::ID id, const ByteVector& data) { + ByteVector msg; + msg.push_back('$'); // preamble1 + msg.push_back('M'); // preamble2 + msg.push_back('<'); // direction + msg.push_back(uint8_t(data.size())); // data size + msg.push_back(uint8_t(id)); // message_id + msg.insert(msg.end(), data.begin(), data.end()); // data + msg.push_back(crcV1(uint8_t(id), data)); // crc + return msg; } -bool Client::respond(const msp::Response &response, const bool wait_ack) { - return respond_raw(uint8_t(response.id()), response.encode(), wait_ack); +uint8_t Client::crcV1(const uint8_t id, const ByteVector& data) { + uint8_t crc = uint8_t(data.size()) ^ id; + for(const uint8_t d : data) { + crc = crc ^ d; + } + return crc; } -bool Client::respond_raw(const uint8_t id, const ByteVector &data, const bool wait_ack) { - // send response - if(!sendData(id, data)) { return false; } +ByteVector Client::packMessageV2(const msp::ID id, const ByteVector& data) { + ByteVector msg; + msg.push_back('$'); // preamble1 + msg.push_back('X'); // preamble2 + msg.push_back('<'); // direction + msg.push_back(0); // flag + msg.push_back(uint8_t(uint16_t(id) & 0xFF)); // message_id low bits + msg.push_back(uint8_t(uint16_t(id) >> 8)); // message_id high bits - if(!wait_ack) - return true; + const uint16_t size = uint16_t(data.size()); + msg.push_back(uint8_t(size & 0xFF)); // data size low bits + msg.push_back(uint8_t(size >> 8)); // data size high bits - // wait for thread to received message - std::unique_lock lock(mutex_cv_ack); - cv_ack.wait(lock, [&]{ - mutex_request.lock(); - const bool received = (request_received.id==id); - // unlock to wait for next message - if(!received) { mutex_request.unlock(); } - return received; - }); + msg.insert(msg.end(), data.begin(), data.end()); // data + msg.push_back(crcV2(0, ByteVector(msg.begin() + 3, msg.end()))); // crc - // check status, expect ACK without payload - const bool success = request_received.status==OK; - mutex_request.unlock(); - return success; + return msg; +} + +uint8_t Client::crcV2(uint8_t crc, const ByteVector& data) { + for(const uint8_t& p : data) { + crc = crcV2(crc, p); + } + return crc; } -uint8_t Client::crc(const uint8_t id, const ByteVector &data) { - uint8_t crc = uint8_t(data.size())^id; - for(const uint8_t d : data) { crc = crc^d; } +uint8_t Client::crcV2(uint8_t crc, const uint8_t& b) { + crc ^= b; + for(int ii = 0; ii < 8; ++ii) { + if(crc & 0x80) { + crc = (crc << 1) ^ 0xD5; + } + else { + crc = crc << 1; + } + } return crc; } -void Client::processOneMessage() { - uint8_t c; - // find start of header - while(true) { - // find '$' - for(c=0; c!='$'; asio::read(pimpl->port, asio::buffer(&c,1))); - // check 'M' - asio::read(pimpl->port, asio::buffer(&c,1)); - if(c=='M') { break; } +void Client::processOneMessage(const asio::error_code& ec, + const std::size_t& bytes_transferred) { + if(log_level_ >= DEBUG) + std::cout << "processOneMessage on " << bytes_transferred << " bytes" + << std::endl; + + if(ec == asio::error::operation_aborted) { + // operation_aborted error probably means the client is being closed + // notify waiting request methods + cv_response.notify_all(); + return; + } + + // ignore and remove header bytes + const uint8_t msg_marker = extractChar(); + if(msg_marker != '$') + std::cerr << "Message marker " << msg_marker << " is not recognised!" + << std::endl; + + // message version + int ver = 0; + const uint8_t ver_marker = extractChar(); + if(ver_marker == 'M') ver = 1; + if(ver_marker == 'X') ver = 2; + if(ver == 0) { + std::cerr << "Version marker " << ver_marker << " is not recognised!" + << std::endl; + } + + ReceivedMessage recv_msg; + if(ver == 2) + recv_msg = processOneMessageV2(); + else + recv_msg = processOneMessageV1(); + + { + std::lock_guard lock2(cv_response_mtx); + std::lock_guard lock(mutex_response); + request_received.reset(new ReceivedMessage(recv_msg)); + } + // notify waiting request methods + cv_response.notify_all(); + + // check subscriptions + { + std::lock_guard lock(mutex_subscriptions); + std::lock_guard lock2(mutex_response); + if(request_received->status == OK && + subscriptions.count(ID(request_received->id))) { + subscriptions.at(ID(request_received->id)) + ->decode(request_received->payload); + } + } + + asio::async_read_until(port, + buffer, + std::bind(&Client::messageReady, + this, + std::placeholders::_1, + std::placeholders::_2), + std::bind(&Client::processOneMessage, + this, + std::placeholders::_1, + std::placeholders::_2)); + + if(log_level_ >= DEBUG) + std::cout << "processOneMessage finished" << std::endl; +} + +std::pair Client::messageReady(iterator begin, iterator end) { + iterator i = begin; + const size_t available = std::distance(begin, end); + + if(available < 2) return std::make_pair(begin, false); + + if(*i == '$' && *(i + 1) == 'M') { + // not even enough data for a header + if(available < 6) return std::make_pair(begin, false); + + const uint8_t payload_size = *(i + 3); + // incomplete xfer + if(available < size_t(5 + payload_size + 1)) + return std::make_pair(begin, false); + + std::advance(i, 5 + payload_size + 1); + } + else if(*i == '$' && *(i + 1) == 'X') { + // not even enough data for a header + if(available < 9) return std::make_pair(begin, false); + + const uint16_t payload_size = uint8_t(*(i + 6)) | uint8_t(*(i + 7)) + << 8; + + // incomplete xfer + if(available < size_t(8 + payload_size + 1)) + return std::make_pair(begin, false); + + std::advance(i, 8 + payload_size + 1); + } + else { + for(; i != end; ++i) { + if(*i == '$') break; + } + // implicitly consume all if $ not found } + return std::make_pair(i, true); +} + +ReceivedMessage Client::processOneMessageV1() { + ReceivedMessage ret; + + ret.status = OK; + // message direction - asio::read(pimpl->port, asio::buffer(&c,1)); - const bool ok_id = (c!='!'); + const uint8_t dir = extractChar(); + const bool ok_id = (dir != '!'); // payload length - uint8_t len; - asio::read(pimpl->port, asio::buffer(&len,1)); - request_received.data.resize(len); + const uint8_t len = extractChar(); + + // message ID + uint8_t id = extractChar(); + ret.id = msp::ID(id); + + if(log_level_ >= WARNING && !ok_id) { + std::cerr << "Message v1 with ID " << size_t(ret.id) + << " is not recognised!" << std::endl; + } + + // payload + for(size_t i(0); i < len; i++) { + ret.payload.push_back(extractChar()); + } + + // CRC + const uint8_t rcv_crc = extractChar(); + const uint8_t exp_crc = crcV1(id, ret.payload); + const bool ok_crc = (rcv_crc == exp_crc); + + if(log_level_ >= WARNING && !ok_crc) { + std::cerr << "Message v1 with ID " << size_t(ret.id) + << " has wrong CRC! (expected: " << size_t(exp_crc) + << ", received: " << size_t(rcv_crc) << ")" << std::endl; + } + + if(!ok_id) { + ret.status = FAIL_ID; + } + else if(!ok_crc) { + ret.status = FAIL_CRC; + } + + return ret; +} + +ReceivedMessage Client::processOneMessageV2() { + ReceivedMessage ret; + + ret.status = OK; + + uint8_t exp_crc = 0; + + // message direction + const uint8_t dir = extractChar(); + if(log_level_ >= DEBUG) std::cout << "dir: " << dir << std::endl; + const bool ok_id = (dir != '!'); + + // flag + const uint8_t flag = extractChar(); + if(log_level_ >= DEBUG) std::cout << "flag: " << flag << std::endl; + exp_crc = crcV2(exp_crc, flag); // message ID - mutex_request.lock(); - asio::read(pimpl->port, asio::buffer(&request_received.id,1)); - mutex_request.unlock(); + const uint8_t id_low = extractChar(); + const uint8_t id_high = extractChar(); + uint16_t id = uint16_t(id_low) | (uint16_t(id_high) << 8); + ret.id = msp::ID(id); + if(log_level_ >= DEBUG) std::cout << "id: " << id << std::endl; + exp_crc = crcV2(exp_crc, id_low); + exp_crc = crcV2(exp_crc, id_high); - if(print_warnings && !ok_id) { - std::cerr << "Message with ID " << size_t(request_received.id) << " is not recognised!" << std::endl; + // payload length + const uint8_t len_low = extractChar(); + const uint8_t len_high = extractChar(); + uint32_t len = uint32_t(len_low) | (uint32_t(len_high) << 8); + exp_crc = crcV2(exp_crc, len_low); + exp_crc = crcV2(exp_crc, len_high); + if(log_level_ >= DEBUG) std::cout << "len: " << len << std::endl; + + if(log_level_ >= WARNING && !ok_id) { + std::cerr << "Message v2 with ID " << ret.id << " is not recognised!" + << std::endl; } // payload - asio::read(pimpl->port, asio::buffer(request_received.data)); + ByteVector data; + for(size_t i(0); i < len; i++) { + ret.payload.push_back(extractChar()); + } + + exp_crc = crcV2(exp_crc, ret.payload); // CRC - uint8_t rcv_crc; - asio::read(pimpl->port, asio::buffer(&rcv_crc,1)); - mutex_request.lock(); - const uint8_t exp_crc = crc(request_received.id, request_received.data); - mutex_request.unlock(); - const bool ok_crc = (rcv_crc==exp_crc); + const uint8_t rcv_crc = extractChar(); + + const bool ok_crc = (rcv_crc == exp_crc); - if(print_warnings && !ok_crc) { - std::cerr << "Message with ID " << size_t(request_received.id) << " has wrong CRC! (expected: " << size_t(exp_crc) << ", received: "<< size_t(rcv_crc) << ")" << std::endl; + if(log_level_ >= WARNING && !ok_crc) { + std::cerr << "Message v2 with ID " << ret.id + << " has wrong CRC! (expected: " << size_t(exp_crc) + << ", received: " << size_t(rcv_crc) << ")" << std::endl; } - mutex_request.lock(); - request_received.status = !ok_id ? FAIL_ID : (!ok_crc ? FAIL_CRC : OK) ; - mutex_request.unlock(); + if(!ok_id) { + ret.status = FAIL_ID; + } + else if(!ok_crc) { + ret.status = FAIL_CRC; + } - // notify waiting request methods - cv_request.notify_one(); - // notify waiting respond methods - cv_ack.notify_one(); + return ret; +} - // check subscriptions - mutex_callbacks.lock(); - mutex_request.lock(); - if(request_received.status==OK && subscriptions.count(ID(request_received.id))) { - // fetch message type, decode payload - msp::Request *const req = subscribed_requests.at(ID(request_received.id)); - req->decode(request_received.data); - mutex_request.unlock(); - // call callback - subscriptions.at(ID(request_received.id))->call(*req); - } - mutex_request.unlock(); - mutex_callbacks.unlock(); -} - -} // namespace client -} // namespace msp +} // namespace client +} // namespace msp diff --git a/src/FlightController.cpp b/src/FlightController.cpp index 06e07d6..729ac4b 100644 --- a/src/FlightController.cpp +++ b/src/FlightController.cpp @@ -1,199 +1,287 @@ #include "FlightController.hpp" - #include namespace fcu { -FlightController::FlightController(const std::string &device, const size_t baudrate) { - client.connect(device, baudrate); - client.start(); -} +FlightController::FlightController() : + msp_version_(1), + armed_(false), + control_source_(ControlSource::NONE), + msp_timer_(std::bind(&FlightController::generateMSP, this), 0.1) {} -FlightController::~FlightController() { - client.stop(); -} +FlightController::~FlightController() { disconnect(); } -void FlightController::waitForConnection() { - std::cout<<"Wait for FC..."< lock(msp_updates_mutex); + flight_mode_ = mode; } + generateMSP(); } -bool FlightController::isFirmware(const FirmwareType firmware_type) { - return firmware == firmware_type; +FlightMode FlightController::getFlightMode() { return flight_mode_; } + +void FlightController::setControlSource(ControlSource source) { + if(source == control_source_) return; + + msp::msg::RxConfig rxConfig(fw_variant_); + if(!client_.sendMessage(rxConfig)) + std::cout << "client_.sendMessage(rxConfig) failed" << std::endl; + std::cout << rxConfig; + + msp::msg::SetRxConfig setRxConfig(fw_variant_); + static_cast(setRxConfig) = + static_cast(rxConfig); + + if(source == ControlSource::SBUS) { + setRxConfig.receiverType = 3; + setRxConfig.serialrx_provider = 2; + } + else if(source == ControlSource::MSP) { + setRxConfig.receiverType = 4; + } + client_.sendMessage(setRxConfig); + + control_source_ = source; } +ControlSource FlightController::getControlSource() { + msp::msg::RxConfig rxConfig(fw_variant_); + client_.sendMessage(rxConfig); + + if(rxConfig.receiverType && rxConfig.receiverType() == 4) + return ControlSource::MSP; + else if(rxConfig.serialrx_provider() == 2) + return ControlSource::SBUS; + return ControlSource::OTHER; +} + +void FlightController::setRPYT(std::array &&rpyt) { + { + std::lock_guard lock(msp_updates_mutex); + rpyt_.swap(rpyt); + } + generateMSP(); +} + +void FlightController::generateMSP() { + std::vector cmds(6, 1000); + // manually remapping from RPYT to TAER (american RC) + // TODO: make this respect channel mapping + { + std::lock_guard lock(msp_updates_mutex); + cmds[0] = (rpyt_[3] * 500) + 1500; + cmds[1] = (rpyt_[0] * 500) + 1500; + cmds[2] = (rpyt_[1] * 500) + 1500; + cmds[3] = (rpyt_[2] * 500) + 1500; + + if(!(uint32_t(flight_mode_.modifier) & + uint32_t(FlightMode::MODIFIER::ARM))) + cmds[4] = 1000; + else if(uint32_t(flight_mode_.secondary) & + uint32_t(FlightMode::SECONDARY_MODE::NAV_ALTHOLD)) + cmds[4] = 2000; + else + cmds[4] = 1500; + + switch(flight_mode_.primary) { + case FlightMode::PRIMARY_MODE::ANGLE: + cmds[5] = 1000; + break; + case FlightMode::PRIMARY_MODE::NAV_POSHOLD: + cmds[5] = 1500; + break; + case FlightMode::PRIMARY_MODE::NAV_RTH: + cmds[5] = 2000; + break; + default: + break; + } + } + setRc(cmds); +} + +bool FlightController::saveSettings() { + msp::msg::WriteEEPROM writeEEPROM(fw_variant_); + return client_.sendMessage(writeEEPROM); +} + +bool FlightController::reboot() { + msp::msg::Reboot reboot(fw_variant_); + return client_.sendMessage(reboot); +} + +msp::FirmwareVariant FlightController::getFwVariant() { return fw_variant_; } + +int FlightController::getProtocolVersion() { return msp_version_; } + +std::string FlightController::getBoardName() { return board_name_; } + void FlightController::initBoxes() { - client.setPrintWarnings(true); // get box names - msp::msg::BoxNames box_names; - if(!client.request(box_names)) + msp::msg::BoxNames box_names(fw_variant_); + if(!client_.sendMessage(box_names)) throw std::runtime_error("Cannot get BoxNames!"); - + std::cout << box_names << std::endl; // get box IDs - msp::msg::BoxIds box_ids; - if(!client.request(box_ids)) + msp::msg::BoxIds box_ids(fw_variant_); + if(!client_.sendMessage(box_ids)) throw std::runtime_error("Cannot get BoxIds!"); + std::cout << box_ids << std::endl; + assert(box_names.box_names.size() == box_ids.box_ids.size()); - assert(box_names.box_names.size()==box_ids.box_ids.size()); - - box_name_ids.clear(); - for(size_t ibox(0); ibox auxs) -{ - if(isFirmwareMultiWii() && hasDynBal()) { - throw std::runtime_error( - "DYNBALANCE is active!\n" - "RC commands will have no effect on motors."); - } - - msp::msg::SetRc rc; + const std::vector auxs) { + msp::msg::SetRawRc rc(fw_variant_); // insert mappable channels - rc.channels.resize(MAX_MAPPABLE_RX_INPUTS); - rc.channels[channel_map[0]] = roll; - rc.channels[channel_map[1]] = pitch; - rc.channels[channel_map[2]] = yaw; - rc.channels[channel_map[3]] = throttle; - rc.channels[channel_map[4]] = aux1; - rc.channels[channel_map[5]] = aux2; - rc.channels[channel_map[6]] = aux3; - rc.channels[channel_map[7]] = aux4; + rc.channels.resize(msp::msg::MAX_MAPPABLE_RX_INPUTS); + rc.channels[channel_map_[0]] = roll; + rc.channels[channel_map_[1]] = pitch; + rc.channels[channel_map_[2]] = yaw; + rc.channels[channel_map_[3]] = throttle; + + rc.channels.emplace_back(aux1); + rc.channels.emplace_back(aux2); + rc.channels.emplace_back(aux3); + rc.channels.emplace_back(aux4); // insert remaining aux channels rc.channels.insert(std::end(rc.channels), std::begin(auxs), std::end(auxs)); // send MSP_SET_RAW_RC without waiting for ACK - return client.respond(rc, false); + return client_.sendMessageNoWait(rc); } bool FlightController::setRc(const std::vector channels) { - msp::msg::SetRc rc; + msp::msg::SetRawRc rc(fw_variant_); rc.channels = channels; - return client.respond(rc, false); + return client_.sendMessageNoWait(rc); } -bool FlightController::setMotors(const std::array &motor_values) { - if(isFirmwareMultiWii() && !hasDynBal()) { - throw std::runtime_error( - "DYNBALANCE is not active!\n" - "Set '#define DYNBALANCE' in your MultiWii 'config.h'"); - } - - msp::msg::SetMotor motor; +bool FlightController::setMotors( + const std::array &motor_values) { + msp::msg::SetMotor motor(fw_variant_); motor.motor = motor_values; - return client.respond(motor); -} - -bool FlightController::arm(const bool arm) { - // arm: - // throttle: 1000 (bottom), yaw: 2000 (right) - // disarm: - // throttle: 1000 (bottom), yaw: 1000 (left) - - const uint16_t yaw = arm ? 2000 : 1000; - - return setRc(1500, 1500, yaw, 1000, 1000, 1000, 1000, 1000); -} - -bool FlightController::arm_block() { - // attempt to arm while FC is disarmed - while(isArmed()==false) { - arm(true); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - return true; -} - -bool FlightController::disarm_block() { - // attempt to disarm while FC is armed - while(isArmed()==true) { - arm(false); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - return true; + return client_.sendMessage(motor); } int FlightController::updateFeatures(const std::set &add, - const std::set &remove) -{ + const std::set &remove) { // get original feature configuration - msp::msg::Feature feature_in; - if(!client.request(feature_in)) - return -1; + msp::msg::Feature feature_in(fw_variant_); + if(!client_.sendMessage(feature_in)) return -1; // update feature configuration - msp::msg::SetFeature feature_out; + msp::msg::SetFeature feature_out(fw_variant_); feature_out.features = feature_in.features; // enable features for(const std::string &a : add) { @@ -205,27 +293,15 @@ int FlightController::updateFeatures(const std::set &add, } // check if feature configuration changed - if(feature_out.features==feature_in.features) - return 0; + if(feature_out.features == feature_in.features) return 0; - if(!client.respond(feature_out)) - return -1; + if(!client_.sendMessage(feature_out)) return -1; // make settings permanent and reboot - if(!writeEEPROM()) - return -1; - if(!reboot()) - return -1; + if(!saveSettings()) return -1; + if(!reboot()) return -1; return 1; } -bool FlightController::reboot() { - return client.respond(msp::msg::Reboot()); -} - -bool FlightController::writeEEPROM() { - return client.respond(msp::msg::WriteEEPROM()); -} - -} // namespace msp +} // namespace fcu diff --git a/src/MSP.cpp b/src/MSP.cpp deleted file mode 100644 index 108d4ce..0000000 --- a/src/MSP.cpp +++ /dev/null @@ -1,316 +0,0 @@ -#include "MSP.hpp" -#include "SerialPortImpl.cpp" - -#include -#include -#include - -namespace msp { - -MSP::MSP() : pimpl(new SerialPortImpl), wait(10) { } - -MSP::MSP(const std::string &device, const size_t baudrate) : pimpl(new SerialPortImpl), wait(10) { - connect(device, baudrate); -} - -MSP::~MSP() { } - -bool MSP::connect(const std::string &device, const size_t baudrate) { - this->device = device; - try { - pimpl->port.open(device); - } - catch(const asio::system_error &e) { - throw NoConnection(device, e.what()); - } - - pimpl->port.set_option(asio::serial_port::baud_rate(uint(baudrate))); - pimpl->port.set_option(asio::serial_port::parity(asio::serial_port::parity::none)); - pimpl->port.set_option(asio::serial_port::character_size(asio::serial_port::character_size(8))); - pimpl->port.set_option(asio::serial_port::stop_bits(asio::serial_port::stop_bits::one)); - - // clear buffer for new session - clear(); - - std::cout<<"Connected to: "<=int(FRAME_SIZE+min_payload_size)) { - DataID pkg = receiveData(); - success = (pkg.id==uint8_t(request.id())); - if(success) - request.decode(pkg.data); - } - } - catch(const MalformedHeader &e) { - std::cerr<', uint8_t(com_state)); - } - } - - // read data size - const uint8_t data_size = read(); - - // get ID of msg - const uint8_t id = read(); - - // read payload data - const ByteVector data = read(data_size); - - // check CRC - const uint8_t rcv_crc = read(); - const uint8_t exp_crc = crc(id, data); - - if(rcv_crc!=exp_crc) - throw WrongCRC(id, exp_crc, rcv_crc); - - return DataID(data,id); -} - -uint8_t MSP::crc(const uint8_t id, const ByteVector &data) { - uint8_t crc = uint8_t(data.size())^id; - - for(uint8_t d : data) - crc = crc^d; - - return crc; -} - -bool MSP::write(const std::vector &data) { - std::lock_guard lock(lock_write); - try { - const std::size_t bytes_written = asio::write(pimpl->port, asio::buffer(data.data(), data.size())); - return (bytes_written==data.size()); - } - catch(const asio::system_error &e) { - throw NoConnection(device, e.what()); - } -} - -size_t MSP::read(std::vector &data) { - std::lock_guard lock(lock_read); - return asio::read(pimpl->port, asio::buffer(data.data(), data.size())); -} - -std::vector MSP::read(std::size_t n_bytes) { - std::vector data(n_bytes); -#if !defined(NDEBUG) - const size_t nread = -#endif - read(data); - assert(nread==n_bytes); - return data; -} - -int MSP::hasData() { -#if __unix__ || __APPLE__ - int available_bytes; - if(ioctl(pimpl->port.native_handle(), FIONREAD, &available_bytes)!=-1) { - return available_bytes; - } - else { - return -1; - } -#elif _WIN32 - COMSTAT comstat; - if (ClearCommError(pimpl->port.native_handle(), NULL, &comstat) == true) { - return comstat.cbInQue; - } - else { - return -1; - } -#else -#warning "hasData() will be unimplemented" -#endif -} - -void MSP::clear() { -#if __unix__ || __APPLE__ - tcflush(pimpl->port.native_handle(),TCIOFLUSH); -#elif _WIN32 - PurgeComm(pimpl->port.native_handle(), PURGE_TXCLEAR); -#else -#warning "clear() will be unimplemented" -#endif -} - -} // namespace msp diff --git a/src/PeriodicTimer.cpp b/src/PeriodicTimer.cpp new file mode 100644 index 0000000..9a43f1f --- /dev/null +++ b/src/PeriodicTimer.cpp @@ -0,0 +1,63 @@ +#include "PeriodicTimer.hpp" + +namespace msp { + +PeriodicTimer::PeriodicTimer(std::function funct, + const double period_seconds) : + funct(funct), + running_(ATOMIC_FLAG_INIT) { + period_us = + std::chrono::duration(size_t(period_seconds * 1e6)); +} + +bool PeriodicTimer::start() { + // only start thread if period is above 0 + if(!(period_us.count() > 0)) return false; + // only start once + if(running_.test_and_set()) return false; + // lock mutex so that the try_lock_until in the new thread always times out + // and loops + mutex_timer.lock(); + // start the thread + thread_ptr = std::shared_ptr(new std::thread([this] { + // log now. + tstart = std::chrono::steady_clock::now(); + while(true) { + // call function + funct(); + // increment the reference time to know when to timeout waiting + tstart += period_us; + // wait until end of period or stop is called + if(mutex_timer.try_lock_until(tstart)) { + // gets here if lock was acquired (means someone called stop and + // manually unlocked the mutex) + mutex_timer.unlock(); + break; + } + } // function over, return + })); + return true; +} + +bool PeriodicTimer::stop() { + bool rc = false; + if(running_.test_and_set()) { + // was already running, so let the thread finish + mutex_timer.unlock(); + if(thread_ptr != nullptr && thread_ptr->joinable()) { + thread_ptr->join(); + } + rc = true; + } + running_.clear(); + return rc; +} + +void PeriodicTimer::setPeriod(const double& period_seconds) { + stop(); + period_us = + std::chrono::duration(size_t(period_seconds * 1e6)); + start(); +} + +} // namespace msp diff --git a/src/SerialPortImpl.cpp b/src/SerialPortImpl.cpp deleted file mode 100644 index fe24090..0000000 --- a/src/SerialPortImpl.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include - -struct msp::SerialPortImpl { - SerialPortImpl() : port(io) { } - - asio::io_service io; /// - -std::ostream& operator<<(std::ostream& s, const msp::msg::ApiVersion& api_version) { - s << "#Api Version:" << std::endl; - s << "API: " << api_version.major << "." << api_version.minor << std::endl; - s << "Protocol: " << api_version.protocol << std::endl; - return s; -} - -std::ostream& operator<<(std::ostream& s, const msp::msg::FcVariant& fc_variant) { - s << "#FC variant:" << std::endl; - s << "Identifier: " << fc_variant.identifier << std::endl; - return s; -} - -std::ostream& operator<<(std::ostream& s, const msp::msg::FcVersion& fc_version) { - s << "#FC version:" << std::endl; - s << "Version: " << fc_version.major << "." << fc_version.minor << "." << fc_version.patch_level << std::endl; - return s; -} - -std::ostream& operator<<(std::ostream& s, const msp::msg::BoardInfo& board_info) { - s << "#Board Info:" << std::endl; - s << "Identifier: " << board_info.identifier << std::endl; - s << "Version: " << board_info.version << std::endl; - s << "Type: " << size_t(board_info.type) << std::endl; - return s; -} - -std::ostream& operator<<(std::ostream& s, const msp::msg::BuildInfo& build_info) { - s << "#Build Info:" << std::endl; - s << "Date: " << build_info.buildDate << std::endl; - s << "Time: " << build_info.buildTime << std::endl; - s << "Git revision: " << build_info.shortGitRevision << std::endl; - return s; -} - -std::ostream& operator<<(std::ostream& s, const msp::msg::Feature& feature) { - s << "#Features:" << std::endl; - for(const std::string &f : feature.features) { - s << f << std::endl; - } - return s; -} - -std::ostream& operator<<(std::ostream& s, const msp::msg::RxMap& rx_map) { - s << "#Channel mapping:" << std::endl; - for(size_t i(0); i +#include "gtest/gtest.h" + +namespace msp { + +// The fixture for testing class Foo. +template class ByteVectorBasicTest : public ::testing::Test {}; +typedef ::testing::Types + basicTypes; +TYPED_TEST_CASE(ByteVectorBasicTest, basicTypes); + +template class ByteVectorScaledTest : public ::testing::Test {}; +typedef ::testing::Types + scaleOutputTypes; +TYPED_TEST_CASE(ByteVectorScaledTest, scaleOutputTypes); + +// Tests that the Foo::Bar() method does Abc. +TEST(ByteVectorBasicTest, Initialzation) { + ByteVector b; + EXPECT_EQ(std::size_t(0), b.size()); + EXPECT_EQ(std::size_t(0), b.unpacking_offset()); + EXPECT_EQ(std::size_t(0), b.unpacking_remaining()); +} + +TYPED_TEST(ByteVectorBasicTest, Pack1zero) { + ByteVector b; + TypeParam ref = 0; + EXPECT_TRUE(b.pack(ref)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(ref)); + EXPECT_EQ(TypeParam(0), ref); +} + +TYPED_TEST(ByteVectorBasicTest, Pack1one) { + ByteVector b; + TypeParam ref = 1; + EXPECT_TRUE(b.pack(ref)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(ref)); + EXPECT_EQ(TypeParam(1), ref); +} + +TYPED_TEST(ByteVectorBasicTest, Pack1max) { + ByteVector b; + TypeParam ref = std::numeric_limits::max(); + EXPECT_TRUE(b.pack(ref)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(ref)); + EXPECT_EQ(std::numeric_limits::max(), ref); +} + +TYPED_TEST(ByteVectorBasicTest, Pack1min) { + ByteVector b; + TypeParam ref = std::numeric_limits::min(); + EXPECT_TRUE(b.pack(ref)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(ref)); + EXPECT_EQ(std::numeric_limits::min(), ref); +} + +TYPED_TEST(ByteVectorBasicTest, Pack1valzero) { + ByteVector b; + Value v; + TypeParam ref = 0; + v = ref; + EXPECT_TRUE(b.pack(v)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(v)); + EXPECT_EQ(ref, v()); +} + +TYPED_TEST(ByteVectorBasicTest, Pack1valone) { + ByteVector b; + Value v; + TypeParam ref = 1; + v = ref; + EXPECT_TRUE(b.pack(v)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(v)); + EXPECT_EQ(ref, v()); +} + +TYPED_TEST(ByteVectorBasicTest, Pack1valmax) { + ByteVector b; + Value v; + TypeParam ref = std::numeric_limits::max(); + v = ref; + EXPECT_TRUE(b.pack(v)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(v)); + EXPECT_EQ(ref, v()); +} + +TYPED_TEST(ByteVectorBasicTest, Pack1valmin) { + ByteVector b; + Value v; + TypeParam ref = std::numeric_limits::min(); + v = ref; + EXPECT_TRUE(b.pack(v)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(v)); + EXPECT_EQ(ref, v()); +} + +TYPED_TEST(ByteVectorBasicTest, Pack10zero) { + ByteVector b; + TypeParam ref = 0; + for(int i = 0; i < 10; ++i) EXPECT_TRUE(b.pack(ref)); + EXPECT_EQ(10 * sizeof(TypeParam), b.size()); + for(int i = 0; i < 10; ++i) { + EXPECT_TRUE(b.unpack(ref)); + EXPECT_EQ(TypeParam(0), ref); + } +} + +TYPED_TEST(ByteVectorBasicTest, Pack10one) { + ByteVector b; + TypeParam ref = 1; + for(int i = 0; i < 10; ++i) EXPECT_TRUE(b.pack(ref)); + EXPECT_EQ(10 * sizeof(TypeParam), b.size()); + for(int i = 0; i < 10; ++i) { + EXPECT_TRUE(b.unpack(ref)); + EXPECT_EQ(TypeParam(1), ref); + } +} + +TYPED_TEST(ByteVectorBasicTest, Pack10max) { + ByteVector b; + TypeParam ref = std::numeric_limits::max(); + for(int i = 0; i < 10; ++i) EXPECT_TRUE(b.pack(ref)); + EXPECT_EQ(10 * sizeof(TypeParam), b.size()); + for(int i = 0; i < 10; ++i) { + EXPECT_TRUE(b.unpack(ref)); + EXPECT_EQ(std::numeric_limits::max(), ref); + } +} + +TYPED_TEST(ByteVectorBasicTest, Pack10min) { + ByteVector b; + TypeParam ref = std::numeric_limits::min(); + for(int i = 0; i < 10; ++i) EXPECT_TRUE(b.pack(ref)); + EXPECT_EQ(10 * sizeof(TypeParam), b.size()); + for(int i = 0; i < 10; ++i) { + EXPECT_TRUE(b.unpack(ref)); + EXPECT_EQ(std::numeric_limits::min(), ref); + } +} + +TYPED_TEST(ByteVectorBasicTest, Pack10valzero) { + ByteVector b; + Value v; + TypeParam ref = 0; + v = ref; + for(int i = 0; i < 10; ++i) EXPECT_TRUE(b.pack(v)); + EXPECT_EQ(10 * sizeof(TypeParam), b.size()); + for(int i = 0; i < 10; ++i) { + EXPECT_TRUE(b.unpack(v)); + EXPECT_EQ(ref, v()); + } +} + +TYPED_TEST(ByteVectorBasicTest, Pack10valone) { + ByteVector b; + Value v; + TypeParam ref = 1; + v = ref; + for(int i = 0; i < 10; ++i) EXPECT_TRUE(b.pack(v)); + EXPECT_EQ(10 * sizeof(TypeParam), b.size()); + for(int i = 0; i < 10; ++i) { + EXPECT_TRUE(b.unpack(v)); + EXPECT_EQ(ref, v()); + } +} + +TYPED_TEST(ByteVectorBasicTest, Pack10valmax) { + ByteVector b; + Value v; + TypeParam ref = std::numeric_limits::max(); + v = ref; + for(int i = 0; i < 10; ++i) EXPECT_TRUE(b.pack(v)); + EXPECT_EQ(10 * sizeof(TypeParam), b.size()); + for(int i = 0; i < 10; ++i) { + EXPECT_TRUE(b.unpack(v)); + EXPECT_EQ(ref, v()); + } +} + +TYPED_TEST(ByteVectorBasicTest, Pack10valmin) { + ByteVector b; + Value v; + TypeParam ref = std::numeric_limits::min(); + v = ref; + for(int i = 0; i < 10; ++i) EXPECT_TRUE(b.pack(v)); + EXPECT_EQ(10 * sizeof(TypeParam), b.size()); + for(int i = 0; i < 10; ++i) { + EXPECT_TRUE(b.unpack(v)); + EXPECT_EQ(ref, v()); + } +} + +TYPED_TEST(ByteVectorScaledTest, Pack1DoubleZero) { + ByteVector b; + double ref = 0.0; + EXPECT_TRUE(b.pack(ref, 1)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(ref, 1)); + EXPECT_DOUBLE_EQ(0, ref); +} + +TYPED_TEST(ByteVectorScaledTest, Pack1DoubleTen) { + ByteVector b; + double ref = 10.0; + EXPECT_TRUE(b.pack(ref, 1)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(ref, 1)); + EXPECT_DOUBLE_EQ(10.f, ref); +} + +TYPED_TEST(ByteVectorScaledTest, Pack1DoubleSaturationMax) { + ByteVector b; + double ref = (double)std::numeric_limits::max() + 1.0; + EXPECT_TRUE(b.pack(ref, 1)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(ref, 1)); + EXPECT_DOUBLE_EQ(std::numeric_limits::max(), ref); +} + +TYPED_TEST(ByteVectorScaledTest, Pack1DoubleSaturationMin) { + ByteVector b; + double ref = (double)std::numeric_limits::min() - 1.0; + EXPECT_TRUE(b.pack(ref, 1)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(ref, 1)); + EXPECT_DOUBLE_EQ(std::numeric_limits::min(), ref); +} + +TYPED_TEST(ByteVectorScaledTest, Pack1FloatZero) { + ByteVector b; + float ref = 0.0; + EXPECT_TRUE(b.pack(ref, 1)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(ref, 1)); + EXPECT_FLOAT_EQ(0.f, ref); +} + +TYPED_TEST(ByteVectorScaledTest, Pack1FloatTen) { + ByteVector b; + float ref = 10.0; + EXPECT_TRUE(b.pack(ref, 1)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(ref, 1)); + EXPECT_FLOAT_EQ(10.f, ref); +} + +TYPED_TEST(ByteVectorScaledTest, Pack1FloatSaturationMax) { + ByteVector b; + float ref = (float)std::numeric_limits::max() + 1.0; + EXPECT_TRUE(b.pack(ref, 1)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(ref, 1)); + EXPECT_FLOAT_EQ(std::numeric_limits::max(), ref); +} + +TYPED_TEST(ByteVectorScaledTest, Pack1FloatSaturationMin) { + ByteVector b; + float ref = (float)std::numeric_limits::min() - 1.0; + EXPECT_TRUE(b.pack(ref, 1)); + EXPECT_EQ(sizeof(TypeParam), b.size()); + EXPECT_TRUE(b.unpack(ref, 1)); + EXPECT_FLOAT_EQ(std::numeric_limits::min(), ref); +} + +} // namespace msp + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/value_test.cpp b/test/value_test.cpp new file mode 100644 index 0000000..f2f098a --- /dev/null +++ b/test/value_test.cpp @@ -0,0 +1,125 @@ +#include "Value.hpp" +#include +#include "ByteVector.hpp" +#include "gtest/gtest.h" + +namespace msp { + +// The fixture for testing class Foo. +template class valueTest : public ::testing::Test { +protected: + // You can remove any or all of the following functions if its body + // is empty. + + valueTest() { + // You can do set-up work for each test here. + } + + virtual ~valueTest() { + // You can do clean-up work that doesn't throw exceptions here. + } + + // If the constructor and destructor are not enough for setting up + // and cleaning up each test, you can define the following methods: + + virtual void SetUp() { + // Code here will be called immediately after the constructor (right + // before each test). + } + + virtual void TearDown() { + // Code here will be called immediately after each test (right + // before the destructor). + } + + // Objects declared here can be used by all tests in the test case for Foo. +}; + +typedef ::testing::Types + numTypes; +TYPED_TEST_CASE(valueTest, numTypes); + +// Tests that the Foo::Bar() method does Abc. +TYPED_TEST(valueTest, Initialzation) { + Value v; + EXPECT_EQ(TypeParam(0), v()); + EXPECT_FALSE(v.set()); +} + +TYPED_TEST(valueTest, AssignmentZero) { + Value v1, v2; + + TypeParam ref = 0; + v1 = ref; + EXPECT_EQ(ref, v1()); + EXPECT_TRUE(v1.set()); + v2 = v1; + EXPECT_EQ(ref, v2()); + EXPECT_TRUE(v2.set()); +} + +TYPED_TEST(valueTest, AssignmentMax) { + Value v1, v2; + + TypeParam ref = std::numeric_limits::max(); + ; + v1 = ref; + EXPECT_EQ(ref, v1()); + EXPECT_TRUE(v1.set()); + v2 = v1; + EXPECT_EQ(ref, v2()); + EXPECT_TRUE(v2.set()); +} + +TYPED_TEST(valueTest, AssignmentMin) { + Value v1, v2; + + TypeParam ref = std::numeric_limits::min(); + ; + v1 = ref; + EXPECT_EQ(ref, v1()); + EXPECT_TRUE(v1.set()); + v2 = v1; + EXPECT_EQ(ref, v2()); + EXPECT_TRUE(v2.set()); +} + +TEST(valueTest, stringInit) { + Value v; + EXPECT_EQ("", v()); + EXPECT_EQ(false, v.set()); +} + +TEST(valueTest, stringAssign) { + Value v1, v2; + v1 = std::string("test"); + EXPECT_EQ("test", v1()); + EXPECT_EQ(true, v1.set()); + v2 = v1; + EXPECT_EQ("test", v2()); + EXPECT_EQ(true, v2.set()); +} + +TEST(valueTest, ByteVecInit) { + Value v; + EXPECT_EQ(true, v().empty()); + EXPECT_EQ(false, v.set()); +} + +TEST(valueTest, ByteVecAssign) { + Value v1, v2; + v1 = ByteVector(1, 1); + EXPECT_EQ(1, v1()[0]); + EXPECT_EQ(true, v1.set()); + v2 = v1; + EXPECT_EQ(1, v2()[0]); + EXPECT_EQ(true, v2.set()); +} + +} // namespace msp + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}