mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
MSP_OSD code changes to support OpenIPC, DJI O3/O4 OSD rendering (#24695)
* 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 <Li.Tianming@example.com> Co-authored-by: Li.Tianming <Li.Tianming>
This commit is contained in:
Li-Tianming
GitHub
parent
38548ded12
commit
815a339684
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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};
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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()
|
||||
{
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user