Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Merge pull request #6062 from machadofelipe/machado_mavlink_update

Browse source 
[MAVLINK] Add new mavlink messages
This commit is contained in:
Konstantin Sharlaimov 2020-10-10 10:52:53 +01:00 • committed by GitHub
commit 04a0a92839
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
155 changed files with 10723 additions and 6148 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/fc/settings.yaml
View file

@ -2547,6 +2547,11 @@ groups:
type: uint8_t
min: 0
max: 255
- name: mavlink_extra3_rate
field: mavlink.extra3_rate
type: uint8_t
min: 0
max: 255
- name: PG_ELERES_CONFIG
type: eleresConfig_t
headers: ["rx/eleres.h"]

303
src/main/io/osd.c
View file

@ -621,10 +621,6 @@ static void osdFormatCraftName(char *buff)
}
}
// Used twice, make sure it's exactly the same string
// to save some memory
#define RC_RX_LINK_LOST_MSG "!RC RX LINK LOST!"
static const char * osdArmingDisabledReasonMessage(void)
{
switch (isArmingDisabledReason()) {
@ -635,18 +631,18 @@ static const char * osdArmingDisabledReasonMessage(void)
// If we're not using sticks, it means the ARM switch
// hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING
// yet
return OSD_MESSAGE_STR("TURN ARM SWITCH OFF");
return OSD_MESSAGE_STR(OSD_MSG_TURN_ARM_SW_OFF);
}
// Not receiving RX data
return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST);
}
return OSD_MESSAGE_STR("DISABLED BY FAILSAFE");
return OSD_MESSAGE_STR(OSD_MSG_DISABLED_BY_FS);
case ARMING_DISABLED_NOT_LEVEL:
return OSD_MESSAGE_STR("AIRCRAFT IS NOT LEVEL");
return OSD_MESSAGE_STR(OSD_MSG_AIRCRAFT_UNLEVEL);
case ARMING_DISABLED_SENSORS_CALIBRATING:
return OSD_MESSAGE_STR("SENSORS CALIBRATING");
return OSD_MESSAGE_STR(OSD_MSG_SENSORS_CAL);
case ARMING_DISABLED_SYSTEM_OVERLOADED:
return OSD_MESSAGE_STR("SYSTEM OVERLOADED");
return OSD_MESSAGE_STR(OSD_MSG_SYS_OVERLOADED);
case ARMING_DISABLED_NAVIGATION_UNSAFE:
#if defined(USE_NAV)
// Check the exact reason
@ -654,67 +650,67 @@ static const char * osdArmingDisabledReasonMessage(void)
case NAV_ARMING_BLOCKER_NONE:
break;
case NAV_ARMING_BLOCKER_MISSING_GPS_FIX:
return OSD_MESSAGE_STR("WAITING FOR GPS FIX");
return OSD_MESSAGE_STR(OSD_MSG_WAITING_GPS_FIX);
case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE:
return OSD_MESSAGE_STR("DISABLE NAVIGATION FIRST");
return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST);
case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR:
return OSD_MESSAGE_STR("FIRST WAYPOINT IS TOO FAR");
return OSD_MESSAGE_STR(OSD_MSG_1ST_WP_TOO_FAR);
case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR:
return OSD_MESSAGE_STR("JUMP WAYPOINT MISCONFIGURED");
return OSD_MESSAGE_STR(OSD_MSG_JUMP_WP_MISCONFIG);
}
#endif
break;
case ARMING_DISABLED_COMPASS_NOT_CALIBRATED:
return OSD_MESSAGE_STR("COMPASS NOT CALIBRATED");
return OSD_MESSAGE_STR(OSD_MSG_MAG_NOT_CAL);
case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED:
return OSD_MESSAGE_STR("ACCELEROMETER NOT CALIBRATED");
return OSD_MESSAGE_STR(OSD_MSG_ACC_NOT_CAL);
case ARMING_DISABLED_ARM_SWITCH:
return OSD_MESSAGE_STR("DISABLE ARM SWITCH FIRST");
return OSD_MESSAGE_STR(OSD_MSG_DISARM_1ST);
case ARMING_DISABLED_HARDWARE_FAILURE:
{
if (!HW_SENSOR_IS_HEALTHY(getHwGyroStatus())) {
return OSD_MESSAGE_STR("GYRO FAILURE");
return OSD_MESSAGE_STR(OSD_MSG_GYRO_FAILURE);
}
if (!HW_SENSOR_IS_HEALTHY(getHwAccelerometerStatus())) {
return OSD_MESSAGE_STR("ACCELEROMETER FAILURE");
return OSD_MESSAGE_STR(OSD_MSG_ACC_FAIL);
}
if (!HW_SENSOR_IS_HEALTHY(getHwCompassStatus())) {
return OSD_MESSAGE_STR("COMPASS FAILURE");
return OSD_MESSAGE_STR(OSD_MSG_MAG_FAIL);
}
if (!HW_SENSOR_IS_HEALTHY(getHwBarometerStatus())) {
return OSD_MESSAGE_STR("BAROMETER FAILURE");
return OSD_MESSAGE_STR(OSD_MSG_BARO_FAIL);
}
if (!HW_SENSOR_IS_HEALTHY(getHwGPSStatus())) {
return OSD_MESSAGE_STR("GPS FAILURE");
return OSD_MESSAGE_STR(OSD_MSG_GPS_FAIL);
}
if (!HW_SENSOR_IS_HEALTHY(getHwRangefinderStatus())) {
return OSD_MESSAGE_STR("RANGE FINDER FAILURE");
return OSD_MESSAGE_STR(OSD_MSG_RANGEFINDER_FAIL);
}
if (!HW_SENSOR_IS_HEALTHY(getHwPitotmeterStatus())) {
return OSD_MESSAGE_STR("PITOT METER FAILURE");
return OSD_MESSAGE_STR(OSD_MSG_PITOT_FAIL);
}
}
return OSD_MESSAGE_STR("HARDWARE FAILURE");
return OSD_MESSAGE_STR(OSD_MSG_HW_FAIL);
case ARMING_DISABLED_BOXFAILSAFE:
return OSD_MESSAGE_STR("FAILSAFE MODE ENABLED");
return OSD_MESSAGE_STR(OSD_MSG_FS_EN);
case ARMING_DISABLED_BOXKILLSWITCH:
return OSD_MESSAGE_STR("KILLSWITCH MODE ENABLED");
return OSD_MESSAGE_STR(OSD_MSG_KILL_SW_EN);
case ARMING_DISABLED_RC_LINK:
return OSD_MESSAGE_STR("NO RC LINK");
return OSD_MESSAGE_STR(OSD_MSG_NO_RC_LINK);
case ARMING_DISABLED_THROTTLE:
return OSD_MESSAGE_STR("THROTTLE IS NOT LOW");
return OSD_MESSAGE_STR(OSD_MSG_THROTTLE_NOT_LOW);
case ARMING_DISABLED_ROLLPITCH_NOT_CENTERED:
return OSD_MESSAGE_STR("ROLLPITCH NOT CENTERED");
return OSD_MESSAGE_STR(OSD_MSG_ROLLPITCH_OFFCENTER);
case ARMING_DISABLED_SERVO_AUTOTRIM:
return OSD_MESSAGE_STR("AUTOTRIM IS ACTIVE");
return OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM_ACTIVE);
case ARMING_DISABLED_OOM:
return OSD_MESSAGE_STR("NOT ENOUGH MEMORY");
return OSD_MESSAGE_STR(OSD_MSG_NOT_ENOUGH_MEMORY);
case ARMING_DISABLED_INVALID_SETTING:
return OSD_MESSAGE_STR("INVALID SETTING");
return OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING);
case ARMING_DISABLED_CLI:
return OSD_MESSAGE_STR("CLI IS ACTIVE");
return OSD_MESSAGE_STR(OSD_MSG_CLI_ACTIVE);
case ARMING_DISABLED_PWM_OUTPUT_ERROR:
return OSD_MESSAGE_STR("PWM INIT ERROR");
return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR);
// Cases without message
case ARMING_DISABLED_CMS_MENU:
FALLTHROUGH;
@ -737,11 +733,11 @@ static const char * osdFailsafePhaseMessage(void)
#ifdef USE_NAV
case FAILSAFE_RETURN_TO_HOME:
// XXX: Keep this in sync with OSD_FLYMODE.
return OSD_MESSAGE_STR("(RTH)");
return OSD_MESSAGE_STR(OSD_MSG_RTH_FS);
#endif
case FAILSAFE_LANDING:
// This should be considered an emergengy landing
return OSD_MESSAGE_STR("(EMERGENCY LANDING)");
return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING_FS);
case FAILSAFE_RX_LOSS_MONITORING:
// Only reachable from FAILSAFE_LANDED, which performs
// a disarm. Since aircraft has been disarmed, we no
@ -776,9 +772,9 @@ static const char * osdFailsafeInfoMessage(void)
{
if (failsafeIsReceivingRxData()) {
// User must move sticks to exit FS mode
return OSD_MESSAGE_STR("!MOVE STICKS TO EXIT FS!");
return OSD_MESSAGE_STR(OSD_MSG_MOVE_EXIT_FS);
}
return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST);
}
static const char * navigationStateMessage(void)
@ -787,22 +783,22 @@ static const char * navigationStateMessage(void)
case MW_NAV_STATE_NONE:
break;
case MW_NAV_STATE_RTH_START:
return OSD_MESSAGE_STR("STARTING RTH");
return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH);
case MW_NAV_STATE_RTH_ENROUTE:
// TODO: Break this up between climb and head home
return OSD_MESSAGE_STR("EN ROUTE TO HOME");
return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME);
case MW_NAV_STATE_HOLD_INFINIT:
// Used by HOLD flight modes. No information to add.
break;
case MW_NAV_STATE_HOLD_TIMED:
// TODO: Maybe we can display a count down
return OSD_MESSAGE_STR("HOLDING WAYPOINT");
return OSD_MESSAGE_STR(OSD_MSG_HOLDING_WAYPOINT);
break;
case MW_NAV_STATE_WP_ENROUTE:
// TODO: Show WP number
return OSD_MESSAGE_STR("TO WP");
return OSD_MESSAGE_STR(OSD_MSG_TO_WP);
case MW_NAV_STATE_PROCESS_NEXT:
return OSD_MESSAGE_STR("PREPARING FOR NEXT WAYPOINT");
return OSD_MESSAGE_STR(OSD_MSG_PREPARE_NEXT_WP);
case MW_NAV_STATE_DO_JUMP:
// Not used
break;
@ -810,18 +806,18 @@ static const char * navigationStateMessage(void)
// Not used
break;
case MW_NAV_STATE_EMERGENCY_LANDING:
return OSD_MESSAGE_STR("EMERGENCY LANDING");
return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING);
case MW_NAV_STATE_LAND_IN_PROGRESS:
return OSD_MESSAGE_STR("LANDING");
return OSD_MESSAGE_STR(OSD_MSG_LANDING);
case MW_NAV_STATE_HOVER_ABOVE_HOME:
if (STATE(FIXED_WING_LEGACY)) {
return OSD_MESSAGE_STR("LOITERING AROUND HOME");
return OSD_MESSAGE_STR(OSD_MSG_LOITERING_HOME);
}
return OSD_MESSAGE_STR("HOVERING");
return OSD_MESSAGE_STR(OSD_MSG_HOVERING);
case MW_NAV_STATE_LANDED:
return OSD_MESSAGE_STR("LANDED");
return OSD_MESSAGE_STR(OSD_MSG_LANDED);
case MW_NAV_STATE_LAND_SETTLE:
return OSD_MESSAGE_STR("PREPARING TO LAND");
return OSD_MESSAGE_STR(OSD_MSG_PREPARING_LAND);
case MW_NAV_STATE_LAND_START_DESCENT:
// Not used
break;
@ -829,15 +825,14 @@ static const char * navigationStateMessage(void)
return NULL;
}
static void osdFormatMessage(char *buff, size_t size, const char *message)
static void osdFormatMessage(char *buff, size_t size, const char *message, bool isCenteredText)
{
// String is always filled with Blanks
memset(buff, SYM_BLANK, size);
if (message) {
int messageLength = strlen(message);
int rem = MAX(0, OSD_MESSAGE_LENGTH - (int)messageLength);
// Don't finish the string at the end of the message,
// write the rest of the blanks.
strncpy(buff + rem / 2, message, MIN(OSD_MESSAGE_LENGTH - rem / 2, messageLength));
size_t messageLength = strlen(message);
int rem = isCenteredText ? MAX(0, (int)size - (int)messageLength) : 0;
strncpy(buff + rem / 2, message, MIN((int)size - rem / 2, (int)messageLength));
}
// Ensure buff is zero terminated
buff[size - 1] = '\0';
@ -2051,99 +2046,7 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_MESSAGES:
{
const char *message = NULL;
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
if (ARMING_FLAG(ARMED)) {
// Aircraft is armed. We might have up to 5
// messages to show.
const char *messages[5];
unsigned messageCount = 0;
if (FLIGHT_MODE(FAILSAFE_MODE)) {
// In FS mode while being armed too
const char *failsafePhaseMessage = osdFailsafePhaseMessage();
const char *failsafeInfoMessage = osdFailsafeInfoMessage();
const char *navStateFSMessage = navigationStateMessage();
if (failsafePhaseMessage) {
messages[messageCount++] = failsafePhaseMessage;
}
if (failsafeInfoMessage) {
messages[messageCount++] = failsafeInfoMessage;
}
if (navStateFSMessage) {
messages[messageCount++] = navStateFSMessage;
}
if (messageCount > 0) {
message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
if (message == failsafeInfoMessage) {
// failsafeInfoMessage is not useful for recovering
// a lost model, but might help avoiding a crash.
// Blink to grab user attention.
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
// We're shoing either failsafePhaseMessage or
// navStateFSMessage. Don't BLINK here since
// having this text available might be crucial
// during a lost aircraft recovery and blinking
// will cause it to be missing from some frames.
}
} else {
if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
const char *navStateMessage = navigationStateMessage();
if (navStateMessage) {
messages[messageCount++] = navStateMessage;
}
} else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
messages[messageCount++] = "AUTOLAUNCH";
} else {
if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
// ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
// when it doesn't require ANGLE mode (required only in FW
// right now). If if requires ANGLE, its display is handled
// by OSD_FLYMODE.
messages[messageCount++] = "(ALTITUDE HOLD)";
}
if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
messages[messageCount++] = "(AUTOTRIM)";
}
if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
messages[messageCount++] = "(AUTOTUNE)";
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
messages[messageCount++] = "(HEADFREE)";
}
}
// Pick one of the available messages. Each message lasts
// a second.
if (messageCount > 0) {
message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
}
}
} else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
unsigned invalidIndex;
// Check if we're unable to arm for some reason
if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
const setting_t *setting = settingGet(invalidIndex);
settingGetName(setting, messageBuf);
for (int ii = 0; messageBuf[ii]; ii++) {
messageBuf[ii] = sl_toupper(messageBuf[ii]);
}
message = messageBuf;
} else {
message = "INVALID SETTING";
TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
}
} else {
if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
message = "UNABLE TO ARM";
TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
} else {
// Show the reason for not arming
message = osdArmingDisabledReasonMessage();
}
}
}
osdFormatMessage(buff, sizeof(buff), message);
elemAttr = osdGetSystemMessage(buff, OSD_MESSAGE_LENGTH, true);
break;
}
@ -3356,4 +3259,106 @@ displayCanvas_t *osdGetDisplayPortCanvas(void)
return NULL;
}
textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenteredText)
{
textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
if (buff != NULL) {
const char *message = NULL;
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
if (ARMING_FLAG(ARMED)) {
// Aircraft is armed. We might have up to 5
// messages to show.
const char *messages[5];
unsigned messageCount = 0;
if (FLIGHT_MODE(FAILSAFE_MODE)) {
// In FS mode while being armed too
const char *failsafePhaseMessage = osdFailsafePhaseMessage();
const char *failsafeInfoMessage = osdFailsafeInfoMessage();
const char *navStateFSMessage = navigationStateMessage();
if (failsafePhaseMessage) {
messages[messageCount++] = failsafePhaseMessage;
}
if (failsafeInfoMessage) {
messages[messageCount++] = failsafeInfoMessage;
}
if (navStateFSMessage) {
messages[messageCount++] = navStateFSMessage;
}
if (messageCount > 0) {
message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
if (message == failsafeInfoMessage) {
// failsafeInfoMessage is not useful for recovering
// a lost model, but might help avoiding a crash.
// Blink to grab user attention.
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
// We're shoing either failsafePhaseMessage or
// navStateFSMessage. Don't BLINK here since
// having this text available might be crucial
// during a lost aircraft recovery and blinking
// will cause it to be missing from some frames.
}
} else {
if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
const char *navStateMessage = navigationStateMessage();
if (navStateMessage) {
messages[messageCount++] = navStateMessage;
}
} else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH);
} else {
if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
// ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
// when it doesn't require ANGLE mode (required only in FW
// right now). If if requires ANGLE, its display is handled
// by OSD_FLYMODE.
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
}
if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM);
}
if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE);
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE);
}
}
// Pick one of the available messages. Each message lasts
// a second.
if (messageCount > 0) {
message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
}
}
} else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
unsigned invalidIndex;
// Check if we're unable to arm for some reason
if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
const setting_t *setting = settingGet(invalidIndex);
settingGetName(setting, messageBuf);
for (int ii = 0; messageBuf[ii]; ii++) {
messageBuf[ii] = sl_toupper(messageBuf[ii]);
}
message = messageBuf;
} else {
message = OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING);
TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
}
} else {
if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
message = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM);
TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
} else {
// Show the reason for not arming
message = osdArmingDisabledReasonMessage();
}
}
}
osdFormatMessage(buff, buff_size, message, isCenteredText);
}
return elemAttr;
}
#endif // OSD

63
src/main/io/osd.h
View file

@ -22,6 +22,7 @@
#include "config/parameter_group.h"
#include "drivers/osd.h"
#include "drivers/display.h"
#ifndef OSD_ALTERNATE_LAYOUT_COUNT
#define OSD_ALTERNATE_LAYOUT_COUNT 3
@ -43,6 +44,59 @@
#define OSD_HOMING_LIM_V2 10
#define OSD_HOMING_LIM_V3 15
// Message defines to be use in OSD and/or telemetry exports
#define OSD_MSG_RC_RX_LINK_LOST "!RC RX LINK LOST!"
#define OSD_MSG_TURN_ARM_SW_OFF "TURN ARM SWITCH OFF"
#define OSD_MSG_DISABLED_BY_FS "DISABLED BY FAILSAFE"
#define OSD_MSG_AIRCRAFT_UNLEVEL "AIRCRAFT IS NOT LEVEL"
#define OSD_MSG_SENSORS_CAL "SENSORS CALIBRATING"
#define OSD_MSG_SYS_OVERLOADED "SYSTEM OVERLOADED"
#define OSD_MSG_WAITING_GPS_FIX "WAITING FOR GPS FIX"
#define OSD_MSG_DISABLE_NAV_FIRST "DISABLE NAVIGATION FIRST"
#define OSD_MSG_1ST_WP_TOO_FAR "FIRST WAYPOINT IS TOO FAR"
#define OSD_MSG_JUMP_WP_MISCONFIG "JUMP WAYPOINT MISCONFIGURED"
#define OSD_MSG_MAG_NOT_CAL "COMPASS NOT CALIBRATED"
#define OSD_MSG_ACC_NOT_CAL "ACCELEROMETER NOT CALIBRATED"
#define OSD_MSG_DISARM_1ST "DISABLE ARM SWITCH FIRST"
#define OSD_MSG_GYRO_FAILURE "GYRO FAILURE"
#define OSD_MSG_ACC_FAIL "ACCELEROMETER FAILURE"
#define OSD_MSG_MAG_FAIL "COMPASS FAILURE"
#define OSD_MSG_BARO_FAIL "BAROMETER FAILURE"
#define OSD_MSG_GPS_FAIL "GPS FAILURE"
#define OSD_MSG_RANGEFINDER_FAIL "RANGE FINDER FAILURE"
#define OSD_MSG_PITOT_FAIL "PITOT METER FAILURE"
#define OSD_MSG_HW_FAIL "HARDWARE FAILURE"
#define OSD_MSG_FS_EN "FAILSAFE MODE ENABLED"
#define OSD_MSG_KILL_SW_EN "KILLSWITCH MODE ENABLED"
#define OSD_MSG_NO_RC_LINK "NO RC LINK"
#define OSD_MSG_THROTTLE_NOT_LOW "THROTTLE IS NOT LOW"
#define OSD_MSG_ROLLPITCH_OFFCENTER "ROLLPITCH NOT CENTERED"
#define OSD_MSG_AUTOTRIM_ACTIVE "AUTOTRIM IS ACTIVE"
#define OSD_MSG_NOT_ENOUGH_MEMORY "NOT ENOUGH MEMORY"
#define OSD_MSG_INVALID_SETTING "INVALID SETTING"
#define OSD_MSG_CLI_ACTIVE "CLI IS ACTIVE"
#define OSD_MSG_PWM_INIT_ERROR "PWM INIT ERROR"
#define OSD_MSG_RTH_FS "(RTH)"
#define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)"
#define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!"
#define OSD_MSG_STARTING_RTH "STARTING RTH"
#define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME"
#define OSD_MSG_HOLDING_WAYPOINT "HOLDING WAYPOINT"
#define OSD_MSG_TO_WP "TO WP"
#define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT"
#define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING"
#define OSD_MSG_LANDING "LANDING"
#define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME"
#define OSD_MSG_HOVERING "HOVERING"
#define OSD_MSG_LANDED "LANDED"
#define OSD_MSG_PREPARING_LAND "PREPARING TO LAND"
#define OSD_MSG_AUTOLAUNCH "AUTOLAUNCH"
#define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)"
#define OSD_MSG_AUTOTRIM "(AUTOTRIM)"
#define OSD_MSG_AUTOTUNE "(AUTOTUNE)"
#define OSD_MSG_HEADFREE "(HEADFREE)"
#define OSD_MSG_UNABLE_ARM "UNABLE TO ARM"
typedef enum {
OSD_RSSI_VALUE,
OSD_MAIN_BATT_VOLTAGE,
@ -317,3 +371,12 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt);
void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D);
// Returns a heading angle in degrees normalized to [0, 360).
int osdGetHeadingAngle(int angle);
/**
* @brief Get the OSD system message
* @param buff pointer to the message buffer
* @param buff_size size of the buffer array
* @param isCenteredText if true, centered text based on buff_size
* @return osd text attributes (Blink, Inverted, Solid)
*/
textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenteredText);

288
src/main/telemetry/mavlink.c Executable file → Normal file
View file

@ -23,6 +23,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
@ -35,16 +36,21 @@
#include "common/color.h"
#include "common/maths.h"
#include "common/utils.h"
#include "common/string_light.h"
#include "config/feature.h"
#include "drivers/serial.h"
#include "drivers/time.h"
#include "drivers/display.h"
#include "drivers/osd_symbols.h"
#include "fc/config.h"
#include "fc/fc_core.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "fc/settings.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
@ -55,8 +61,10 @@
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/serial.h"
#include "io/osd.h"
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "rx/rx.h"
@ -66,11 +74,18 @@
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/pitotmeter.h"
#include "sensors/diagnostics.h"
#include "sensors/sensors.h"
#include "sensors/temperature.h"
#include "sensors/esc_sensor.h"
#include "telemetry/mavlink.h"
#include "telemetry/telemetry.h"
#include "blackbox/blackbox_io.h"
#include "scheduler/scheduler.h"
// mavlink library uses unnames unions that's causes GCC to complain if -Wpedantic is used
// until this is resolved in mavlink library - ignore -Wpedantic for mavlink code
#pragma GCC diagnostic push
@ -148,7 +163,8 @@ static uint8_t mavRates[] = {
[MAV_DATA_STREAM_RC_CHANNELS] = 5, // 5Hz
[MAV_DATA_STREAM_POSITION] = 2, // 2Hz
[MAV_DATA_STREAM_EXTRA1] = 10, // 10Hz
[MAV_DATA_STREAM_EXTRA2] = 2 // 2Hz
[MAV_DATA_STREAM_EXTRA2] = 2, // 2Hz
[MAV_DATA_STREAM_EXTRA3] = 1 // 1Hz
};
#define MAXSTREAMS (sizeof(mavRates) / sizeof(mavRates[0]))
@ -162,7 +178,7 @@ static mavlink_status_t mavRecvStatus;
static uint8_t mavSystemId = 1;
static uint8_t mavComponentId = MAV_COMP_ID_SYSTEM_CONTROL;
APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode)
static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode)
{
switch (flightMode)
{
@ -175,12 +191,22 @@ APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode)
case FLM_RTH: return COPTER_MODE_RTL;
case FLM_MISSION: return COPTER_MODE_AUTO;
case FLM_LAUNCH: return COPTER_MODE_THROW;
case FLM_FAILSAFE: return COPTER_MODE_RTL;
case FLM_FAILSAFE:
{
if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) {
return COPTER_MODE_RTL;
} else if (failsafePhase() == FAILSAFE_LANDING) {
return COPTER_MODE_LAND;
} else {
// There is no valid mapping to ArduCopter
return COPTER_MODE_ENUM_END;
}
}
default: return COPTER_MODE_ENUM_END;
}
}
APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode)
static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode)
{
switch (flightMode)
{
@ -195,13 +221,23 @@ APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode)
case FLM_MISSION: return PLANE_MODE_AUTO;
case FLM_CRUISE: return PLANE_MODE_CRUISE;
case FLM_LAUNCH: return PLANE_MODE_TAKEOFF;
case FLM_FAILSAFE: return PLANE_MODE_RTL;
case FLM_FAILSAFE:
{
if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) {
return PLANE_MODE_RTL;
}
else if (failsafePhase() == FAILSAFE_LANDING) {
return PLANE_MODE_AUTO;
}
else {
// There is no valid mapping to ArduPlane
return PLANE_MODE_ENUM_END;
}
}
default: return PLANE_MODE_ENUM_END;
}
}
static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum)
{
uint8_t rate = (uint8_t) mavRates[streamNum];
@ -265,6 +301,7 @@ static void configureMAVLinkStreamRates(void)
mavRates[MAV_DATA_STREAM_POSITION] = telemetryConfig()->mavlink.position_rate;
mavRates[MAV_DATA_STREAM_EXTRA1] = telemetryConfig()->mavlink.extra1_rate;
mavRates[MAV_DATA_STREAM_EXTRA2] = telemetryConfig()->mavlink.extra2_rate;
mavRates[MAV_DATA_STREAM_EXTRA3] = telemetryConfig()->mavlink.extra3_rate;
}
void checkMAVLinkTelemetryState(void)
@ -294,35 +331,126 @@ static void mavlinkSendMessage(void)
void mavlinkSendSystemStatus(void)
{
uint32_t onboardControlAndSensors = 35843;
// Receiver is assumed to be always present
uint32_t onboard_control_sensors_present = (MAV_SYS_STATUS_SENSOR_RC_RECEIVER);
// GYRO and RC are assumed as minimium requirements
uint32_t onboard_control_sensors_enabled = (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_RC_RECEIVER);
uint32_t onboard_control_sensors_health = 0;
/*
onboard_control_sensors_present Bitmask
fedcba9876543210
1000110000000011 For all = 35843
0001000000000100 With Mag = 4100
0010000000001000 With Baro = 8200
0100000000100000 With GPS = 16416
0000001111111111
*/
if (getHwGyroStatus() == HW_SENSOR_OK) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
// Missing presence will report as sensor unhealthy
onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
}
if (sensors(SENSOR_MAG)) onboardControlAndSensors |= 4100;
if (sensors(SENSOR_BARO)) onboardControlAndSensors |= 8200;
if (sensors(SENSOR_GPS)) onboardControlAndSensors |= 16416;
hardwareSensorStatus_e accStatus = getHwAccelerometerStatus();
if (accStatus == HW_SENSOR_OK) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
} else if (accStatus == HW_SENSOR_UNHEALTHY) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
} else if (accStatus == HW_SENSOR_UNAVAILABLE) {
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
}
hardwareSensorStatus_e compassStatus = getHwCompassStatus();
if (compassStatus == HW_SENSOR_OK) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG;
onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
} else if (compassStatus == HW_SENSOR_UNHEALTHY) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG;
} else if (compassStatus == HW_SENSOR_UNAVAILABLE) {
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG;
}
hardwareSensorStatus_e baroStatus = getHwBarometerStatus();
if (baroStatus == HW_SENSOR_OK) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
} else if (baroStatus == HW_SENSOR_UNHEALTHY) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
} else if (baroStatus == HW_SENSOR_UNAVAILABLE) {
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
}
hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus();
if (pitotStatus == HW_SENSOR_OK) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
} else if (pitotStatus == HW_SENSOR_UNHEALTHY) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
} else if (pitotStatus == HW_SENSOR_UNAVAILABLE) {
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
hardwareSensorStatus_e gpsStatus = getHwGPSStatus();
if (gpsStatus == HW_SENSOR_OK) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS;
onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
} else if (gpsStatus == HW_SENSOR_UNHEALTHY) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS;
} else if (gpsStatus == HW_SENSOR_UNAVAILABLE) {
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS;
}
hardwareSensorStatus_e opFlowStatus = getHwOpticalFlowStatus();
if (opFlowStatus == HW_SENSOR_OK) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
} else if (opFlowStatus == HW_SENSOR_UNHEALTHY) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
} else if (opFlowStatus == HW_SENSOR_UNAVAILABLE) {
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
hardwareSensorStatus_e rangefinderStatus = getHwRangefinderStatus();
if (rangefinderStatus == HW_SENSOR_OK) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
} else if (rangefinderStatus == HW_SENSOR_UNHEALTHY) {
onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
} else if (rangefinderStatus == HW_SENSOR_UNAVAILABLE) {
onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
if (rxIsReceivingSignal() && rxAreFlightChannelsValid()) {
onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
#ifdef USE_BLACKBOX
// BLACKBOX is assumed enabled and present for boards with capability
onboard_control_sensors_present |= MAV_SYS_STATUS_LOGGING;
onboard_control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
// Unhealthy only for cases with not enough space to record
if (!isBlackboxDeviceFull()) {
onboard_control_sensors_health |= MAV_SYS_STATUS_LOGGING;
}
#endif
mavlink_msg_sys_status_pack(mavSystemId, mavComponentId, &mavSendMsg,
// onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present.
//Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure,
// 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position,
// 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization,
// 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
onboardControlAndSensors,
//Value of 0: not present. Value of 1: present. Indices according MAV_SYS_STATUS_SENSOR
onboard_control_sensors_present,
// onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled
onboardControlAndSensors,
onboard_control_sensors_enabled,
// onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error.
onboardControlAndSensors & 1023,
onboard_control_sensors_health,
// load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
0,
constrain(averageSystemLoadPercent*10, 0, 1000),
// voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
feature(FEATURE_VBAT) ? getBatteryVoltage() * 10 : 0,
// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
@ -431,14 +559,14 @@ void mavlinkSendPosition(timeUs_t currentTimeUs)
#else
gpsSol.llh.alt * 10,
#endif
// Ground X Speed (Latitude), expressed as m/s * 100
0,
// Ground Y Speed (Longitude), expressed as m/s * 100
0,
// Ground Z Speed (Altitude), expressed as m/s * 100
0,
// heading Current heading in degrees, in compass units (0..360, 0=north)
DECIDEGREES_TO_DEGREES(attitude.values.yaw)
// [cm/s] Ground X Speed (Latitude, positive north)
getEstimatedActualVelocity(X),
// [cm/s] Ground Y Speed (Longitude, positive east)
getEstimatedActualVelocity(Y),
// [cm/s] Ground Z Speed (Altitude, positive down)
getEstimatedActualVelocity(Z),
// [cdeg] Vehicle heading (yaw angle) (0.0..359.99 degrees, 0=north)
DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw)
);
mavlinkSendMessage();
@ -507,6 +635,11 @@ void mavlinkSendHUDAndHeartbeat(void)
}
#endif
int16_t thr = rxGetChannelValue(THROTTLE);
if (navigationIsControllingThrottle()) {
thr = rcCommand[THROTTLE];
}
mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg,
// airspeed Current airspeed in m/s
mavAirSpeed,
@ -515,7 +648,7 @@ void mavlinkSendHUDAndHeartbeat(void)
// heading Current heading in degrees, in compass units (0..360, 0=north)
DECIDEGREES_TO_DEGREES(attitude.values.yaw),
// throttle Current throttle setting in integer percent, 0 to 100
scaleRange(constrain(rxGetChannelValue(THROTTLE), PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100),
scaleRange(constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100),
// alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate)
mavAltitude,
// climb Current climb rate in meters/second
@ -602,6 +735,82 @@ void mavlinkSendHUDAndHeartbeat(void)
mavlinkSendMessage();
}
void mavlinkSendBatteryTemperatureStatusText(void)
{
uint16_t batteryVoltages[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN];
memset(batteryVoltages, UINT16_MAX, sizeof(batteryVoltages));
if (feature(FEATURE_VBAT)) {
uint8_t batteryCellCount = getBatteryCellCount();
if (batteryCellCount > 0) {
for (int cell=0; (cell < batteryCellCount) && (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN); cell++) {
batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10;
}
}
else {
batteryVoltages[0] = getBatteryVoltage() * 10;
}
}
else {
batteryVoltages[0] = 0;
}
mavlink_msg_battery_status_pack(mavSystemId, mavComponentId, &mavSendMsg,
// id Battery ID
0,
// battery_function Function of the battery
MAV_BATTERY_FUNCTION_UNKNOWN,
// type Type (chemistry) of the battery
MAV_BATTERY_TYPE_UNKNOWN,
// temperature Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature
INT16_MAX,
// voltages Battery voltage of cells, in millivolts (1 = 1 millivolt). Cells above the valid cell count for this battery should have the UINT16_MAX value.
batteryVoltages,
// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
isAmperageConfigured() ? getAmperage() : -1,
// current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate
isAmperageConfigured() ? getMAhDrawn() : -1,
// energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate
isAmperageConfigured() ? getMWhDrawn()*36 : -1,
// battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery);
feature(FEATURE_VBAT) ? calculateBatteryPercentage() : -1);
mavlinkSendMessage();
int16_t temperature;
sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature);
mavlink_msg_scaled_pressure_pack(mavSystemId, mavComponentId, &mavSendMsg,
millis(),
0,
0,
temperature * 10);
mavlinkSendMessage();
// FIXME - Status text is limited to boards with USE_OSD
#ifdef USE_OSD
char buff[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {" "};
textAttributes_t elemAttr = osdGetSystemMessage(buff, sizeof(buff), false);
if (buff[0] != SYM_BLANK) {
MAV_SEVERITY severity = MAV_SEVERITY_NOTICE;
if (TEXT_ATTRIBUTES_HAVE_BLINK(elemAttr)) {
severity = MAV_SEVERITY_CRITICAL;
} else if TEXT_ATTRIBUTES_HAVE_INVERTED(elemAttr) {
severity = MAV_SEVERITY_WARNING;
}
mavlink_msg_statustext_pack(mavSystemId, mavComponentId, &mavSendMsg,
(uint8_t)severity,
buff);
mavlinkSendMessage();
}
#endif
}
void processMAVLinkTelemetry(timeUs_t currentTimeUs)
{
// is executed @ TELEMETRY_MAVLINK_MAXRATE rate
@ -626,6 +835,11 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs)
if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) {
mavlinkSendHUDAndHeartbeat();
}
if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3)) {
mavlinkSendBatteryTemperatureStatusText();
}
}
static bool handleIncoming_MISSION_CLEAR_ALL(void)

3
src/main/telemetry/telemetry.c
View file

@ -81,7 +81,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
.rc_channels_rate = 5,
.position_rate = 2,
.extra1_rate = 10,
.extra2_rate = 2
.extra2_rate = 2,
.extra3_rate = 1
}
);

1
src/main/telemetry/telemetry.h
View file

@ -82,6 +82,7 @@ typedef struct telemetryConfig_s {
uint8_t position_rate;
uint8_t extra1_rate;
uint8_t extra2_rate;
uint8_t extra3_rate;
} mavlink;
} telemetryConfig_t;