forked from Archive/PX4-Autopilot
MSP OSD Revision
This commit is contained in:
Daniel M. Sahu
Daniel Agar
parent
770f8080c0
commit
779e738143
|
|
@ -24,7 +24,7 @@ CONFIG_DRIVERS_IRLOCK=y
|
|||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_COMMON_OPTICAL_FLOW=y
|
||||
CONFIG_DRIVERS_OSD=y
|
||||
CONFIG_COMMON_OSD=y
|
||||
CONFIG_DRIVERS_PCA9685=y
|
||||
CONFIG_DRIVERS_PCA9685_PWM_OUT=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
|
|
|
|||
|
|
@ -23,7 +23,7 @@ CONFIG_DRIVERS_IRLOCK=y
|
|||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_COMMON_OPTICAL_FLOW=y
|
||||
CONFIG_DRIVERS_OSD=y
|
||||
CONFIG_COMMON_OSD=y
|
||||
CONFIG_DRIVERS_PCA9685=y
|
||||
CONFIG_DRIVERS_PCA9685_PWM_OUT=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ CONFIG_COMMON_LIGHT=y
|
|||
CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_COMMON_OPTICAL_FLOW=y
|
||||
CONFIG_DRIVERS_OSD_MSP_OSD=y
|
||||
CONFIG_COMMON_OSD=y
|
||||
CONFIG_DRIVERS_PCA9685=y
|
||||
CONFIG_DRIVERS_PCA9685_PWM_OUT=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ CONFIG_DRIVERS_IRLOCK=y
|
|||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_COMMON_OPTICAL_FLOW=y
|
||||
CONFIG_DRIVERS_OSD=y
|
||||
CONFIG_COMMON_OSD=y
|
||||
CONFIG_DRIVERS_PCA9685=y
|
||||
CONFIG_DRIVERS_PCA9685_PWM_OUT=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@ CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
|
|||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_COMMON_OPTICAL_FLOW=y
|
||||
CONFIG_COMMON_OSD=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA228=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA238=y
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@ CONFIG_BOARD_TESTING=y
|
|||
CONFIG_BOARD_ETHERNET=y
|
||||
CONFIG_DRIVERS_CAMERA_TRIGGER=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_OSD_MSP_OSD=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
|
|
|
|||
|
|
@ -31,15 +31,18 @@
|
|||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(MessageDisplay)
|
||||
|
||||
px4_add_module(
|
||||
MODULE drivers__osd__msp_osd
|
||||
MAIN msp_osd
|
||||
SRCS
|
||||
msp_defines.h
|
||||
msp_osd.cpp
|
||||
msp_osd.hpp
|
||||
MspV1.cpp
|
||||
MspV1.hpp
|
||||
uorb_to_msp.cpp
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
DEPENDS
|
||||
message_display
|
||||
px4_work_queue
|
||||
)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
|
|
@ -31,4 +31,12 @@
|
|||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(atxxxx)
|
||||
add_compile_options(-Wno-stringop-truncation) # full message intentional string truncation
|
||||
|
||||
px4_add_library(message_display
|
||||
MessageDisplay.cpp
|
||||
MessageDisplay.hpp
|
||||
)
|
||||
|
||||
# unit testing
|
||||
px4_add_unit_gtest(SRC MessageDisplayTest.cpp LINKLIBS message_display)
|
||||
|
|
@ -0,0 +1,157 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* Implementation of MessageDisplay class.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include "MessageDisplay.hpp"
|
||||
|
||||
namespace msp_osd
|
||||
{
|
||||
|
||||
void MessageDisplay::set(const MessageDisplayType mode, const char *string)
|
||||
{
|
||||
switch (mode) {
|
||||
case MessageDisplayType::WARNING:
|
||||
if (strcmp(warning_msg, string) != 0) {
|
||||
warning_msg[MSG_BUFFER_SIZE - 1] = '\0';
|
||||
strncpy(warning_msg, string, MSG_BUFFER_SIZE - 1);
|
||||
updated_ = true;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case MessageDisplayType::FLIGHT_MODE:
|
||||
if (strcmp(flight_mode_msg, string) != 0) {
|
||||
flight_mode_msg[MSG_BUFFER_SIZE - 1] = '\0';
|
||||
strncpy(flight_mode_msg, string, MSG_BUFFER_SIZE - 1);
|
||||
updated_ = true;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case MessageDisplayType::ARMING:
|
||||
if (strcmp(arming_msg, string) != 0) {
|
||||
arming_msg[MSG_BUFFER_SIZE - 1] = '\0';
|
||||
strncpy(arming_msg, string, MSG_BUFFER_SIZE - 1);
|
||||
updated_ = true;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case MessageDisplayType::STATUS:
|
||||
if (strcmp(status_msg, string) != 0) {
|
||||
status_msg[MSG_BUFFER_SIZE - 1] = '\0';
|
||||
strncpy(status_msg, string, MSG_BUFFER_SIZE - 1);
|
||||
updated_ = true;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case MessageDisplayType::HEADING:
|
||||
if (strcmp(heading_msg, string) != 0) {
|
||||
heading_msg[MSG_BUFFER_SIZE - 1] = '\0';
|
||||
strncpy(heading_msg, string, MSG_BUFFER_SIZE - 1);
|
||||
updated_ = true;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
// PX4_ERR("Received unsupported message display mode.");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void MessageDisplay::get(char *string, const uint32_t current_time)
|
||||
{
|
||||
// clear input sting
|
||||
string[0] = '\0';
|
||||
|
||||
// check if we should update the full message (and reset display)
|
||||
if (updated_) {
|
||||
// full_message = "Flight Mode: " + flight_mode_msg + " - ARMED: " + arming_msg + " - STATUS: " + status_msg + " - WARNING: " + warning_msg + " ";
|
||||
|
||||
// reset and construct full message
|
||||
strcpy(full_message, "");
|
||||
strncat(full_message, flight_mode_msg, 3); // first three characters of Flight Mode
|
||||
strcat(full_message, "|");
|
||||
strncat(full_message, arming_msg, 4); // first four characters of Arming Message
|
||||
strcat(full_message, "|");
|
||||
strncat(full_message, heading_msg, 2); // first two characters of Heading
|
||||
|
||||
// add a warning message, if it's not empty
|
||||
if (strlen(warning_msg) != 0) {
|
||||
// copy as much of warning message as we can fit
|
||||
strcat(full_message, " WARN: ");
|
||||
const size_t chars_used {20};
|
||||
strncat(full_message, warning_msg, MSG_BUFFER_SIZE - chars_used - FULL_MSG_LENGTH - 1);
|
||||
|
||||
// pad with one full length of terminal whitespace
|
||||
for (unsigned i = 0; i != FULL_MSG_LENGTH; ++i) {
|
||||
strcat(full_message, " ");
|
||||
}
|
||||
}
|
||||
|
||||
// reset display variables
|
||||
index = 0;
|
||||
updated_ = false;
|
||||
}
|
||||
|
||||
// handle edge case where full message is short
|
||||
if (strlen(full_message) < FULL_MSG_LENGTH) {
|
||||
strncpy(string, full_message, FULL_MSG_LENGTH);
|
||||
string[FULL_MSG_LENGTH] = '\0';
|
||||
return;
|
||||
}
|
||||
|
||||
// check if we should update the sub-message (giving extra time to the beginning)
|
||||
uint32_t dt = current_time - last_update_;
|
||||
|
||||
if ((index == 0 && dt >= dwell_) || (index != 0 && dt >= period_)) {
|
||||
// scroll through message by updating index
|
||||
if (++index > strlen(full_message) - FULL_MSG_LENGTH) {
|
||||
index = 0;
|
||||
}
|
||||
|
||||
// save timestamp
|
||||
last_update_ = current_time;
|
||||
}
|
||||
|
||||
// reset update flag and return latest message
|
||||
strncpy(string, full_message + index, FULL_MSG_LENGTH);
|
||||
string[FULL_MSG_LENGTH] = '\0';
|
||||
}
|
||||
|
||||
} // namespace msp_osd
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* Class for displaying messages to a limited screen.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <string.h>
|
||||
#include <stdint.h>
|
||||
|
||||
namespace msp_osd
|
||||
{
|
||||
|
||||
// character size limitations
|
||||
#define MSG_BUFFER_SIZE 250
|
||||
|
||||
// size of available characters, accounting for null terminator
|
||||
// note: the craft_name seems to think it has 15 chars. From testing
|
||||
// that seems incorrect
|
||||
#define FULL_MSG_LENGTH 12
|
||||
#define FULL_MSG_BUFFER 13
|
||||
|
||||
// supported message types
|
||||
enum MessageDisplayType {
|
||||
WARNING,
|
||||
FLIGHT_MODE,
|
||||
ARMING,
|
||||
STATUS,
|
||||
HEADING
|
||||
};
|
||||
|
||||
// display information
|
||||
class MessageDisplay
|
||||
{
|
||||
// working information
|
||||
char warning_msg[MSG_BUFFER_SIZE] {""};
|
||||
char flight_mode_msg[MSG_BUFFER_SIZE] {"???"};
|
||||
char arming_msg[MSG_BUFFER_SIZE] {"????"};
|
||||
char heading_msg[MSG_BUFFER_SIZE] {"??"};
|
||||
// currently unused:
|
||||
char status_msg[MSG_BUFFER_SIZE] {""};
|
||||
|
||||
// the full message and the part we're currently displaying
|
||||
char full_message[MSG_BUFFER_SIZE] {"INITIALIZING"};
|
||||
|
||||
// current index we're displaying
|
||||
uint16_t index{0};
|
||||
|
||||
// last update timestamp
|
||||
uint64_t last_update_{0};
|
||||
bool updated_{false};
|
||||
|
||||
// dwell duration update period (us)
|
||||
uint64_t period_{125'000};
|
||||
uint64_t dwell_{500'000};
|
||||
|
||||
public:
|
||||
MessageDisplay() = default;
|
||||
MessageDisplay(const uint64_t period, const uint64_t dwell) : period_(period), dwell_(dwell) {}
|
||||
|
||||
// set the given string
|
||||
void set(const MessageDisplayType mode, const char *string);
|
||||
|
||||
// get the latest subsection of the message
|
||||
void get(char *string, const uint32_t current_time);
|
||||
|
||||
// update local parameters
|
||||
void set_period(const uint64_t period) { period_ = period; }
|
||||
void set_dwell(const uint64_t dwell) { dwell_ = dwell; }
|
||||
};
|
||||
|
||||
|
||||
} // namespace msp_osd
|
||||
|
|
@ -0,0 +1,164 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "MessageDisplay.hpp"
|
||||
|
||||
// default parameters
|
||||
#define PERIOD 125000
|
||||
#define DWELL 500000
|
||||
|
||||
// use base namespace
|
||||
using namespace msp_osd;
|
||||
|
||||
// core test setup class
|
||||
class MessageDisplayTest : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
// test copy of the message display object
|
||||
std::unique_ptr<MessageDisplay> md_;
|
||||
|
||||
// setup a new test
|
||||
void SetUp() override
|
||||
{
|
||||
// construct new default display
|
||||
md_ = std::make_unique<MessageDisplay>(PERIOD, DWELL);
|
||||
return;
|
||||
}
|
||||
};
|
||||
|
||||
// check basic construction
|
||||
TEST_F(MessageDisplayTest, testMessageDisplayConstruction)
|
||||
{
|
||||
// make sure we have an object!
|
||||
ASSERT_TRUE(static_cast<bool>(md_));
|
||||
|
||||
// verify default message
|
||||
char message[FULL_MSG_LENGTH];
|
||||
md_->get(message, 0);
|
||||
EXPECT_STREQ(message, "INITIALIZING");
|
||||
}
|
||||
|
||||
// check setting and getting various status messages
|
||||
TEST_F(MessageDisplayTest, testMessageDisplayStatic)
|
||||
{
|
||||
// make sure we have an object!
|
||||
ASSERT_TRUE(static_cast<bool>(md_));
|
||||
|
||||
// initialize output string
|
||||
char message[FULL_MSG_BUFFER];
|
||||
|
||||
// set a random flight mode that's too short
|
||||
md_->set(MessageDisplayType::FLIGHT_MODE, "S");
|
||||
md_->get(message, 0);
|
||||
EXPECT_STREQ(message, "S|????|??");
|
||||
|
||||
// set a random flight mode that's too long
|
||||
md_->set(MessageDisplayType::FLIGHT_MODE, "SOSSOS");
|
||||
md_->get(message, 0);
|
||||
EXPECT_STREQ(message, "SOS|????|??");
|
||||
|
||||
// set a random flight mode that's just right
|
||||
md_->set(MessageDisplayType::FLIGHT_MODE, "SOS");
|
||||
md_->get(message, 0);
|
||||
EXPECT_STREQ(message, "SOS|????|??");
|
||||
|
||||
// set a random arming status that's too short
|
||||
md_->set(MessageDisplayType::ARMING, "DS");
|
||||
md_->get(message, 0);
|
||||
EXPECT_STREQ(message, "SOS|DS|??");
|
||||
|
||||
// set a random arming status that's too long
|
||||
md_->set(MessageDisplayType::ARMING, "DISARM");
|
||||
md_->get(message, 0);
|
||||
EXPECT_STREQ(message, "SOS|DISA|??");
|
||||
|
||||
// set a random arming status that's just right
|
||||
md_->set(MessageDisplayType::ARMING, "DSRM");
|
||||
md_->get(message, 0);
|
||||
EXPECT_STREQ(message, "SOS|DSRM|??");
|
||||
|
||||
// set a random heading that's too short
|
||||
md_->set(MessageDisplayType::HEADING, "N");
|
||||
md_->get(message, 0);
|
||||
EXPECT_STREQ(message, "SOS|DSRM|N");
|
||||
|
||||
// set a random heading that's too short
|
||||
md_->set(MessageDisplayType::HEADING, "NBNW");
|
||||
md_->get(message, 0);
|
||||
EXPECT_STREQ(message, "SOS|DSRM|NB");
|
||||
|
||||
// set a random heading that's too short
|
||||
md_->set(MessageDisplayType::HEADING, "NW");
|
||||
md_->get(message, 0);
|
||||
EXPECT_STREQ(message, "SOS|DSRM|NW");
|
||||
}
|
||||
|
||||
// check setting and getting for a warning message
|
||||
TEST_F(MessageDisplayTest, testMessageDisplayWarning)
|
||||
{
|
||||
// make sure we have an object!
|
||||
ASSERT_TRUE(static_cast<bool>(md_));
|
||||
|
||||
// set just a warning message, but a very long one
|
||||
md_->set(MessageDisplayType::WARNING,
|
||||
"Lorem ipsum dolor sit amet, consectetur adipiscing elit, sed do eiusmod tempor incididunt ut labore et dolore magna aliqua. Ut enim ad minim veniam, quis nostrud exercitation ullamco laboris nisi ut aliquip ex ea commodo consequat. Duis aute irure dolor in reprehenderit in voluptate velit esse cillum dolore eu fugiat nulla pariatur. Excepteur sint occaecat cupidatat non proident, sunt in culpa qui officia deserunt mollit anim id est laborum.");
|
||||
|
||||
// the ground truth message is the following:
|
||||
const char ground_truth[MSG_BUFFER_SIZE] =
|
||||
"???|????|?? WARN: Lorem ipsum dolor sit amet, consectetur adipiscing elit, sed do eiusmod tempor incididunt ut labore et dolore magna aliqua. Ut enim ad minim veniam, quis nostrud exercitation ullamco laboris nisi ut aliquip ex ea comm ";
|
||||
|
||||
// walk through the message as it scrolls
|
||||
char message[FULL_MSG_BUFFER]; // output message
|
||||
char correct[FULL_MSG_BUFFER]; // correct value of output message
|
||||
uint32_t stamp = DWELL - 1; // start at the end of the DWELL time
|
||||
|
||||
for (size_t i = 0; i != MSG_BUFFER_SIZE - FULL_MSG_LENGTH; ++i) {
|
||||
// get updated message for this time
|
||||
md_->get(message, stamp);
|
||||
|
||||
// get substring that we should be seeing
|
||||
strncpy(correct, &ground_truth[i], FULL_MSG_LENGTH);
|
||||
EXPECT_STREQ(message, correct);
|
||||
|
||||
// update time
|
||||
stamp += PERIOD;
|
||||
}
|
||||
|
||||
// verify that we wrap around as expected at the end
|
||||
md_->get(message, stamp);
|
||||
strncpy(correct, ground_truth, FULL_MSG_LENGTH);
|
||||
EXPECT_STREQ(message, correct);
|
||||
}
|
||||
|
||||
|
|
@ -62,7 +62,7 @@ struct msp_message_descriptor_t {
|
|||
uint8_t message_size;
|
||||
};
|
||||
|
||||
#define MSP_DESCRIPTOR_COUNT 12
|
||||
#define MSP_DESCRIPTOR_COUNT 11
|
||||
const msp_message_descriptor_t msp_message_descriptors[MSP_DESCRIPTOR_COUNT] = {
|
||||
{MSP_OSD_CONFIG, true, sizeof(msp_osd_config_t)},
|
||||
{MSP_NAME, true, sizeof(msp_name_t)},
|
||||
|
|
@ -75,7 +75,6 @@ const msp_message_descriptor_t msp_message_descriptors[MSP_DESCRIPTOR_COUNT] = {
|
|||
{MSP_COMP_GPS, true, sizeof(msp_comp_gps_t)},
|
||||
{MSP_ESC_SENSOR_DATA, true, sizeof(msp_esc_sensor_data_dji_t)},
|
||||
{MSP_MOTOR_TELEMETRY, true, sizeof(msp_motor_telemetry_t)},
|
||||
{MSP_FC_VARIANT, true, sizeof(msp_fc_variant_t)},
|
||||
};
|
||||
|
||||
#define MSP_FRAME_START_SIZE 5
|
||||
|
|
@ -84,7 +83,7 @@ bool MspV1::Send(const uint8_t message_id, const void *payload)
|
|||
{
|
||||
uint32_t payload_size = 0;
|
||||
|
||||
msp_message_descriptor_t *desc = NULL;
|
||||
msp_message_descriptor_t *desc = nullptr;
|
||||
|
||||
for (int i = 0; i < MSP_DESCRIPTOR_COUNT; i++) {
|
||||
if (message_id == msp_message_descriptors[i].message_id) {
|
||||
|
|
|
|||
|
|
@ -41,6 +41,6 @@ public:
|
|||
bool Send(const uint8_t message_id, const void *payload);
|
||||
|
||||
private:
|
||||
int _fd;
|
||||
int _fd{-1};
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,85 @@
|
|||
module_name: MSP OSD
|
||||
serial_config:
|
||||
- command: msp_osd start -d ${SERIAL_DEV}
|
||||
port_config_param:
|
||||
name: MSP_OSD_CONFIG
|
||||
group: OSD
|
||||
|
||||
parameters:
|
||||
- group: OSD
|
||||
definitions:
|
||||
|
||||
# OSD Parameters
|
||||
OSD_SYMBOLS:
|
||||
description:
|
||||
short: OSD Symbol Selection
|
||||
long: |
|
||||
Configure / toggle support display options.
|
||||
type: bitmask
|
||||
bit:
|
||||
0: CRAFT_NAME
|
||||
1: DISARMED
|
||||
2: GPS_LAT
|
||||
3: GPS_LON
|
||||
4: GPS_SATS
|
||||
5: GPS_SPEED
|
||||
6: HOME_DIST
|
||||
7: HOME_DIR
|
||||
8: MAIN_BATT_VOLTAGE
|
||||
9: CURRENT_DRAW
|
||||
10: MAH_DRAWN
|
||||
11: RSSI_VALUE
|
||||
12: ALTITUDE
|
||||
13: NUMERICAL_VARIO
|
||||
# the following are reserved but not currently functional
|
||||
14: (unused) FLYMODE
|
||||
15: (unused) ESC_TMP
|
||||
16: (unused) PITCH_ANGLE
|
||||
17: (unused) ROLL_ANGLE
|
||||
18: (unused) CROSSHAIRS
|
||||
19: (unused) AVG_CELL_VOLTAGE
|
||||
20: (unused) HORIZON_SIDEBARS
|
||||
21: (unused) POWER
|
||||
default: 16383
|
||||
|
||||
# OSD Log Level
|
||||
OSD_LOG_LEVEL:
|
||||
description:
|
||||
short: OSD Warning Level
|
||||
long: |
|
||||
Minimum security of log level to display on the OSD.
|
||||
type: int32
|
||||
bit:
|
||||
0: EMERGENCY
|
||||
1: ALERT
|
||||
2: CRITICAL
|
||||
3: ERROR
|
||||
4: WARNING
|
||||
5: NOTICE
|
||||
6: INFO
|
||||
7: DEBUG
|
||||
8: NONE
|
||||
default: 3
|
||||
|
||||
# Scroll Speed
|
||||
OSD_SCROLL_RATE:
|
||||
description:
|
||||
short: OSD Scroll Rate (ms)
|
||||
long: |
|
||||
Scroll rate in milliseconds for OSD messages longer than available character width.
|
||||
This is lower-bounded by the nominal loop rate of this module.
|
||||
type: int32
|
||||
min: 100
|
||||
max: 1000
|
||||
default: 125
|
||||
|
||||
# Dwell TIme
|
||||
OSD_DWELL_TIME:
|
||||
description:
|
||||
short: OSD Dwell Time (ms)
|
||||
long: |
|
||||
Amount of time in milliseconds to dwell at the beginning of the display, when scrolling.
|
||||
type: int32
|
||||
min: 100
|
||||
max: 10000
|
||||
default: 500
|
||||
|
|
@ -33,8 +33,6 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4
|
||||
|
||||
// requests & replies
|
||||
#define MSP_API_VERSION 1
|
||||
#define MSP_FC_VARIANT 2
|
||||
|
|
|
|||
|
|
@ -31,6 +31,13 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* Notes:
|
||||
* - Currently there's a lot of wasted processing here if certain displays are enabled.
|
||||
* A relatively low-hanging fruit would be figuring out which display elements require
|
||||
* information from what UORB topics and disable if the information isn't displayed.
|
||||
* (this is complicated by the fact that it's not a one-to-one mapping...)
|
||||
*/
|
||||
|
||||
#include "msp_osd.hpp"
|
||||
|
||||
#include "msp_defines.h"
|
||||
|
|
@ -54,7 +61,6 @@
|
|||
#include <uORB/topics/airspeed_validated.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
#include "MspV1.hpp"
|
||||
|
|
@ -63,20 +69,17 @@
|
|||
//in betaflight configurator set OSD elements to your desired positions and in CLI type "set osd" to retreieve the numbers.
|
||||
//234 -> not visible. Horizontally 2048-2074(spacing 1), vertically 2048-2528(spacing 32). 26 characters X 15 lines
|
||||
|
||||
// Currently working elements
|
||||
// Currently working elements positions (hardcoded)
|
||||
|
||||
// Left
|
||||
const uint16_t osd_gps_lat_pos = 2048;
|
||||
const uint16_t osd_gps_lon_pos = 2080;
|
||||
uint16_t osd_gps_sats_pos = 2112;
|
||||
const uint16_t osd_gps_sats_pos = 2112;
|
||||
const uint16_t osd_rssi_value_pos = 2176;
|
||||
const uint16_t osd_flymode_pos = 234;
|
||||
const uint16_t osd_esc_tmp_pos = 234;
|
||||
|
||||
// Center
|
||||
const uint16_t osd_home_dir_pos = 2093;
|
||||
const uint16_t osd_craft_name_pos = 2543;
|
||||
const uint16_t osd_horizon_sidebars_pos = 234;
|
||||
const uint16_t osd_disarmed_pos = 2125;
|
||||
|
||||
// Right
|
||||
|
|
@ -87,56 +90,19 @@ const uint16_t osd_altitude_pos = 2233;
|
|||
const uint16_t osd_numerical_vario_pos = 2267;
|
||||
const uint16_t osd_gps_speed_pos = 2299;
|
||||
const uint16_t osd_home_dist_pos = 2331;
|
||||
const uint16_t osd_power_pos = 234;
|
||||
|
||||
// Disabled
|
||||
const uint16_t osd_pitch_angle_pos = 234;
|
||||
const uint16_t osd_roll_angle_pos = 234;
|
||||
const uint16_t osd_crosshairs_pos = 234;
|
||||
const uint16_t osd_avg_cell_voltage_pos = 234;
|
||||
|
||||
// Not implemented or not available
|
||||
const uint16_t osd_throttle_pos_pos = 234;
|
||||
const uint16_t osd_vtx_channel_pos = 234;
|
||||
const uint16_t osd_roll_pids_pos = 234;
|
||||
const uint16_t osd_pitch_pids_pos = 234;
|
||||
const uint16_t osd_yaw_pids_pos = 234;
|
||||
const uint16_t osd_pidrate_profile_pos = 234;
|
||||
const uint16_t osd_warnings_pos = 234;
|
||||
const uint16_t osd_debug_pos = 234;
|
||||
const uint16_t osd_artificial_horizon_pos = 234;
|
||||
const uint16_t osd_item_timer_1_pos = 234;
|
||||
const uint16_t osd_item_timer_2_pos = 234;
|
||||
const uint16_t osd_main_batt_usage_pos = 234;
|
||||
const uint16_t osd_numerical_heading_pos = 234;
|
||||
const uint16_t osd_compass_bar_pos = 234;
|
||||
const uint16_t osd_esc_rpm_pos = 234;
|
||||
const uint16_t osd_remaining_time_estimate_pos = 234;
|
||||
const uint16_t osd_rtc_datetime_pos = 234;
|
||||
const uint16_t osd_adjustment_range_pos = 234;
|
||||
const uint16_t osd_core_temperature_pos = 234;
|
||||
const uint16_t osd_anti_gravity_pos = 234;
|
||||
const uint16_t osd_g_force_pos = 234;
|
||||
const uint16_t osd_motor_diag_pos = 234;
|
||||
const uint16_t osd_log_status_pos = 234;
|
||||
const uint16_t osd_flip_arrow_pos = 234;
|
||||
const uint16_t osd_link_quality_pos = 234;
|
||||
const uint16_t osd_flight_dist_pos = 234;
|
||||
const uint16_t osd_stick_overlay_left_pos = 234;
|
||||
const uint16_t osd_stick_overlay_right_pos = 234;
|
||||
const uint16_t osd_display_name_pos = 234;
|
||||
const uint16_t osd_esc_rpm_freq_pos = 234;
|
||||
const uint16_t osd_rate_profile_name_pos = 234;
|
||||
const uint16_t osd_pid_profile_name_pos = 234;
|
||||
const uint16_t osd_profile_name_pos = 234;
|
||||
const uint16_t osd_rssi_dbm_value_pos = 234;
|
||||
const uint16_t osd_rc_channels_pos = 234;
|
||||
|
||||
MspOsd::MspOsd() :
|
||||
MspOsd::MspOsd(const char *device) :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1),
|
||||
_msp(NULL)
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
|
||||
{
|
||||
_display.set_period(_param_osd_scroll_rate.get() * 1000ULL);
|
||||
_display.set_dwell(_param_osd_dwell_time.get() * 1000ULL);
|
||||
|
||||
// back up device name for connection later
|
||||
strcpy(_device, device);
|
||||
|
||||
// _is_initialized = true;
|
||||
PX4_INFO("MSP OSD prepared to run on %s", _device);
|
||||
}
|
||||
|
||||
MspOsd::~MspOsd()
|
||||
|
|
@ -155,7 +121,6 @@ void MspOsd::SendConfig()
|
|||
msp_osd_config_t msp_osd_config;
|
||||
|
||||
msp_osd_config.units = 0;
|
||||
|
||||
msp_osd_config.osd_item_count = 56;
|
||||
msp_osd_config.osd_stat_count = 24;
|
||||
msp_osd_config.osd_timer_count = 2;
|
||||
|
|
@ -164,63 +129,70 @@ void MspOsd::SendConfig()
|
|||
msp_osd_config.osdprofileindex = 1; // 1
|
||||
msp_osd_config.overlay_radio_mode = 0; // 0
|
||||
|
||||
msp_osd_config.osd_rssi_value_pos = osd_rssi_value_pos;
|
||||
msp_osd_config.osd_main_batt_voltage_pos = osd_main_batt_voltage_pos;
|
||||
msp_osd_config.osd_crosshairs_pos = osd_crosshairs_pos;
|
||||
msp_osd_config.osd_artificial_horizon_pos = osd_artificial_horizon_pos;
|
||||
msp_osd_config.osd_horizon_sidebars_pos = osd_horizon_sidebars_pos;
|
||||
msp_osd_config.osd_item_timer_1_pos = osd_item_timer_1_pos;
|
||||
msp_osd_config.osd_item_timer_2_pos = osd_item_timer_2_pos;
|
||||
msp_osd_config.osd_flymode_pos = osd_flymode_pos;
|
||||
msp_osd_config.osd_craft_name_pos = osd_craft_name_pos;
|
||||
msp_osd_config.osd_throttle_pos_pos = osd_throttle_pos_pos;
|
||||
msp_osd_config.osd_vtx_channel_pos = osd_vtx_channel_pos;
|
||||
msp_osd_config.osd_current_draw_pos = osd_current_draw_pos;
|
||||
msp_osd_config.osd_mah_drawn_pos = osd_mah_drawn_pos;
|
||||
msp_osd_config.osd_gps_speed_pos = osd_gps_speed_pos;
|
||||
msp_osd_config.osd_gps_sats_pos = osd_gps_sats_pos;
|
||||
msp_osd_config.osd_altitude_pos = osd_altitude_pos;
|
||||
msp_osd_config.osd_roll_pids_pos = osd_roll_pids_pos;
|
||||
msp_osd_config.osd_pitch_pids_pos = osd_pitch_pids_pos;
|
||||
msp_osd_config.osd_yaw_pids_pos = osd_yaw_pids_pos;
|
||||
msp_osd_config.osd_power_pos = osd_power_pos;
|
||||
msp_osd_config.osd_pidrate_profile_pos = osd_pidrate_profile_pos;
|
||||
msp_osd_config.osd_warnings_pos = osd_warnings_pos;
|
||||
msp_osd_config.osd_avg_cell_voltage_pos = osd_avg_cell_voltage_pos;
|
||||
msp_osd_config.osd_gps_lon_pos = osd_gps_lon_pos;
|
||||
msp_osd_config.osd_gps_lat_pos = osd_gps_lat_pos;
|
||||
msp_osd_config.osd_debug_pos = osd_debug_pos;
|
||||
msp_osd_config.osd_pitch_angle_pos = osd_pitch_angle_pos;
|
||||
msp_osd_config.osd_roll_angle_pos = osd_roll_angle_pos;
|
||||
msp_osd_config.osd_main_batt_usage_pos = osd_main_batt_usage_pos;
|
||||
msp_osd_config.osd_disarmed_pos = osd_disarmed_pos;
|
||||
msp_osd_config.osd_home_dir_pos = osd_home_dir_pos;
|
||||
msp_osd_config.osd_home_dist_pos = osd_home_dist_pos;
|
||||
msp_osd_config.osd_numerical_heading_pos = osd_numerical_heading_pos;
|
||||
msp_osd_config.osd_numerical_vario_pos = osd_numerical_vario_pos;
|
||||
msp_osd_config.osd_compass_bar_pos = osd_compass_bar_pos;
|
||||
msp_osd_config.osd_esc_tmp_pos = osd_esc_tmp_pos;
|
||||
msp_osd_config.osd_esc_rpm_pos = osd_esc_rpm_pos;
|
||||
msp_osd_config.osd_remaining_time_estimate_pos = osd_remaining_time_estimate_pos;
|
||||
msp_osd_config.osd_rtc_datetime_pos = osd_rtc_datetime_pos;
|
||||
msp_osd_config.osd_adjustment_range_pos = osd_adjustment_range_pos;
|
||||
msp_osd_config.osd_core_temperature_pos = osd_core_temperature_pos;
|
||||
msp_osd_config.osd_anti_gravity_pos = osd_anti_gravity_pos;
|
||||
msp_osd_config.osd_g_force_pos = osd_g_force_pos;
|
||||
msp_osd_config.osd_motor_diag_pos = osd_motor_diag_pos;
|
||||
msp_osd_config.osd_log_status_pos = osd_log_status_pos;
|
||||
msp_osd_config.osd_flip_arrow_pos = osd_flip_arrow_pos;
|
||||
msp_osd_config.osd_link_quality_pos = osd_link_quality_pos;
|
||||
msp_osd_config.osd_flight_dist_pos = osd_flight_dist_pos;
|
||||
msp_osd_config.osd_stick_overlay_left_pos = osd_stick_overlay_left_pos;
|
||||
msp_osd_config.osd_stick_overlay_right_pos = osd_stick_overlay_right_pos;
|
||||
msp_osd_config.osd_display_name_pos = osd_display_name_pos;
|
||||
msp_osd_config.osd_esc_rpm_freq_pos = osd_esc_rpm_freq_pos;
|
||||
msp_osd_config.osd_rate_profile_name_pos = osd_rate_profile_name_pos;
|
||||
msp_osd_config.osd_pid_profile_name_pos = osd_pid_profile_name_pos;
|
||||
msp_osd_config.osd_profile_name_pos = osd_profile_name_pos;
|
||||
msp_osd_config.osd_rssi_dbm_value_pos = osd_rssi_dbm_value_pos;
|
||||
msp_osd_config.osd_rc_channels_pos = osd_rc_channels_pos;
|
||||
// display conditional elements
|
||||
msp_osd_config.osd_craft_name_pos = enabled(SymbolIndex::CRAFT_NAME) ? osd_craft_name_pos : LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_disarmed_pos = enabled(SymbolIndex::DISARMED) ? osd_disarmed_pos : LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_gps_lat_pos = enabled(SymbolIndex::GPS_LAT) ? osd_gps_lat_pos : LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_gps_lon_pos = enabled(SymbolIndex::GPS_LON) ? osd_gps_lon_pos : LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_gps_sats_pos = enabled(SymbolIndex::GPS_SATS) ? osd_gps_sats_pos : LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_gps_speed_pos = enabled(SymbolIndex::GPS_SPEED) ? osd_gps_speed_pos : LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_home_dist_pos = enabled(SymbolIndex::HOME_DIST) ? osd_home_dist_pos : LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_home_dir_pos = enabled(SymbolIndex::HOME_DIR) ? osd_home_dir_pos : LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_main_batt_voltage_pos = enabled(SymbolIndex::MAIN_BATT_VOLTAGE) ? osd_main_batt_voltage_pos :
|
||||
LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_current_draw_pos = enabled(SymbolIndex::CURRENT_DRAW) ? osd_current_draw_pos : LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_mah_drawn_pos = enabled(SymbolIndex::MAH_DRAWN) ? osd_mah_drawn_pos : LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_rssi_value_pos = enabled(SymbolIndex::RSSI_VALUE) ? osd_rssi_value_pos : LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_altitude_pos = enabled(SymbolIndex::ALTITUDE) ? osd_altitude_pos : LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_numerical_vario_pos = enabled(SymbolIndex::NUMERICAL_VARIO) ? osd_numerical_vario_pos :
|
||||
LOCATION_HIDDEN;
|
||||
|
||||
// possibly available, but not currently used
|
||||
msp_osd_config.osd_flymode_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_esc_tmp_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_pitch_angle_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_roll_angle_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_crosshairs_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_avg_cell_voltage_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_horizon_sidebars_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_power_pos = LOCATION_HIDDEN;
|
||||
|
||||
// Not implemented or not available
|
||||
msp_osd_config.osd_artificial_horizon_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_item_timer_1_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_item_timer_2_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_throttle_pos_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_vtx_channel_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_roll_pids_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_pitch_pids_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_yaw_pids_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_pidrate_profile_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_warnings_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_debug_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_main_batt_usage_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_numerical_heading_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_compass_bar_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_esc_rpm_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_remaining_time_estimate_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_rtc_datetime_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_adjustment_range_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_core_temperature_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_anti_gravity_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_g_force_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_motor_diag_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_log_status_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_flip_arrow_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_link_quality_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_flight_dist_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_stick_overlay_left_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_stick_overlay_right_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_display_name_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_esc_rpm_freq_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_rate_profile_name_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_pid_profile_name_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_profile_name_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_rssi_dbm_value_pos = LOCATION_HIDDEN;
|
||||
msp_osd_config.osd_rc_channels_pos = LOCATION_HIDDEN;
|
||||
|
||||
_msp.Send(MSP_OSD_CONFIG, &msp_osd_config);
|
||||
}
|
||||
|
|
@ -239,16 +211,16 @@ void MspOsd::Run()
|
|||
parameter_update_s param_update;
|
||||
_parameter_update_sub.copy(¶m_update);
|
||||
updateParams(); // update module parameters (in DEFINE_PARAMETERS)
|
||||
parameters_update();
|
||||
}
|
||||
|
||||
// perform first time initialization, if needed
|
||||
if (!_is_initialized) {
|
||||
_is_initialized = true;
|
||||
|
||||
struct termios t;
|
||||
|
||||
_msp_fd = open("/dev/ttyS3", O_RDWR | O_NONBLOCK);
|
||||
_msp_fd = open(_device, O_RDWR | O_NONBLOCK);
|
||||
|
||||
if (_msp_fd < 0) {
|
||||
_performance_data.initialization_problems = true;
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -262,200 +234,203 @@ void MspOsd::Run()
|
|||
|
||||
_msp = MspV1(_msp_fd);
|
||||
|
||||
PX4_WARN("Startup");
|
||||
_is_initialized = true;
|
||||
}
|
||||
|
||||
msp_battery_state_t battery_state = {0};
|
||||
msp_name_t name = {0};
|
||||
msp_status_BF_t status_BF = {0};
|
||||
msp_analog_t analog = {0};
|
||||
msp_raw_gps_t raw_gps = {0};
|
||||
msp_comp_gps_t comp_gps = {0};
|
||||
msp_attitude_t attitude = {0};
|
||||
msp_altitude_t altitude = {0};
|
||||
msp_esc_sensor_data_dji_t esc_sensor_data = {0};
|
||||
//msp_motor_telemetry_t motor_telemetry = {0};
|
||||
msp_fc_variant_t variant = {0};
|
||||
// avoid premature pessimization; if skip processing if we're effectively disabled
|
||||
if (_param_osd_symbols.get() == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
//power_monitor_sub.update(&power_monitor_struct);
|
||||
_battery_status_sub.update(&_battery_status_struct);
|
||||
_vehicle_status_sub.update(&_vehicle_status_struct);
|
||||
_vehicle_gps_position_sub.update(&_vehicle_gps_position_struct);
|
||||
_airspeed_validated_sub.update(&_airspeed_validated_struct);
|
||||
_vehicle_air_data_sub.update(&_vehicle_air_data_struct);
|
||||
_home_position_sub.update(&_home_position_struct);
|
||||
_vehicle_global_position_sub.update(&_vehicle_global_position_struct);
|
||||
_vehicle_local_position_sub.update(&_vehicle_local_position_struct);
|
||||
_vehicle_attitude_sub.update(&_vehicle_attitude_struct);
|
||||
_estimator_status_sub.update(&_estimator_status_struct);
|
||||
_input_rc_sub.update(&_input_rc_struct);
|
||||
// update display message
|
||||
{
|
||||
vehicle_status_s vehicle_status{};
|
||||
_vehicle_status_sub.copy(&vehicle_status);
|
||||
|
||||
matrix::Eulerf euler_attitude(matrix::Quatf(_vehicle_attitude_struct.q));
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
_vehicle_attitude_sub.copy(&vehicle_attitude);
|
||||
|
||||
memcpy(variant.flightControlIdentifier, "BTFL", sizeof(variant.flightControlIdentifier));
|
||||
_msp.Send(MSP_FC_VARIANT, &variant);
|
||||
log_message_s log_message{};
|
||||
_log_message_sub.copy(&log_message);
|
||||
|
||||
// MSP_NAME
|
||||
snprintf(name.craft_name, sizeof(name.craft_name), "> %i", _x);
|
||||
name.craft_name[14] = '\0';
|
||||
_msp.Send(MSP_NAME, &name);
|
||||
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);
|
||||
}
|
||||
|
||||
// MSP_FC_VARIANT
|
||||
{
|
||||
const auto msg = msp_osd::construct_FC_VARIANT();
|
||||
this->Send(MSP_FC_VARIANT, &msg);
|
||||
}
|
||||
|
||||
// MSP_STATUS
|
||||
if (_vehicle_status_struct.arming_state == _vehicle_status_struct.ARMING_STATE_ARMED) {
|
||||
status_BF.flight_mode_flags |= ARM_ACRO_BF;
|
||||
{
|
||||
vehicle_status_s vehicle_status{};
|
||||
_vehicle_status_sub.copy(&vehicle_status);
|
||||
|
||||
switch (_vehicle_status_struct.nav_state) {
|
||||
case _vehicle_status_struct.NAVIGATION_STATE_MANUAL:
|
||||
status_BF.flight_mode_flags |= 0;
|
||||
break;
|
||||
|
||||
case _vehicle_status_struct.NAVIGATION_STATE_ACRO:
|
||||
status_BF.flight_mode_flags |= 0;
|
||||
break;
|
||||
|
||||
case _vehicle_status_struct.NAVIGATION_STATE_STAB:
|
||||
status_BF.flight_mode_flags |= STAB_BF;
|
||||
break;
|
||||
|
||||
case _vehicle_status_struct.NAVIGATION_STATE_AUTO_RTL:
|
||||
status_BF.flight_mode_flags |= RESC_BF;
|
||||
break;
|
||||
|
||||
case _vehicle_status_struct.NAVIGATION_STATE_TERMINATION:
|
||||
status_BF.flight_mode_flags |= FS_BF;
|
||||
break;
|
||||
|
||||
default:
|
||||
status_BF.flight_mode_flags = 0;
|
||||
break;
|
||||
}
|
||||
const auto msg = msp_osd::construct_STATUS(vehicle_status);
|
||||
this->Send(MSP_STATUS, &msg);
|
||||
}
|
||||
|
||||
status_BF.arming_disable_flags_count = 1;
|
||||
status_BF.arming_disable_flags = !(_vehicle_status_struct.arming_state == _vehicle_status_struct.ARMING_STATE_ARMED);
|
||||
_msp.Send(MSP_STATUS, &status_BF);
|
||||
|
||||
// MSP_ANALOG
|
||||
analog.vbat = _battery_status_struct.voltage_v * 10; // bottom right... v * 10
|
||||
analog.rssi = (uint16_t)((_input_rc_struct.rssi * 1023.0f) / 100.0f);
|
||||
analog.amperage = _battery_status_struct.current_a * 100; // main amperage
|
||||
analog.mAhDrawn = _battery_status_struct.discharged_mah; // unused
|
||||
_msp.Send(MSP_ANALOG, &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);
|
||||
}
|
||||
|
||||
// MSP_BATTERY_STATE
|
||||
battery_state.amperage = _battery_status_struct.current_a; // not used?
|
||||
battery_state.batteryVoltage = (uint16_t)(_battery_status_struct.voltage_v * 400.0f); // OK
|
||||
battery_state.mAhDrawn = _battery_status_struct.discharged_mah ; // OK
|
||||
battery_state.batteryCellCount = _battery_status_struct.cell_count;
|
||||
battery_state.batteryCapacity = _battery_status_struct.capacity; // not used?
|
||||
{
|
||||
battery_status_s battery_status{};
|
||||
_battery_status_sub.copy(&battery_status);
|
||||
|
||||
// Voltage color 0==white, 1==red
|
||||
if (_battery_status_struct.voltage_v < 14.4f) {
|
||||
battery_state.batteryState = 1;
|
||||
|
||||
} else {
|
||||
battery_state.batteryState = 0;
|
||||
const auto msg = msp_osd::construct_BATTERY_STATE(battery_status);
|
||||
this->Send(MSP_BATTERY_STATE, &msg);
|
||||
}
|
||||
|
||||
battery_state.legacyBatteryVoltage = _battery_status_struct.voltage_v * 10;
|
||||
_msp.Send(MSP_BATTERY_STATE, &battery_state);
|
||||
|
||||
// MSP_RAW_GPS
|
||||
if (_vehicle_gps_position_struct.fix_type >= 2) {
|
||||
raw_gps.lat = _vehicle_gps_position_struct.lat;
|
||||
raw_gps.lon = _vehicle_gps_position_struct.lon;
|
||||
raw_gps.alt = _vehicle_gps_position_struct.alt / 10;
|
||||
//raw_gps.groundCourse = vehicle_gps_position_struct
|
||||
altitude.estimatedActualPosition = _vehicle_gps_position_struct.alt / 10;
|
||||
{
|
||||
sensor_gps_s vehicle_gps_position{};
|
||||
_vehicle_gps_position_sub.copy(&vehicle_gps_position);
|
||||
|
||||
} else {
|
||||
raw_gps.lat = 0;
|
||||
raw_gps.lon = 0;
|
||||
raw_gps.alt = 0;
|
||||
altitude.estimatedActualPosition = 0;
|
||||
}
|
||||
airspeed_validated_s airspeed_validated{};
|
||||
_airspeed_validated_sub.copy(&airspeed_validated);
|
||||
|
||||
if (_vehicle_gps_position_struct.fix_type == 0
|
||||
|| _vehicle_gps_position_struct.fix_type == 1) {
|
||||
raw_gps.fixType = MSP_GPS_NO_FIX;
|
||||
|
||||
} else if (_vehicle_gps_position_struct.fix_type == 2) {
|
||||
raw_gps.fixType = MSP_GPS_FIX_2D;
|
||||
|
||||
} else if (_vehicle_gps_position_struct.fix_type >= 3 && _vehicle_gps_position_struct.fix_type <= 5) {
|
||||
raw_gps.fixType = MSP_GPS_FIX_3D;
|
||||
|
||||
} else {
|
||||
raw_gps.fixType = MSP_GPS_NO_FIX;
|
||||
}
|
||||
|
||||
//raw_gps.hdop = vehicle_gps_position_struct.hdop
|
||||
raw_gps.numSat = _vehicle_gps_position_struct.satellites_used;
|
||||
|
||||
if (_airspeed_validated_struct.airspeed_sensor_measurement_valid
|
||||
&& _airspeed_validated_struct.indicated_airspeed_m_s != NAN
|
||||
&& _airspeed_validated_struct.indicated_airspeed_m_s > 0) {
|
||||
raw_gps.groundSpeed = _airspeed_validated_struct.indicated_airspeed_m_s * 100;
|
||||
|
||||
} else {
|
||||
raw_gps.groundSpeed = 0;
|
||||
}
|
||||
|
||||
//PX4_WARN("%f\r\n", (double)_battery_status_struct.current_a);
|
||||
_msp.Send(MSP_RAW_GPS, &raw_gps);
|
||||
|
||||
// Calculate distance and direction to home
|
||||
if (_home_position_struct.valid_hpos
|
||||
&& _home_position_struct.valid_lpos
|
||||
&& _estimator_status_struct.solution_status_flags & (1 << 4)) {
|
||||
float bearing_to_home = get_bearing_to_next_waypoint(_vehicle_global_position_struct.lat,
|
||||
_vehicle_global_position_struct.lon,
|
||||
_home_position_struct.lat, _home_position_struct.lon);
|
||||
|
||||
float distance_to_home = get_distance_to_next_waypoint(_vehicle_global_position_struct.lat,
|
||||
_vehicle_global_position_struct.lon,
|
||||
_home_position_struct.lat, _home_position_struct.lon);
|
||||
|
||||
comp_gps.distanceToHome = (int16_t)distance_to_home; // meters
|
||||
comp_gps.directionToHome = bearing_to_home; // degrees
|
||||
|
||||
} else {
|
||||
comp_gps.distanceToHome = 0; // meters
|
||||
comp_gps.directionToHome = 0; // degrees
|
||||
const auto msg = msp_osd::construct_RAW_GPS(
|
||||
vehicle_gps_position,
|
||||
airspeed_validated);
|
||||
this->Send(MSP_RAW_GPS, &msg);
|
||||
}
|
||||
|
||||
// MSP_COMP_GPS
|
||||
comp_gps.heartbeat = _heartbeat;
|
||||
_heartbeat = !_heartbeat;
|
||||
_msp.Send(MSP_COMP_GPS, &comp_gps);
|
||||
{
|
||||
// update heartbeat
|
||||
_heartbeat = !_heartbeat;
|
||||
|
||||
// MSP_ATTITUDE
|
||||
attitude.pitch = math::degrees(euler_attitude.theta()) * 10;
|
||||
attitude.roll = math::degrees(euler_attitude.phi()) * 10;
|
||||
attitude.yaw = math::degrees(euler_attitude.psi()) * 10;
|
||||
_msp.Send(MSP_ATTITUDE, &attitude);
|
||||
home_position_s home_position{};
|
||||
_home_position_sub.copy(&home_position);
|
||||
|
||||
// MSP_ALTITUDE
|
||||
if (_estimator_status_struct.solution_status_flags & (1 << 5)) {
|
||||
altitude.estimatedActualVelocity = -_vehicle_local_position_struct.vz * 10; //m/s to cm/s
|
||||
estimator_status_s estimator_status{};
|
||||
_estimator_status_sub.copy(&estimator_status);
|
||||
|
||||
} else {
|
||||
altitude.estimatedActualVelocity = 0;
|
||||
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,
|
||||
estimator_status,
|
||||
vehicle_global_position,
|
||||
_heartbeat);
|
||||
this->Send(MSP_COMP_GPS, &msg);
|
||||
}
|
||||
|
||||
_msp.Send(MSP_ALTITUDE, &altitude);
|
||||
// MSP_ATTITUDE
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
// MSP_ALTITUDE
|
||||
{
|
||||
sensor_gps_s vehicle_gps_position{};
|
||||
_vehicle_gps_position_sub.copy(&vehicle_gps_position);
|
||||
|
||||
estimator_status_s estimator_status{};
|
||||
_estimator_status_sub.copy(&estimator_status);
|
||||
|
||||
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,
|
||||
estimator_status,
|
||||
vehicle_local_position);
|
||||
this->Send(MSP_ALTITUDE, &msg);
|
||||
}
|
||||
|
||||
// MSP_MOTOR_TELEMETRY
|
||||
esc_sensor_data.rpm = 0;
|
||||
esc_sensor_data.temperature = 50;
|
||||
_msp.Send(MSP_ESC_SENSOR_DATA, &esc_sensor_data);
|
||||
{
|
||||
const auto msg = msp_osd::construct_ESC_SENSOR_DATA();
|
||||
this->Send(MSP_ESC_SENSOR_DATA, &msg);
|
||||
}
|
||||
|
||||
// send full configuration
|
||||
SendConfig();
|
||||
}
|
||||
|
||||
void MspOsd::Send(const unsigned int message_type, const void *payload)
|
||||
{
|
||||
if (_msp.Send(message_type, payload)) {
|
||||
_performance_data.successful_sends++;
|
||||
|
||||
} else {
|
||||
_performance_data.unsuccessful_sends++;
|
||||
}
|
||||
}
|
||||
|
||||
void MspOsd::parameters_update()
|
||||
{
|
||||
// update our display rate and dwell time
|
||||
_display.set_period(hrt_abstime(_param_osd_scroll_rate.get() * 1000ULL));
|
||||
_display.set_dwell(hrt_abstime(_param_osd_dwell_time.get() * 1000ULL));
|
||||
}
|
||||
|
||||
bool MspOsd::enabled(const SymbolIndex &symbol)
|
||||
{
|
||||
return _param_osd_symbols.get() & (1u << symbol);
|
||||
}
|
||||
|
||||
int MspOsd::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
MspOsd *instance = new MspOsd();
|
||||
// initialize device
|
||||
const char *device = nullptr;
|
||||
bool error_flag = false;
|
||||
|
||||
// loop through input arguments
|
||||
int myoptind = 1;
|
||||
int ch;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'd':
|
||||
device = myoptarg;
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_WARN("unrecognized flag");
|
||||
error_flag = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (error_flag) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
if (!device) {
|
||||
PX4_ERR("Missing device");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
MspOsd *instance = new MspOsd(device);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
|
|
@ -478,7 +453,17 @@ int MspOsd::task_spawn(int argc, char *argv[])
|
|||
|
||||
int MspOsd::print_status()
|
||||
{
|
||||
PX4_INFO("Running");
|
||||
PX4_INFO("Running on %s", _device);
|
||||
PX4_INFO("\tinitialized: %d", _is_initialized);
|
||||
PX4_INFO("\tinitialization issues: %d", _performance_data.initialization_problems);
|
||||
PX4_INFO("\tscroll rate: %d", static_cast<int>(_param_osd_scroll_rate.get()));
|
||||
PX4_INFO("\tsuccessful sends: %lu", _performance_data.successful_sends);
|
||||
PX4_INFO("\tunsuccessful sends: %lu", _performance_data.unsuccessful_sends);
|
||||
|
||||
// print current display string
|
||||
char msg[FULL_MSG_BUFFER];
|
||||
_display.get(msg, hrt_absolute_time());
|
||||
PX4_INFO("Current message: \n\t%s", msg);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -41,29 +41,67 @@
|
|||
#include <uORB/SubscriptionInterval.hpp>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/airspeed_validated.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
#include <uORB/topics/log_message.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/power_monitor.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/airspeed_validated.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
#include "MspV1.hpp"
|
||||
#include "MessageDisplay/MessageDisplay.hpp"
|
||||
#include "uorb_to_msp.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
// location to "hide" unused display elements
|
||||
#define LOCATION_HIDDEN 234;
|
||||
|
||||
struct PerformanceData {
|
||||
bool initialization_problems{false};
|
||||
long unsigned int successful_sends{0};
|
||||
long unsigned int unsuccessful_sends{0};
|
||||
};
|
||||
|
||||
// mapping from symbol name to bit in the parameter bitmask
|
||||
// @TODO investigate params; it seems like this should be available directly?
|
||||
enum SymbolIndex : uint8_t {
|
||||
CRAFT_NAME = 0,
|
||||
DISARMED = 1,
|
||||
GPS_LAT = 2,
|
||||
GPS_LON = 3,
|
||||
GPS_SATS = 4,
|
||||
GPS_SPEED = 5,
|
||||
HOME_DIST = 6,
|
||||
HOME_DIR = 7,
|
||||
MAIN_BATT_VOLTAGE = 8,
|
||||
CURRENT_DRAW = 9,
|
||||
MAH_DRAWN = 10,
|
||||
RSSI_VALUE = 11,
|
||||
ALTITUDE = 12,
|
||||
NUMERICAL_VARIO = 13,
|
||||
FLYMODE = 14,
|
||||
ESC_TMP = 15,
|
||||
PITCH_ANGLE = 16,
|
||||
ROLL_ANGLE = 17,
|
||||
CROSSHAIRS = 18,
|
||||
AVG_CELL_VOLTAGE = 19,
|
||||
HORIZON_SIDEBARS = 20,
|
||||
POWER = 21
|
||||
};
|
||||
|
||||
class MspOsd : public ModuleBase<MspOsd>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
MspOsd();
|
||||
MspOsd(const char *device);
|
||||
|
||||
~MspOsd() override;
|
||||
|
||||
|
|
@ -84,44 +122,55 @@ public:
|
|||
private:
|
||||
void Run() override;
|
||||
|
||||
MspV1 _msp;
|
||||
int _msp_fd{-1};
|
||||
|
||||
bool _is_initialized{false};
|
||||
|
||||
//uORB::Subscription power_monitor_sub(ORB_ID(power_monitor));
|
||||
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
|
||||
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
||||
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
|
||||
uORB::Subscription _home_position_sub{ORB_ID(home_position)};
|
||||
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _input_rc_sub{ORB_ID(input_rc)};
|
||||
|
||||
struct battery_status_s _battery_status_struct = {0};
|
||||
struct vehicle_status_s _vehicle_status_struct;
|
||||
struct sensor_gps_s _vehicle_gps_position_struct = {0};
|
||||
struct airspeed_validated_s _airspeed_validated_struct = {0};
|
||||
struct vehicle_air_data_s _vehicle_air_data_struct = {0};
|
||||
struct home_position_s _home_position_struct = {0};
|
||||
struct vehicle_global_position_s _vehicle_global_position_struct = {0};
|
||||
struct vehicle_attitude_s _vehicle_attitude_struct = {0};
|
||||
struct estimator_status_s _estimator_status_struct = {0};
|
||||
struct vehicle_local_position_s _vehicle_local_position_struct = {0};
|
||||
struct input_rc_s _input_rc_struct = {0};
|
||||
|
||||
// update a single display element in the display
|
||||
void Send(const unsigned int message_type, const void *payload);
|
||||
|
||||
// send full configuration to MSP (triggers the actual update)
|
||||
void SendConfig();
|
||||
void SendTelemetry();
|
||||
|
||||
uint8_t _x{0};
|
||||
bool _heartbeat{false};
|
||||
// perform actions required for local updates
|
||||
void parameters_update();
|
||||
|
||||
// convenience function to check if a given symbol is enabled
|
||||
bool enabled(const SymbolIndex &symbol);
|
||||
|
||||
MspV1 _msp{0};
|
||||
int _msp_fd{-1};
|
||||
|
||||
msp_osd::MessageDisplay _display{};
|
||||
|
||||
bool _is_initialized{false};
|
||||
|
||||
// subscriptions to desired vehicle display information
|
||||
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
||||
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
||||
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
|
||||
uORB::Subscription _home_position_sub{ORB_ID(home_position)};
|
||||
uORB::Subscription _input_rc_sub{ORB_ID(input_rc)};
|
||||
uORB::Subscription _log_message_sub{ORB_ID(log_message)};
|
||||
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
|
||||
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
// local heartbeat
|
||||
bool _heartbeat{false};
|
||||
|
||||
// parameters
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::OSD_SYMBOLS>) _param_osd_symbols,
|
||||
(ParamInt<px4::params::OSD_SCROLL_RATE>) _param_osd_scroll_rate,
|
||||
(ParamInt<px4::params::OSD_DWELL_TIME>) _param_osd_dwell_time,
|
||||
(ParamInt<px4::params::OSD_LOG_LEVEL>) _param_osd_log_level
|
||||
)
|
||||
|
||||
// metadata
|
||||
char _device[64] {};
|
||||
PerformanceData _performance_data{};
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,439 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* uorb_to_msp.cpp
|
||||
*
|
||||
* Implementation file for UORB -> MSP conversion functions.
|
||||
*/
|
||||
|
||||
// includes for mathematical manipulation
|
||||
#include <math.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
// clock access
|
||||
#include <px4_platform_common/defines.h>
|
||||
using namespace time_literals;
|
||||
|
||||
#include "uorb_to_msp.hpp"
|
||||
|
||||
namespace msp_osd
|
||||
{
|
||||
|
||||
msp_name_t construct_display_message(const vehicle_status_s &vehicle_status,
|
||||
const vehicle_attitude_s &vehicle_attitude,
|
||||
const log_message_s &log_message,
|
||||
const int log_level,
|
||||
MessageDisplay &display)
|
||||
{
|
||||
// initialize result
|
||||
msp_name_t display_message {0};
|
||||
|
||||
const auto now = hrt_absolute_time();
|
||||
static uint64_t last_warning_stamp {0};
|
||||
|
||||
// update arming state, flight mode, and warnings, if current
|
||||
if (vehicle_status.timestamp < (now - 1_s)) {
|
||||
display.set(MessageDisplayType::ARMING, "???");
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "???");
|
||||
|
||||
} else {
|
||||
// display armed / disarmed
|
||||
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
display.set(MessageDisplayType::ARMING, "ARM");
|
||||
|
||||
} else {
|
||||
display.set(MessageDisplayType::ARMING, "DSRM");
|
||||
}
|
||||
|
||||
// display flight mode
|
||||
switch (vehicle_status.nav_state) {
|
||||
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "MANUAL");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "ALTCTL");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "POSCTL");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "AUTO_MISSION");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "AUTO_LOITER");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "AUTO_RTL");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_UNUSED:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "UNUSED");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_ACRO:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "ACRO");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_UNUSED1:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "UNUSED1");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "DESCEND");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "TERMINATION");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "OFFBOARD");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_STAB:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "STAB");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_UNUSED2:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "UNUSED2");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "AUTO_TAKEOFF");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "AUTO_LAND");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "AUTO_FOLLOW_TARGET");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "AUTO_PRECLAND");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_ORBIT:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "ORBIT");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "AUTO_VTOL_TAKEOFF");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_MAX:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "MAX");
|
||||
break;
|
||||
|
||||
default:
|
||||
display.set(MessageDisplayType::FLIGHT_MODE, "???");
|
||||
}
|
||||
}
|
||||
|
||||
// display, if updated
|
||||
if (log_message.severity <= log_level) {
|
||||
display.set(MessageDisplayType::WARNING, log_message.text);
|
||||
last_warning_stamp = now;
|
||||
|
||||
} else if (now - last_warning_stamp > 30_s) {
|
||||
// clear warning after timeout
|
||||
display.set(MessageDisplayType::WARNING, "");
|
||||
last_warning_stamp = now;
|
||||
}
|
||||
|
||||
// update heading, if relatively recent
|
||||
if (vehicle_attitude.timestamp < (now - 1_s)) {
|
||||
display.set(MessageDisplayType::HEADING, "N?");
|
||||
|
||||
} else {
|
||||
// convert to YAW
|
||||
matrix::Eulerf euler_attitude(matrix::Quatf(vehicle_attitude.q));
|
||||
const auto yaw = math::degrees(euler_attitude.psi());
|
||||
|
||||
// display north direction
|
||||
if (yaw <= 22.5f) {
|
||||
display.set(MessageDisplayType::HEADING, "N");
|
||||
|
||||
} else if (yaw <= 67.5f) {
|
||||
display.set(MessageDisplayType::HEADING, "NE");
|
||||
|
||||
} else if (yaw <= 112.5f) {
|
||||
display.set(MessageDisplayType::HEADING, "E");
|
||||
|
||||
} else if (yaw <= 157.5f) {
|
||||
display.set(MessageDisplayType::HEADING, "SE");
|
||||
|
||||
} else if (yaw <= 202.5f) {
|
||||
display.set(MessageDisplayType::HEADING, "S");
|
||||
|
||||
} else if (yaw <= 247.5f) {
|
||||
display.set(MessageDisplayType::HEADING, "SW");
|
||||
|
||||
} else if (yaw <= 292.5f) {
|
||||
display.set(MessageDisplayType::HEADING, "W");
|
||||
|
||||
} else if (yaw <= 337.5f) {
|
||||
display.set(MessageDisplayType::HEADING, "NW");
|
||||
|
||||
} else if (yaw <= 360.0f) {
|
||||
display.set(MessageDisplayType::HEADING, "N");
|
||||
}
|
||||
}
|
||||
|
||||
// update message and return
|
||||
display.get(display_message.craft_name, hrt_absolute_time());
|
||||
return display_message;
|
||||
}
|
||||
|
||||
msp_fc_variant_t construct_FC_VARIANT()
|
||||
{
|
||||
// initialize result
|
||||
msp_fc_variant_t variant{};
|
||||
|
||||
memcpy(variant.flightControlIdentifier, "BTFL", sizeof(variant.flightControlIdentifier));
|
||||
return variant;
|
||||
}
|
||||
|
||||
msp_status_BF_t construct_STATUS(const vehicle_status_s &vehicle_status)
|
||||
{
|
||||
// initialize result
|
||||
msp_status_BF_t status_BF = {0};
|
||||
|
||||
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
status_BF.flight_mode_flags |= ARM_ACRO_BF;
|
||||
|
||||
switch (vehicle_status.nav_state) {
|
||||
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
|
||||
status_BF.flight_mode_flags |= 0;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_ACRO:
|
||||
status_BF.flight_mode_flags |= 0;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_STAB:
|
||||
status_BF.flight_mode_flags |= STAB_BF;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
|
||||
status_BF.flight_mode_flags |= RESC_BF;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
|
||||
status_BF.flight_mode_flags |= FS_BF;
|
||||
break;
|
||||
|
||||
default:
|
||||
status_BF.flight_mode_flags = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
status_BF.arming_disable_flags_count = 1;
|
||||
status_BF.arming_disable_flags = !(vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
||||
return status_BF;
|
||||
}
|
||||
|
||||
msp_analog_t construct_ANALOG(const battery_status_s &battery_status, const input_rc_s &input_rc)
|
||||
{
|
||||
// initialize result
|
||||
msp_analog_t analog {0};
|
||||
|
||||
analog.vbat = battery_status.voltage_v * 10; // bottom right... v * 10
|
||||
analog.rssi = (uint16_t)((input_rc.rssi * 1023.0f) / 100.0f);
|
||||
analog.amperage = battery_status.current_a * 100; // main amperage
|
||||
analog.mAhDrawn = battery_status.discharged_mah; // unused
|
||||
|
||||
return analog;
|
||||
}
|
||||
|
||||
msp_battery_state_t construct_BATTERY_STATE(const battery_status_s &battery_status)
|
||||
{
|
||||
// initialize result
|
||||
msp_battery_state_t battery_state = {0};
|
||||
|
||||
// MSP_BATTERY_STATE
|
||||
battery_state.amperage = battery_status.current_a; // not used?
|
||||
battery_state.batteryVoltage = (uint16_t)(battery_status.voltage_v * 400.0f); // OK
|
||||
battery_state.mAhDrawn = battery_status.discharged_mah ; // OK
|
||||
battery_state.batteryCellCount = battery_status.cell_count;
|
||||
battery_state.batteryCapacity = battery_status.capacity; // not used?
|
||||
|
||||
// Voltage color 0==white, 1==red
|
||||
if (battery_status.voltage_v < 14.4f) {
|
||||
battery_state.batteryState = 1;
|
||||
|
||||
} else {
|
||||
battery_state.batteryState = 0;
|
||||
}
|
||||
|
||||
battery_state.legacyBatteryVoltage = battery_status.voltage_v * 10;
|
||||
return battery_state;
|
||||
}
|
||||
|
||||
msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position,
|
||||
const airspeed_validated_s &airspeed_validated)
|
||||
{
|
||||
// initialize result
|
||||
msp_raw_gps_t raw_gps {0};
|
||||
|
||||
if (vehicle_gps_position.fix_type >= 2) {
|
||||
raw_gps.lat = vehicle_gps_position.lat;
|
||||
raw_gps.lon = vehicle_gps_position.lon;
|
||||
raw_gps.alt = vehicle_gps_position.alt / 10;
|
||||
//raw_gps.groundCourse = vehicle_gps_position_struct
|
||||
|
||||
} else {
|
||||
raw_gps.lat = 0;
|
||||
raw_gps.lon = 0;
|
||||
raw_gps.alt = 0;
|
||||
}
|
||||
|
||||
if (vehicle_gps_position.fix_type == 0
|
||||
|| vehicle_gps_position.fix_type == 1) {
|
||||
raw_gps.fixType = MSP_GPS_NO_FIX;
|
||||
|
||||
} else if (vehicle_gps_position.fix_type == 2) {
|
||||
raw_gps.fixType = MSP_GPS_FIX_2D;
|
||||
|
||||
} else if (vehicle_gps_position.fix_type >= 3 && vehicle_gps_position.fix_type <= 5) {
|
||||
raw_gps.fixType = MSP_GPS_FIX_3D;
|
||||
|
||||
} else {
|
||||
raw_gps.fixType = MSP_GPS_NO_FIX;
|
||||
}
|
||||
|
||||
//raw_gps.hdop = vehicle_gps_position_struct.hdop
|
||||
raw_gps.numSat = vehicle_gps_position.satellites_used;
|
||||
|
||||
if (airspeed_validated.airspeed_sensor_measurement_valid
|
||||
&& airspeed_validated.indicated_airspeed_m_s != NAN
|
||||
&& airspeed_validated.indicated_airspeed_m_s > 0) {
|
||||
raw_gps.groundSpeed = airspeed_validated.indicated_airspeed_m_s * 100;
|
||||
|
||||
} else {
|
||||
raw_gps.groundSpeed = 0;
|
||||
}
|
||||
|
||||
return raw_gps;
|
||||
}
|
||||
|
||||
msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
|
||||
const estimator_status_s &estimator_status,
|
||||
const vehicle_global_position_s &vehicle_global_position,
|
||||
const bool heartbeat)
|
||||
{
|
||||
// initialize result
|
||||
msp_comp_gps_t comp_gps {0};
|
||||
|
||||
// Calculate distance and direction to home
|
||||
if (home_position.valid_hpos
|
||||
&& home_position.valid_lpos
|
||||
&& estimator_status.solution_status_flags & (1 << 4)) {
|
||||
float bearing_to_home = get_bearing_to_next_waypoint(vehicle_global_position.lat,
|
||||
vehicle_global_position.lon,
|
||||
home_position.lat, home_position.lon);
|
||||
|
||||
float distance_to_home = get_distance_to_next_waypoint(vehicle_global_position.lat,
|
||||
vehicle_global_position.lon,
|
||||
home_position.lat, home_position.lon);
|
||||
|
||||
comp_gps.distanceToHome = (int16_t)distance_to_home; // meters
|
||||
comp_gps.directionToHome = bearing_to_home; // degrees
|
||||
|
||||
} else {
|
||||
comp_gps.distanceToHome = 0; // meters
|
||||
comp_gps.directionToHome = 0; // degrees
|
||||
}
|
||||
|
||||
comp_gps.heartbeat = heartbeat;
|
||||
return comp_gps;
|
||||
}
|
||||
|
||||
msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude)
|
||||
{
|
||||
// initialize results
|
||||
msp_attitude_t attitude {0};
|
||||
|
||||
// convert from quaternion to RPY
|
||||
matrix::Eulerf euler_attitude(matrix::Quatf(vehicle_attitude.q));
|
||||
attitude.pitch = math::degrees(euler_attitude.theta()) * 10;
|
||||
attitude.roll = math::degrees(euler_attitude.phi()) * 10;
|
||||
attitude.yaw = math::degrees(euler_attitude.psi()) * 10;
|
||||
|
||||
return attitude;
|
||||
}
|
||||
|
||||
msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
|
||||
const estimator_status_s &estimator_status,
|
||||
const vehicle_local_position_s &vehicle_local_position)
|
||||
{
|
||||
// initialize result
|
||||
msp_altitude_t altitude {0};
|
||||
|
||||
if (vehicle_gps_position.fix_type >= 2) {
|
||||
altitude.estimatedActualPosition = vehicle_gps_position.alt / 10;
|
||||
|
||||
} else {
|
||||
altitude.estimatedActualPosition = 0;
|
||||
}
|
||||
|
||||
if (estimator_status.solution_status_flags & (1 << 5)) {
|
||||
altitude.estimatedActualVelocity = -vehicle_local_position.vz * 10; //m/s to cm/s
|
||||
|
||||
} else {
|
||||
altitude.estimatedActualVelocity = 0;
|
||||
}
|
||||
|
||||
return altitude;
|
||||
}
|
||||
|
||||
msp_esc_sensor_data_dji_t construct_ESC_SENSOR_DATA()
|
||||
{
|
||||
// initialize result
|
||||
msp_esc_sensor_data_dji_t esc_sensor_data {0};
|
||||
|
||||
esc_sensor_data.rpm = 0;
|
||||
esc_sensor_data.temperature = 50;
|
||||
|
||||
return esc_sensor_data;
|
||||
}
|
||||
|
||||
} // namespace msp_osd
|
||||
|
|
@ -0,0 +1,112 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* uorb_to_msp.hpp
|
||||
*
|
||||
* Declaration of functions which translate UORB messages into MSP specific structures.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
// basic types
|
||||
#include <cmath>
|
||||
|
||||
// UORB topic structs
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/power_monitor.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/airspeed_validated.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
#include <uORB/topics/log_message.h>
|
||||
|
||||
// PX4 events interface
|
||||
#include <px4_platform_common/events.h>
|
||||
|
||||
// MSP structs
|
||||
#include "msp_defines.h"
|
||||
#include "MessageDisplay/MessageDisplay.hpp"
|
||||
|
||||
namespace msp_osd
|
||||
{
|
||||
|
||||
// construct an MSP_NAME struct
|
||||
// note: this is actually how we display _all_ string information
|
||||
msp_name_t construct_display_message(const vehicle_status_s &vehicle_status,
|
||||
const vehicle_attitude_s &vehicle_attitude,
|
||||
const log_message_s &log_message,
|
||||
const int log_level,
|
||||
MessageDisplay &display);
|
||||
|
||||
// construct an MSP_FC_VARIANT struct
|
||||
msp_fc_variant_t construct_FC_VARIANT();
|
||||
|
||||
// construct an MSP_STATUS struct
|
||||
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);
|
||||
|
||||
// construct an MSP_BATTERY_STATE struct
|
||||
msp_battery_state_t construct_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);
|
||||
|
||||
// construct an MSP_COMP_GPS struct
|
||||
msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
|
||||
const estimator_status_s &estimator_status,
|
||||
const vehicle_global_position_s &vehicle_global_position,
|
||||
const bool heartbeat);
|
||||
|
||||
// construct an MSP_ATTITUDE struct
|
||||
msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude);
|
||||
|
||||
// construct an MSP_ALTITUDE struct
|
||||
msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
|
||||
const estimator_status_s &estimator_status,
|
||||
const vehicle_local_position_s &vehicle_local_position);
|
||||
|
||||
// construct an MSP_ESC_SENSOR_DATA struct
|
||||
msp_esc_sensor_data_dji_t construct_ESC_SENSOR_DATA();
|
||||
|
||||
} // namespace msp_osd
|
||||
Loading…
Reference in New Issue