forked from Archive/PX4-Autopilot
mcharleb
commit
21a5dfc828
|
|
@ -9,19 +9,33 @@ import sys
|
|||
|
||||
raw_messages = glob.glob(sys.argv[1]+"/msg/*.msg")
|
||||
messages = []
|
||||
message_floats = []
|
||||
message_elements = []
|
||||
|
||||
|
||||
for index,m in enumerate(raw_messages):
|
||||
temp_list = []
|
||||
temp_list_floats = []
|
||||
temp_list_uint64 = []
|
||||
temp_list_bool = []
|
||||
if("actuator_control" not in m and "pwm_input" not in m and "position_setpoint" not in m):
|
||||
temp_list = []
|
||||
f = open(m,'r')
|
||||
for line in f.readlines():
|
||||
if(line.split(' ')[0] == "float32"):
|
||||
temp_list.append(line.split(' ')[1].split('\t')[0].split('\n')[0])
|
||||
temp_list.append(("float",line.split(' ')[1].split('\t')[0].split('\n')[0]))
|
||||
elif(line.split(' ')[0] == "uint64"):
|
||||
temp_list.append(("uint64",line.split(' ')[1].split('\t')[0].split('\n')[0]))
|
||||
elif (line.split(' ')[0] == "bool"):
|
||||
temp_list.append(("bool",line.split(' ')[1].split('\t')[0].split('\n')[0]))
|
||||
elif (line.split(' ')[0] == "uint8") and len(line.split('=')) == 1:
|
||||
temp_list.append(("uint8",line.split(' ')[1].split('\t')[0].split('\n')[0]))
|
||||
elif ('float32[' in line.split(' ')[0]):
|
||||
num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0])
|
||||
temp_list.append(("float_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats))
|
||||
|
||||
f.close()
|
||||
messages.append(m.split('/')[-1].split('.')[0])
|
||||
message_floats.append(temp_list)
|
||||
message_elements.append(temp_list)
|
||||
|
||||
num_messages = len(messages);
|
||||
|
||||
print
|
||||
|
|
@ -76,6 +90,7 @@ print """
|
|||
#include <cstring>
|
||||
#include <uORB/uORB.h>
|
||||
#include <string.h>
|
||||
#include <stdint.h>
|
||||
"""
|
||||
for m in messages:
|
||||
print "#include <uORB/topics/%s.h>" % m
|
||||
|
|
@ -104,10 +119,28 @@ for index,m in enumerate(messages[1:]):
|
|||
print "\t\tID = ORB_ID(%s);" % m
|
||||
print "\t\tstruct %s_s container;" % m
|
||||
print "\t\tmemset(&container, 0, sizeof(container));"
|
||||
print "\t\tbool updated;"
|
||||
print "\t\tfor(uint32_t i = 0;i<num_msgs;i++) {"
|
||||
print "\t\t\torb_check(sub,&updated);"
|
||||
print "\t\t\tupdated = true;"
|
||||
print "\t\t\tif(updated) {"
|
||||
print "\t\t\torb_copy(ID,sub,&container);"
|
||||
for item in message_floats[index+1]:
|
||||
print "\t\t\tprintf(\"%s: %%f\\n \",container.%s);" % (item, item)
|
||||
for item in message_elements[index+1]:
|
||||
if item[0] == "float":
|
||||
print "\t\t\tprintf(\"%s: %%f\\n \",container.%s);" % (item[1], item[1])
|
||||
elif item[0] == "float_array":
|
||||
print "\t\t\tprintf(\"%s:\");" % item[1]
|
||||
print "\t\t\tfor (int j=0;j<%d;j++) {" % item[2]
|
||||
print "\t\t\t\tprintf(\"%%f \",container.%s[j]);" % item[1]
|
||||
print "\t\t\t}"
|
||||
print "\t\t\tprintf(\"\\n\");"
|
||||
elif item[0] == "uint64":
|
||||
print "\t\t\tprintf(\"%s: %%f\\n \",(float)container.%s);" % (item[1], item[1])
|
||||
elif item[0] == "uint8":
|
||||
print "\t\t\tprintf(\"%s: %%f\\n \",(float)container.%s);" % (item[1], item[1])
|
||||
elif item[0] == "bool":
|
||||
print "\t\t\tprintf(\"%s: %%s\\n \",container.%s ? \"True\" : \"False\");" % (item[1], item[1])
|
||||
print "\t\t\t}"
|
||||
print "\t\t}"
|
||||
print "\t} else {"
|
||||
print "\t\t printf(\" Topic did not match any known topics\\n\");"
|
||||
|
|
|
|||
|
|
@ -37,4 +37,10 @@
|
|||
|
||||
MODULE_COMMAND = simulator
|
||||
|
||||
SRCS = simulator.cpp
|
||||
SRCS = simulator.cpp
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed
|
||||
|
||||
EXTRACFLAGS = -Wno-packed
|
||||
|
|
|
|||
|
|
@ -99,6 +99,90 @@ int Simulator::start(int argc, char *argv[])
|
|||
return ret;
|
||||
}
|
||||
|
||||
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;
|
||||
|
|
@ -168,13 +252,36 @@ void Simulator::publishSensorsCombined() {
|
|||
#ifndef __PX4_QURT
|
||||
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);
|
||||
|
||||
// get samples from external provider
|
||||
struct sockaddr_in myaddr;
|
||||
struct sockaddr_in srcaddr;
|
||||
socklen_t addrlen = sizeof(srcaddr);
|
||||
int len, fd;
|
||||
const int buflen = 200;
|
||||
const int port = 9876;
|
||||
const int port = 14550;
|
||||
unsigned char buf[buflen];
|
||||
|
||||
if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
|
||||
|
|
@ -192,6 +299,38 @@ void Simulator::updateSamples()
|
|||
return;
|
||||
}
|
||||
|
||||
// wait for new mavlink messages to arrive
|
||||
for (;;) {
|
||||
len = 0;
|
||||
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
|
||||
//printf("Publishing SensorsCombined\n");
|
||||
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);
|
||||
}
|
||||
|
||||
/*
|
||||
for (;;) {
|
||||
len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen);
|
||||
if (len > 0) {
|
||||
|
|
@ -212,6 +351,7 @@ void Simulator::updateSamples()
|
|||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -40,11 +40,14 @@
|
|||
|
||||
#include <semaphore.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <v1.0/mavlink_types.h>
|
||||
#include <v1.0/common/mavlink.h>
|
||||
|
||||
namespace simulator {
|
||||
|
||||
|
|
@ -151,13 +154,24 @@ public:
|
|||
bool getMPUReport(uint8_t *buf, int len);
|
||||
bool getBaroSample(uint8_t *buf, int len);
|
||||
private:
|
||||
Simulator() : _accel(1), _mpu(1), _baro(1) {}
|
||||
Simulator() :
|
||||
_accel(1),
|
||||
_mpu(1),
|
||||
_baro(1),
|
||||
_sensor_combined_pub(-1),
|
||||
_manual_control_sp_pub(-1),
|
||||
_sensor{},
|
||||
_manual_control_sp{}
|
||||
{}
|
||||
~Simulator() { _instance=NULL; }
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
void updateSamples();
|
||||
#endif
|
||||
void publishSensorsCombined();
|
||||
void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu);
|
||||
void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg);
|
||||
void handle_message(mavlink_message_t *msg);
|
||||
|
||||
static Simulator *_instance;
|
||||
|
||||
|
|
@ -170,5 +184,9 @@ private:
|
|||
orb_advert_t _gyro_pub;
|
||||
orb_advert_t _mag_pub;
|
||||
orb_advert_t _sensor_combined_pub;
|
||||
orb_advert_t _manual_control_sp_pub;
|
||||
|
||||
struct sensor_combined_s _sensor;
|
||||
struct manual_control_setpoint_s _manual_control_sp;
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue