From 815a339684cae5cbfd3cdf538401166dc598ae74 Mon Sep 17 00:00:00 2001 From: Li-Tianming <87600827+Li-Tianming@users.noreply.github.com> Date: Sat, 17 May 2025 03:33:40 +0800 Subject: [PATCH] MSP_OSD code changes to support OpenIPC, DJI O3/O4 OSD rendering (#24695) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Change MSP_OSD message content and rendering process * Finish MSP_OSD battery message construct * Finish MSP_OSD `display message` construct HOL|DSAM|N * Finish MSP_FC_VARIANT(0x02) message BTFL * Finish MSP_OSD RSSI message 📶10% * Finish MSP_OSD GPS message 🛰 10 🌐000.000000 🌐00.000000 * Finish MSP_OSD PITCH ROLL message 🔃-10.5 🔁13.2 * Change struct filed name * Change OSD message postion * Finish MSP_OSD PITCH Altitude message 🔝15.2 * Finish MSP_OSD distanceToHome message 🏠5000 * Add Hide/Show option for ALT and homeDist * Format the code by `make format` * Clean up stray text * Remove other commented out dead code * Change `sprintf()` to `snprintf()` * Add msg field comment in `display_message` construct * Init str buffer to 0, Change refresh rate back to 100ms * Explicit conversion float to double --------- Co-authored-by: Li.Tianming Co-authored-by: Li.Tianming --- boards/hkust/nxt-dual/default.px4board | 1 + .../msp_osd/MessageDisplay/MessageDisplay.cpp | 7 + src/drivers/osd/msp_osd/MspV1.cpp | 27 ++- src/drivers/osd/msp_osd/MspV1.hpp | 1 + src/drivers/osd/msp_osd/msp_defines.h | 92 +++++++++ src/drivers/osd/msp_osd/msp_osd.cpp | 137 ++++++++----- src/drivers/osd/msp_osd/msp_osd.hpp | 1 + src/drivers/osd/msp_osd/uorb_to_msp.cpp | 193 ++++++++++++++++++ src/drivers/osd/msp_osd/uorb_to_msp.hpp | 20 ++ 9 files changed, 428 insertions(+), 51 deletions(-) diff --git a/boards/hkust/nxt-dual/default.px4board b/boards/hkust/nxt-dual/default.px4board index cab44e7812..4327646414 100644 --- a/boards/hkust/nxt-dual/default.px4board +++ b/boards/hkust/nxt-dual/default.px4board @@ -17,6 +17,7 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y diff --git a/src/drivers/osd/msp_osd/MessageDisplay/MessageDisplay.cpp b/src/drivers/osd/msp_osd/MessageDisplay/MessageDisplay.cpp index 3b431cb357..61ae0ce6f0 100644 --- a/src/drivers/osd/msp_osd/MessageDisplay/MessageDisplay.cpp +++ b/src/drivers/osd/msp_osd/MessageDisplay/MessageDisplay.cpp @@ -56,6 +56,13 @@ void MessageDisplay::set(const MessageDisplayType mode, const char *string) if (strcmp(flight_mode_msg, string) != 0) { flight_mode_msg[MSG_BUFFER_SIZE - 1] = '\0'; strncpy(flight_mode_msg, string, MSG_BUFFER_SIZE - 1); + + for (int i = 0; i < (MSG_BUFFER_SIZE - 1); ++i) { + if (flight_mode_msg[i] >= 'a' && flight_mode_msg[i] <= 'z') { + flight_mode_msg[i] = flight_mode_msg[i] - 32; // toupper + } + } + updated_ = true; } diff --git a/src/drivers/osd/msp_osd/MspV1.cpp b/src/drivers/osd/msp_osd/MspV1.cpp index c6b4d3fa56..ae13e408eb 100644 --- a/src/drivers/osd/msp_osd/MspV1.cpp +++ b/src/drivers/osd/msp_osd/MspV1.cpp @@ -107,7 +107,32 @@ bool MspV1::Send(const uint8_t message_id, const void *payload) packet[0] = '$'; packet[1] = 'M'; - packet[2] = '<'; + packet[2] = '>'; + packet[3] = payload_size; + packet[4] = message_id; + + crc = payload_size ^ message_id; + + memcpy(packet + MSP_FRAME_START_SIZE, payload, payload_size); + + for (uint32_t i = 0; i < payload_size; i ++) { + crc ^= packet[MSP_FRAME_START_SIZE + i]; + } + + packet[MSP_FRAME_START_SIZE + payload_size] = crc; + + int packet_size = MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE; + return write(_fd, packet, packet_size) == packet_size; +} + +bool MspV1::Send(const uint8_t message_id, const void *payload, uint32_t payload_size) +{ + uint8_t packet[MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE]; + uint8_t crc; + + packet[0] = '$'; + packet[1] = 'M'; + packet[2] = '>'; packet[3] = payload_size; packet[4] = message_id; diff --git a/src/drivers/osd/msp_osd/MspV1.hpp b/src/drivers/osd/msp_osd/MspV1.hpp index 6c6ad524ef..a94b21bab5 100644 --- a/src/drivers/osd/msp_osd/MspV1.hpp +++ b/src/drivers/osd/msp_osd/MspV1.hpp @@ -39,6 +39,7 @@ public: MspV1(int fd); int GetMessageSize(int message_type); bool Send(const uint8_t message_id, const void *payload); + bool Send(const uint8_t message_id, const void *payload, uint32_t payload_size); private: int _fd{-1}; diff --git a/src/drivers/osd/msp_osd/msp_defines.h b/src/drivers/osd/msp_osd/msp_defines.h index fb2e83a7cc..317c0b51f2 100644 --- a/src/drivers/osd/msp_osd/msp_defines.h +++ b/src/drivers/osd/msp_osd/msp_defines.h @@ -111,6 +111,7 @@ #define MSP_MODE_TURNASSIST 27 #define MSP_MODE_NAVLAUNCH 28 #define MSP_MODE_AUTOTRIM 29 +#define MSP_CMD_DISPLAYPORT 182 struct msp_esc_sensor_data_t { uint8_t motor_count; @@ -288,6 +289,25 @@ struct msp_attitude_t { int16_t yaw; } __attribute__((packed)); +struct msp_rendor_pitch_t { + uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed + uint8_t screenYPosition; + uint8_t screenXPosition; + uint8_t iconAttrs = 0x00; + uint8_t iconIndex = 0x15; //PITCH icon + + char str[6]; // -00.0 +} __attribute__((packed)); + +struct msp_rendor_roll_t { + uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed + uint8_t screenYPosition; + uint8_t screenXPosition; + uint8_t iconAttrs = 0x00; + uint8_t iconIndex = 0x14; //ROLL icon + + char str[6]; // -00.0 +} __attribute__((packed)); // MSP_ALTITUDE reply struct msp_altitude_t { @@ -297,6 +317,17 @@ struct msp_altitude_t { } __attribute__((packed)); +struct msp_rendor_altitude_t { + uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed + uint8_t screenYPosition; + uint8_t screenXPosition; + uint8_t iconAttrs = 0x00; + uint8_t iconIndex = 0x7F; //ALT icon + + char str[8]; // -0000.0 // 9999.9 meter +} __attribute__((packed)); + + // MSP_SONAR_ALTITUDE reply struct msp_sonar_altitude_t { int32_t altitude; @@ -311,6 +342,16 @@ struct msp_analog_t { int16_t amperage; // send amperage in 0.01 A steps, range is -320A to 320A } __attribute__((packed)); +struct msp_rendor_rssi_t { + uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed + uint8_t screenYPosition; + uint8_t screenXPosition; + uint8_t iconAttrs = 0x00; + uint8_t iconIndex = 0x01; //RSSI icon + + char str[4]; // 100% +} __attribute__((packed)); + // MSP_ARMING_CONFIG reply struct msp_arming_config_t { @@ -392,6 +433,38 @@ struct msp_raw_gps_t { uint16_t hdop; } __attribute__((packed)); +struct msp_rendor_latitude_t { + uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed + uint8_t screenYPosition; + uint8_t screenXPosition; + uint8_t iconAttrs = 0x00; + uint8_t iconIndex = 0x89; //LAT icon + + char str[11]; // -00.0000000 +} __attribute__((packed)); + + +struct msp_rendor_longitude_t { + uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed + uint8_t screenYPosition; + uint8_t screenXPosition; + uint8_t iconAttrs = 0x00; + uint8_t iconIndex = 0x98; //LON icon + + char str[12]; // -000.0000000 +} __attribute__((packed)); + +struct msp_rendor_satellites_used_t { + uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed + uint8_t screenYPosition; + uint8_t screenXPosition; + uint8_t iconAttrs = 0x00; + uint8_t iconIndex = 0x1E; //satellites icon + uint8_t iconIndex2 = 0x1F; //satellites icon + + char str[2]; // 99 +} __attribute__((packed)); + // MSP_COMP_GPS reply struct msp_comp_gps_t { @@ -400,6 +473,16 @@ struct msp_comp_gps_t { uint8_t heartbeat; // toggles 0 and 1 for each change } __attribute__((packed)); +struct msp_rendor_distanceToHome_t { + uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed + uint8_t screenYPosition; + uint8_t screenXPosition; + uint8_t iconAttrs = 0x00; // + uint8_t iconIndex = 0x71; //distanceToHome icon + + char str[6]; // 65536 +} __attribute__((packed)); + // values for msp_nav_status_t.mode #define MSP_NAV_STATUS_MODE_NONE 0 @@ -788,6 +871,15 @@ struct msp_battery_state_t { uint16_t batteryVoltage; } __attribute__((packed)); +struct msp_rendor_battery_state_t { + uint8_t subCommand; // 0x03 write string. fixed + uint8_t screenYPosition; + uint8_t screenXPosition; + uint8_t iconAttrs; + uint8_t iconIndex; + char str[5]; +} __attribute__((packed)); + // MSP_STATUS reply customized for BF/DJI struct msp_status_BF_t { uint16_t task_delta_time; diff --git a/src/drivers/osd/msp_osd/msp_osd.cpp b/src/drivers/osd/msp_osd/msp_osd.cpp index e6e7f236f7..25b9aa7c96 100644 --- a/src/drivers/osd/msp_osd/msp_osd.cpp +++ b/src/drivers/osd/msp_osd/msp_osd.cpp @@ -111,6 +111,21 @@ const uint16_t osd_current_draw_pos = 2103; const uint16_t osd_numerical_vario_pos = LOCATION_HIDDEN; +#define OSD_GRID_COL_MAX (59) // From betaflight-configurator OSD tab +#define OSD_GRID_ROW_MAX (21) // From betaflight-configurator OSD tab + +typedef enum { + MSP_DP_HEARTBEAT = 0, // Release the display after clearing and updating + MSP_DP_RELEASE = 1, // Release the display after clearing and updating + MSP_DP_CLEAR_SCREEN = 2, // Clear the display + MSP_DP_WRITE_STRING = 3, // Write a string at given coordinates + MSP_DP_DRAW_SCREEN = 4, // Trigger a screen draw + MSP_DP_OPTIONS = 5, // Not used by Betaflight. Reserved by Ardupilot and INAV + MSP_DP_SYS = 6, // Display system element displayportSystemElement_e at given coordinates + MSP_DP_COUNT, +} displayportMspSubCommand; + + MspOsd::MspOsd(const char *device) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) @@ -224,7 +239,7 @@ void MspOsd::SendConfig() _msp.Send(MSP_OSD_CONFIG, &msp_osd_config); } - +// extract it to MSPOSD_BF_Run() and MSPOSD_DJIFPV_Run() for compatibility? void MspOsd::Run() { if (should_exit()) { @@ -270,6 +285,12 @@ void MspOsd::Run() return; } + uint8_t subcmd = MSP_DP_HEARTBEAT; + this->Send(MSP_CMD_DISPLAYPORT, &subcmd, 1); + + subcmd = MSP_DP_CLEAR_SCREEN; + this->Send(MSP_CMD_DISPLAYPORT, &subcmd, 1); + // update display message { vehicle_status_s vehicle_status{}; @@ -280,43 +301,39 @@ void MspOsd::Run() log_message_s log_message{}; _log_message_sub.copy(&log_message); - + // TODO re-wirte this function? const auto display_message = msp_osd::construct_display_message( vehicle_status, vehicle_attitude, log_message, _param_osd_log_level.get(), _display); - this->Send(MSP_NAME, &display_message); + + char msg[sizeof(msp_name_t) + 5] = {0}; + int index = 0; + msg[index++] = MSP_DP_WRITE_STRING; + msg[index++] = 0x02; // row position + msg[index++] = 0x14; // colum position + msg[index++] = 0; // Icon attr + msg[index++] = 0x03; // Icon index > + memcpy(&msg[index++], &display_message, sizeof(msp_name_t)); + this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msg)); } // MSP_FC_VARIANT { const auto msg = msp_osd::construct_FC_VARIANT(); - this->Send(MSP_FC_VARIANT, &msg); - } - - // MSP_STATUS - { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); - - const auto msg = msp_osd::construct_STATUS(vehicle_status); - this->Send(MSP_STATUS, &msg); + this->Send(MSP_FC_VARIANT, &msg, sizeof(msg)); } // MSP_ANALOG { - battery_status_s battery_status{}; - _battery_status_sub.copy(&battery_status); - - input_rc_s input_rc{}; - _input_rc_sub.copy(&input_rc); - - const auto msg = msp_osd::construct_ANALOG( - battery_status, - input_rc); - this->Send(MSP_ANALOG, &msg); + if (enabled(SymbolIndex::RSSI_VALUE)) { + input_rc_s input_rc{}; + _input_rc_sub.copy(&input_rc); + const auto msg = msp_osd::construct_rendor_RSSI(input_rc); + this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_rssi_t)); + } } // MSP_BATTERY_STATE @@ -324,8 +341,9 @@ void MspOsd::Run() battery_status_s battery_status{}; _battery_status_sub.copy(&battery_status); - const auto msg = msp_osd::construct_BATTERY_STATE(battery_status); - this->Send(MSP_BATTERY_STATE, &msg); + const auto msg = msp_osd::construct_rendor_BATTERY_STATE(battery_status); + this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_battery_state_t)); + } // MSP_RAW_GPS @@ -333,32 +351,35 @@ void MspOsd::Run() sensor_gps_s vehicle_gps_position{}; _vehicle_gps_position_sub.copy(&vehicle_gps_position); - airspeed_validated_s airspeed_validated{}; - _airspeed_validated_sub.copy(&airspeed_validated); + if (enabled(SymbolIndex::GPS_LAT)) { + const auto msg = msp_osd::construct_rendor_GPS_LAT(vehicle_gps_position); + this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_latitude_t)); + } - const auto msg = msp_osd::construct_RAW_GPS( - vehicle_gps_position, - airspeed_validated); - this->Send(MSP_RAW_GPS, &msg); + if (enabled(SymbolIndex::GPS_LON)) { + const auto msg = msp_osd::construct_rendor_GPS_LON(vehicle_gps_position); + this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_longitude_t)); + } + + if (enabled(SymbolIndex::GPS_SATS)) { + const auto msg = msp_osd::construct_rendor_GPS_NUM(vehicle_gps_position); + this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_satellites_used_t)); + } } // MSP_COMP_GPS { - // update heartbeat - _heartbeat = !_heartbeat; - home_position_s home_position{}; _home_position_sub.copy(&home_position); vehicle_global_position_s vehicle_global_position{}; _vehicle_global_position_sub.copy(&vehicle_global_position); - // construct and send message - const auto msg = msp_osd::construct_COMP_GPS( - home_position, - vehicle_global_position, - _heartbeat); - this->Send(MSP_COMP_GPS, &msg); + if (enabled(SymbolIndex::HOME_DIST)) { + const auto msg = msp_osd::construct_rendor_distanceToHome(home_position, vehicle_global_position); + + this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_distanceToHome_t)); + } } // MSP_ATTITUDE @@ -366,10 +387,17 @@ void MspOsd::Run() vehicle_attitude_s vehicle_attitude{}; _vehicle_attitude_sub.copy(&vehicle_attitude); - const auto msg = msp_osd::construct_ATTITUDE(vehicle_attitude); - this->Send(MSP_ATTITUDE, &msg); + { + const auto msg = msp_osd::construct_rendor_PITCH(vehicle_attitude); + this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_pitch_t)); + } + { + const auto msg = msp_osd::construct_rendor_ROLL(vehicle_attitude); + this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_roll_t)); + } } + // MSP_ALTITUDE { sensor_gps_s vehicle_gps_position{}; @@ -378,19 +406,19 @@ void MspOsd::Run() vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); - // construct and send message - const auto msg = msp_osd::construct_ALTITUDE(vehicle_gps_position, vehicle_local_position); - this->Send(MSP_ALTITUDE, &msg); + if (enabled(SymbolIndex::ALTITUDE)) { + const auto msg = msp_osd::construct_Rendor_ALTITUDE(vehicle_gps_position, vehicle_local_position); + + this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_altitude_t)); + } } // MSP_MOTOR_TELEMETRY { - const auto msg = msp_osd::construct_ESC_SENSOR_DATA(); - this->Send(MSP_ESC_SENSOR_DATA, &msg); - } - // send full configuration - SendConfig(); + } + subcmd = MSP_DP_DRAW_SCREEN; + this->Send(MSP_CMD_DISPLAYPORT, &subcmd, 1); } void MspOsd::Send(const unsigned int message_type, const void *payload) @@ -402,6 +430,15 @@ void MspOsd::Send(const unsigned int message_type, const void *payload) _performance_data.unsuccessful_sends++; } } +void MspOsd::Send(const unsigned int message_type, const void *payload, int32_t payload_size) +{ + if (_msp.Send(message_type, payload, payload_size)) { + _performance_data.successful_sends++; + + } else { + _performance_data.unsuccessful_sends++; + } +} void MspOsd::parameters_update() { diff --git a/src/drivers/osd/msp_osd/msp_osd.hpp b/src/drivers/osd/msp_osd/msp_osd.hpp index b35e7ff135..b39cdb60f6 100644 --- a/src/drivers/osd/msp_osd/msp_osd.hpp +++ b/src/drivers/osd/msp_osd/msp_osd.hpp @@ -123,6 +123,7 @@ private: // update a single display element in the display void Send(const unsigned int message_type, const void *payload); + void Send(const unsigned int message_type, const void *payload, int32_t payload_size); // send full configuration to MSP (triggers the actual update) void SendConfig(); diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.cpp b/src/drivers/osd/msp_osd/uorb_to_msp.cpp index 121b9ee778..e64842fb88 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.cpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.cpp @@ -51,6 +51,16 @@ using namespace time_literals; namespace msp_osd { +typedef enum { + MSP_DP_HEARTBEAT = 0, // Release the display after clearing and updating + MSP_DP_RELEASE = 1, // Release the display after clearing and updating + MSP_DP_CLEAR_SCREEN = 2, // Clear the display + MSP_DP_WRITE_STRING = 3, // Write a string at given coordinates + MSP_DP_DRAW_SCREEN = 4, // Trigger a screen draw + MSP_DP_OPTIONS = 5, // Not used by Betaflight. Reserved by Ardupilot and INAV + MSP_DP_SYS = 6, // Display system element displayportSystemElement_e at given coordinates + MSP_DP_COUNT, +} displayportMspSubCommand; msp_name_t construct_display_message(const vehicle_status_s &vehicle_status, const vehicle_attitude_s &vehicle_attitude, @@ -198,6 +208,24 @@ msp_analog_t construct_ANALOG(const battery_status_s &battery_status, const inpu return analog; } +msp_rendor_rssi_t construct_rendor_RSSI(const input_rc_s &input_rc) +{ + msp_rendor_rssi_t rssi; + rssi.screenYPosition = 0x02; + rssi.screenXPosition = 0x02; + + int len = snprintf(&rssi.str[0], sizeof(rssi.str) - 1, "%d", input_rc.link_quality); + + if (len >= 3) { + rssi.str[3] = '%'; + + } else { + rssi.str[len] = '%'; + } + + return rssi; +} + msp_battery_state_t construct_BATTERY_STATE(const battery_status_s &battery_status) { // initialize result @@ -222,6 +250,36 @@ msp_battery_state_t construct_BATTERY_STATE(const battery_status_s &battery_stat return battery_state; } +msp_rendor_battery_state_t construct_rendor_BATTERY_STATE(const battery_status_s &battery_status) +{ + // initialize result + msp_rendor_battery_state_t battery_state = {0}; + + battery_state.subCommand = MSP_DP_WRITE_STRING; // 3 write string. fixed + battery_state.screenYPosition = 0x04; + battery_state.screenXPosition = 0x02; + battery_state.iconAttrs = 0x00; + + float sigle_cell_v = battery_status.voltage_v / battery_status.cell_count; + + if (sigle_cell_v > 4.0f) { + battery_state.iconIndex = 0x91; // Full battery Icon + + } else if ((sigle_cell_v <= 4.0f) && (sigle_cell_v > 3.5f)) { + battery_state.iconIndex = 0x93; // Half battery Icon + + } else if ((sigle_cell_v <= 3.5f) && (sigle_cell_v > 3.2f)) { + battery_state.iconIndex = 0x95; // Empty battery Icon + + } else { + battery_state.iconIndex = 0x96; // Dead battery Icon + } + + snprintf(&battery_state.str[0], sizeof(battery_state.str), "%.1fV", (double)sigle_cell_v); + return battery_state; +} + + msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, const airspeed_validated_s &airspeed_validated) { @@ -279,6 +337,53 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, return raw_gps; } +msp_rendor_latitude_t construct_rendor_GPS_LAT(const sensor_gps_s &vehicle_gps_position) +{ + msp_rendor_latitude_t lat; + + lat.screenYPosition = 0x0A; + lat.screenXPosition = 0x29; + + if (vehicle_gps_position.fix_type >= 2) { + snprintf(&lat.str[0], sizeof(lat.str), "%.6f", vehicle_gps_position.latitude_deg); + + } else { + snprintf(&lat.str[0], sizeof(lat.str), "%.6f", 0.0); + } + + return lat; +} + +msp_rendor_longitude_t construct_rendor_GPS_LON(const sensor_gps_s &vehicle_gps_position) +{ + msp_rendor_longitude_t lon; + + lon.screenYPosition = 0x09; + lon.screenXPosition = 0x29; + + if (vehicle_gps_position.fix_type >= 2) { + snprintf(&lon.str[0], sizeof(lon.str), "%.6f", vehicle_gps_position.longitude_deg); + + } else { + snprintf(&lon.str[0], sizeof(lon.str), "%.6f", -0.0); + } + + return lon; +} + +msp_rendor_satellites_used_t construct_rendor_GPS_NUM(const sensor_gps_s &vehicle_gps_position) +{ + msp_rendor_satellites_used_t num; + + num.screenYPosition = 0x08; + num.screenXPosition = 0x29; + + snprintf(&num.str[0], sizeof(num.str), "%d", vehicle_gps_position.satellites_used); + + return num; +} + + msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, const vehicle_global_position_s &vehicle_global_position, const bool heartbeat) @@ -315,6 +420,34 @@ msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, return comp_gps; } +msp_rendor_distanceToHome_t construct_rendor_distanceToHome(const home_position_s &home_position, + const vehicle_global_position_s &vehicle_global_position) +{ + msp_rendor_distanceToHome_t distance; + + distance.screenYPosition = 0x08; + distance.screenXPosition = 0x02; + + int16_t dist_i = 0; + + if (home_position.valid_hpos + && home_position.valid_lpos + && (hrt_elapsed_time(&vehicle_global_position.timestamp) < 1_s)) { + + float distance_to_home = get_distance_to_next_waypoint(vehicle_global_position.lat, + vehicle_global_position.lon, + home_position.lat, home_position.lon); + + dist_i = (int16_t)distance_to_home; // meters + + } + + memset(&distance.str[0], 0, sizeof(distance.str)); + snprintf(&distance.str[0], sizeof(distance.str), "%d", dist_i); // 65536 + + return distance; +} + msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude) { // initialize results @@ -339,6 +472,43 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude) return attitude; } +msp_rendor_pitch_t construct_rendor_PITCH(const vehicle_attitude_s &vehicle_attitude) +{ + // initialize results + msp_rendor_pitch_t pit; + + pit.screenYPosition = 0x0D; + pit.screenXPosition = 0x29; + + // convert from quaternion to RPY + matrix::Eulerf euler_attitude(matrix::Quatf(vehicle_attitude.q)); + double pitch_deg = (double)math::degrees(euler_attitude.theta()); + // attitude.roll = math::degrees(euler_attitude.phi()) * 10; + + snprintf(&pit.str[0], sizeof(pit.str), "%.1f", pitch_deg); + + return pit; +} + +msp_rendor_roll_t construct_rendor_ROLL(const vehicle_attitude_s &vehicle_attitude) +{ + // initialize results + msp_rendor_roll_t roll; + + roll.screenYPosition = 0x0E; + roll.screenXPosition = 0x29; + + // convert from quaternion to RPY + matrix::Eulerf euler_attitude(matrix::Quatf(vehicle_attitude.q)); + // double pitch = (double)math::degrees(euler_attitude.theta()); + double roll_deg = (double)math::degrees(euler_attitude.phi()); + + snprintf(&roll.str[0], sizeof(roll.str), "%.1f", roll_deg); + + return roll; +} + + msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, const vehicle_local_position_s &vehicle_local_position) { @@ -362,6 +532,29 @@ msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, return altitude; } +msp_rendor_altitude_t construct_Rendor_ALTITUDE(const sensor_gps_s &vehicle_gps_position, + const vehicle_local_position_s &vehicle_local_position) +{ + msp_rendor_altitude_t altitude; + + altitude.screenYPosition = 0x06; + altitude.screenXPosition = 0x02; + + double alt; + + if (vehicle_gps_position.fix_type >= 2) { + alt = vehicle_gps_position.altitude_msl_m; + + } else { + alt = (double)(vehicle_local_position.z * -1.0f); + } + + memset(&altitude.str[0], 0, sizeof(altitude.str)); + snprintf(&altitude.str[0], sizeof(altitude.str), "%.1f", alt); + + return altitude; +} + msp_esc_sensor_data_dji_t construct_ESC_SENSOR_DATA() { // initialize result diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.hpp b/src/drivers/osd/msp_osd/uorb_to_msp.hpp index 1f5cdb3b6d..dfef00b4cc 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.hpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.hpp @@ -84,9 +84,13 @@ msp_status_BF_t construct_STATUS(const vehicle_status_s &vehicle_status); // construct an MSP_ANALOG struct msp_analog_t construct_ANALOG(const battery_status_s &battery_status, const input_rc_s &input_rc); +msp_rendor_rssi_t construct_rendor_RSSI(const input_rc_s &input_rc); + // construct an MSP_BATTERY_STATE struct msp_battery_state_t construct_BATTERY_STATE(const battery_status_s &battery_status); +msp_rendor_battery_state_t construct_rendor_BATTERY_STATE(const battery_status_s &battery_status); + // construct an MSP_RAW_GPS struct msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, const airspeed_validated_s &airspeed_validated); @@ -96,13 +100,29 @@ msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, const vehicle_global_position_s &vehicle_global_position, const bool heartbeat); +msp_rendor_latitude_t construct_rendor_GPS_LAT(const sensor_gps_s &vehicle_gps_position); + +msp_rendor_longitude_t construct_rendor_GPS_LON(const sensor_gps_s &vehicle_gps_position); + +msp_rendor_satellites_used_t construct_rendor_GPS_NUM(const sensor_gps_s &vehicle_gps_position); + // construct an MSP_ATTITUDE struct msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude); +msp_rendor_pitch_t construct_rendor_PITCH(const vehicle_attitude_s &vehicle_attitude); + +msp_rendor_roll_t construct_rendor_ROLL(const vehicle_attitude_s &vehicle_attitude); + // construct an MSP_ALTITUDE struct msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, const vehicle_local_position_s &vehicle_local_position); +msp_rendor_altitude_t construct_Rendor_ALTITUDE(const sensor_gps_s &vehicle_gps_position, + const vehicle_local_position_s &vehicle_local_position); + +msp_rendor_distanceToHome_t construct_rendor_distanceToHome(const home_position_s &home_position, + const vehicle_global_position_s &vehicle_global_position); + // construct an MSP_ESC_SENSOR_DATA struct msp_esc_sensor_data_dji_t construct_ESC_SENSOR_DATA();