Skip to content

Commit 0f5c34d

Browse files
error414error414
committed
implement minimal need for softrf
1 parent ccdbecf commit 0f5c34d

6 files changed

Lines changed: 100 additions & 52 deletions

File tree

‎docs/Settings.md‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2742,6 +2742,16 @@ MAVLink System ID
27422742

27432743
---
27442744

2745+
### mavlink_system_time_rate
2746+
2747+
Rate of the system time message for MAVLink telemetry
2748+
2749+
| Default | Min | Max |
2750+
| --- | --- | --- |
2751+
| 1 | 0 | 255 |
2752+
2753+
---
2754+
27452755
### mavlink_version
27462756

27472757
Version of MAVLink to use

‎lib/main/MAVLink/common/common.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -987,6 +987,8 @@ typedef enum MAV_DATA_STREAM
987987
MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */
988988
MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */
989989
MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION_INT messages. | */
990+
MAV_DATA_STREAM_SYSTEM_TIME=7, /* Enable SYSTEM_TIME messages. | */
991+
MAV_DATA_STREAM_HEARTBEAT=8, /* Enable HEARTBEAT messages. | */
990992
MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */
991993
MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */
992994
MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */

‎src/main/fc/settings.yaml‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3272,6 +3272,13 @@ groups:
32723272
min: 0
32733273
max: 255
32743274
default_value: 1
3275+
- name: mavlink_system_time_rate
3276+
description: "Rate of the system time message for MAVLink telemetry"
3277+
field: mavlink.system_time_rate
3278+
type: uint8_t
3279+
min: 0
3280+
max: 255
3281+
default_value: 1
32753282
- name: mavlink_version
32763283
field: mavlink.version
32773284
description: "Version of MAVLink to use"

‎src/main/telemetry/mavlink.c‎

Lines changed: 79 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -172,7 +172,9 @@ static uint8_t mavRates[] = {
172172
[MAV_DATA_STREAM_POSITION] = 2, // 2Hz
173173
[MAV_DATA_STREAM_EXTRA1] = 3, // 3Hz
174174
[MAV_DATA_STREAM_EXTRA2] = 2, // 2Hz, HEARTBEATs are important
175-
[MAV_DATA_STREAM_EXTRA3] = 1 // 1Hz
175+
[MAV_DATA_STREAM_EXTRA3] = 1, // 1Hz
176+
[MAV_DATA_STREAM_SYSTEM_TIME] = 1, // 1Hz
177+
[MAV_DATA_STREAM_HEARTBEAT] = 1, // 1Hz
176178
};
177179

178180
#define MAXSTREAMS (sizeof(mavRates) / sizeof(mavRates[0]))
@@ -330,6 +332,7 @@ static void configureMAVLinkStreamRates(void)
330332
mavRates[MAV_DATA_STREAM_EXTRA1] = telemetryConfig()->mavlink.extra1_rate;
331333
mavRates[MAV_DATA_STREAM_EXTRA2] = telemetryConfig()->mavlink.extra2_rate;
332334
mavRates[MAV_DATA_STREAM_EXTRA3] = telemetryConfig()->mavlink.extra3_rate;
335+
mavRates[MAV_DATA_STREAM_SYSTEM_TIME] = telemetryConfig()->mavlink.system_time_rate;
333336
}
334337

335338
void checkMAVLinkTelemetryState(void)
@@ -704,52 +707,22 @@ void mavlinkSendAttitude(void)
704707
mavlinkSendMessage();
705708
}
706709

707-
void mavlinkSendHUDAndHeartbeat(void)
710+
void mavlinkSendSystemTime(void)
708711
{
709-
float mavAltitude = 0;
710-
float mavGroundSpeed = 0;
711-
float mavAirSpeed = 0;
712-
float mavClimbRate = 0;
712+
uint64_t timeUnixUsec = 0;
713+
rtcTime_t rtcTime;
713714

714-
#if defined(USE_GPS)
715-
// use ground speed if source available
716-
if (sensors(SENSOR_GPS)
717-
#ifdef USE_GPS_FIX_ESTIMATION
718-
|| STATE(GPS_ESTIMATED_FIX)
719-
#endif
720-
) {
721-
mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
715+
if (rtcGet(&rtcTime)) {
716+
timeUnixUsec = (uint64_t)rtcTime * 1000ULL + (uint64_t)(micros() % 1000); // extrapolation to uS
717+
//timeUnixUsec = (uint64_t)rtcTime * 1000ULL; // mS resolution
722718
}
723-
#endif
724-
725-
#if defined(USE_PITOT)
726-
if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
727-
mavAirSpeed = getAirspeedEstimate() / 100.0f;
728-
}
729-
#endif
730-
731-
// select best source for altitude
732-
mavAltitude = getEstimatedActualPosition(Z) / 100.0f;
733-
mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f;
734-
735-
int16_t thr = getThrottlePercent(osdUsingScaledThrottle());
736-
mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg,
737-
// airspeed Current airspeed in m/s
738-
mavAirSpeed,
739-
// groundspeed Current ground speed in m/s
740-
mavGroundSpeed,
741-
// heading Current heading in degrees, in compass units (0..360, 0=north)
742-
DECIDEGREES_TO_DEGREES(attitude.values.yaw),
743-
// throttle Current throttle setting in integer percent, 0 to 100
744-
thr,
745-
// alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate)
746-
mavAltitude,
747-
// climb Current climb rate in meters/second
748-
mavClimbRate);
749719

720+
mavlink_msg_system_time_pack(mavSystemId, mavComponentId, &mavSendMsg, timeUnixUsec, millis());
750721
mavlinkSendMessage();
722+
}
751723

752-
724+
void mavlinkSendHeartbeat(void)
725+
{
753726
uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
754727
if (ARMING_FLAG(ARMED))
755728
mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
@@ -821,16 +794,62 @@ void mavlinkSendHUDAndHeartbeat(void)
821794
}
822795

823796
mavlink_msg_heartbeat_pack(mavSystemId, mavComponentId, &mavSendMsg,
824-
// type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
825-
mavSystemType,
826-
// autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
827-
mavType,
828-
// base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
829-
mavModes,
830-
// custom_mode A bitfield for use for autopilot-specific flags.
831-
mavCustomMode,
832-
// system_status System status flag, see MAV_STATE ENUM
833-
mavSystemState);
797+
// type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
798+
mavSystemType,
799+
// autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
800+
mavType,
801+
// base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
802+
mavModes,
803+
// custom_mode A bitfield for use for autopilot-specific flags.
804+
mavCustomMode,
805+
// system_status System status flag, see MAV_STATE ENUM
806+
mavSystemState);
807+
808+
mavlinkSendMessage();
809+
}
810+
811+
void mavlinkSendHUD(void)
812+
{
813+
float mavAltitude = 0;
814+
float mavGroundSpeed = 0;
815+
float mavAirSpeed = 0;
816+
float mavClimbRate = 0;
817+
818+
#if defined(USE_GPS)
819+
// use ground speed if source available
820+
if (sensors(SENSOR_GPS)
821+
#ifdef USE_GPS_FIX_ESTIMATION
822+
|| STATE(GPS_ESTIMATED_FIX)
823+
#endif
824+
) {
825+
mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
826+
}
827+
#endif
828+
829+
#if defined(USE_PITOT)
830+
if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
831+
mavAirSpeed = getAirspeedEstimate() / 100.0f;
832+
}
833+
#endif
834+
835+
// select best source for altitude
836+
mavAltitude = getEstimatedActualPosition(Z) / 100.0f;
837+
mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f;
838+
839+
int16_t thr = getThrottlePercent(osdUsingScaledThrottle());
840+
mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg,
841+
// airspeed Current airspeed in m/s
842+
mavAirSpeed,
843+
// groundspeed Current ground speed in m/s
844+
mavGroundSpeed,
845+
// heading Current heading in degrees, in compass units (0..360, 0=north)
846+
DECIDEGREES_TO_DEGREES(attitude.values.yaw),
847+
// throttle Current throttle setting in integer percent, 0 to 100
848+
thr,
849+
// alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate)
850+
mavAltitude,
851+
// climb Current climb rate in meters/second
852+
mavClimbRate);
834853

835854
mavlinkSendMessage();
836855
}
@@ -952,13 +971,21 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs)
952971
}
953972

954973
if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) {
955-
mavlinkSendHUDAndHeartbeat();
974+
mavlinkSendHUD();
956975
}
957976

958977
if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3)) {
959978
mavlinkSendBatteryTemperatureStatusText();
960979
}
961980

981+
if (mavlinkStreamTrigger(MAV_DATA_STREAM_HEARTBEAT)) {
982+
mavlinkSendHeartbeat();
983+
}
984+
985+
if (mavlinkStreamTrigger(MAV_DATA_STREAM_SYSTEM_TIME)) {
986+
mavlinkSendSystemTime();
987+
}
988+
962989
}
963990

964991
static bool handleIncoming_MISSION_CLEAR_ALL(void)

‎src/main/telemetry/telemetry.c‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -94,6 +94,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
9494
.extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT,
9595
.extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT,
9696
.extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT,
97+
.system_time_rate = SETTING_MAVLINK_SYSTEM_TIME_RATE_DEFAULT,
9798
.version = SETTING_MAVLINK_VERSION_DEFAULT,
9899
.min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT,
99100
.radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT,

‎src/main/telemetry/telemetry.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,7 @@ typedef struct telemetryConfig_s {
8484
uint8_t extra1_rate;
8585
uint8_t extra2_rate;
8686
uint8_t extra3_rate;
87+
uint8_t system_time_rate;
8788
uint8_t version;
8889
uint8_t min_txbuff;
8990
uint8_t radio_type;

0 commit comments

Comments
 (0)