dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mpu9250: main handle init and alloc failures separately

This commit is contained in:
Daniel Agar 2020-01-04 11:10:14 -05:00
parent 25cbcca84a
commit 5dfc8f1362
1 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/drivers/imu/mpu9250/mpu9250_main.cpp
View File

@ -83,7 +83,7 @@ struct mpu9250_bus_option {
};
// find a bus structure for a busid
static struct mpu9250_bus_option *find_bus(MPU9250_BUS busid)
static mpu9250_bus_option *find_bus(MPU9250_BUS busid)
{
for (mpu9250_bus_option &bus_option : bus_options) {
if ((busid == MPU9250_BUS::ALL ||
@ -119,10 +119,15 @@ static bool start_bus(mpu9250_bus_option &bus, enum Rotation rotation)
MPU9250 *dev = new MPU9250(interface, mag_interface, rotation);
if (dev == nullptr || (dev->init() != PX4_OK)) {
if (dev == nullptr) {
PX4_ERR("alloc failed");
delete mag_interface;
return false;
}
if (dev->init() != PX4_OK) {
PX4_ERR("driver start failed");
delete dev;
delete interface;
delete mag_interface;
return false;
}
@ -147,11 +152,11 @@ static int start(MPU9250_BUS busid, enum Rotation rotation)
}
if (start_bus(bus_option, rotation)) {
return PX4_OK;
return 0;
}
}
return PX4_ERROR;
return -1;
}
static int stop(MPU9250_BUS busid)
@ -164,10 +169,10 @@ static int stop(MPU9250_BUS busid)
} else {
PX4_WARN("driver not running");
return PX4_ERROR;
return -1;
}
return PX4_OK;
return 0;
}
static int status(MPU9250_BUS busid)
@ -176,11 +181,11 @@ static int status(MPU9250_BUS busid)
if (bus != nullptr && bus->dev != nullptr) {
bus->dev->print_info();
return PX4_OK;
return 0;
}
PX4_WARN("driver not running");
return PX4_ERROR;
return -1;
}
static int usage()
@ -208,7 +213,7 @@ extern "C" int mpu9250_main(int argc, char *argv[])
MPU9250_BUS busid = MPU9250_BUS::ALL;
enum Rotation rotation = ROTATION_NONE;
while ((ch = px4_getopt(argc, argv, "XISstMR:", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "XISstR:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'X':
busid = MPU9250_BUS::I2C_EXTERNAL;