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

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

@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 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>
* Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -42,6 +39,7 @@
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <geo/geo.h>
@ -200,7 +198,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
crosstrack_error->bearing = 0.0f;
// 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_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@ -251,29 +249,29 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
crosstrack_error->bearing = 0.0f;
// 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) {
bearing_sector_start = arc_start_bearing;
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 {
bearing_sector_end = arc_start_bearing;
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;
// 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
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_sector) {
@ -375,18 +373,24 @@ __EXPORT float _wrap_pi(float bearing)
}
int c = 0;
while (bearing > M_PI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing <= -M_PI_F) {
bearing += M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
@ -399,18 +403,24 @@ __EXPORT float _wrap_2pi(float bearing)
}
int c = 0;
while (bearing > M_TWOPI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing <= 0.0f) {
bearing += M_TWOPI_F;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
@ -423,18 +433,24 @@ __EXPORT float _wrap_180(float bearing)
}
int c = 0;
while (bearing > 180.0f) {
bearing -= 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing <= -180.0f) {
bearing += 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
@ -447,18 +463,24 @@ __EXPORT float _wrap_360(float bearing)
}
int c = 0;
while (bearing > 360.0f) {
bearing -= 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing <= 0.0f) {
bearing += 360.0f;
if (c++ > 3)
if (c++ > 3) {
return NAN;
}
}
return bearing;
}

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

@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 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>
* Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -42,6 +39,7 @@
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.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>
*/

50
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.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -40,6 +39,8 @@
* 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).
* 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>
@ -355,8 +356,9 @@ MulticopterPositionControl::parameters_update(bool force)
orb_check(_params_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(parameter_update), _params_sub, &param_upd);
}
if (updated || force) {
param_get(_params_handles.thr_min, &_params.thr_min);
@ -412,34 +414,40 @@ MulticopterPositionControl::poll_subscriptions()
orb_check(_att_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
}
orb_check(_att_sp_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
}
orb_check(_control_mode_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
orb_check(_manual_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
}
orb_check(_arming_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
}
orb_check(_local_pos_sub, &updated);
if (updated)
if (updated) {
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
}
}
float
MulticopterPositionControl::scale_control(float ctl, float end, float dz)
@ -550,8 +558,9 @@ MulticopterPositionControl::task_main()
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
/* timed out - periodic check for _task_should_exit */
if (pret == 0)
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do */
if (pret < 0) {
@ -653,8 +662,9 @@ MulticopterPositionControl::task_main()
bool 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);
}
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
@ -832,9 +842,10 @@ MulticopterPositionControl::task_main()
/* limit max tilt and min lift when landing */
tilt_max = _params.land_tilt_max;
if (thr_min < 0.0f)
if (thr_min < 0.0f) {
thr_min = 0.0f;
}
}
/* limit min lift */
if (-thrust_sp(2) < thr_min) {
@ -924,9 +935,10 @@ MulticopterPositionControl::task_main()
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
/* protection against flipping on ground when landing */
if (thrust_int(2) > 0.0f)
if (thrust_int(2) > 0.0f) {
thrust_int(2) = 0.0f;
}
}
/* calculate attitude setpoint from thrust vector */
if (_control_mode.flag_control_velocity_enabled) {
@ -1045,18 +1057,21 @@ MulticopterPositionControl::start()
int mc_pos_control_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1) {
errx(1, "usage: mc_pos_control {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (pos_control::g_control != nullptr)
if (pos_control::g_control != nullptr) {
errx(1, "already running");
}
pos_control::g_control = new MulticopterPositionControl;
if (pos_control::g_control == nullptr)
if (pos_control::g_control == nullptr) {
errx(1, "alloc failed");
}
if (OK != pos_control::g_control->start()) {
delete pos_control::g_control;
@ -1068,8 +1083,9 @@ int mc_pos_control_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "stop")) {
if (pos_control::g_control == nullptr)
if (pos_control::g_control == nullptr) {
errx(1, "not running");
}
delete pos_control::g_control;
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.
* Author: Anton Babushkin <rk3dov@gmail.com>
* Copyright (C) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -35,6 +34,8 @@
/**
* @file position_estimator_inav_main.c
* Model-identification based position estimator for multirotors
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <unistd.h>
@ -95,8 +96,9 @@ static void usage(const char *reason);
*/
static void usage(const char *reason)
{
if (reason)
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
exit(1);
@ -112,8 +114,9 @@ static void usage(const char *reason)
*/
int position_estimator_inav_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
@ -125,8 +128,9 @@ int position_estimator_inav_main(int argc, char *argv[])
verbose_mode = false;
if (argc > 1)
if (!strcmp(argv[2], "-v"))
if (!strcmp(argv[2], "-v")) {
verbose_mode = true;
}
thread_should_exit = false;
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);
}
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");
if (f) {
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]);
@ -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);
free(s);
}
fsync(fileno(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 */
// TODO make this more fuzzy
if (!flow_accurate)
if (!flow_accurate) {
w_flow *= 0.05f;
}
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.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -35,6 +34,9 @@
/**
* @file vehicle_local_position.h
* 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_