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

Merge pull request #726 from PX4/mavlink2_hil 

mavlink HIL with normal autostart scripts
This commit is contained in:
Lorenz Meier 2014-03-17 16:44:51 +01:00
parent 762e8f5289 67298d47c1
commit c4ce5b9286
14 changed files with 119 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -5,7 +5,7 @@
echo "Starting MAVLink on this USB console"
mavlink start -r 5000 -d /dev/ttyACM0
mavlink start -r 10000 -d /dev/ttyACM0
# Exit shell to make it available to MAVLink
exit

2
ROMFS/px4fmu_common/init.d/rcS
View File

@ -393,7 +393,7 @@ then
if [ $HIL == yes ]
then
sleep 1
set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0 -m hil"
set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0"
usleep 5000
else
# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s

2
src/drivers/device/cdev.cpp
View File

@ -124,7 +124,7 @@ CDev::register_class_devname(const char *class_devname)
if (ret == OK) break;
} else {
char name[32];
snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
ret = register_driver(name, &fops, 0666, (void *)this);
if (ret == OK) break;
}

5
src/drivers/gps/gps.cpp
View File

@ -232,6 +232,11 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCRESET:
cmd_reset();
break;
default:
/* give it to parent if no one wants it */
ret = CDev::ioctl(filp, cmd, arg);
break;
}
unlock();

21
src/drivers/px4io/px4io.cpp
View File

@ -1332,12 +1332,15 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
battery_status.discharged_mah = _battery_mamphour_total;
_battery_last_timestamp = battery_status.timestamp;
/* lazily publish the battery voltage */
if (_to_battery > 0) {
orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
/* the announced battery status would conflict with the simulated battery status in HIL */
if (!(_pub_blocked)) {
/* lazily publish the battery voltage */
if (_to_battery > 0) {
orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
} else {
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
} else {
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
}
}
}
@ -1959,8 +1962,7 @@ PX4IO::print_status()
}
int
PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
/* Make it obvious that file * isn't used here */
PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
{
int ret = OK;
@ -2372,8 +2374,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
break;
default:
/* not a recognized value */
ret = -ENOTTY;
/* see if the parent class can make any use of it */
ret = CDev::ioctl(filep, cmd, arg);
break;
}
return ret;

2
src/drivers/rgbled/rgbled.cpp
View File

@ -242,6 +242,8 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
return OK;
default:
/* see if the parent class can make any use of it */
ret = CDev::ioctl(filp, cmd, arg);
break;
}

4
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -393,7 +393,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp;
sensor_last_timestamp[1] = raw.accelerometer_timestamp;
}
hrt_abstime vel_t = 0;
@ -445,7 +445,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp;
sensor_last_timestamp[2] = raw.magnetometer_timestamp;
}
z_k[6] = raw.magnetometer_ga[0];

4
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
View File

@ -539,7 +539,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp;
sensor_last_timestamp[1] = raw.accelerometer_timestamp;
}
acc[0] = raw.accelerometer_m_s2[0];
@ -550,7 +550,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp;
sensor_last_timestamp[2] = raw.magnetometer_timestamp;
}
mag[0] = raw.magnetometer_ga[0];

53
src/modules/commander/state_machine_helper.cpp
View File

@ -44,6 +44,7 @@
#include <stdbool.h>
#include <dirent.h>
#include <fcntl.h>
#include <string.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@ -309,10 +310,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
bool valid_transition = false;
int ret = ERROR;
warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
if (current_status->hil_state == new_state) {
warnx("Hil state not changed");
valid_transition = true;
} else {
@ -340,23 +338,60 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/* list directory */
DIR *d;
struct dirent *direntry;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
char devname[24];
while ((direntry = readdir(d)) != NULL) {
int sensfd = ::open(direntry->d_name, 0);
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
/* skip serial ports */
if (!strncmp("tty", direntry->d_name, 3)) {
continue;
}
/* skip mtd devices */
if (!strncmp("mtd", direntry->d_name, 3)) {
continue;
}
/* skip ram devices */
if (!strncmp("ram", direntry->d_name, 3)) {
continue;
}
/* skip MMC devices */
if (!strncmp("mmc", direntry->d_name, 3)) {
continue;
}
/* skip mavlink */
if (!strcmp("mavlink", direntry->d_name)) {
continue;
}
/* skip console */
if (!strcmp("console", direntry->d_name)) {
continue;
}
/* skip null */
if (!strcmp("null", direntry->d_name)) {
continue;
}
snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
int sensfd = ::open(devname, 0);
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue;
}
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
close(sensfd);
printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
closedir(d);
warnx("directory listing ok (FS mounted and readable)");
} else {
/* failed opening dir */
warnx("FAILED LISTING DEVICE ROOT DIRECTORY");

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

@ -200,7 +200,8 @@ Mavlink::Mavlink() :
_task_should_exit(false),
_mavlink_fd(-1),
_task_running(false),
_mavlink_hil_enabled(false),
_hil_enabled(false),
_is_usb_uart(false),
_main_loop_delay(1000),
_subscriptions(nullptr),
_streams(nullptr),
@ -577,17 +578,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
return -1;
}
/*
* Setup hardware flow control. If the port has no RTS pin this call will fail,
* which is not an issue, but requires a separate call so we can fail silently.
*/
(void)tcgetattr(_uart_fd, &uart_config);
uart_config.c_cflag |= CRTS_IFLOW;
(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
if (!_is_usb_uart) {
/*
* Setup hardware flow control. If the port has no RTS pin this call will fail,
* which is not an issue, but requires a separate call so we can fail silently.
*/
(void)tcgetattr(_uart_fd, &uart_config);
uart_config.c_cflag |= CRTS_IFLOW;
(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
/* setup output flow control */
if (enable_flow_control(true)) {
warnx("ERR FLOW CTRL EN");
/* setup output flow control */
if (enable_flow_control(true)) {
warnx("ERR FLOW CTRL EN");
}
}
return _uart_fd;
@ -596,6 +599,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
int
Mavlink::enable_flow_control(bool enabled)
{
// We can't do this on USB - skip
if (_is_usb_uart) {
return OK;
}
struct termios uart_config;
int ret = tcgetattr(_uart_fd, &uart_config);
if (enabled) {
@ -617,38 +625,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;
@ -1514,10 +1501,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_chan(mavlink_system.sysid, mavlink_system.compid, _channel, &msg, &statustext);
mavlink_missionlib_send_message(&msg);
mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
return OK;
} else {
@ -1625,8 +1610,7 @@ Mavlink::task_main(int argc, char *argv[])
int ch;
_baudrate = 57600;
_datarate = 0;
_mode = MODE_OFFBOARD;
_mode = MAVLINK_MODE_NORMAL;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
@ -1667,17 +1651,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;
@ -1713,42 +1688,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 bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate);
@ -1756,10 +1709,9 @@ Mavlink::task_main(int argc, char *argv[])
fflush(stdout);
struct termios uart_config_original;
bool usb_uart;
/* default values for arguments */
_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &usb_uart);
_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart);
if (_uart_fd < 0) {
warn("could not open %s", _device_name);
@ -1801,7 +1753,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);
@ -1813,20 +1765,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;
}
@ -1855,12 +1793,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 */

11
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; };
bool get_flow_control_enabled() { return _flow_control_enabled; }
@ -209,7 +207,8 @@ 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 */
bool _is_usb_uart; /**< Port is USB */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */

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

@ -108,8 +108,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
_hil_counter(0),
_hil_frames(0),
_old_timestamp(0),
_hil_local_proj_inited(0),
@ -647,7 +645,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
}
/* increment counters */
_hil_counter++;
_hil_frames++;
/* print HIL sensors rate */
@ -854,7 +851,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_battery_status.timestamp = timestamp;
hil_battery_status.voltage_v = 11.1f;
hil_battery_status.voltage_filtered_v = 11.1f;
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);

1
src/modules/mavlink/mavlink_receiver.h
View File

@ -139,7 +139,6 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
int _hil_counter;
int _hil_frames;
uint64_t _old_timestamp;
bool _hil_local_proj_inited;

3
src/modules/sensors/sensors.cpp
View File

@ -1592,8 +1592,7 @@ Sensors::task_main()
/* check parameters for updates */
parameter_update_poll();
/* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */
raw.timestamp = hrt_absolute_time();
/* the timestamp of the raw struct is updated by the gyro_poll() method */
/* copy most recent sensor data */
gyro_poll(raw);