fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Fix bug when changing to high latency telemetry

Browse Source 
Due to the old heartbeat of the high latency telemetry when activating it after flying sufficiently long in normal telemetry it is first detected as lost until the first message is sent.
By updating the heartbeat to the current time on switching this issue is avoided.

Also includes a small style fix for the HIGH_LATENCY2 stream
This commit is contained in:
acfloria 2018-03-29 11:05:13 +02:00 committed by Beat Küng
parent 1124a397f2
commit b95d65df53
2 changed files with 15 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/commander/commander.cpp
View File

@ -4154,7 +4154,7 @@ void Commander::poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout)
_telemetry_high_latency[i] = false;
}
if (telemetry.heartbeat_time > 0) {
if (telemetry.heartbeat_time > 0 && (_telemetry_last_heartbeat[i] < telemetry.heartbeat_time)) {
_telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
}
}
@ -4274,6 +4274,13 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32
vehicle_cmd.target_system = status.system_id;
vehicle_cmd.target_component = 0;
// set heartbeat to current time for high latency so that the first message can be transmitted
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (_telemetry_high_latency[i]) {
_telemetry_last_heartbeat[i] = hrt_absolute_time();
}
}
if (_vehicle_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vehicle_cmd);

14
src/modules/mavlink/mavlink_messages.cpp
View File

@ -4448,7 +4448,7 @@ protected:
void update_airspeed(float update_rate)
{
struct airspeed_s airspeed = {};
airspeed_s airspeed = {};
if (_airspeed_sub->update(&airspeed)) {
_airspeed.add_value(airspeed.indicated_airspeed_m_s, update_rate);
@ -4458,7 +4458,7 @@ protected:
void update_tecs_status(float update_rate)
{
struct tecs_status_s tecs_status = {};
tecs_status_s tecs_status = {};
if (_tecs_status_sub->update(&tecs_status)) {
_airspeed_sp.add_value(tecs_status.airspeedSp, update_rate);
@ -4467,7 +4467,7 @@ protected:
void update_battery_status(float update_rate)
{
struct battery_status_s battery = {};
battery_status_s battery = {};
if (_battery_sub->update(&battery)) {
_battery.add_value(battery.remaining, update_rate);
@ -4476,7 +4476,7 @@ protected:
void update_global_position(float update_rate)
{
struct vehicle_global_position_s global_pos = {};
vehicle_global_position_s global_pos = {};
if (_global_pos_sub->update(&global_pos)) {
_climb_rate.add_value(fabsf(global_pos.vel_d), update_rate);
@ -4486,7 +4486,7 @@ protected:
void update_gps(float update_rate)
{
struct vehicle_gps_position_s gps = {};
vehicle_gps_position_s gps = {};
if (_gps_sub->update(&gps)) {
_eph.add_value(gps.eph, update_rate);
@ -4496,7 +4496,7 @@ protected:
void update_vehicle_status(float update_rate)
{
struct vehicle_status_s status = {};
vehicle_status_s status = {};
if (_status_sub->update(&status)) {
if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
@ -4521,7 +4521,7 @@ protected:
void update_wind_estimate(float update_rate)
{
struct wind_estimate_s wind = {};
wind_estimate_s wind = {};
if (_wind_sub->update(&wind)) {
_windspeed.add_value(sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east),