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

Simulator: Fix code style

Browse Source
This commit is contained in:
Lorenz Meier
2015-09-20 11:35:52 +02:00
parent af11a22472
commit 30d0dab8e6
3 changed files with 190 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator_mavlink.cpp
+78 -55
View File
@@ -71,7 +71,8 @@ static socklen_t _addrlen = sizeof(_srcaddr);
using namespace simulator;
void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
{
float out[8] = {};
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
@@ -96,6 +97,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
out[i] = 0.0f;
}
}
} else {
// convert back to range [-1, 1]
for (unsigned i = 0; i < 8; i++) {
@@ -122,29 +124,31 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
actuator_msg.nav_mode = 0;
}
void Simulator::send_controls() {
void Simulator::send_controls()
{
mavlink_hil_controls_t msg;
pack_actuator_message(msg);
//PX4_WARN("Sending HIL_CONTROLS msg");
send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
}
static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels) {
static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels)
{
rc->timestamp_publication = hrt_absolute_time();
rc->timestamp_last_signal = hrt_absolute_time();
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);
*/
/* 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;
@@ -166,7 +170,8 @@ static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t
rc->values[17] = rc_channels->chan18_raw;
}
void Simulator::update_sensors(mavlink_hil_sensor_t *imu) {
void Simulator::update_sensors(mavlink_hil_sensor_t *imu)
{
// write sensor data to memory so that drivers can copy data from there
RawMPUData mpu;
mpu.accel_x = imu->xacc;
@@ -208,11 +213,12 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) {
write_airspeed_data((void *)&airspeed);
}
void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
void Simulator::update_gps(mavlink_hil_gps_t *gps_sim)
{
RawGPSData gps;
gps.lat = gps_sim->lat;
gps.lon = gps_sim->lon;
gps.alt = gps_sim->alt;
gps.lat = gps_sim->lat;
gps.lon = gps_sim->lon;
gps.alt = gps_sim->alt;
gps.eph = gps_sim->eph;
gps.epv = gps_sim->epv;
gps.vel = gps_sim->vel;
@@ -226,23 +232,28 @@ void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
write_gps_data((void *)&gps);
}
void Simulator::handle_message(mavlink_message_t *msg, bool publish) {
switch(msg->msgid) {
void Simulator::handle_message(mavlink_message_t *msg, bool publish)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_HIL_SENSOR:
mavlink_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu);
if (publish) {
publish_sensor_topics(&imu);
}
update_sensors(&imu);
break;
case MAVLINK_MSG_ID_HIL_GPS:
mavlink_hil_gps_t gps_sim;
mavlink_msg_hil_gps_decode(msg, &gps_sim);
if (publish) {
//PX4_WARN("FIXME: Need to publish GPS topic. Not done yet.");
}
update_gps(&gps_sim);
break;
@@ -253,17 +264,20 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) {
// publish message
if (publish) {
if(_rc_channels_pub == nullptr) {
if (_rc_channels_pub == nullptr) {
_rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input);
} else {
orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input);
}
}
break;
}
}
void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) {
void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
{
component_ID = 0;
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -280,7 +294,7 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8
buf[5] = msgid;
/* payload */
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],msg, payload_len);
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len);
/* checksum */
uint16_t checksum;
@@ -292,44 +306,51 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
ssize_t len = sendto(_fd, buf, packet_len, 0, (struct sockaddr *)&_srcaddr, _addrlen);
if (len <= 0) {
PX4_WARN("Failed sending mavlink message");
}
}
void Simulator::poll_topics() {
void Simulator::poll_topics()
{
// copy new actuator data if available
bool updated;
orb_check(_actuator_outputs_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
}
orb_check(_vehicle_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
}
void *Simulator::sending_trampoline(void *) {
void *Simulator::sending_trampoline(void *)
{
_instance->send();
return 0; // why do I have to put this???
}
void Simulator::send() {
void Simulator::send()
{
px4_pollfd_struct_t fds[1];
fds[0].fd = _actuator_outputs_sub;
fds[0].events = POLLIN;
int pret;
while(true) {
while (true) {
// wait for up to 100ms for data
pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
//timed out
if (pret == 0)
if (pret == 0) {
continue;
}
// this is undesirable but not much we can do
if (pret < 0) {
@@ -349,23 +370,19 @@ void Simulator::send() {
void Simulator::initializeSensorData()
{
struct baro_report baro;
memset(&baro,0,sizeof(baro));
struct baro_report baro = {};
baro.pressure = 120000.0f;
// acceleration report
struct accel_report accel;
memset(&accel,0,sizeof(accel));
struct accel_report accel = {};
accel.z = 9.81f;
accel.range_m_s2 = 80.0f;
// gyro report
struct gyro_report gyro;
memset(&gyro, 0 ,sizeof(gyro));
struct gyro_report gyro = {};
// mag report
struct mag_report mag;
memset(&mag, 0 ,sizeof(mag));
struct mag_report mag = {};
}
void Simulator::pollForMAVLinkMessages(bool publish)
@@ -410,6 +427,7 @@ void Simulator::pollForMAVLinkMessages(bool publish)
if (serial_fd < 0) {
PX4_INFO("Not using %s for radio control input. Assuming joystick input via MAVLink.", PIXHAWK_DEVICE);
} else {
// tell the device to stream some messages
@@ -435,10 +453,12 @@ void Simulator::pollForMAVLinkMessages(bool publish)
// this is important for the UDP communication to work
int pret = -1;
PX4_INFO("Waiting for initial data on UDP. Please start the flight simulator to proceed..");
while (pret <= 0) {
pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100);
pret = ::poll(&fds[0], (sizeof(fds[0]) / sizeof(fds[0])), 100);
}
PX4_INFO("Found initial message, pret = %d",pret);
PX4_INFO("Found initial message, pret = %d", pret);
_initialized = true;
// reset system time
(void)hrt_reset();
@@ -460,11 +480,12 @@ void Simulator::pollForMAVLinkMessages(bool publish)
// wait for new mavlink messages to arrive
while (true) {
pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100);
pret = ::poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
//timed out
if (pret == 0)
if (pret == 0) {
continue;
}
// this is undesirable but not much we can do
if (pret < 0) {
@@ -477,13 +498,13 @@ void Simulator::pollForMAVLinkMessages(bool publish)
// got data from simulator
if (fds[0].revents & POLLIN) {
len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
if (len > 0) {
mavlink_message_t msg;
mavlink_status_t status;
for (int i = 0; i < len; ++i)
{
if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status))
{
for (int i = 0; i < len; ++i) {
if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) {
// have a message, handle it
handle_message(&msg, publish);
}
@@ -494,13 +515,13 @@ void Simulator::pollForMAVLinkMessages(bool publish)
// got data from PIXHAWK
if (fds[1].revents & POLLIN) {
len = ::read(serial_fd, serial_buf, sizeof(serial_buf));
if (len > 0) {
mavlink_message_t msg;
mavlink_status_t status;
for (int i = 0; i < len; ++i)
{
if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status))
{
for (int i = 0; i < len; ++i) {
if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) {
// have a message, handle it
handle_message(&msg, publish);
}
@@ -612,15 +633,17 @@ int openUart(const char *uart_name, int baud)
return uart_fd;
}
int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) {
int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
{
//uint64_t timestamp = imu->time_usec;
uint64_t timestamp = hrt_absolute_time();
//uint64_t timestamp = imu->time_usec;
uint64_t timestamp = hrt_absolute_time();
if ((imu->fields_updated & 0x1FFF) != 0x1FFF) {
PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated. Got %08x", imu->fields_updated);
}
if((imu->fields_updated & 0x1FFF)!=0x1FFF) {
PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated. Got %08x",imu->fields_updated);
}
/*
static int count=0;
static uint64_t last_timestamp=0;
@@ -717,5 +740,5 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) {
}
}
return OK;
return OK;
}