fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

SITL: switch to HIL_ACTUATOR_CONTROLS mavlink message & add pwm_out_sim support for 16 outputs

Browse Source
This commit is contained in:
Beat Küng 2016-09-01 16:34:46 +02:00
parent 41913c4a80
commit 010c9e937b
7 changed files with 179 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
posix-configs/SITL/init/ekf2/typhoon_h480
View File

@ -50,7 +50,7 @@ accelsim start
barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
pwm_out_sim mode_pwm16
sleep 1
sensors start
commander start

2
posix-configs/SITL/init/lpe/typhoon_h480
View File

@ -50,7 +50,7 @@ accelsim start
barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
pwm_out_sim mode_pwm16
sleep 1
sensors start
commander start

13
src/drivers/pwm_out_sim/pwm_out_sim.cpp
View File

@ -471,6 +471,10 @@ PWMSim::task_main()
num_outputs = 8;
break;
case MODE_16PWM:
num_outputs = 16;
break;
default:
num_outputs = 0;
break;
@ -729,7 +733,11 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
if (_mode == MODE_8PWM) {
if (_mode == MODE_16PWM) {
*(unsigned *)arg = 16;
} else if (_mode == MODE_8PWM) {
*(unsigned *)arg = 8;
} else if (_mode == MODE_4PWM) {
@ -1000,6 +1008,9 @@ pwm_out_sim_main(int argc, char *argv[])
} else if (!strcmp(verb, "mode_port2_pwm16")) {
new_mode = PORT2_8PWM;
} else if (!strcmp(verb, "mode_pwm16")) {
new_mode = PORT2_16PWM;
}
/* was a new mode set? */

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

@ -851,13 +851,13 @@ Mavlink::set_hil_enabled(bool hil_enabled)
/* enable HIL */
if (hil_enabled && !_hil_enabled) {
_hil_enabled = true;
configure_stream("HIL_CONTROLS", 200.0f);
configure_stream("HIL_ACTUATOR_CONTROLS", 200.0f);
}
/* disable HIL */
if (!hil_enabled && _hil_enabled) {
_hil_enabled = false;
configure_stream("HIL_CONTROLS", 0.0f);
configure_stream("HIL_ACTUATOR_CONTROLS", 0.0f);
} else {
ret = ERROR;

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

@ -2053,6 +2053,7 @@ protected:
};
//TODO: this is deprecated (09.2016). Remove it some time in the future...
class MavlinkStreamHILControls : public MavlinkStream
{
public:
@ -2216,6 +2217,158 @@ protected:
}
};
class MavlinkStreamHILActuatorControls : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamHILActuatorControls::get_name_static();
}
static const char *get_name_static()
{
return "HIL_ACTUATOR_CONTROLS";
}
static uint8_t get_id_static()
{
return MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS;
}
uint8_t get_id()
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamHILActuatorControls(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_HIL_ACTUATOR_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 */
MavlinkStreamHILActuatorControls(MavlinkStreamHILActuatorControls &);
MavlinkStreamHILActuatorControls &operator = (const MavlinkStreamHILActuatorControls &);
protected:
explicit MavlinkStreamHILActuatorControls(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)
{}
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
struct actuator_outputs_s act;
bool 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;
mavlink_hil_actuator_controls_t msg;
get_mavlink_mode_state(&status, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
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 < 16; i++) {
if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i < n) {
/* scale PWM out 900..2100 us to 0..1 for rotors */
msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
} else {
/* scale PWM out 900..2100 us to -1..1 for other channels */
msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
}
} else {
/* send 0 when disarmed and for disabled channels */
msg.controls[i] = 0.0f;
}
}
} else {
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
for (unsigned i = 0; i < 16; i++) {
if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i != 3) {
/* scale PWM out 900..2100 us to -1..1 for normal channels */
msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
} else {
/* scale PWM out 900..2100 us to 0..1 for throttle */
msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
}
} else {
/* set 0 for disabled channels */
msg.controls[i] = 0.0f;
}
}
}
msg.time_usec = hrt_absolute_time();
msg.mode = mavlink_base_mode;
msg.flags = 0;
mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg);
}
}
};
class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream
{
@ -3314,6 +3467,7 @@ const StreamListItem *streams_list[] = {
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),
new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static, &MavlinkStreamAttitudeTarget::get_id_static),

2
src/modules/simulator/simulator.h
View File

@ -325,7 +325,7 @@ private:
void send_controls();
void pollForMAVLinkMessages(bool publish, int udp_port);
void pack_actuator_message(mavlink_hil_controls_t &actuator_msg, unsigned index);
void pack_actuator_message(mavlink_hil_actuator_controls_t &actuator_msg, unsigned index);
void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
void update_sensors(mavlink_hil_sensor_t *imu);
void update_gps(mavlink_hil_gps_t *gps_sim);

27
src/modules/simulator/simulator_mavlink.cpp
View File

@ -76,37 +76,26 @@ const unsigned mode_flag_custom = 1;
using namespace simulator;
void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg, unsigned index)
void Simulator::pack_actuator_message(mavlink_hil_actuator_controls_t &actuator_msg, unsigned index)
{
// reset state
memset(&actuator_msg, 0, sizeof(actuator_msg));
actuator_msg.time_usec = hrt_absolute_time();
float out[8] = {};
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
for (unsigned i = 0; i < (sizeof(out) / sizeof(out[0])); i++) {
for (unsigned i = 0; i < MAVLINK_MSG_HIL_ACTUATOR_CONTROLS_FIELD_CONTROLS_LEN; i++) {
// scale PWM out 900..2100 us to -1..1 */
out[i] = (_actuators[index].output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
actuator_msg.controls[i] = (_actuators[index].output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
if (!PX4_ISFINITE(out[i])) {
out[i] = -1.0f;
if (!PX4_ISFINITE(actuator_msg.controls[i])) {
actuator_msg.controls[i] = -1.0f;
}
}
actuator_msg.roll_ailerons = out[0];
actuator_msg.pitch_elevator = out[1];
actuator_msg.yaw_rudder = out[2];
actuator_msg.throttle = out[3];
actuator_msg.aux1 = out[4];
actuator_msg.aux2 = out[5];
actuator_msg.aux3 = out[6];
actuator_msg.aux4 = out[7];
actuator_msg.mode = mode_flag_custom;
actuator_msg.mode |= (armed) ? mode_flag_armed : 0;
actuator_msg.nav_mode = index; // XXX this indicates the output group in our use of the message
actuator_msg.flags = 0;
}
void Simulator::send_controls()
@ -117,9 +106,9 @@ void Simulator::send_controls()
continue;
}
mavlink_hil_controls_t msg;
mavlink_hil_actuator_controls_t msg;
pack_actuator_message(msg, i);
send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
send_mavlink_message(MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS, &msg, 200);
}
}