fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 05:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Simulator MAVLink: Forward port cleanly

Browse Source
This commit is contained in:
Lorenz Meier
2016-06-17 22:44:42 +02:00
parent 46ec1e6b95
commit f11d42aab3
1 changed files with 19 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator_mavlink.cpp
+19 -47
View File
@@ -72,60 +72,41 @@ static socklen_t _addrlen = sizeof(_srcaddr);
static bool actuators_on = false;
static hrt_abstime batt_sim_start = 0;
const unsigned mode_flag_armed = 128; // following MAVLink spec
const unsigned mode_flag_custom = 1;
using namespace simulator;
void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
{
// 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 now we only support quadrotors
unsigned n = 4;
for (unsigned i = 0; i < (sizeof(out) / sizeof(out[0])); i++) {
// scale PWM out 900..2100 us to -1..1 */
out[i] = (_actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
if (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) {
for (unsigned i = 0; i < 8; i++) {
if (_actuators.output[i] > PWM_DEFAULT_MIN / 2) {
if (i < n) {
// scale PWM out 900..2100 us to 0..1 for rotors */
out[i] = (_actuators.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
} else {
// scale PWM out 900..2100 us to -1..1 for other channels */
out[i] = (_actuators.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 {
// convert back to range [-1, 1]
for (unsigned i = 0; i < 8; i++) {
out[i] = (_actuators.output[i] - 1500) / 600.0f;
if (!PX4_ISFINITE(out[i])) {
out[i] = -1.0f;
}
}
// if vehicle status has not yet been updated, set actuator commands to zero
// this is to prevent the simulation getting into a bad state
if (_vehicle_status.timestamp == 0) {
memset(out, 0, sizeof(out));
}
actuators_on = (out[3] > 0.1f);
actuator_msg.time_usec = hrt_absolute_time();
actuator_msg.roll_ailerons = out[0];
actuator_msg.pitch_elevator = (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) ? out[1] : -out[1];
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 = _actuators.output[6] > PWM_DEFAULT_MIN / 2 ? out[6] : -1.0f;;
actuator_msg.aux3 = out[6];
actuator_msg.aux4 = out[7];
actuator_msg.mode = 0; // need to put something here
actuator_msg.mode = mode_flag_custom;
actuator_msg.mode |= (armed) ? mode_flag_armed : 0;
actuator_msg.nav_mode = 0;
}
@@ -143,17 +124,6 @@ static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t
rc->channel_count = rc_channels->chancount;
rc->rssi = rc_channels->rssi;
/* PX4_WARN("RC: %d, %d, %d, %d, %d, %d, %d, %d",
rc_channels->chan1_raw,
rc_channels->chan2_raw,
rc_channels->chan3_raw,
rc_channels->chan4_raw,
rc_channels->chan5_raw,
rc_channels->chan6_raw,
rc_channels->chan7_raw,
rc_channels->chan8_raw);
*/
rc->values[0] = rc_channels->chan1_raw;
rc->values[1] = rc_channels->chan2_raw;
rc->values[2] = rc_channels->chan3_raw;
@@ -552,6 +522,8 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
// send hearbeat
mavlink_heartbeat_t hb = {};
hb.autopilot = 12;
hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0;
send_mavlink_message(MAVLINK_MSG_ID_HEARTBEAT, &hb, 200);
if (len > 0) {