|
|
@ -1,6 +1,6 @@ |
|
|
|
/****************************************************************************
|
|
|
|
/****************************************************************************
|
|
|
|
* |
|
|
|
* |
|
|
|
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. |
|
|
|
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. |
|
|
|
* |
|
|
|
* |
|
|
|
* Redistribution and use in source and binary forms, with or without |
|
|
|
* Redistribution and use in source and binary forms, with or without |
|
|
|
* modification, are permitted provided that the following conditions |
|
|
|
* modification, are permitted provided that the following conditions |
|
|
@ -58,7 +58,8 @@ int px4_simple_app_main(int argc, char *argv[]) |
|
|
|
|
|
|
|
|
|
|
|
/* subscribe to sensor_combined topic */ |
|
|
|
/* subscribe to sensor_combined topic */ |
|
|
|
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); |
|
|
|
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); |
|
|
|
orb_set_interval(sensor_sub_fd, 1000); |
|
|
|
/* limit the update rate to 5 Hz */ |
|
|
|
|
|
|
|
orb_set_interval(sensor_sub_fd, 200); |
|
|
|
|
|
|
|
|
|
|
|
/* advertise attitude topic */ |
|
|
|
/* advertise attitude topic */ |
|
|
|
struct vehicle_attitude_s att; |
|
|
|
struct vehicle_attitude_s att; |
|
|
@ -82,14 +83,13 @@ int px4_simple_app_main(int argc, char *argv[]) |
|
|
|
/* handle the poll result */ |
|
|
|
/* handle the poll result */ |
|
|
|
if (poll_ret == 0) { |
|
|
|
if (poll_ret == 0) { |
|
|
|
/* this means none of our providers is giving us data */ |
|
|
|
/* this means none of our providers is giving us data */ |
|
|
|
PX4_ERR("[px4_simple_app] Got no data within a second"); |
|
|
|
PX4_ERR("Got no data within a second"); |
|
|
|
|
|
|
|
|
|
|
|
} else if (poll_ret < 0) { |
|
|
|
} else if (poll_ret < 0) { |
|
|
|
/* this is seriously bad - should be an emergency */ |
|
|
|
/* this is seriously bad - should be an emergency */ |
|
|
|
if (error_counter < 10 || error_counter % 50 == 0) { |
|
|
|
if (error_counter < 10 || error_counter % 50 == 0) { |
|
|
|
/* use a counter to prevent flooding (and slowing us down) */ |
|
|
|
/* use a counter to prevent flooding (and slowing us down) */ |
|
|
|
PX4_ERR("[px4_simple_app] ERROR return value from poll(): %d" |
|
|
|
PX4_ERR("ERROR return value from poll(): %d", poll_ret); |
|
|
|
, poll_ret); |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
error_counter++; |
|
|
|
error_counter++; |
|
|
@ -101,7 +101,7 @@ int px4_simple_app_main(int argc, char *argv[]) |
|
|
|
struct sensor_combined_s raw; |
|
|
|
struct sensor_combined_s raw; |
|
|
|
/* copy sensors raw data into local buffer */ |
|
|
|
/* copy sensors raw data into local buffer */ |
|
|
|
orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); |
|
|
|
orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); |
|
|
|
PX4_WARN("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f", |
|
|
|
PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f", |
|
|
|
(double)raw.accelerometer_m_s2[0], |
|
|
|
(double)raw.accelerometer_m_s2[0], |
|
|
|
(double)raw.accelerometer_m_s2[1], |
|
|
|
(double)raw.accelerometer_m_s2[1], |
|
|
|
(double)raw.accelerometer_m_s2[2]); |
|
|
|
(double)raw.accelerometer_m_s2[2]); |
|
|
|