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

position_estimator_inav: major optimization, poll only attitude topic, publish at 100 Hz

Browse Source
This commit is contained in:
Anton Babushkin
2013-10-07 11:59:23 +02:00
parent 73bf8af2b3
commit c8c74ea776
1 changed files with 28 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+28 -26
View File
@@ -79,7 +79,7 @@ static bool verbose_mode = false;
static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s
static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s
static const uint32_t updates_counter_len = 1000000; static const uint32_t updates_counter_len = 1000000;
static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz
__EXPORT int position_estimator_inav_main(int argc, char *argv[]); __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@@ -308,14 +308,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
/* main loop */ /* main loop */
struct pollfd fds[7] = { struct pollfd fds[1] = {
{ .fd = parameter_update_sub, .events = POLLIN },
{ .fd = actuator_sub, .events = POLLIN },
{ .fd = armed_sub, .events = POLLIN },
{ .fd = vehicle_attitude_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN },
{ .fd = sensor_combined_sub, .events = POLLIN },
{ .fd = optical_flow_sub, .events = POLLIN },
{ .fd = vehicle_gps_position_sub, .events = POLLIN }
}; };
if (!thread_should_exit) { if (!thread_should_exit) {
@@ -323,7 +317,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
while (!thread_should_exit) { while (!thread_should_exit) {
int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
hrt_abstime t = hrt_absolute_time(); hrt_abstime t = hrt_absolute_time();
if (ret < 0) { if (ret < 0) {
@@ -333,36 +327,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
continue; continue;
} else if (ret > 0) { } else if (ret > 0) {
/* act on attitude updates */
/* vehicle attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
bool updated;
/* parameter update */ /* parameter update */
if (fds[0].revents & POLLIN) { orb_check(parameter_update_sub, &updated);
/* read from param to clear updated flag */
if (updated) {
struct parameter_update_s update; struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
&update);
/* update parameters */
parameters_update(&pos_inav_param_handles, &params); parameters_update(&pos_inav_param_handles, &params);
} }
/* actuator */ /* actuator */
if (fds[1].revents & POLLIN) { orb_check(actuator_sub, &updated);
if (updated) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
} }
/* armed */ /* armed */
if (fds[2].revents & POLLIN) { orb_check(armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
} }
/* vehicle attitude */
if (fds[3].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
}
/* sensor combined */ /* sensor combined */
if (fds[4].revents & POLLIN) { orb_check(sensor_combined_sub, &updated);
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_counter > accel_counter) { if (sensor.accelerometer_counter > accel_counter) {
if (att.R_valid) { if (att.R_valid) {
/* correct accel bias, now only for Z */ /* correct accel bias, now only for Z */
@@ -399,7 +399,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
/* optical flow */ /* optical flow */
if (fds[5].revents & POLLIN) { orb_check(optical_flow_sub, &updated);
if (updated) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) {
@@ -436,7 +437,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
/* vehicle GPS position */ /* vehicle GPS position */
if (fds[6].revents & POLLIN) { orb_check(vehicle_gps_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3) { if (gps.fix_type >= 3) {
/* hysteresis for GPS quality */ /* hysteresis for GPS quality */