fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 04:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mpu9250: main handle init and alloc failures separately

Browse Source
This commit is contained in:
Daniel Agar
2020-01-04 11:41:56 -05:00
parent 25cbcca84a
commit 5dfc8f1362
1 changed files with 15 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/mpu9250/mpu9250_main.cpp
+15 -10
View File
@@ -83,7 +83,7 @@ struct mpu9250_bus_option {
}; };
// find a bus structure for a busid // 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) { for (mpu9250_bus_option &bus_option : bus_options) {
if ((busid == MPU9250_BUS::ALL || 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); 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"); PX4_ERR("driver start failed");
delete dev; delete dev;
delete interface;
delete mag_interface; delete mag_interface;
return false; return false;
} }
@@ -147,11 +152,11 @@ static int start(MPU9250_BUS busid, enum Rotation rotation)
} }
if (start_bus(bus_option, rotation)) { if (start_bus(bus_option, rotation)) {
return PX4_OK; return 0;
} }
} }
return PX4_ERROR; return -1;
} }
static int stop(MPU9250_BUS busid) static int stop(MPU9250_BUS busid)
@@ -164,10 +169,10 @@ static int stop(MPU9250_BUS busid)
} else { } else {
PX4_WARN("driver not running"); PX4_WARN("driver not running");
return PX4_ERROR; return -1;
} }
return PX4_OK; return 0;
} }
static int status(MPU9250_BUS busid) static int status(MPU9250_BUS busid)
@@ -176,11 +181,11 @@ static int status(MPU9250_BUS busid)
if (bus != nullptr && bus->dev != nullptr) { if (bus != nullptr && bus->dev != nullptr) {
bus->dev->print_info(); bus->dev->print_info();
return PX4_OK; return 0;
} }
PX4_WARN("driver not running"); PX4_WARN("driver not running");
return PX4_ERROR; return -1;
} }
static int usage() static int usage()
@@ -208,7 +213,7 @@ extern "C" int mpu9250_main(int argc, char *argv[])
MPU9250_BUS busid = MPU9250_BUS::ALL; MPU9250_BUS busid = MPU9250_BUS::ALL;
enum Rotation rotation = ROTATION_NONE; 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) { switch (ch) {
case 'X': case 'X':
busid = MPU9250_BUS::I2C_EXTERNAL; busid = MPU9250_BUS::I2C_EXTERNAL;