mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 09:22:18 +00:00
Simulator: refactored mavlink additions
QuRT does not support UDP so moved the mavlink specific code to a new file that is not built for the qurt target Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
@@ -56,9 +56,6 @@ static px4_task_t g_sim_task = -1;
|
||||
|
||||
Simulator *Simulator::_instance = NULL;
|
||||
|
||||
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
|
||||
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
|
||||
|
||||
Simulator *Simulator::getInstance()
|
||||
{
|
||||
return _instance;
|
||||
@@ -101,167 +98,6 @@ int Simulator::start(int argc, char *argv[])
|
||||
return ret;
|
||||
}
|
||||
|
||||
void Simulator::poll_topics() {
|
||||
// copy new 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_attitude_sub, &updated);
|
||||
if(updated) {
|
||||
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude);
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::send_data() {
|
||||
// check if it's time to send new data
|
||||
hrt_abstime time_now = hrt_absolute_time();
|
||||
if (time_now - _time_last >= (hrt_abstime)(_interval * 1000)) {
|
||||
_time_last = time_now;
|
||||
mavlink_message_t msg;
|
||||
pack_actuator_message(&msg);
|
||||
send_mavlink_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg, 200);
|
||||
// can add more messages here, can also setup different timings
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::pack_actuator_message(mavlink_message_t *msg) {
|
||||
// pack message and send
|
||||
mavlink_servo_output_raw_t actuator_msg;
|
||||
|
||||
actuator_msg.time_usec = hrt_absolute_time();
|
||||
actuator_msg.port = 8; // hardcoded for now
|
||||
actuator_msg.servo1_raw = _actuators.output[0];
|
||||
actuator_msg.servo2_raw = _actuators.output[1];
|
||||
actuator_msg.servo3_raw = _actuators.output[2];
|
||||
actuator_msg.servo4_raw = _actuators.output[3];
|
||||
actuator_msg.servo5_raw = _actuators.output[4];
|
||||
actuator_msg.servo6_raw = _actuators.output[5];
|
||||
actuator_msg.servo7_raw = _actuators.output[6];
|
||||
actuator_msg.servo8_raw = _actuators.output[7];
|
||||
|
||||
// encode the message
|
||||
mavlink_msg_servo_output_raw_encode(1, 100, msg, &actuator_msg);
|
||||
}
|
||||
|
||||
void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) {
|
||||
uint8_t payload_len = mavlink_message_lengths[msgid];
|
||||
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
||||
|
||||
/* header */
|
||||
buf[0] = MAVLINK_STX;
|
||||
buf[1] = payload_len;
|
||||
/* no idea which numbers should be here*/
|
||||
buf[2] = 100;
|
||||
buf[3] = 1;
|
||||
buf[4] = component_ID;
|
||||
buf[5] = msgid;
|
||||
|
||||
/* payload */
|
||||
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],&msg, payload_len);
|
||||
|
||||
/* checksum */
|
||||
uint16_t checksum;
|
||||
crc_init(&checksum);
|
||||
crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
|
||||
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
|
||||
|
||||
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
|
||||
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
|
||||
|
||||
ssize_t len = sendto(_fd, buf, sizeof(buf), 0, (struct sockaddr *)&_srcaddr, _addrlen);
|
||||
if (len <= 0) {
|
||||
PX4_WARN("Failed sending mavlink message");
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu) {
|
||||
hrt_abstime timestamp = hrt_absolute_time();
|
||||
sensor->timestamp = timestamp;
|
||||
sensor->gyro_raw[0] = imu->xgyro * 1000.0f;
|
||||
sensor->gyro_raw[1] = imu->ygyro * 1000.0f;
|
||||
sensor->gyro_raw[2] = imu->zgyro * 1000.0f;
|
||||
sensor->gyro_rad_s[0] = imu->xgyro;
|
||||
sensor->gyro_rad_s[1] = imu->ygyro;
|
||||
sensor->gyro_rad_s[2] = imu->zgyro;
|
||||
|
||||
sensor->accelerometer_raw[0] = imu->xacc; // mg2ms2;
|
||||
sensor->accelerometer_raw[1] = imu->yacc; // mg2ms2;
|
||||
sensor->accelerometer_raw[2] = imu->zacc; // mg2ms2;
|
||||
sensor->accelerometer_m_s2[0] = imu->xacc;
|
||||
sensor->accelerometer_m_s2[1] = imu->yacc;
|
||||
sensor->accelerometer_m_s2[2] = imu->zacc;
|
||||
sensor->accelerometer_mode = 0; // TODO what is this?
|
||||
sensor->accelerometer_range_m_s2 = 32.7f; // int16
|
||||
sensor->accelerometer_timestamp = timestamp;
|
||||
sensor->timestamp = timestamp;
|
||||
|
||||
sensor->adc_voltage_v[0] = 0.0f;
|
||||
sensor->adc_voltage_v[1] = 0.0f;
|
||||
sensor->adc_voltage_v[2] = 0.0f;
|
||||
|
||||
sensor->magnetometer_raw[0] = imu->xmag * 1000.0f;
|
||||
sensor->magnetometer_raw[1] = imu->ymag * 1000.0f;
|
||||
sensor->magnetometer_raw[2] = imu->zmag * 1000.0f;
|
||||
sensor->magnetometer_ga[0] = imu->xmag;
|
||||
sensor->magnetometer_ga[1] = imu->ymag;
|
||||
sensor->magnetometer_ga[2] = imu->zmag;
|
||||
sensor->magnetometer_range_ga = 32.7f; // int16
|
||||
sensor->magnetometer_mode = 0; // TODO what is this
|
||||
sensor->magnetometer_cuttoff_freq_hz = 50.0f;
|
||||
sensor->magnetometer_timestamp = timestamp;
|
||||
|
||||
sensor->baro_pres_mbar = imu->abs_pressure;
|
||||
sensor->baro_alt_meter = imu->pressure_alt;
|
||||
sensor->baro_temp_celcius = imu->temperature;
|
||||
sensor->baro_timestamp = timestamp;
|
||||
|
||||
sensor->differential_pressure_pa = imu->diff_pressure * 1e2f; //from hPa to Pa
|
||||
sensor->differential_pressure_timestamp = timestamp;
|
||||
}
|
||||
|
||||
void Simulator::fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg) {
|
||||
manual->timestamp = hrt_absolute_time();
|
||||
manual->x = man_msg->x / 1000.0f;
|
||||
manual->y = man_msg->y / 1000.0f;
|
||||
manual->r = man_msg->r / 1000.0f;
|
||||
manual->z = man_msg->z / 1000.0f;
|
||||
}
|
||||
|
||||
void Simulator::handle_message(mavlink_message_t *msg) {
|
||||
switch(msg->msgid) {
|
||||
case MAVLINK_MSG_ID_HIGHRES_IMU:
|
||||
mavlink_highres_imu_t imu;
|
||||
mavlink_msg_highres_imu_decode(msg, &imu);
|
||||
fill_sensors_from_imu_msg(&_sensor, &imu);
|
||||
|
||||
// publish message
|
||||
if(_sensor_combined_pub < 0) {
|
||||
_sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor);
|
||||
} else {
|
||||
orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor);
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_MANUAL_CONTROL:
|
||||
|
||||
mavlink_manual_control_t man_ctrl_sp;
|
||||
mavlink_msg_manual_control_decode(msg, &man_ctrl_sp);
|
||||
fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp);
|
||||
|
||||
// publish message
|
||||
if(_manual_control_sp_pub < 0) {
|
||||
_manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp);
|
||||
} else {
|
||||
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::publishSensorsCombined() {
|
||||
|
||||
struct baro_report baro;
|
||||
@@ -327,160 +163,6 @@ void Simulator::publishSensorsCombined() {
|
||||
}
|
||||
}
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
|
||||
void *Simulator::sending_trampoline(void *) {
|
||||
_instance->send();
|
||||
return 0; // why do I have to put this???
|
||||
}
|
||||
|
||||
void Simulator::send() {
|
||||
px4_pollfd_struct_t fds[1];
|
||||
fds[0].fd = _actuator_outputs_sub;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
_time_last = hrt_absolute_time();
|
||||
|
||||
while(true) {
|
||||
// wait for up to 100ms for data
|
||||
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
|
||||
|
||||
//timed out
|
||||
if (pret == 0)
|
||||
continue;
|
||||
|
||||
// this is undesirable but not much we can do
|
||||
if (pret < 0) {
|
||||
PX4_WARN("poll error %d, %d", pret, errno);
|
||||
// sleep a bit before next try
|
||||
usleep(100000);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (fds[0].revents & POLLIN) {
|
||||
// got new data to read, update all topics
|
||||
poll_topics();
|
||||
send_data();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Simulator::updateSamples()
|
||||
{
|
||||
struct baro_report baro;
|
||||
memset(&baro,0,sizeof(baro));
|
||||
baro.pressure = 120000.0f;
|
||||
|
||||
// acceleration report
|
||||
struct accel_report accel;
|
||||
memset(&accel,0,sizeof(accel));
|
||||
accel.z = 9.81f;
|
||||
accel.range_m_s2 = 80.0f;
|
||||
|
||||
// gyro report
|
||||
struct gyro_report gyro;
|
||||
memset(&gyro, 0 ,sizeof(gyro));
|
||||
|
||||
// mag report
|
||||
struct mag_report mag;
|
||||
memset(&mag, 0 ,sizeof(mag));
|
||||
|
||||
// init publishers
|
||||
_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
|
||||
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
|
||||
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
|
||||
_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
|
||||
|
||||
// subscribe to topics
|
||||
_actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
|
||||
_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
|
||||
// try to setup udp socket for communcation with simulator
|
||||
memset((char *)&_myaddr, 0, sizeof(_myaddr));
|
||||
_myaddr.sin_family = AF_INET;
|
||||
_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||
_myaddr.sin_port = htons(_port);
|
||||
|
||||
if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
|
||||
PX4_WARN("create socket failed\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
|
||||
PX4_WARN("bind failed\n");
|
||||
return;
|
||||
}
|
||||
|
||||
// create a thread for sending data to the simulator
|
||||
pthread_t sender_thread;
|
||||
|
||||
// initialize threads
|
||||
pthread_attr_t sender_thread_attr;
|
||||
pthread_attr_init(&sender_thread_attr);
|
||||
pthread_attr_setstacksize(&sender_thread_attr, 1000);
|
||||
|
||||
struct sched_param param;
|
||||
(void)pthread_attr_getschedparam(&sender_thread_attr, ¶m);
|
||||
|
||||
/* low priority */
|
||||
param.sched_priority = SCHED_PRIORITY_DEFAULT - 30;
|
||||
(void)pthread_attr_setschedparam(&sender_thread_attr, ¶m);
|
||||
pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);
|
||||
pthread_attr_destroy(&sender_thread_attr);
|
||||
|
||||
struct pollfd socket_fds;
|
||||
socket_fds.fd = _fd;
|
||||
socket_fds.events = POLLIN;
|
||||
|
||||
int len = 0;
|
||||
// wait for new mavlink messages to arrive
|
||||
while (true) {
|
||||
|
||||
int socket_pret = ::poll(&socket_fds, (size_t)1, 100);
|
||||
|
||||
//timed out
|
||||
if (socket_pret == 0)
|
||||
continue;
|
||||
|
||||
// this is undesirable but not much we can do
|
||||
if (socket_pret < 0) {
|
||||
PX4_WARN("poll error %d, %d", socket_pret, errno);
|
||||
// sleep a bit before next try
|
||||
usleep(100000);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (socket_fds.revents & POLLIN) {
|
||||
len = recvfrom(_fd, _buf, _buflen, 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))
|
||||
{
|
||||
// have a message, handle it
|
||||
handle_message(&msg);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// publish these messages so that attitude estimator does not complain
|
||||
hrt_abstime time_last = hrt_absolute_time();
|
||||
baro.timestamp = time_last;
|
||||
accel.timestamp = time_last;
|
||||
gyro.timestamp = time_last;
|
||||
mag.timestamp = time_last;
|
||||
// publish the sensor values
|
||||
orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro);
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro);
|
||||
orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static void usage()
|
||||
{
|
||||
PX4_WARN("Usage: simulator {start -[sc] |stop}");
|
||||
@@ -502,19 +184,7 @@ int simulator_main(int argc, char *argv[])
|
||||
{
|
||||
int ret = 0;
|
||||
if (argc == 3 && strcmp(argv[1], "start") == 0) {
|
||||
if (strcmp(argv[2], "-s") == 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);
|
||||
}
|
||||
else if (strcmp(argv[2], "-p") == 0) {
|
||||
if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) {
|
||||
if (g_sim_task >= 0) {
|
||||
warnx("Simulator already started");
|
||||
return 0;
|
||||
|
||||
Reference in New Issue
Block a user