From 4de777fd2fa5e83129b29aadac3714f16f5d87c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Sep 2015 12:55:18 +0200 Subject: [PATCH 1/4] Topic listener: Fix code generation --- Tools/generate_listener.py | 41 +++++++++++++++++++++++++++++++------- 1 file changed, 34 insertions(+), 7 deletions(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 4e5b8ab088..1b55528372 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -20,17 +20,32 @@ for index,m in enumerate(raw_messages): temp_list = [] f = open(m,'r') for line in f.readlines(): - if(line.split(' ')[0] == "float32"): + if ('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)) + if ('uint64[' in line.split(' ')[0]): + num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) + temp_list.append(("uint64_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) + elif(line.split(' ')[0] == "float32"): temp_list.append(("float",line.split(' ')[1].split('\t')[0].split('\n')[0])) - elif(line.split(' ')[0] == "uint64"): + elif(line.split(' ')[0] == "uint64") and len(line.split('=')) == 1: temp_list.append(("uint64",line.split(' ')[1].split('\t')[0].split('\n')[0])) - elif (line.split(' ')[0] == "bool"): + elif(line.split(' ')[0] == "uint32") and len(line.split('=')) == 1: + temp_list.append(("uint32",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "uint16") and len(line.split('=')) == 1: + temp_list.append(("uint16",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "int64") and len(line.split('=')) == 1: + temp_list.append(("int64",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "int32") and len(line.split('=')) == 1: + temp_list.append(("int32",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "int16") and len(line.split('=')) == 1: + temp_list.append(("int16",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif (line.split(' ')[0] == "bool") and len(line.split('=')) == 1: 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)) + elif (line.split(' ')[0] == "int8") and len(line.split('=')) == 1: + temp_list.append(("int8",line.split(' ')[1].split('\t')[0].split('\n')[0])) f.close() messages.append(m.split('/')[-1].split('.')[0]) @@ -131,12 +146,24 @@ for index,m in enumerate(messages[1:]): print("\t\t\tprintf(\"%s: %%f\\n \",(double)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\tfor (int j = 0; j < %d; j++) {" % item[2]) print("\t\t\t\tprintf(\"%%f \",(double)container.%s[j]);" % item[1]) print("\t\t\t}") print("\t\t\tprintf(\"\\n\");") elif item[0] == "uint64": print("\t\t\tprintf(\"%s: %%\" PRIu64 \"\\n \",container.%s);" % (item[1], item[1])) + elif item[0] == "uint64_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(\"%%\" PRIu64 \" \",container.%s[j]);" % item[1]) + print("\t\t\t}") + print("\t\t\tprintf(\"\\n\");") + elif item[0] == "int64": + print("\t\t\tprintf(\"%s: %%\" PRI64 \"\\n \",container.%s);" % (item[1], item[1])) + elif item[0] == "int32": + print("\t\t\tprintf(\"%s: %%d\\n \",container.%s);" % (item[1], item[1])) + elif item[0] == "uint32": + print("\t\t\tprintf(\"%s: %%d\\n \",container.%s);" % (item[1], item[1])) elif item[0] == "uint8": print("\t\t\tprintf(\"%s: %%u\\n \",container.%s);" % (item[1], item[1])) elif item[0] == "bool": From 324c786e0af8e16f1cb7fa900494405be3d532fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Sep 2015 12:55:32 +0200 Subject: [PATCH 2/4] Simulator: Init sensor data diligently --- src/modules/simulator/simulator_mavlink.cpp | 34 +++++++++++++++------ 1 file changed, 25 insertions(+), 9 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index c9833addda..7f55db47bb 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -370,19 +370,35 @@ void Simulator::send() void Simulator::initializeSensorData() { - struct baro_report baro = {}; - baro.pressure = 120000.0f; + // write sensor data to memory so that drivers can copy data from there + RawMPUData mpu = {}; + mpu.accel_z = 9.81f; - // acceleration report - struct accel_report accel = {}; + write_MPU_data((void *)&mpu); + + RawAccelData accel = {}; accel.z = 9.81f; - accel.range_m_s2 = 80.0f; - // gyro report - struct gyro_report gyro = {}; + write_accel_data((void *)&accel); - // mag report - struct mag_report mag = {}; + RawMagData mag = {}; + mag.x = 0.4f; + mag.y = 0.0f; + mag.z = 0.6f; + + write_mag_data((void *)&mag); + + RawBaroData baro = {}; + // calculate air pressure from altitude (valid for low altitude) + baro.pressure = 120000.0f; + baro.altitude = 0.0f; + baro.temperature = 25.0f; + + write_baro_data((void *)&baro); + + RawAirspeedData airspeed {}; + + write_airspeed_data((void *)&airspeed); } void Simulator::pollForMAVLinkMessages(bool publish) From 59f71452d71ca6ad512a722946e67dc89db3b18b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Sep 2015 12:55:48 +0200 Subject: [PATCH 3/4] Sensor sim: Initialize sensor data to zero --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 9 +++++---- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 14 ++++++++------ 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index cc299b5847..fa0a256605 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -448,6 +448,9 @@ ACCELSIM::init() { int ret = ERROR; + struct mag_report mrp = {}; + struct accel_report arp = {}; + /* do SIM init first */ if (VDev::init() != OK) { PX4_WARN("SIM init failed"); @@ -478,7 +481,6 @@ ACCELSIM::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct mag_report mrp; _mag_reports->get(&mrp); /* measurement will have generated a report, publish */ @@ -493,7 +495,6 @@ 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 = {}; _accel_reports->get(&arp); /* measurement will have generated a report, publish */ @@ -1014,7 +1015,7 @@ ACCELSIM::measure() } raw_accel_report; #pragma pack(pop) - accel_report accel_report; + accel_report accel_report = {}; /* start the performance counter */ perf_begin(_accel_sample_perf); @@ -1023,7 +1024,7 @@ ACCELSIM::measure() memset(&raw_accel_report, 0, sizeof(raw_accel_report)); raw_accel_report.cmd = DIR_READ | ACC_READ; - if(OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) { + if (OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) { return; } diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 850bf0f9d1..50cc5091d8 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -419,6 +419,9 @@ GYROSIM::init() return ret; } + struct accel_report arp = {}; + struct gyro_report grp = {}; + /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) { @@ -466,7 +469,6 @@ GYROSIM::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp = {}; _accel_reports->get(&arp); /* measurement will have generated a report, publish */ @@ -482,7 +484,6 @@ GYROSIM::init() /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp = {}; _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, @@ -511,8 +512,9 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) if (cmd == MPUREAD) { // Get data from the simulator Simulator *sim = Simulator::getInstance(); - if (sim == NULL) + if (sim == NULL) { return ENODEV; + } // FIXME - not sure what interrupt status should be recv[1] = 0; @@ -1012,7 +1014,7 @@ GYROSIM::measure() x++; } #endif - struct MPUReport mpu_report; + struct MPUReport mpu_report = {}; /* start measuring */ perf_begin(_sample_perf); @@ -1031,8 +1033,8 @@ GYROSIM::measure() /* * Report buffers. */ - accel_report arb; - gyro_report grb; + accel_report arb = {}; + gyro_report grb = {}; // for now use local time but this should be the timestamp of the simulator grb.timestamp = hrt_absolute_time(); From 1c27b9fa41503afc445d1acbdabaf44c45aeba2d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Sep 2015 12:56:03 +0200 Subject: [PATCH 4/4] Topic listener: Re-enable in build --- src/systemcmds/topic_listener/CMakeLists.txt | 34 ++++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/systemcmds/topic_listener/CMakeLists.txt b/src/systemcmds/topic_listener/CMakeLists.txt index 3d98a6d4f4..2f720757ac 100644 --- a/src/systemcmds/topic_listener/CMakeLists.txt +++ b/src/systemcmds/topic_listener/CMakeLists.txt @@ -31,23 +31,23 @@ # ############################################################################ -#add_custom_command(OUTPUT topic_listener.cpp -# COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/generate_listener.py ${CMAKE_SOURCE_DIR} > topic_listener.cpp -# ) +add_custom_command(OUTPUT topic_listener.cpp + COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/generate_listener.py ${CMAKE_SOURCE_DIR} > topic_listener.cpp + ) -#add_custom_target(generate_topic_listener -# DEPENDS topic_listener.cpp) +add_custom_target(generate_topic_listener + DEPENDS topic_listener.cpp) -#px4_add_module( -# MODULE systemcmds__topic_listener -# MAIN listener -# STACK 1800 -# COMPILE_FLAGS -# -Os -# SRCS -# topic_listener.cpp -# DEPENDS -# platforms__common -# generate_topic_listener -# ) +px4_add_module( + MODULE systemcmds__topic_listener + MAIN listener + STACK 1800 + COMPILE_FLAGS + -Os + SRCS + topic_listener.cpp + DEPENDS + platforms__common + generate_topic_listener + ) # vim: set noet ft=cmake fenc=utf-8 ff=unix :