diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index de0691cb34..c68e4d681c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2438,174 +2438,6 @@ protected: } }; - -//TODO: this is deprecated (09.2016). Remove it some time in the future... -class MavlinkStreamHILControls : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamHILControls::get_name_static(); - } - - static const char *get_name_static() - { - return "HIL_CONTROLS"; - } - - static uint16_t get_id_static() - { - return MAVLINK_MSG_ID_HIL_CONTROLS; - } - - uint16_t get_id() - { - return get_id_static(); - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamHILControls(mavlink); - } - - unsigned get_size() - { - return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - -private: - MavlinkOrbSubscription *_status_sub; - uint64_t _status_time; - - MavlinkOrbSubscription *_act_sub; - uint64_t _act_time; - - /* do not allow top copying this class */ - MavlinkStreamHILControls(MavlinkStreamHILControls &); - MavlinkStreamHILControls &operator = (const MavlinkStreamHILControls &); - -protected: - explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink), - _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), - _status_time(0), - _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))), - _act_time(0) - {} - - bool send(const hrt_abstime t) - { - vehicle_status_s status = {}; - actuator_outputs_s act = {}; - - bool updated = false; - updated |= _act_sub->update(&_act_time, &act); - updated |= _status_sub->update(&_status_time, &status); - - if (updated && (status.arming_state == vehicle_status_s::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, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - float out[8]; - - const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; - - unsigned system_type = _mavlink->get_system_type(); - - /* scale outputs depending on system type */ - if (system_type == MAV_TYPE_QUADROTOR || - system_type == MAV_TYPE_HEXAROTOR || - system_type == MAV_TYPE_OCTOROTOR || - system_type == MAV_TYPE_VTOL_DUOROTOR || - system_type == MAV_TYPE_VTOL_QUADROTOR) { - - /* multirotors: set number of rotor outputs depending on type */ - - unsigned n; - - switch (system_type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; - - case MAV_TYPE_HEXAROTOR: - n = 6; - break; - - case MAV_TYPE_VTOL_DUOROTOR: - n = 2; - break; - - case MAV_TYPE_VTOL_QUADROTOR: - n = 4; - break; - - default: - n = 8; - break; - } - - for (unsigned i = 0; i < 8; i++) { - if (act.output[i] > PWM_DEFAULT_MIN / 2) { - if (i < n) { - /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */ - out[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); - - } else { - /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); - } - - } else { - /* send 0 when disarmed and for disabled channels */ - out[i] = 0.0f; - } - } - - } else { - /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ - - for (unsigned i = 0; i < 8; i++) { - if (act.output[i] > PWM_DEFAULT_MIN / 2) { - if (i != 3) { - /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for normal channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); - - } else { - /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for throttle */ - out[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); - } - - } else { - /* set 0 for disabled channels */ - out[i] = 0.0f; - } - } - } - - mavlink_hil_controls_t msg = {}; - - msg.time_usec = hrt_absolute_time(); - msg.roll_ailerons = out[0]; - msg.pitch_elevator = out[1]; - msg.yaw_rudder = out[2]; - msg.throttle = out[3]; - msg.aux1 = out[4]; - msg.aux2 = out[5]; - msg.aux3 = out[6]; - msg.aux4 = out[7]; - msg.mode = mavlink_base_mode; - msg.nav_mode = 0; - - mavlink_msg_hil_controls_send_struct(_mavlink->get_channel(), &msg); - } - - return updated; - } -}; - class MavlinkStreamHILActuatorControls : public MavlinkStream { public: @@ -4430,7 +4262,6 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static, &MavlinkStreamServoOutputRaw<1>::get_id_static), new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static, &MavlinkStreamServoOutputRaw<2>::get_id_static), new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static, &MavlinkStreamServoOutputRaw<3>::get_id_static), - new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static, &MavlinkStreamHILControls::get_id_static), new StreamListItem(&MavlinkStreamHILActuatorControls::new_instance, &MavlinkStreamHILActuatorControls::get_name_static, &MavlinkStreamHILActuatorControls::get_id_static), new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static, &MavlinkStreamPositionTargetGlobalInt::get_id_static), new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static, &MavlinkStreamLocalPositionSetpoint::get_id_static),