void send(const hrt_abstime t)
{
if (gps_sub->update(t)) {
mavlink_msg_gps_raw_int_send(_channel,
gps->timestamp_position,
gps->fix_type,
gps->lat,
gps->lon,
gps->alt,
cm_uint16_from_m_float(gps->eph_m),
cm_uint16_from_m_float(gps->epv_m),
gps->vel_m_s * 100.0f,
_wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps->satellites_visible);
}
}
void send(const hrt_abstime t)
{
struct actuator_outputs_s act;
if (act_sub->update(&act_time, &act)) {
mavlink_msg_servo_output_raw_send(_channel,
act.timestamp / 1000,
N,
act.output[0],
act.output[1],
act.output[2],
act.output[3],
act.output[4],
act.output[5],
act.output[6],
act.output[7]);
}
}
void send(const hrt_abstime t)
{
struct vehicle_gps_position_s gps;
if (gps_sub->update(&gps_time, &gps)) {
mavlink_msg_gps_raw_int_send(_channel,
gps.timestamp_position,
gps.fix_type + (gps.fix_quality << 4),
gps.lat,
gps.lon,
gps.alt,
cm_uint16_from_m_float(gps.eph),
cm_uint16_from_m_float(gps.epv),
gps.vel_m_s * 100.0f,
_wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps.satellites_used);
}
}
void send(const hrt_abstime t)
{
status_sub->update(t);
mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 100.0f,
status->battery_remaining * 100.0f,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
status->errors_count2,
status->errors_count3,
status->errors_count4);
}
void send(const hrt_abstime t)
{
if (range_sub->update(t)) {
uint8_t type;
switch (range->type) {
case RANGE_FINDER_TYPE_LASER:
type = MAV_DISTANCE_SENSOR_LASER;
break;
}
uint8_t id = 0;
uint8_t orientation = 0;
uint8_t covariance = 20;
mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
}
}
void send(const hrt_abstime t)
{
struct sensor_combined_s sensor;
if (sensor_sub->update(&sensor_time, &sensor)) {
uint16_t fields_updated = 0;
if (accel_timestamp != sensor.accelerometer_timestamp) {
/* mark first three dimensions as changed */
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
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);
}
}
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
if (status_sub->update(&status)) {
mavlink_msg_sys_status_send(_channel,
status.onboard_control_sensors_present,
status.onboard_control_sensors_enabled,
status.onboard_control_sensors_health,
status.load * 1000.0f,
status.battery_voltage * 1000.0f,
status.battery_current * 100.0f,
status.battery_remaining * 100.0f,
status.drop_rate_comm,
status.errors_comm,
status.errors_count1,
status.errors_count2,
status.errors_count3,
status.errors_count4);
}
}
void send(const hrt_abstime t)
{
if (att_ctrl_sub->update(t)) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"rll ctrl ",
att_ctrl->control[0]);
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"ptch ctrl ",
att_ctrl->control[1]);
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"yaw ctrl ",
att_ctrl->control[2]);
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"thr ctrl ",
att_ctrl->control[3]);
}
}
void send(const hrt_abstime t)
{
if (rc_sub->update(t)) {
const unsigned port_width = 8;
for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(_channel,
rc->timestamp_publication / 1000,
i,
(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) + 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) + 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) + 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->rssi);
}
}
}
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
struct position_setpoint_triplet_s pos_sp_triplet;
struct actuator_outputs_s act;
bool updated = act_sub->update(&act_time, &act);
updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet);
updated |= status_sub->update(&status_time, &status);
if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
/* translate the current syste state to mavlink state and mode */
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);
if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
mavlink_system.type == MAV_TYPE_HEXAROTOR ||
mavlink_system.type == MAV_TYPE_OCTOROTOR) {
/* set number of valid outputs depending on vehicle type */
unsigned n;
switch (mavlink_system.type) {
case MAV_TYPE_QUADROTOR:
n = 4;
break;
case MAV_TYPE_HEXAROTOR:
n = 6;
break;
default:
n = 8;
break;
}
/* scale / assign outputs depending on system type */
float out[8];
for (unsigned i = 0; i < 8; i++) {
if (i < n) {
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
/* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
} else {
/* send 0 when disarmed */
out[i] = 0.0f;
}
} else {
out[i] = -1.0f;
}
}
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
mavlink_base_mode,
0);
} else {
/* fixed wing: scale all channels except throttle -1 .. 1
* because we know that we set the mixers up this way
*/
float out[8];
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
for (unsigned i = 0; i < 8; i++) {
if (i != 3) {
/* scale fake PWM out 900..2100 us to -1..+1 for normal channels */
out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
} else {
/* scale fake PWM out 900..2100 us to 0..1 for throttle */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
}
}
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
mavlink_base_mode,
0);
}
}
}
void subscribe(Mavlink *mavlink)
{
pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint));
pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
home = (struct home_position_s *)home_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position));
gps = (struct vehicle_gps_position_s *)gps_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint));
manual = (struct manual_control_setpoint_s *)manual_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
status = (struct vehicle_status_s *)status_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow));
flow = (struct optical_flow_s *)flow_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
range = (struct range_finder_report *)range_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
debug = (struct debug_key_value_s *)debug_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
att = (struct vehicle_attitude_s *)att_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint));
att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined));
sensor = (struct sensor_combined_s *)sensor_sub->get_data();
}
void subscribe(Mavlink *mavlink)
{
rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc));
rc = (struct rc_input_values *)rc_sub->get_data();
}
