mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-24 10:43:34 +08:00
Initial build for Crazyflie 2.0
Working crazyflie firmware build * Console on USART3 * Could not disable building PX4IO firmware, currently commented out Don't build PX4IO firmware if the board doesn't ask for it Added crazyflie motor driver Fixed wrong register CLK_SEL is in PWR_MGMT_1 Initial I2C/SPI MPU9250 device * Tested with I2C * Need to add error checking * Intermittent crash on stop call Working ak8963 mag driver Functional lps25h driver. Work in progress. Works well enough to probe and allow sensors task to start. Added serial port test module HACK! Get sensors module working Set crazyflie PWM range Extend baudrate for Crazyflie's NRF radio Added dummy tone alarm to allow for init Added autostart script for Crazyflie
This commit is contained in:
Tim Dyer
+198
-83
@@ -89,61 +89,111 @@ extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); }
|
||||
namespace mpu9250
|
||||
{
|
||||
|
||||
MPU9250 *g_dev_int; // on internal bus
|
||||
MPU9250 *g_dev_ext; // on external bus
|
||||
enum MPU9250_BUS {
|
||||
MPU9250_BUS_ALL = 0,
|
||||
MPU9250_BUS_I2C_INTERNAL,
|
||||
MPU9250_BUS_I2C_EXTERNAL,
|
||||
MPU9250_BUS_SPI_INTERNAL,
|
||||
MPU9250_BUS_SPI_EXTERNAL
|
||||
};
|
||||
|
||||
void start(bool, enum Rotation);
|
||||
void stop(bool);
|
||||
void test(bool);
|
||||
void reset(bool);
|
||||
void info(bool);
|
||||
void regdump(bool);
|
||||
void testerror(bool);
|
||||
/*
|
||||
list of supported bus configurations
|
||||
*/
|
||||
|
||||
struct mpu9250_bus_option {
|
||||
enum MPU9250_BUS busid;
|
||||
const char *accelpath;
|
||||
const char *gyropath;
|
||||
const char *magpath;
|
||||
MPU9250_constructor interface_constructor;
|
||||
uint8_t busnum;
|
||||
MPU6000 *dev;
|
||||
} bus_options[] = {
|
||||
#if defined (USE_I2C)
|
||||
# if defined(PX4_I2C_BUS_ONBOARD)
|
||||
{ MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
|
||||
# endif
|
||||
# if defined(PX4_I2C_BUS_EXPANSION)
|
||||
{ MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
|
||||
# endif
|
||||
#endif
|
||||
#ifdef PX4_SPIDEV_MPU
|
||||
{ MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
|
||||
#endif
|
||||
#if defined(PX4_SPI_BUS_EXT)
|
||||
{ MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT &MPU9250_SPI_interface, PX4_SPI_BUS_EXT, NULL },
|
||||
#endif
|
||||
};
|
||||
|
||||
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
|
||||
|
||||
|
||||
void start(enum MPU9250_BUS busid, enum Rotation rotation);
|
||||
bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation);
|
||||
struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid);
|
||||
void stop(enum MPU9250_BUS busid);
|
||||
void test(enum MPU9250_BUS busid);
|
||||
void reset(enum MPU9250_BUS busid);
|
||||
void info(enum MPU9250_BUS busid);
|
||||
void regdump(enum MPU9250_BUS busid);
|
||||
void testerror(enum MPU9250_BUS busid);
|
||||
void usage();
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*
|
||||
* This function only returns if the driver is up and running
|
||||
* or failed to detect the sensor.
|
||||
* find a bus structure for a busid
|
||||
*/
|
||||
void
|
||||
start(bool external_bus, enum Rotation rotation)
|
||||
struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid)
|
||||
{
|
||||
int fd;
|
||||
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
|
||||
const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL;
|
||||
const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO;
|
||||
const char *path_mag = external_bus ? MPU_DEVICE_PATH_MAG_EXT : MPU_DEVICE_PATH_MAG;
|
||||
|
||||
if (*g_dev_ptr != nullptr)
|
||||
/* if already started, the still command succeeded */
|
||||
{
|
||||
errx(0, "already started");
|
||||
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||
if ((busid == MPU9250_BUS_ALL ||
|
||||
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
|
||||
return bus_options[i];
|
||||
}
|
||||
}
|
||||
|
||||
/* create the driver */
|
||||
if (external_bus) {
|
||||
#ifdef PX4_SPI_BUS_EXT
|
||||
*g_dev_ptr = new MPU9250(PX4_SPI_BUS_EXT, path_accel, path_gyro, path_mag, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
|
||||
#else
|
||||
errx(0, "External SPI not available");
|
||||
#endif
|
||||
errx(1, "bus %u not started", (unsigned)busid);
|
||||
}
|
||||
|
||||
} else {
|
||||
*g_dev_ptr = new MPU9250(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, path_mag, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
|
||||
/**
|
||||
* start driver for a specific bus option
|
||||
*/
|
||||
bool
|
||||
start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int device_type, bool external)
|
||||
{
|
||||
int fd = -1;
|
||||
|
||||
if (bus.dev != nullptr) {
|
||||
warnx("%s SPI not available", external ? "External" : "Internal");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (*g_dev_ptr == nullptr) {
|
||||
goto fail;
|
||||
device::Device *interface = bus.interface_constructor(bus.busnum, device_type, external);
|
||||
|
||||
if (interface == nullptr) {
|
||||
warnx("no device on bus %u", (unsigned)bus.busid);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (OK != (*g_dev_ptr)->init()) {
|
||||
if (interface->init() != OK) {
|
||||
delete interface;
|
||||
warnx("no device on bus %u", (unsigned)bus.busid);
|
||||
return false;
|
||||
}
|
||||
|
||||
bus.dev = new MPU9250(interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, device_type);
|
||||
|
||||
if (bus.dev == nullptr) {
|
||||
delete interface;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (OK != bus.dev->init()) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(path_accel, O_RDONLY);
|
||||
fd = open(bus.accelpath, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
goto fail;
|
||||
@@ -153,27 +203,67 @@ start(bool external_bus, enum Rotation rotation)
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (ioctl(fd, ACCELIOCSRANGE, range) < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
exit(0);
|
||||
return true;
|
||||
|
||||
fail:
|
||||
|
||||
if (*g_dev_ptr != nullptr) {
|
||||
delete(*g_dev_ptr);
|
||||
*g_dev_ptr = nullptr;
|
||||
if (fd >= 0) {
|
||||
close(fd);
|
||||
}
|
||||
|
||||
if (bus.dev != nullptr) {
|
||||
delete(bus.dev);
|
||||
bus.dev = nullptr;
|
||||
}
|
||||
|
||||
errx(1, "driver start failed");
|
||||
}
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*
|
||||
* This function only returns if the driver is up and running
|
||||
* or failed to detect the sensor.
|
||||
*/
|
||||
void
|
||||
stop(bool external_bus)
|
||||
start(enum MPU9250_BUS busid, enum Rotation rotation, int range, int device_type, bool external)
|
||||
{
|
||||
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
|
||||
|
||||
if (*g_dev_ptr != nullptr) {
|
||||
delete *g_dev_ptr;
|
||||
*g_dev_ptr = nullptr;
|
||||
bool started = false;
|
||||
|
||||
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||
if (busid == MPU9250_BUS_ALL && bus_options[i].dev != NULL) {
|
||||
// this device is already started
|
||||
continue;
|
||||
}
|
||||
|
||||
if (busid != MPU9250_BUS_ALL && bus_options[i].busid != busid) {
|
||||
// not the one that is asked for
|
||||
continue;
|
||||
}
|
||||
|
||||
started |= start_bus(bus_options[i], rotation, range, device_type, external);
|
||||
}
|
||||
|
||||
exit(started ? 0 : 1);
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
stop(enum MPU9250_BUS busid)
|
||||
{
|
||||
struct mpu9250_bus_option &bus = find_bus(busid);
|
||||
|
||||
|
||||
if (bus.dev != nullptr) {
|
||||
delete bus.dev;
|
||||
bus.dev = nullptr;
|
||||
|
||||
} else {
|
||||
/* warn, but not an error */
|
||||
@@ -189,35 +279,33 @@ stop(bool external_bus)
|
||||
* and automatic modes.
|
||||
*/
|
||||
void
|
||||
test(bool external_bus)
|
||||
test(enum MPU9250_BUS busid)
|
||||
{
|
||||
const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL;
|
||||
const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO;
|
||||
const char *path_mag = external_bus ? MPU_DEVICE_PATH_MAG_EXT : MPU_DEVICE_PATH_MAG;
|
||||
struct mpu9250_bus_option &bus = find_bus(busid);
|
||||
accel_report a_report;
|
||||
gyro_report g_report;
|
||||
mag_report m_report;
|
||||
ssize_t sz;
|
||||
|
||||
/* get the driver */
|
||||
int fd = open(path_accel, O_RDONLY);
|
||||
int fd = open(bus.accelpath, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
err(1, "%s open failed (try 'm start')", path_accel);
|
||||
err(1, "%s open failed (try 'm start')", bus.accelpath);
|
||||
}
|
||||
|
||||
/* get the driver */
|
||||
int fd_gyro = open(path_gyro, O_RDONLY);
|
||||
int fd_gyro = open(bus.gyropath, O_RDONLY);
|
||||
|
||||
if (fd_gyro < 0) {
|
||||
err(1, "%s open failed", path_gyro);
|
||||
err(1, "%s open failed", bus.gyropath);
|
||||
}
|
||||
|
||||
/* get the driver */
|
||||
int fd_mag = open(path_mag, O_RDONLY);
|
||||
int fd_mag = open(bus.magpath, O_RDONLY);
|
||||
|
||||
if (fd_mag < 0) {
|
||||
err(1, "%s open failed", path_mag);
|
||||
err(1, "%s open failed", bus.magpath);
|
||||
}
|
||||
|
||||
/* reset to manual polling */
|
||||
@@ -292,7 +380,7 @@ test(bool external_bus)
|
||||
|
||||
/* XXX add poll-rate tests here too */
|
||||
|
||||
reset(external_bus);
|
||||
reset(busid);
|
||||
errx(0, "PASS");
|
||||
}
|
||||
|
||||
@@ -300,10 +388,10 @@ test(bool external_bus)
|
||||
* Reset the driver.
|
||||
*/
|
||||
void
|
||||
reset(bool external_bus)
|
||||
reset(enum MPU9250_BUS busid)
|
||||
{
|
||||
const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL;
|
||||
int fd = open(path_accel, O_RDONLY);
|
||||
struct mpu9250_bus_option &bus = find_bus(busid);
|
||||
int fd = open(bus.accelpath, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
err(1, "failed ");
|
||||
@@ -326,15 +414,17 @@ reset(bool external_bus)
|
||||
* Print a little info about the driver.
|
||||
*/
|
||||
void
|
||||
info(bool external_bus)
|
||||
info(enum MPU9250_BUS busid)
|
||||
{
|
||||
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
|
||||
struct mpu9250_bus_option &bus = find_bus(busid);
|
||||
|
||||
if (*g_dev_ptr == nullptr) {
|
||||
|
||||
if (bus.dev == nullptr) {
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
|
||||
(*g_dev_ptr)->print_info();
|
||||
printf("state @ %p\n", bus.dev);
|
||||
bus.dev->print_info();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
@@ -343,15 +433,17 @@ info(bool external_bus)
|
||||
* Dump the register information
|
||||
*/
|
||||
void
|
||||
regdump(bool external_bus)
|
||||
regdump(enum MPU9250_BUS busid)
|
||||
{
|
||||
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
|
||||
struct mpu9250_bus_option &bus = find_bus(busid);
|
||||
|
||||
if (*g_dev_ptr == nullptr) {
|
||||
|
||||
if (bus.dev == nullptr) {
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
|
||||
(*g_dev_ptr)->print_registers();
|
||||
printf("regdump @ %p\n", bus.dev);
|
||||
bus.dev->print_registers();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
@@ -360,15 +452,16 @@ regdump(bool external_bus)
|
||||
* deliberately produce an error to test recovery
|
||||
*/
|
||||
void
|
||||
testerror(bool external_bus)
|
||||
testerror(enum MPU9250_BUS busid)
|
||||
{
|
||||
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
|
||||
struct mpu9250_bus_option &bus = find_bus(busid);
|
||||
|
||||
if (*g_dev_ptr == nullptr) {
|
||||
|
||||
if (bus.dev == nullptr) {
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
|
||||
(*g_dev_ptr)->test_error();
|
||||
bus.dev->test_error();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
@@ -380,6 +473,7 @@ usage()
|
||||
warnx("options:");
|
||||
warnx(" -X (external bus)");
|
||||
warnx(" -R rotation");
|
||||
warnx(" -a accel range (in g)");
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -387,70 +481,91 @@ usage()
|
||||
int
|
||||
mpu9250_main(int argc, char *argv[])
|
||||
{
|
||||
bool external_bus = false;
|
||||
enum MPU9250_BUS busid = MPU9250_BUS_ALL;
|
||||
int ch;
|
||||
bool external = false;
|
||||
enum Rotation rotation = ROTATION_NONE;
|
||||
int accel_range = 8;
|
||||
|
||||
/* jump over start/off/etc and look at options first */
|
||||
while ((ch = getopt(argc, argv, "XR:")) != EOF) {
|
||||
while ((ch = getopt(argc, argv, "XISsR:a:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'X':
|
||||
external_bus = true;
|
||||
busid = MPU9260_BUS_I2C_EXTERNAL;
|
||||
break;
|
||||
|
||||
case 'I':
|
||||
busid = MPU9250_BUS_I2C_INTERNAL;
|
||||
break;
|
||||
|
||||
case 'S':
|
||||
busid = MPU9250_BUS_SPI_EXTERNAL;
|
||||
break;
|
||||
|
||||
case 's':
|
||||
busid = MPU9260_BUS_SPI_INTERNAL;
|
||||
break;
|
||||
|
||||
case 'R':
|
||||
rotation = (enum Rotation)atoi(optarg);
|
||||
break;
|
||||
|
||||
case 'a':
|
||||
accel_range = atoi(optarg);
|
||||
break;
|
||||
|
||||
default:
|
||||
mpu9250::usage();
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
external = (busid == MPU9250_BUS_I2C_EXTERNAL || busid == MPU9250_BUS_SPI_EXTERNAL);
|
||||
|
||||
const char *verb = argv[optind];
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
|
||||
*/
|
||||
if (!strcmp(verb, "start")) {
|
||||
mpu9250::start(external_bus, rotation);
|
||||
mpu9250::start(busid, rotation, accel_range, device_type, external);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
mpu9250::stop(external_bus);
|
||||
mpu9250::stop(busid);
|
||||
}
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(verb, "test")) {
|
||||
mpu9250::test(external_bus);
|
||||
mpu9250::test(busid);
|
||||
}
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
if (!strcmp(verb, "reset")) {
|
||||
mpu9250::reset(external_bus);
|
||||
mpu9250::reset(busid);
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(verb, "info")) {
|
||||
mpu9250::info(external_bus);
|
||||
mpu9250::info(busid);
|
||||
}
|
||||
|
||||
/*
|
||||
* Print register information.
|
||||
*/
|
||||
if (!strcmp(verb, "regdump")) {
|
||||
mpu9250::regdump(external_bus);
|
||||
mpu9250::regdump(busid);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "testerror")) {
|
||||
mpu9250::testerror(external_bus);
|
||||
mpu9250::testerror(busid);
|
||||
}
|
||||
|
||||
mpu9250::usage();
|
||||
|
||||
Reference in New Issue
Block a user