Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

mavlink delete deprecated HIL_CONTROLS

Browse Source
This commit is contained in:
Daniel Agar
2018-03-27 13:10:16 -04:00
parent 3db287ba67
commit d40d165b4b
1 changed files with 0 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
-169
View File
@@ -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),