dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

telemetry_statur: use 4 separate topics

This commit is contained in:
Anton Babushkin 2014-07-06 16:08:37 +02:00
parent d4eae37e47
commit bd5d3ebf70
7 changed files with 163 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -767,7 +767,6 @@ int commander_thread_main(int argc, char *argv[])
hrt_abstime last_idle_time = 0;
hrt_abstime start_time = 0;
hrt_abstime latest_heartbeat = 0;
bool status_changed = true;
bool param_init_forced = true;
@ -797,10 +796,16 @@ int commander_thread_main(int argc, char *argv[])
struct offboard_control_setpoint_s sp_offboard;
memset(&sp_offboard, 0, sizeof(sp_offboard));
/* Subscribe to telemetry status */
int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
struct telemetry_status_s telemetry;
memset(&telemetry, 0, sizeof(telemetry));
/* Subscribe to telemetry status topics */
int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
telemetry_last_heartbeat[i] = 0;
telemetry_lost[i] = true;
}
/* Subscribe to global position */
int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@ -882,7 +887,6 @@ int commander_thread_main(int argc, char *argv[])
bool arming_state_changed = false;
bool main_state_changed = false;
bool failsafe_old = false;
bool system_checked = false;
while (!thread_should_exit) {
@ -939,15 +943,6 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
}
/* Perform system checks (again) once params are loaded and MAVLink is up. */
if (!system_checked && mavlink_fd &&
(telemetry.heartbeat_time > 0) &&
(hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) {
(void)rc_calibration_check(mavlink_fd);
system_checked = true;
}
orb_check(sp_man_sub, &updated);
if (updated) {
@ -960,10 +955,26 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
}
orb_check(telemetry_sub, &updated);
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
orb_check(telemetry_subs[i], &updated);
if (updated) {
orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry);
if (updated) {
struct telemetry_status_s telemetry;
memset(&telemetry, 0, sizeof(telemetry));
orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry);
/* perform system checks when new telemetry link connected */
if (mavlink_fd &&
telemetry_last_heartbeat[i] == 0 &&
telemetry.heartbeat_time > 0 &&
hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) {
(void)rc_calibration_check(mavlink_fd);
}
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
}
}
orb_check(sensor_sub, &updated);
@ -1367,28 +1378,40 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* data link check */
if (telemetry.heartbeat_time >= latest_heartbeat) {
if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
/* data links check */
bool have_link = false;
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
/* handle the case where data link was regained */
if (status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "#audio: data link regained");
status.data_link_lost = false;
status_changed = true;
if (telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i);
telemetry_lost[i] = false;
}
/* Only consider data link with most recent heartbeat */
latest_heartbeat = telemetry.heartbeat_time;
have_link = true;
} else {
if (!status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
status.data_link_lost = true;
status_changed = true;
if (!telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i);
telemetry_lost[i] = true;
}
}
}
if (have_link) {
/* handle the case where data link was regained */
if (status.data_link_lost) {
status.data_link_lost = false;
status_changed = true;
}
} else {
if (!status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST");
status.data_link_lost = true;
status_changed = true;
}
}
/* handle commands last, as the system needs to be updated to handle them */
orb_check(cmd_sub, &updated);

80
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -421,32 +421,35 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
void
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
{
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(msg, &rstatus);
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(msg, &rstatus);
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi;
tstatus.txbuf = rstatus.txbuf;
tstatus.noise = rstatus.noise;
tstatus.remote_noise = rstatus.remnoise;
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi;
tstatus.txbuf = rstatus.txbuf;
tstatus.noise = rstatus.noise;
tstatus.remote_noise = rstatus.remnoise;
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
/* this means that heartbeats alone won't be published to the radio status no more */
_radio_status_available = true;
}
/* this means that heartbeats alone won't be published to the radio status no more */
_radio_status_available = true;
}
void
@ -475,28 +478,31 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
void
MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
{
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
/* ignore own heartbeats, accept only heartbeats from GCS */
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
_telemetry_heartbeat_time = hrt_absolute_time();
/* ignore own heartbeats, accept only heartbeats from GCS */
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
_telemetry_heartbeat_time = hrt_absolute_time();
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
if (!_radio_status_available) {
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
if (!_radio_status_available) {
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = _telemetry_heartbeat_time;
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
tstatus.timestamp = _telemetry_heartbeat_time;
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
}
}
}

31
src/modules/sdlog2/sdlog2.c
View File

@ -979,7 +979,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GVSP_s log_GVSP;
struct log_BATT_s log_BATT;
struct log_DIST_s log_DIST;
struct log_TELE_s log_TELE;
struct log_TEL_s log_TEL;
struct log_EST0_s log_EST0;
struct log_EST1_s log_EST1;
struct log_PWR_s log_PWR;
@ -1019,7 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int esc_sub;
int global_vel_sp_sub;
int battery_sub;
int telemetry_sub;
int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
int range_finder_sub;
int estimator_status_sub;
int tecs_status_sub;
@ -1049,7 +1049,9 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
}
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
@ -1479,16 +1481,19 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- TELEMETRY --- */
if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
log_msg.msg_type = LOG_TELE_MSG;
log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
log_msg.body.log_TELE.noise = buf.telemetry.noise;
log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
LOGBUFFER_WRITE_AND_COUNT(TELE);
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) {
log_msg.msg_type = LOG_TEL0_MSG + i;
log_msg.body.log_TEL.rssi = buf.telemetry.rssi;
log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi;
log_msg.body.log_TEL.noise = buf.telemetry.noise;
log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise;
log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors;
log_msg.body.log_TEL.fixed = buf.telemetry.fixed;
log_msg.body.log_TEL.txbuf = buf.telemetry.txbuf;
log_msg.body.log_TEL.heartbeat_time = buf.telemetry.heartbeat_time;
LOGBUFFER_WRITE_AND_COUNT(TEL);
}
}
/* --- BOTTOM DISTANCE --- */

8
src/modules/sdlog2/sdlog2_format.h
View File

@ -91,6 +91,14 @@ struct log_format_s {
.labels = _labels \
}
#define LOG_FORMAT_S(_name, _struct_name, _format, _labels) { \
.type = LOG_##_name##_MSG, \
.length = sizeof(struct log_##_struct_name##_s) + LOG_PACKET_HEADER_LEN, \
.name = #_name, \
.format = _format, \
.labels = _labels \
}
#define LOG_FORMAT_MSG 0x80
#define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s)

35
src/modules/sdlog2/sdlog2_messages.h
View File

@ -276,18 +276,7 @@ struct log_DIST_s {
uint8_t flags;
};
/* --- TELE - TELEMETRY STATUS --- */
#define LOG_TELE_MSG 22
struct log_TELE_s {
uint8_t rssi;
uint8_t remote_rssi;
uint8_t noise;
uint8_t remote_noise;
uint16_t rxerrors;
uint16_t fixed;
uint8_t txbuf;
};
// ID 22 available
// ID 23 available
/* --- PWR - ONBOARD POWER SYSTEM --- */
@ -385,6 +374,23 @@ struct log_EST1_s {
float s[16];
};
/* --- TEL0..3 - TELEMETRY STATUS --- */
#define LOG_TEL0_MSG 34
#define LOG_TEL1_MSG 35
#define LOG_TEL2_MSG 36
#define LOG_TEL3_MSG 37
struct log_TEL_s {
uint8_t rssi;
uint8_t remote_rssi;
uint8_t noise;
uint8_t remote_noise;
uint16_t rxerrors;
uint16_t fixed;
uint8_t txbuf;
uint64_t heartbeat_time;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@ -432,7 +438,10 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
LOG_FORMAT_S(TEL0, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
LOG_FORMAT_S(TEL1, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
LOG_FORMAT_S(TEL2, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
LOG_FORMAT_S(TEL3, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),

5
src/modules/uORB/objects_common.cpp
View File

@ -186,7 +186,10 @@ ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
#include "topics/telemetry_status.h"
ORB_DEFINE(telemetry_status, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_2, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_3, struct telemetry_status_s);
#include "topics/debug_key_value.h"
ORB_DEFINE(debug_key_value, struct debug_key_value_s);

14
src/modules/uORB/topics/telemetry_status.h
View File

@ -72,6 +72,18 @@ struct telemetry_status_s {
* @}
*/
ORB_DECLARE(telemetry_status);
ORB_DECLARE(telemetry_status_0);
ORB_DECLARE(telemetry_status_1);
ORB_DECLARE(telemetry_status_2);
ORB_DECLARE(telemetry_status_3);
#define TELEMETRY_STATUS_ORB_ID_NUM 4
static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_ID_NUM] = {
ORB_ID(telemetry_status_0),
ORB_ID(telemetry_status_1),
ORB_ID(telemetry_status_2),
ORB_ID(telemetry_status_3),
};
#endif /* TOPIC_TELEMETRY_STATUS_H */