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

mavlink: use "normal" mode for HIL operation, only add HIL_CONTROLS stream when HIL enabled

This commit is contained in:
Anton Babushkin 2014-03-11 23:03:49 +04:00
parent 0bcda7f8a6
commit 1fd7b89d3c
2 changed files with 26 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
src/modules/mavlink/mavlink_main.cpp
View File

@ -165,7 +165,7 @@ Mavlink::Mavlink() :
_task_should_exit(false),
_mavlink_fd(-1),
_task_running(false),
_mavlink_hil_enabled(false),
_hil_enabled(false),
_main_loop_delay(1000),
_subscriptions(nullptr),
_streams(nullptr),
@ -548,38 +548,17 @@ Mavlink::set_hil_enabled(bool hil_enabled)
{
int ret = OK;
/* Enable HIL */
if (hil_enabled && !_mavlink_hil_enabled) {
_mavlink_hil_enabled = true;
/* ramp up some HIL-related subscriptions */
unsigned hil_rate_interval;
if (_baudrate < 19200) {
/* 10 Hz */
hil_rate_interval = 100;
} else if (_baudrate < 38400) {
/* 10 Hz */
hil_rate_interval = 100;
} else if (_baudrate < 115200) {
/* 20 Hz */
hil_rate_interval = 50;
} else {
/* 200 Hz */
hil_rate_interval = 5;
}
// orb_set_interval(subs.spa_sub, hil_rate_interval);
// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
/* enable HIL */
if (hil_enabled && !_hil_enabled) {
_hil_enabled = true;
float rate_mult = _datarate / 1000.0f;
configure_stream("HIL_CONTROLS", 15.0f * rate_mult);
}
if (!hil_enabled && _mavlink_hil_enabled) {
_mavlink_hil_enabled = false;
// orb_set_interval(subs.spa_sub, 200);
/* disable HIL */
if (!hil_enabled && _hil_enabled) {
_hil_enabled = false;
configure_stream("HIL_CONTROLS", 0.0f);
} else {
ret = ERROR;
@ -1445,10 +1424,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
if (i > 1) {
/* Enforce null termination */
statustext.text[i] = '\0';
mavlink_message_t msg;
mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
mavlink_missionlib_send_message(&msg);
mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
return OK;
} else {
@ -1560,9 +1537,7 @@ Mavlink::task_main(int argc, char *argv[])
int ch;
_baudrate = 57600;
_datarate = 0;
_channel = MAVLINK_COMM_0;
_mode = MODE_OFFBOARD;
_mode = MAVLINK_MODE_NORMAL;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
@ -1603,17 +1578,8 @@ Mavlink::task_main(int argc, char *argv[])
// break;
case 'm':
if (strcmp(optarg, "offboard") == 0) {
_mode = MODE_OFFBOARD;
} else if (strcmp(optarg, "onboard") == 0) {
_mode = MODE_ONBOARD;
} else if (strcmp(optarg, "hil") == 0) {
_mode = MODE_HIL;
} else if (strcmp(optarg, "custom") == 0) {
_mode = MODE_CUSTOM;
if (strcmp(optarg, "custom") == 0) {
_mode = MAVLINK_MODE_CUSTOM;
}
break;
@ -1649,42 +1615,20 @@ Mavlink::task_main(int argc, char *argv[])
/* inform about mode */
switch (_mode) {
case MODE_CUSTOM:
case MAVLINK_MODE_NORMAL:
warnx("mode: NORMAL");
break;
case MAVLINK_MODE_CUSTOM:
warnx("mode: CUSTOM");
break;
case MODE_OFFBOARD:
warnx("mode: OFFBOARD");
break;
case MODE_ONBOARD:
warnx("mode: ONBOARD");
break;
case MODE_HIL:
warnx("mode: HIL");
break;
default:
warnx("ERROR: Unknown mode");
break;
}
switch (_mode) {
case MODE_OFFBOARD:
case MODE_HIL:
case MODE_CUSTOM:
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
break;
case MODE_ONBOARD:
_mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA;
break;
default:
_mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
break;
}
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
warnx("data rate: %d bytes/s", _datarate);
warnx("port: %s, baudrate: %d", _device_name, _baudrate);
@ -1741,7 +1685,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("HEARTBEAT", 1.0f);
switch (_mode) {
case MODE_OFFBOARD:
case MAVLINK_MODE_NORMAL:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
@ -1753,20 +1697,6 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
break;
case MODE_HIL:
/* HIL mode normally runs at high data rate, rate_mult ~ 10 */
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
configure_stream("ATTITUDE", 2.0f * rate_mult);
configure_stream("VFR_HUD", 2.0f * rate_mult);
configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_INT", 1.0f * rate_mult);
configure_stream("LOCAL_POSITION_NED", 1.0f * rate_mult);
configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
configure_stream("HIL_CONTROLS", 15.0f * rate_mult);
break;
default:
break;
}
@ -1795,12 +1725,7 @@ Mavlink::task_main(int argc, char *argv[])
if (status_sub->update(t)) {
/* switch HIL mode if required */
if (status->hil_state == HIL_STATE_ON) {
set_hil_enabled(true);
} else if (status->hil_state == HIL_STATE_OFF) {
set_hil_enabled(false);
}
set_hil_enabled(status->hil_state == HIL_STATE_ON);
}
/* check for requested subscriptions */

10
src/modules/mavlink/mavlink_main.h
View File

@ -145,16 +145,14 @@ public:
const char *_device_name;
enum MAVLINK_MODE {
MODE_CUSTOM = 0,
MODE_OFFBOARD,
MODE_ONBOARD,
MODE_HIL
MAVLINK_MODE_NORMAL = 0,
MAVLINK_MODE_CUSTOM
};
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode() { return _mode; }
bool get_hil_enabled() { return _mavlink_hil_enabled; };
bool get_hil_enabled() { return _hil_enabled; };
/**
* Handle waypoint related messages.
@ -200,7 +198,7 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
/* states */
bool _mavlink_hil_enabled; /**< Hardware In the Loop mode */
bool _hil_enabled; /**< Hardware In the Loop mode */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */