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

Merged cmake-2

Browse Source
This commit is contained in:
Lorenz Meier 2015-09-20 11:37:28 +02:00
commit 08c56085b3
5 changed files with 193 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
src/modules/simulator/simulator.cpp
View File

@ -45,7 +45,7 @@
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/types.h>
#include <drivers/drv_led.h>
#include "simulator.h"
@ -91,27 +91,33 @@ bool Simulator::getAirspeedSample(uint8_t *buf, int len)
return _airspeed.copyData(buf, len);
}
void Simulator::write_MPU_data(void *buf) {
void Simulator::write_MPU_data(void *buf)
{
_mpu.writeData(buf);
}
void Simulator::write_accel_data(void *buf) {
void Simulator::write_accel_data(void *buf)
{
_accel.writeData(buf);
}
void Simulator::write_mag_data(void *buf) {
void Simulator::write_mag_data(void *buf)
{
_mag.writeData(buf);
}
void Simulator::write_baro_data(void *buf) {
void Simulator::write_baro_data(void *buf)
{
_baro.writeData(buf);
}
void Simulator::write_gps_data(void *buf) {
void Simulator::write_gps_data(void *buf)
{
_gps.writeData(buf);
}
void Simulator::write_airspeed_data(void *buf) {
void Simulator::write_airspeed_data(void *buf)
{
_airspeed.writeData(buf);
}
@ -119,20 +125,23 @@ int Simulator::start(int argc, char *argv[])
{
int ret = 0;
_instance = new Simulator();
if (_instance) {
drv_led_start();
if (argv[2][1] == 's') {
_instance->initializeSensorData();
#ifndef __PX4_QURT
// Update sensor data
_instance->pollForMAVLinkMessages(false);
#endif
} else {
// Update sensor data
_instance->pollForMAVLinkMessages(true);
}
}
else {
} else {
PX4_WARN("Simulator creation failed");
ret = 1;
}
@ -153,52 +162,54 @@ __END_DECLS
extern "C" {
int simulator_main(int argc, char *argv[])
{
int ret = 0;
if (argc == 3 && strcmp(argv[1], "start") == 0) {
if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) {
if (g_sim_task >= 0) {
warnx("Simulator already started");
return 0;
}
g_sim_task = px4_task_spawn_cmd("Simulator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
Simulator::start,
argv);
int simulator_main(int argc, char *argv[])
{
int ret = 0;
// now wait for the command to complete
while(true) {
if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) {
break;
} else {
usleep(100000);
if (argc == 3 && strcmp(argv[1], "start") == 0) {
if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) {
if (g_sim_task >= 0) {
warnx("Simulator already started");
return 0;
}
g_sim_task = px4_task_spawn_cmd("Simulator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
Simulator::start,
argv);
// now wait for the command to complete
while (true) {
if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) {
break;
} else {
usleep(100000);
}
}
} else {
usage();
ret = -EINVAL;
}
}
else
{
} else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
if (g_sim_task < 0) {
PX4_WARN("Simulator not running");
} else {
px4_task_delete(g_sim_task);
g_sim_task = -1;
}
} else {
usage();
ret = -EINVAL;
}
}
else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
if (g_sim_task < 0) {
PX4_WARN("Simulator not running");
}
else {
px4_task_delete(g_sim_task);
g_sim_task = -1;
}
}
else {
usage();
ret = -EINVAL;
return ret;
}
return ret;
}
}

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

@ -53,24 +53,25 @@
#include <uORB/uORB.h>
#include <v1.0/mavlink_types.h>
#include <v1.0/common/mavlink.h>
namespace simulator {
namespace simulator
{
// FIXME - what is the endianness of these on actual device?
#pragma pack(push, 1)
struct RawAccelData {
float temperature;
float x;
float y;
float z;
float temperature;
float x;
float y;
float z;
};
#pragma pack(pop)
#pragma pack(push, 1)
struct RawMagData {
float temperature;
float x;
float y;
float z;
float temperature;
float x;
float y;
float z;
};
#pragma pack(pop)
@ -103,22 +104,23 @@ struct RawAirspeedData {
#pragma pack(push, 1)
struct RawGPSData {
int32_t lat;
int32_t lon;
int32_t alt;
uint16_t eph;
uint16_t epv;
uint16_t vel;
int16_t vn;
int16_t ve;
int16_t vd;
uint16_t cog;
uint8_t fix_type;
uint8_t satellites_visible;
int32_t lat;
int32_t lon;
int32_t alt;
uint16_t eph;
uint16_t epv;
uint16_t vel;
int16_t vn;
int16_t ve;
int16_t vd;
uint16_t cog;
uint8_t fix_type;
uint8_t satellites_visible;
};
#pragma pack(pop)
template <typename RType> class Report {
template <typename RType> class Report
{
public:
Report(int readers) :
_readidx(0),
@ -135,6 +137,7 @@ public:
if (len != _report_len) {
return false;
}
read_lock();
memcpy(outbuf, &_buf[_readidx], _report_len);
read_unlock();
@ -153,13 +156,13 @@ protected:
void read_unlock() { px4_sem_post(&_lock); }
void write_lock()
{
for (int i=0; i<_max_readers; i++) {
for (int i = 0; i < _max_readers; i++) {
px4_sem_wait(&_lock);
}
}
void write_unlock()
{
for (int i=0; i<_max_readers; i++) {
for (int i = 0; i < _max_readers; i++) {
px4_sem_post(&_lock);
}
}
@ -173,7 +176,8 @@ protected:
};
class Simulator {
class Simulator
{
public:
static Simulator *getInstance();
@ -211,32 +215,32 @@ public:
private:
Simulator() :
_accel(1),
_mpu(1),
_baro(1),
_mag(1),
_gps(1),
_airspeed(1),
_accel_pub(nullptr),
_baro_pub(nullptr),
_gyro_pub(nullptr),
_mag_pub(nullptr),
_initialized(false)
_accel(1),
_mpu(1),
_baro(1),
_mag(1),
_gps(1),
_airspeed(1),
_accel_pub(nullptr),
_baro_pub(nullptr),
_gyro_pub(nullptr),
_mag_pub(nullptr),
_initialized(false)
#ifndef __PX4_QURT
,
_rc_channels_pub(nullptr),
_actuator_outputs_sub(-1),
_vehicle_attitude_sub(-1),
_manual_sub(-1),
_vehicle_status_sub(-1),
_rc_input{},
_actuators{},
_attitude{},
_manual{},
_vehicle_status{}
,
_rc_channels_pub(nullptr),
_actuator_outputs_sub(-1),
_vehicle_attitude_sub(-1),
_manual_sub(-1),
_vehicle_status_sub(-1),
_rc_input{},
_actuators{},
_attitude{},
_manual{},
_vehicle_status{}
#endif
{}
~Simulator() { _instance=NULL; }
~Simulator() { _instance = NULL; }
void initializeSensorData();

133
src/modules/simulator/simulator_mavlink.cpp
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;
}

2
src/platforms/posix/drivers/accelsim/accelsim.cpp
View File

@ -493,7 +493,7 @@ ACCELSIM::init()
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
struct accel_report arp = {};
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */

4
src/platforms/posix/drivers/gyrosim/gyrosim.cpp
View File

@ -466,7 +466,7 @@ GYROSIM::init()
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
struct accel_report arp = {};
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
@ -482,7 +482,7 @@ GYROSIM::init()
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
struct gyro_report grp = {};
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,