px4通过iic读取超声波数据程序

主要思路是继承iic类，然后初始化，读取数据，实测有用。刚学不久，如有错误，欢迎指正

主程序：

#include <termios.h>

#include <drivers/drv_hrt.h>

#include <systemlib/mavlink_log.h>

#include <px4_posix.h>

//#include

//#include

#include “drivers/drv_pwm_output.h”

#include <uORB/topics/offboard_control_mode.h>

#include <uORB/topics/position_setpoint_triplet.h>

#include <unistd.h>

#include <stdio.h>

#include <uORB/topics/home_position.h>

#include <string.h>

#include <stdlib.h>

#include <px4_config.h>

#include <px4_tasks.h>

#include <px4_getopt.h>

#include <px4_defines.h>

#include <px4_log.h>

#include <uORB/topics/vehicle_status.h>

#include <uORB/topics/manual_control_setpoint.h>

#include <uORB/topics/vehicle_command.h>

#include <uORB/topics/vehicle_global_position.h>

#include <uORB/topics/vehicle_local_position.h>

#include <uORB/topics/vehicle_command_ack.h>

//#include <geo/geo.h>

#include <poll.h>

#include <uORB/topics/rc_channels.h>

//#include <uORB/topics/output_pwm.h>

//#include <nuttx/fs/ioctl.h>

#include “drivers/drv_pwm_output.h”

#include “systemlib/err.h”

#include <sys/ioctl.h>

//#include “systemlib/systemlib.h”

//#include “systemlib/param/param.h”

#include <sys/stat.h>

#include <fcntl.h>

#include <arch/board/board.h>

//#include <drivers/drv_gpio.h>

#include <sys/types.h>

#include <sched.h>

#include <errno.h>

#include <assert.h>

#include <drivers/drv_input_capture.h>

#include <px4_config.h>

#include <px4_tasks.h>

#include <px4_posix.h>

#include <unistd.h>

#include <stdio.h>

#include <poll.h>

#include <string.h>

#include <math.h>

// system libraries

#include <parameters/param.h>

#include <systemlib/err.h>

#include <perf/perf_counter.h>

// internal libraries

#include <lib/mathlib/mathlib.h>

#include <matrix/math.hpp>

#include <lib/ecl/geo/geo.h>

// Include uORB and the required topics for this app

#include <uORB/uORB.h>

#include <uORB/topics/sensor_combined.h> // this topics hold the acceleration data

#include <uORB/topics/actuator_controls.h> // this topic gives the actuators control input

#include <uORB/topics/vehicle_attitude.h> // this topic holds the orientation of the hippocampus

#include <uORB/topics/safety.h>

#include <uORB/topics/a02.h>

#include <uORB/topics/a03pid_out.h>

#include “commander/commander_helper.h”

#include <drivers/drv_hrt.h>

#include <drivers/drv_tone_alarm.h>

#include <drivers/drv_pwm_output.h>

#include<lib/pid/pid.h>

#include <drivers/device/i2c.h>

#include"04workqueue.h"

#include <uORB/topics/distance_sensor.h>

#include <px4_config.h>

#include <px4_getopt.h>

#include <px4_workqueue.h>

#include <containers/Array.hpp>

#include <drivers/device/i2c.h>

#include <sys/types.h>

#include <stdint.h>

#include <stdlib.h>

#include <stdbool.h>

#include <semaphore.h>

#include <string.h>

#include <fcntl.h>

#include <poll.h>

#include <stdio.h>

#include <math.h>

#include <unistd.h>

#include <perf/perf_counter.h>

#include <drivers/drv_hrt.h>

#include <drivers/drv_range_finder.h>

#include <drivers/device/ringbuffer.h>

#include <uORB/uORB.h>

#include <uORB/topics/distance_sensor.h>

#include <board_config.h>

#define SENSOR_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */

#define PX4FMU_DEVICE_PATH “/dev/px4fmu”

#define MYIIC_PATH “/dev/xxxxxx”

extern “C” __EXPORT int d_main(int argc, char *argv[]);//主函数

workqueuetest ::workqueuetest(int bus = PX4_I2C_BUS_EXPANSION2, int address = SENSOR_BASEADDR) :

I2C(“xxxxxx”, MYIIC_PATH, bus, address, 100000)

{}

int workqueuetest::start()

{

int ret = PX4_ERROR;

/* do I2C init (and probe) first */

if (I2C::init() != OK) {

return ret;

}

uint8_t val[2] = {0, 0};

ret = transfer(nullptr, 0, &val[0], 2);

if (ret < 0) {

PX4_DEBUG(“error reading from sensor: %d”, ret);

PX4_INFO(“read err”);

return ret;

}

uint16_t distance_cm = val[0] << 8 | val[1];

float distance_m = float(distance_cm) * 1e-2f;

PX4_INFO("%1.5f",(double)distance_m);

return 0;

}

int d_main(int argc, char *argv[])

{

workqueuetest a0;

a0.start();

return 0;

}

头文件

#include <drivers/device/i2c.h>

class workqueuetest :public device::I2C

{

public:

workqueuetest(int bus, int address);

int start();

};

cmakelist

px4_add_module(

MODULE modules__04workqueue

MAIN d

STACK_MAIN 2000

SRCS

04workqueue.cpp

DEPENDS

)

固件是px4 1.9.0