diff options
Diffstat (limited to 'src/modules/mavlink/orb_listener.c')
-rw-r--r-- | src/modules/mavlink/orb_listener.c | 89 |
1 files changed, 63 insertions, 26 deletions
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index edb8761b8..53d86ec00 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -71,7 +71,8 @@ struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; -struct actuator_controls_effective_s actuators_0; +struct actuator_controls_effective_s actuators_effective_0; +struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; struct mavlink_subscriptions mavlink_subs; @@ -112,6 +113,7 @@ static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); +static void l_vehicle_attitude_controls_effective(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); static void l_optical_flow(const struct listener *l); @@ -138,6 +140,7 @@ static const struct listener listeners[] = { {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, + {l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, @@ -272,19 +275,23 @@ l_vehicle_status(const struct listener *l) orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* enable or disable HIL */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, - mavlink_mode, - v_status.state_machine, + mavlink_base_mode, + mavlink_custom_mode, mavlink_state); } @@ -334,7 +341,7 @@ l_global_position(const struct listener *l) int16_t vz = (int16_t)(global_pos.vz * 100.0f); /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); + uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, @@ -470,8 +477,9 @@ l_actuator_outputs(const struct listener *l) /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* HIL message as per MAVLink spec */ @@ -488,7 +496,7 @@ l_actuator_outputs(const struct listener *l) -1, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { @@ -502,7 +510,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { @@ -516,7 +524,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_mode, + mavlink_base_mode, 0); } else { @@ -530,7 +538,7 @@ l_actuator_outputs(const struct listener *l) (act_outputs.output[5] - 1500.0f) / 500.0f, (act_outputs.output[6] - 1500.0f) / 500.0f, (act_outputs.output[7] - 1500.0f) / 500.0f, - mavlink_mode, + mavlink_base_mode, 0); } } @@ -562,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l) } void -l_vehicle_attitude_controls(const struct listener *l) +l_vehicle_attitude_controls_effective(const struct listener *l) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl0 ", - actuators_0.control_effective[0]); + actuators_effective_0.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl1 ", - actuators_0.control_effective[1]); + actuators_effective_0.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl2 ", - actuators_0.control_effective[2]); + actuators_effective_0.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl3 ", - actuators_0.control_effective[3]); + actuators_effective_0.control_effective[3]); + } +} + +void +l_vehicle_attitude_controls(const struct listener *l) +{ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); + + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl0 ", + actuators_0.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl1 ", + actuators_0.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators_0.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl3 ", + actuators_0.control[3]); } } @@ -632,12 +666,12 @@ l_airspeed(const struct listener *l) orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); - float alt = global_pos.alt; - float climb = global_pos.vz; + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); + float alt = global_pos.relative_alt; + float climb = -global_pos.vz; - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, - ((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } static void * @@ -673,7 +707,7 @@ uorb_receive_thread(void *arg) /* handle the poll result */ if (poll_ret == 0) { - mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + /* silent */ } else if (poll_ret < 0) { mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); @@ -768,7 +802,10 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */ + + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ |