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

Merged mavlink2_hil

This commit is contained in:
Lorenz Meier 2014-03-17 13:22:06 +01:00
parent c256ba54a0 2db7d30400
commit d2feafaac9
9 changed files with 337 additions and 238 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -124,7 +124,7 @@ CDev::register_class_devname(const char *class_devname)
if (ret == OK) break; if (ret == OK) break;
} else { } else {
char name[32]; 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); ret = register_driver(name, &fops, 0666, (void *)this);
if (ret == OK) break; 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: case SENSORIOCRESET:
cmd_reset(); cmd_reset();
break; break;
default:
/* give it to parent if no one wants it */
ret = CDev::ioctl(filp, cmd, arg);
break;
} }
unlock(); unlock();

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

@ -445,7 +445,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
// XXX write this out to perf regs // XXX write this out to perf regs
/* keep track of sensor updates */ /* keep track of sensor updates */
uint32_t sensor_last_count[3] = {0, 0, 0};
uint64_t sensor_last_timestamp[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_so3_params so3_comp_params; struct attitude_estimator_so3_params so3_comp_params;

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

@ -44,6 +44,7 @@
#include <stdbool.h> #include <stdbool.h>
#include <dirent.h> #include <dirent.h>
#include <fcntl.h> #include <fcntl.h>
#include <string.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.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; bool valid_transition = false;
int ret = ERROR; int ret = ERROR;
warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
if (current_status->hil_state == new_state) { if (current_status->hil_state == new_state) {
warnx("Hil state not changed");
valid_transition = true; valid_transition = true;
} else { } else {
@ -340,23 +338,60 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/* list directory */ /* list directory */
DIR *d; DIR *d;
struct dirent *direntry;
d = opendir("/dev"); d = opendir("/dev");
if (d) { if (d) {
struct dirent *direntry;
char devname[24];
while ((direntry = readdir(d)) != NULL) { while ((direntry = readdir(d)) != NULL) {
int sensfd = ::open(direntry->d_name, 0); /* skip serial ports */
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); 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); 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); closedir(d);
warnx("directory listing ok (FS mounted and readable)");
} else { } else {
/* failed opening dir */ /* failed opening dir */
warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); warnx("FAILED LISTING DEVICE ROOT DIRECTORY");

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

@ -201,6 +201,7 @@ Mavlink::Mavlink() :
_mavlink_fd(-1), _mavlink_fd(-1),
_task_running(false), _task_running(false),
_hil_enabled(false), _hil_enabled(false),
_is_usb_uart(false),
_main_loop_delay(1000), _main_loop_delay(1000),
_subscriptions(nullptr), _subscriptions(nullptr),
_streams(nullptr), _streams(nullptr),
@ -577,17 +578,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
return -1; return -1;
} }
/* 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. * 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)tcgetattr(_uart_fd, &uart_config);
(void)tcsetattr(_uart_fd, TCSANOW, &uart_config); uart_config.c_cflag |= CRTS_IFLOW;
(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
/* setup output flow control */ /* setup output flow control */
if (enable_flow_control(true)) { if (enable_flow_control(true)) {
warnx("ERR FLOW CTRL EN"); warnx("ERR FLOW CTRL EN");
}
} }
return _uart_fd; return _uart_fd;
@ -596,6 +599,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
int int
Mavlink::enable_flow_control(bool enabled) Mavlink::enable_flow_control(bool enabled)
{ {
// We can't do this on USB - skip
if (_is_usb_uart) {
return OK;
}
struct termios uart_config; struct termios uart_config;
int ret = tcgetattr(_uart_fd, &uart_config); int ret = tcgetattr(_uart_fd, &uart_config);
if (enabled) { if (enabled) {
@ -1701,10 +1709,9 @@ Mavlink::task_main(int argc, char *argv[])
fflush(stdout); fflush(stdout);
struct termios uart_config_original; struct termios uart_config_original;
bool usb_uart;
/* default values for arguments */ /* 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) { if (_uart_fd < 0) {
warn("could not open %s", _device_name); warn("could not open %s", _device_name);

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

@ -208,6 +208,7 @@ private:
/* states */ /* states */
bool _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 */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */

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

@ -216,8 +216,8 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
status_sub->update(t); (void)status_sub->update(t);
pos_sp_triplet_sub->update(t); (void)pos_sp_triplet_sub->update(t);
uint8_t mavlink_state = 0; uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0; uint8_t mavlink_base_mode = 0;
@ -261,22 +261,23 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
status_sub->update(t); if (status_sub->update(t)) {
mavlink_msg_sys_status_send(_channel, mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present, status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled, status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health, status->onboard_control_sensors_health,
status->load * 1000.0f, status->load * 1000.0f,
status->battery_voltage * 1000.0f, status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f, status->battery_current * 1000.0f,
status->battery_remaining, status->battery_remaining,
status->drop_rate_comm, status->drop_rate_comm,
status->errors_comm, status->errors_comm,
status->errors_count1, status->errors_count1,
status->errors_count2, status->errors_count2,
status->errors_count3, status->errors_count3,
status->errors_count4); status->errors_count4);
}
} }
}; };
@ -316,42 +317,43 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
sensor_sub->update(t); if (sensor_sub->update(t)) {
uint16_t fields_updated = 0; uint16_t fields_updated = 0;
if (accel_timestamp != sensor->accelerometer_timestamp) { if (accel_timestamp != sensor->accelerometer_timestamp) {
/* mark first three dimensions as changed */ /* mark first three dimensions as changed */
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
accel_timestamp = sensor->accelerometer_timestamp; accel_timestamp = sensor->accelerometer_timestamp;
}
if (gyro_timestamp != sensor->timestamp) {
/* mark second group dimensions as changed */
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
gyro_timestamp = sensor->timestamp;
}
if (mag_timestamp != sensor->magnetometer_timestamp) {
/* mark third group dimensions as changed */
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
mag_timestamp = sensor->magnetometer_timestamp;
}
if (baro_timestamp != sensor->baro_timestamp) {
/* mark last group dimensions as changed */
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
baro_timestamp = sensor->baro_timestamp;
}
mavlink_msg_highres_imu_send(_channel,
sensor->timestamp,
sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
sensor->baro_pres_mbar, sensor->differential_pressure_pa,
sensor->baro_alt_meter, sensor->baro_temp_celcius,
fields_updated);
} }
if (gyro_timestamp != sensor->timestamp) {
/* mark second group dimensions as changed */
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
gyro_timestamp = sensor->timestamp;
}
if (mag_timestamp != sensor->magnetometer_timestamp) {
/* mark third group dimensions as changed */
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
mag_timestamp = sensor->magnetometer_timestamp;
}
if (baro_timestamp != sensor->baro_timestamp) {
/* mark last group dimensions as changed */
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
baro_timestamp = sensor->baro_timestamp;
}
mavlink_msg_highres_imu_send(_channel,
sensor->timestamp,
sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
sensor->baro_pres_mbar, sensor->differential_pressure_pa,
sensor->baro_alt_meter, sensor->baro_temp_celcius,
fields_updated);
} }
}; };
@ -382,12 +384,13 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
att_sub->update(t); if (att_sub->update(t)) {
mavlink_msg_attitude_send(_channel, mavlink_msg_attitude_send(_channel,
att->timestamp / 1000, att->timestamp / 1000,
att->roll, att->pitch, att->yaw, att->roll, att->pitch, att->yaw,
att->rollspeed, att->pitchspeed, att->yawspeed); att->rollspeed, att->pitchspeed, att->yawspeed);
}
} }
}; };
@ -418,17 +421,18 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
att_sub->update(t); if (att_sub->update(t)) {
mavlink_msg_attitude_quaternion_send(_channel, mavlink_msg_attitude_quaternion_send(_channel,
att->timestamp / 1000, att->timestamp / 1000,
att->q[0], att->q[0],
att->q[1], att->q[1],
att->q[2], att->q[2],
att->q[3], att->q[3],
att->rollspeed, att->rollspeed,
att->pitchspeed, att->pitchspeed,
att->yawspeed); att->yawspeed);
}
} }
}; };
@ -483,23 +487,26 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
att_sub->update(t); bool updated = att_sub->update(t);
pos_sub->update(t); updated |= pos_sub->update(t);
armed_sub->update(t); updated |= armed_sub->update(t);
act_sub->update(t); updated |= act_sub->update(t);
airspeed_sub->update(t); updated |= airspeed_sub->update(t);
float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); if (updated) {
uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_channel, float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
airspeed->true_airspeed_m_s, uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
groundspeed, float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
heading,
throttle, mavlink_msg_vfr_hud_send(_channel,
pos->alt, airspeed->true_airspeed_m_s,
-pos->vel_d); groundspeed,
heading,
throttle,
pos->alt,
-pos->vel_d);
}
} }
}; };
@ -530,19 +537,20 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
gps_sub->update(t); if (gps_sub->update(t)) {
mavlink_msg_gps_raw_int_send(_channel, mavlink_msg_gps_raw_int_send(_channel,
gps->timestamp_position, gps->timestamp_position,
gps->fix_type, gps->fix_type,
gps->lat, gps->lat,
gps->lon, gps->lon,
gps->alt, gps->alt,
cm_uint16_from_m_float(gps->eph_m), cm_uint16_from_m_float(gps->eph_m),
cm_uint16_from_m_float(gps->epv_m), cm_uint16_from_m_float(gps->epv_m),
gps->vel_m_s * 100.0f, gps->vel_m_s * 100.0f,
_wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps->satellites_visible); gps->satellites_visible);
}
} }
}; };
@ -579,10 +587,11 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
pos_sub->update(t); bool updated = pos_sub->update(t);
home_sub->update(t); updated |= home_sub->update(t);
mavlink_msg_global_position_int_send(_channel, if (updated) {
mavlink_msg_global_position_int_send(_channel,
pos->timestamp / 1000, pos->timestamp / 1000,
pos->lat * 1e7, pos->lat * 1e7,
pos->lon * 1e7, pos->lon * 1e7,
@ -592,6 +601,7 @@ protected:
pos->vel_e * 100.0f, pos->vel_e * 100.0f,
pos->vel_d * 100.0f, pos->vel_d * 100.0f,
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
}
} }
}; };
@ -622,16 +632,17 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
pos_sub->update(t); if (pos_sub->update(t)) {
mavlink_msg_local_position_ned_send(_channel, mavlink_msg_local_position_ned_send(_channel,
pos->timestamp / 1000, pos->timestamp / 1000,
pos->x, pos->x,
pos->y, pos->y,
pos->z, pos->z,
pos->vx, pos->vx,
pos->vy, pos->vy,
pos->vz); pos->vz);
}
} }
}; };
@ -662,12 +673,17 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
home_sub->update(t);
mavlink_msg_gps_global_origin_send(_channel, /* we're sending the GPS home periodically to ensure the
(int32_t)(home->lat * 1e7), * the GCS does pick it up at one point */
(int32_t)(home->lon * 1e7), if (home_sub->is_published()) {
(int32_t)(home->alt) * 1000.0f); home_sub->update(t);
mavlink_msg_gps_global_origin_send(_channel,
(int32_t)(home->lat * 1e7),
(int32_t)(home->lon * 1e7),
(int32_t)(home->alt) * 1000.0f);
}
} }
}; };
@ -713,19 +729,20 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
act_sub->update(t); if (act_sub->update(t)) {
mavlink_msg_servo_output_raw_send(_channel, mavlink_msg_servo_output_raw_send(_channel,
act->timestamp / 1000, act->timestamp / 1000,
_n, _n,
act->output[0], act->output[0],
act->output[1], act->output[1],
act->output[2], act->output[2],
act->output[3], act->output[3],
act->output[4], act->output[4],
act->output[5], act->output[5],
act->output[6], act->output[6],
act->output[7]); act->output[7]);
}
} }
}; };
@ -768,57 +785,60 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
status_sub->update(t); bool updated = status_sub->update(t);
pos_sp_triplet_sub->update(t); updated |= pos_sp_triplet_sub->update(t);
act_sub->update(t); updated |= act_sub->update(t);
/* translate the current syste state to mavlink state and mode */ if (updated) {
uint8_t mavlink_state;
uint8_t mavlink_base_mode;
uint32_t mavlink_custom_mode;
get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* set number of valid outputs depending on vehicle type */ /* translate the current syste state to mavlink state and mode */
unsigned n; uint8_t mavlink_state;
uint8_t mavlink_base_mode;
uint32_t mavlink_custom_mode;
get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
switch (mavlink_system.type) { /* set number of valid outputs depending on vehicle type */
case MAV_TYPE_QUADROTOR: unsigned n;
n = 4;
break;
case MAV_TYPE_HEXAROTOR: switch (mavlink_system.type) {
n = 6; case MAV_TYPE_QUADROTOR:
break; n = 4;
break;
default: case MAV_TYPE_HEXAROTOR:
n = 8; n = 6;
break; break;
}
/* scale / assign outputs depending on system type */ default:
float out[8]; n = 8;
break;
}
for (unsigned i = 0; i < 8; i++) { /* scale / assign outputs depending on system type */
if (i < n) { float out[8];
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
/* scale fake PWM out 900..1200 us to 0..1*/ for (unsigned i = 0; i < 8; i++) {
out[i] = (act->output[i] - 900.0f) / 1200.0f; if (i < n) {
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
/* scale fake PWM out 900..1200 us to 0..1*/
out[i] = (act->output[i] - 900.0f) / 1200.0f;
} else {
/* send 0 when disarmed */
out[i] = 0.0f;
}
} else { } else {
/* send 0 when disarmed */ out[i] = -1.0f;
out[i] = 0.0f;
} }
} else {
out[i] = -1.0f;
} }
}
mavlink_msg_hil_controls_send(_channel, mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(), hrt_absolute_time(),
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
mavlink_base_mode, mavlink_base_mode,
0); 0);
}
} }
}; };
@ -849,14 +869,15 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
pos_sp_triplet_sub->update(t); if (pos_sp_triplet_sub->update(t)) {
mavlink_msg_global_position_setpoint_int_send(_channel, mavlink_msg_global_position_setpoint_int_send(_channel,
MAV_FRAME_GLOBAL, MAV_FRAME_GLOBAL,
(int32_t)(pos_sp_triplet->current.lat * 1e7), (int32_t)(pos_sp_triplet->current.lat * 1e7),
(int32_t)(pos_sp_triplet->current.lon * 1e7), (int32_t)(pos_sp_triplet->current.lon * 1e7),
(int32_t)(pos_sp_triplet->current.alt * 1000), (int32_t)(pos_sp_triplet->current.alt * 1000),
(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
}
} }
}; };
@ -925,14 +946,15 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
att_sp_sub->update(t); if (att_sp_sub->update(t)) {
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
att_sp->timestamp / 1000, att_sp->timestamp / 1000,
att_sp->roll_body, att_sp->roll_body,
att_sp->pitch_body, att_sp->pitch_body,
att_sp->yaw_body, att_sp->yaw_body,
att_sp->thrust); att_sp->thrust);
}
} }
}; };
@ -963,14 +985,15 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
att_rates_sp_sub->update(t); if (att_rates_sp_sub->update(t)) {
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
att_rates_sp->timestamp / 1000, att_rates_sp->timestamp / 1000,
att_rates_sp->roll, att_rates_sp->roll,
att_rates_sp->pitch, att_rates_sp->pitch,
att_rates_sp->yaw, att_rates_sp->yaw,
att_rates_sp->thrust); att_rates_sp->thrust);
}
} }
}; };
@ -1001,24 +1024,25 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
rc_sub->update(t); if (rc_sub->update(t)) {
const unsigned port_width = 8; const unsigned port_width = 8;
for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */ /* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(_channel, mavlink_msg_rc_channels_raw_send(_channel,
rc->timestamp_publication / 1000, rc->timestamp_publication / 1000,
i, i,
(rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
rc->rssi); rc->rssi);
}
} }
} }
}; };
@ -1050,15 +1074,16 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
manual_sub->update(t); if (manual_sub->update(t)) {
mavlink_msg_manual_control_send(_channel, mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid, mavlink_system.sysid,
manual->roll * 1000, manual->roll * 1000,
manual->pitch * 1000, manual->pitch * 1000,
manual->yaw * 1000, manual->yaw * 1000,
manual->throttle * 1000, manual->throttle * 1000,
0); 0);
}
} }
}; };
@ -1089,15 +1114,16 @@ protected:
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
flow_sub->update(t); if (flow_sub->update(t)) {
mavlink_msg_optical_flow_send(_channel, mavlink_msg_optical_flow_send(_channel,
flow->timestamp, flow->timestamp,
flow->sensor_id, flow->sensor_id,
flow->flow_raw_x, flow->flow_raw_y, flow->flow_raw_x, flow->flow_raw_y,
flow->flow_comp_x_m, flow->flow_comp_y_m, flow->flow_comp_x_m, flow->flow_comp_y_m,
flow->quality, flow->quality,
flow->ground_distance_m); flow->ground_distance_m);
}
} }
}; };

21
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@ -46,11 +46,15 @@
#include "mavlink_orb_subscription.h" #include "mavlink_orb_subscription.h"
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr) MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
_fd(orb_subscribe(_topic)),
_published(false),
_topic(topic),
_last_check(0),
next(nullptr)
{ {
_data = malloc(topic->o_size); _data = malloc(topic->o_size);
memset(_data, 0, topic->o_size); memset(_data, 0, topic->o_size);
_fd = orb_subscribe(_topic);
} }
MavlinkOrbSubscription::~MavlinkOrbSubscription() MavlinkOrbSubscription::~MavlinkOrbSubscription()
@ -87,3 +91,16 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
return false; return false;
} }
bool
MavlinkOrbSubscription::is_published()
{
bool updated;
orb_check(_fd, &updated);
if (updated) {
_published = true;
}
return _published;
}

9
src/modules/mavlink/mavlink_orb_subscription.h
View File

@ -54,12 +54,21 @@ public:
~MavlinkOrbSubscription(); ~MavlinkOrbSubscription();
bool update(const hrt_abstime t); bool update(const hrt_abstime t);
/**
* Check if the topic has been published.
*
* This call will return true if the topic was ever published.
* @param true if the topic has been published at least once.
*/
bool is_published();
void *get_data(); void *get_data();
const orb_id_t get_topic(); const orb_id_t get_topic();
private: private:
const orb_id_t _topic; const orb_id_t _topic;
int _fd; int _fd;
bool _published;
void *_data; void *_data;
hrt_abstime _last_check; hrt_abstime _last_check;
}; };