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

copyright and code style fixes

This commit is contained in:
Anton Babushkin 2014-03-18 00:10:38 +04:00
parent c266124099
commit 068b7526b7
5 changed files with 113 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
src/lib/geo/geo.c
View File

@ -1,9 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* *
* 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
@ -42,6 +39,7 @@
* @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#include <geo/geo.h> #include <geo/geo.h>
@ -142,7 +140,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
return theta; return theta;
} }
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e) __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
{ {
double lat_now_rad = lat_now * M_DEG_TO_RAD; double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD;
@ -157,7 +155,7 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double
*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); *v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
} }
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e) __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
{ {
double lat_now_rad = lat_now * M_DEG_TO_RAD; double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD;
@ -183,7 +181,7 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
{ {
// This function returns the distance to the nearest point on the track line. Distance is positive if current // This function returns the distance to the nearest point on the track line. Distance is positive if current
// position is right of the track and negative if left of the track as seen from a point on the track line // position is right of the track and negative if left of the track as seen from a point on the track line
@ -200,7 +198,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
crosstrack_error->bearing = 0.0f; crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad // Return error if arguments are bad
if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value; if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@ -231,8 +229,8 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
} }
__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep) float radius, float arc_start_bearing, float arc_sweep)
{ {
// This function returns the distance to the nearest point on the track arc. Distance is positive if current // This function returns the distance to the nearest point on the track arc. Distance is positive if current
// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
@ -251,29 +249,29 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
crosstrack_error->bearing = 0.0f; crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad // Return error if arguments are bad
if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value; if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; }
if (arc_sweep >= 0) { if (arc_sweep >= 0) {
bearing_sector_start = arc_start_bearing; bearing_sector_start = arc_start_bearing;
bearing_sector_end = arc_start_bearing + arc_sweep; bearing_sector_end = arc_start_bearing + arc_sweep;
if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F; if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; }
} else { } else {
bearing_sector_end = arc_start_bearing; bearing_sector_end = arc_start_bearing;
bearing_sector_start = arc_start_bearing - arc_sweep; bearing_sector_start = arc_start_bearing - arc_sweep;
if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F; if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; }
} }
in_sector = false; in_sector = false;
// Case where sector does not span zero // Case where sector does not span zero
if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; }
// Case where sector does span zero // Case where sector does span zero
if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true; if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; }
// If in the sector then calculate distance and bearing to closest point // If in the sector then calculate distance and bearing to closest point
if (in_sector) { if (in_sector) {
@ -329,8 +327,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
} }
__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
double lat_next, double lon_next, float alt_next, double lat_next, double lon_next, float alt_next,
float *dist_xy, float *dist_z) float *dist_xy, float *dist_z)
{ {
double current_x_rad = lat_next / 180.0 * M_PI; double current_x_rad = lat_next / 180.0 * M_PI;
double current_y_rad = lon_next / 180.0 * M_PI; double current_y_rad = lon_next / 180.0 * M_PI;
@ -354,8 +352,8 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now
__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, __EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
float x_next, float y_next, float z_next, float x_next, float y_next, float z_next,
float *dist_xy, float *dist_z) float *dist_xy, float *dist_z)
{ {
float dx = x_now - x_next; float dx = x_now - x_next;
float dy = y_now - y_next; float dy = y_now - y_next;
@ -375,17 +373,23 @@ __EXPORT float _wrap_pi(float bearing)
} }
int c = 0; int c = 0;
while (bearing > M_PI_F) { while (bearing > M_PI_F) {
bearing -= M_TWOPI_F; bearing -= M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN; return NAN;
}
} }
c = 0; c = 0;
while (bearing <= -M_PI_F) { while (bearing <= -M_PI_F) {
bearing += M_TWOPI_F; bearing += M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN; return NAN;
}
} }
return bearing; return bearing;
@ -399,17 +403,23 @@ __EXPORT float _wrap_2pi(float bearing)
} }
int c = 0; int c = 0;
while (bearing > M_TWOPI_F) { while (bearing > M_TWOPI_F) {
bearing -= M_TWOPI_F; bearing -= M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN; return NAN;
}
} }
c = 0; c = 0;
while (bearing <= 0.0f) { while (bearing <= 0.0f) {
bearing += M_TWOPI_F; bearing += M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN; return NAN;
}
} }
return bearing; return bearing;
@ -423,17 +433,23 @@ __EXPORT float _wrap_180(float bearing)
} }
int c = 0; int c = 0;
while (bearing > 180.0f) { while (bearing > 180.0f) {
bearing -= 360.0f; bearing -= 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN; return NAN;
}
} }
c = 0; c = 0;
while (bearing <= -180.0f) { while (bearing <= -180.0f) {
bearing += 360.0f; bearing += 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN; return NAN;
}
} }
return bearing; return bearing;
@ -447,17 +463,23 @@ __EXPORT float _wrap_360(float bearing)
} }
int c = 0; int c = 0;
while (bearing > 360.0f) { while (bearing > 360.0f) {
bearing -= 360.0f; bearing -= 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN; return NAN;
}
} }
c = 0; c = 0;
while (bearing <= 0.0f) { while (bearing <= 0.0f) {
bearing += 360.0f; bearing += 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN; return NAN;
}
} }
return bearing; return bearing;

24
src/lib/geo/geo.h
View File

@ -1,9 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* *
* 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
@ -42,6 +39,7 @@
* @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
*/ */
@ -123,30 +121,30 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
*/ */
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e); __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e); __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res); __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res);
__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep); float radius, float arc_start_bearing, float arc_sweep);
/* /*
* Calculate distance in global frame * Calculate distance in global frame
*/ */
__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
double lat_next, double lon_next, float alt_next, double lat_next, double lon_next, float alt_next,
float *dist_xy, float *dist_z); float *dist_xy, float *dist_z);
/* /*
* Calculate distance in local frame (NED) * Calculate distance in local frame (NED)
*/ */
__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, __EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
float x_next, float y_next, float z_next, float x_next, float y_next, float z_next,
float *dist_xy, float *dist_z); float *dist_xy, float *dist_z);
__EXPORT float _wrap_180(float bearing); __EXPORT float _wrap_180(float bearing);
__EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_360(float bearing);

54
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
* *
* 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
@ -40,6 +39,8 @@
* Output of velocity controller is thrust vector that splitted to thrust direction * Output of velocity controller is thrust vector that splitted to thrust direction
* (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
* Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
@ -355,8 +356,9 @@ MulticopterPositionControl::parameters_update(bool force)
orb_check(_params_sub, &updated); orb_check(_params_sub, &updated);
if (updated) if (updated) {
orb_copy(ORB_ID(parameter_update), _params_sub, &param_upd); orb_copy(ORB_ID(parameter_update), _params_sub, &param_upd);
}
if (updated || force) { if (updated || force) {
param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_min, &_params.thr_min);
@ -412,33 +414,39 @@ MulticopterPositionControl::poll_subscriptions()
orb_check(_att_sub, &updated); orb_check(_att_sub, &updated);
if (updated) if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
}
orb_check(_att_sp_sub, &updated); orb_check(_att_sp_sub, &updated);
if (updated) if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
}
orb_check(_control_mode_sub, &updated); orb_check(_control_mode_sub, &updated);
if (updated) if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
orb_check(_manual_sub, &updated); orb_check(_manual_sub, &updated);
if (updated) if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
}
orb_check(_arming_sub, &updated); orb_check(_arming_sub, &updated);
if (updated) if (updated) {
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
}
orb_check(_local_pos_sub, &updated); orb_check(_local_pos_sub, &updated);
if (updated) if (updated) {
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
}
} }
float float
@ -550,8 +558,9 @@ MulticopterPositionControl::task_main()
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
/* timed out - periodic check for _task_should_exit */ /* timed out - periodic check for _task_should_exit */
if (pret == 0) if (pret == 0) {
continue; continue;
}
/* this is undesirable but not much we can do */ /* this is undesirable but not much we can do */
if (pret < 0) { if (pret < 0) {
@ -653,8 +662,9 @@ MulticopterPositionControl::task_main()
bool updated; bool updated;
orb_check(_pos_sp_triplet_sub, &updated); orb_check(_pos_sp_triplet_sub, &updated);
if (updated) if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) { if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */ /* in case of interrupted mission don't go to waypoint but stay at current position */
@ -663,8 +673,8 @@ MulticopterPositionControl::task_main()
/* project setpoint to local frame */ /* project setpoint to local frame */
map_projection_project(&_ref_pos, map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&_pos_sp.data[0], &_pos_sp.data[1]); &_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
/* update yaw setpoint if needed */ /* update yaw setpoint if needed */
@ -832,8 +842,9 @@ MulticopterPositionControl::task_main()
/* limit max tilt and min lift when landing */ /* limit max tilt and min lift when landing */
tilt_max = _params.land_tilt_max; tilt_max = _params.land_tilt_max;
if (thr_min < 0.0f) if (thr_min < 0.0f) {
thr_min = 0.0f; thr_min = 0.0f;
}
} }
/* limit min lift */ /* limit min lift */
@ -924,8 +935,9 @@ MulticopterPositionControl::task_main()
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
/* protection against flipping on ground when landing */ /* protection against flipping on ground when landing */
if (thrust_int(2) > 0.0f) if (thrust_int(2) > 0.0f) {
thrust_int(2) = 0.0f; thrust_int(2) = 0.0f;
}
} }
/* calculate attitude setpoint from thrust vector */ /* calculate attitude setpoint from thrust vector */
@ -1045,18 +1057,21 @@ MulticopterPositionControl::start()
int mc_pos_control_main(int argc, char *argv[]) int mc_pos_control_main(int argc, char *argv[])
{ {
if (argc < 1) if (argc < 1) {
errx(1, "usage: mc_pos_control {start|stop|status}"); errx(1, "usage: mc_pos_control {start|stop|status}");
}
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (pos_control::g_control != nullptr) if (pos_control::g_control != nullptr) {
errx(1, "already running"); errx(1, "already running");
}
pos_control::g_control = new MulticopterPositionControl; pos_control::g_control = new MulticopterPositionControl;
if (pos_control::g_control == nullptr) if (pos_control::g_control == nullptr) {
errx(1, "alloc failed"); errx(1, "alloc failed");
}
if (OK != pos_control::g_control->start()) { if (OK != pos_control::g_control->start()) {
delete pos_control::g_control; delete pos_control::g_control;
@ -1068,8 +1083,9 @@ int mc_pos_control_main(int argc, char *argv[])
} }
if (!strcmp(argv[1], "stop")) { if (!strcmp(argv[1], "stop")) {
if (pos_control::g_control == nullptr) if (pos_control::g_control == nullptr) {
errx(1, "not running"); errx(1, "not running");
}
delete pos_control::g_control; delete pos_control::g_control;
pos_control::g_control = nullptr; pos_control::g_control = nullptr;

22
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 Anton Babushkin. All rights reserved. * Copyright (C) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
* *
* 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
@ -35,6 +34,8 @@
/** /**
* @file position_estimator_inav_main.c * @file position_estimator_inav_main.c
* Model-identification based position estimator for multirotors * Model-identification based position estimator for multirotors
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#include <unistd.h> #include <unistd.h>
@ -95,8 +96,9 @@ static void usage(const char *reason);
*/ */
static void usage(const char *reason) static void usage(const char *reason)
{ {
if (reason) if (reason) {
fprintf(stderr, "%s\n", reason); fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
exit(1); exit(1);
@ -112,8 +114,9 @@ static void usage(const char *reason)
*/ */
int position_estimator_inav_main(int argc, char *argv[]) int position_estimator_inav_main(int argc, char *argv[])
{ {
if (argc < 1) if (argc < 1) {
usage("missing command"); usage("missing command");
}
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (thread_running) { if (thread_running) {
@ -125,8 +128,9 @@ int position_estimator_inav_main(int argc, char *argv[])
verbose_mode = false; verbose_mode = false;
if (argc > 1) if (argc > 1)
if (!strcmp(argv[2], "-v")) if (!strcmp(argv[2], "-v")) {
verbose_mode = true; verbose_mode = true;
}
thread_should_exit = false; thread_should_exit = false;
position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
@ -163,8 +167,10 @@ int position_estimator_inav_main(int argc, char *argv[])
exit(1); exit(1);
} }
void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
{
FILE *f = fopen("/fs/microsd/inav.log", "a"); FILE *f = fopen("/fs/microsd/inav.log", "a");
if (f) { if (f) {
char *s = malloc(256); char *s = malloc(256);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]);
@ -173,6 +179,7 @@ void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3],
fwrite(s, 1, n, f); fwrite(s, 1, n, f);
free(s); free(s);
} }
fsync(fileno(f)); fsync(fileno(f));
fclose(f); fclose(f);
} }
@ -505,8 +512,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* if flow is not accurate, reduce weight for it */ /* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy // TODO make this more fuzzy
if (!flow_accurate) if (!flow_accurate) {
w_flow *= 0.05f; w_flow *= 0.05f;
}
flow_valid = true; flow_valid = true;

4
src/modules/uORB/topics/vehicle_local_position.h
View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* *
* 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
@ -35,6 +34,9 @@
/** /**
* @file vehicle_local_position.h * @file vehicle_local_position.h
* Definition of the local fused NED position uORB topic. * Definition of the local fused NED position uORB topic.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_ #ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_