From 2284a7e985b174dab4b3c1666d9f019d9479a230 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Mar 2014 22:19:50 +0400 Subject: [PATCH 01/53] geo lib: major rewrite of map_projection_XXX functions --- src/lib/geo/geo.c | 130 ++++++++++++---------------------------------- src/lib/geo/geo.h | 14 +++-- 2 files changed, 43 insertions(+), 101 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9b3e202e6e..fc4e5cd1b5 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -52,124 +52,58 @@ #include #include +/* + * Azimuthal Equidistant Projection + * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html + */ -/* values for map projection */ -static double phi_1; -static double sin_phi_1; -static double cos_phi_1; -static double lambda_0; -static double scale; - -__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - phi_1 = lat_0 / 180.0 * M_PI; - lambda_0 = lon_0 / 180.0 * M_PI; - - sin_phi_1 = sin(phi_1); - cos_phi_1 = cos(phi_1); - - /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale - - /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ - - double lat1 = phi_1; - double lon1 = lambda_0; - - double lat2 = phi_1 + 0.5 / 180 * M_PI; - double lon2 = lambda_0 + 0.5 / 180 * M_PI; - double sin_lat_2 = sin(lat2); - double cos_lat_2 = cos(lat2); - double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH; - - /* 2) calculate distance rho on plane */ - double k_bar = 0; - double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); - - if (0 != c) - k_bar = c / sin(c); - - double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane - double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); - double rho = sqrt(pow(x2, 2) + pow(y2, 2)); - - scale = d / rho; + ref->lat = lat_0 / 180.0 * M_PI; + ref->lon = lon_0 / 180.0 * M_PI; + ref->sin_lat = sin(ref->lat); + ref->cos_lat = cos(ref->lat); } -__EXPORT void map_projection_project(double lat, double lon, float *x, float *y) +__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) { - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - double phi = lat / 180.0 * M_PI; - double lambda = lon / 180.0 * M_PI; + double lat_rad = lat / 180.0 * M_PI; + double lon_rad = lon / 180.0 * M_PI; - double sin_phi = sin(phi); - double cos_phi = cos(phi); + double sin_lat = sin(lat_rad); + double cos_lat = cos(lat_rad); + double cos_d_lon = cos(lon_rad - ref->lon); - double k_bar = 0; - /* using small angle approximation (formula in comment is without aproximation) */ - double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); + double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double k = (c == 0.0) ? 1.0 : (c / sin(c)); - if (0 != c) - k_bar = c / sin(c); - - /* using small angle approximation (formula in comment is without aproximation) */ - *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; - *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; - -// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); + *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * (cos_lat * sin(lon_rad - ref->lon)) * CONSTANTS_RADIUS_OF_EARTH; } -__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon) +__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - - double x_descaled = x / scale; - double y_descaled = y / scale; - - double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); + float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; + float y_rad = y / CONSTANTS_RADIUS_OF_EARTH; + double c = sqrtf(x_rad * x_rad + y_rad * y_rad); double sin_c = sin(c); double cos_c = cos(c); - double lat_sphere = 0; + double lat_rad; + double lon_rad; - if (c != 0) - lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); - else - lat_sphere = asin(cos_c * sin_phi_1); - -// printf("lat_sphere = %.10f\n",lat_sphere); - - double lon_sphere = 0; - - if (phi_1 == M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); - - } else if (phi_1 == -M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); + if (c != 0.0) { + lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); + lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); } else { - - lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); - //using small angle approximation -// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); -// if(denominator != 0) -// { -// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); -// } -// else -// { -// ... -// } + lat_rad = ref->lat; + lon_rad = ref->lon; } -// printf("lon_sphere = %.10f\n",lon_sphere); - - *lat = lat_sphere * 180.0 / M_PI; - *lon = lon_sphere * 180.0 / M_PI; - + *lat = lat_rad * 180.0 / M_PI; + *lon = lon_rad * 180.0 / M_PI; } diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 94afb4df0a..a66bd10e4d 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -67,6 +67,14 @@ struct crosstrack_error_s { float bearing; // Bearing in radians to closest point on line/arc } ; +/* lat/lon are in radians */ +struct map_projection_reference_s { + double lat; + double lon; + double sin_lat; + double cos_lat; +}; + /** * Initializes the map transformation. * @@ -74,7 +82,7 @@ struct crosstrack_error_s { * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_init(double lat_0, double lon_0); +__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0); /** * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane @@ -83,7 +91,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0); * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_project(double lat, double lon, float *x, float *y); +__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); /** * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system @@ -93,7 +101,7 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y) * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); +__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); /** * Returns the distance to the next waypoint in meters. From 3d5f52678fa093a248d824828fcafe12ac2f8f15 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Mar 2014 22:20:41 +0400 Subject: [PATCH 02/53] Use updated map_projection_XXX functions in apps --- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 10 ++++------ src/modules/att_pos_estimator_ekf/KalmanNav.hpp | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 5 +++-- .../position_estimator_inav_main.c | 8 +++++--- 4 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 668bac5d9c..a4d5560c71 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -97,6 +97,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : { using namespace math; + memset(&ref, 0, sizeof(ref)); + F.zero(); G.zero(); V.zero(); @@ -247,11 +249,7 @@ void KalmanNav::update() lat0 = lat; lon0 = lon; alt0 = alt; - // XXX map_projection has internal global - // states that multiple things could change, - // should make map_projection take reference - // lat/lon and not have init - map_projection_init(lat0, lon0); + map_projection_init(&ref, lat0, lon0); _positionInitialized = true; warnx("initialized EKF state with GPS"); warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", @@ -327,7 +325,7 @@ void KalmanNav::updatePublications() float x; float y; bool landed = alt < (alt0 + 0.1); // XXX improve? - map_projection_project(lat, lon, &x, &y); + map_projection_project(&ref, lat, lon, &x, &y); _localPos.timestamp = _pubTimeStamp; _localPos.xy_valid = true; _localPos.z_valid = true; diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 46ee4b6c8e..5021b99275 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include @@ -164,6 +165,7 @@ protected: // parameters float alt; /**< altitude, meters */ double lat0, lon0; /**< reference latitude and longitude */ + struct map_projection_reference_s ref; /**< local projection reference */ float alt0; /**< refeerence altitude (ground height) */ control::BlockParamFloat _vGyro; /**< gyro process noise */ control::BlockParamFloat _vAccel; /**< accelerometer process noise */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1dbe564955..624740237c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -132,6 +132,7 @@ static orb_advert_t telemetry_status_pub = -1; static int32_t lat0 = 0; static int32_t lon0 = 0; static double alt0 = 0; +struct map_projection_reference_s hil_ref; static void handle_message(mavlink_message_t *msg) @@ -653,7 +654,7 @@ handle_message(mavlink_message_t *msg) bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve? double lat = hil_state.lat*1e-7; double lon = hil_state.lon*1e-7; - map_projection_project(lat, lon, &x, &y); + map_projection_project(&hil_ref, lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; @@ -679,7 +680,7 @@ handle_message(mavlink_message_t *msg) lat0 = hil_state.lat; lon0 = hil_state.lon; alt0 = hil_state.alt / 1000.0f; - map_projection_init(hil_state.lat, hil_state.lon); + map_projection_init(&hil_ref, hil_state.lat, hil_state.lon); } /* Calculate Rotation Matrix */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index a14354138b..eddf6e94ea 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -206,6 +206,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool ref_inited = false; hrt_abstime ref_init_start = 0; const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix + struct map_projection_reference_s ref; + memset(&ref, 0, sizeof(ref)); uint16_t accel_updates = 0; uint16_t baro_updates = 0; @@ -560,7 +562,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_timestamp = t; /* initialize projection */ - map_projection_init(lat, lon); + map_projection_init(&ref, lat, lon); warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); } @@ -569,7 +571,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + map_projection_project(&ref, gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - x_est[0]; corr_gps[1][0] = gps_proj[1] - y_est[0]; @@ -836,7 +838,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global) { double est_lat, est_lon; - map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = est_lat; global_pos.lon = est_lon; global_pos.time_gps_usec = gps.time_gps_usec; From c0c54f01cb351afc4184c35a0308686392af1a14 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Mar 2014 22:42:52 +0400 Subject: [PATCH 03/53] mc_pos_control: operate in local projection instead of global frame --- .../mc_pos_control/mc_pos_control_main.cpp | 207 ++++++++---------- 1 file changed, 96 insertions(+), 111 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 78d06ba5b5..3d05b37d81 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -62,9 +62,10 @@ #include #include #include -#include +#include #include #include +#include #include #include #include @@ -114,20 +115,21 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ - int _global_pos_sub; /**< vehicle local position */ + int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ - orb_advert_t _pos_sp_triplet_pub; /**< position setpoint triplet publication */ - orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */ + orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ + orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_global_position_s _global_pos; /**< vehicle global position */ + struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ + struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct { @@ -172,14 +174,15 @@ private: math::Vector<3> sp_offs_max; } _params; - double _lat_sp; - double _lon_sp; - float _alt_sp; + struct map_projection_reference_s _ref_pos; + float _ref_alt; + hrt_abstime _ref_timestamp; - bool _reset_lat_lon_sp; + bool _reset_pos_sp; bool _reset_alt_sp; - bool _use_global_alt; /**< switch between global (AMSL) and barometric altitudes */ + math::Vector<3> _pos; + math::Vector<3> _pos_sp; math::Vector<3> _vel; math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ @@ -202,9 +205,13 @@ private: static float scale_control(float ctl, float end, float dz); /** - * Reset lat/lon to current position + * Update reference for local position projection */ - void reset_lat_lon_sp(); + void update_ref(); + /** + * Reset position setpoint to current position + */ + void reset_pos_sp(); /** * Reset altitude setpoint to current altitude @@ -252,31 +259,32 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_sub(-1), _manual_sub(-1), _arming_sub(-1), - _global_pos_sub(-1), + _local_pos_sub(-1), _pos_sp_triplet_sub(-1), /* publications */ _att_sp_pub(-1), - _pos_sp_triplet_pub(-1), + _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), - _lat_sp(0.0), - _lon_sp(0.0), - _alt_sp(0.0f), + _ref_alt(0.0f), + _ref_timestamp(0), - _reset_lat_lon_sp(true), - _reset_alt_sp(true), - _use_global_alt(false) + _reset_pos_sp(true), + _reset_alt_sp(true) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); memset(&_arming, 0, sizeof(_arming)); - memset(&_global_pos, 0, sizeof(_global_pos)); + memset(&_local_pos, 0, sizeof(_local_pos)); memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); + memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); + memset(&_ref_pos, 0, sizeof(_ref_pos)); + _params.pos_p.zero(); _params.vel_p.zero(); _params.vel_i.zero(); @@ -285,6 +293,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _params.vel_ff.zero(); _params.sp_offs_max.zero(); + _pos.zero(); + _pos_sp.zero(); _vel.zero(); _vel_sp.zero(); _vel_prev.zero(); @@ -425,10 +435,10 @@ MulticopterPositionControl::poll_subscriptions() if (updated) orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); - orb_check(_global_pos_sub, &updated); + orb_check(_local_pos_sub, &updated); if (updated) - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); } float @@ -452,13 +462,25 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) } void -MulticopterPositionControl::reset_lat_lon_sp() +MulticopterPositionControl::update_ref() { - if (_reset_lat_lon_sp) { - _reset_lat_lon_sp = false; - _lat_sp = _global_pos.lat; - _lon_sp = _global_pos.lon; - mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp); + if (_local_pos.ref_timestamp != _ref_timestamp) { + _ref_timestamp = _local_pos.ref_timestamp; + // TODO mode position setpoint in assisted modes + + map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); + _ref_alt = _local_pos.ref_alt; + } +} + +void +MulticopterPositionControl::reset_pos_sp() +{ + if (_reset_pos_sp) { + _reset_pos_sp = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -467,25 +489,8 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt; - mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp); - } -} - -void -MulticopterPositionControl::select_alt(bool global) -{ - if (global != _use_global_alt) { - _use_global_alt = global; - - if (global) { - /* switch from barometric to global altitude */ - _alt_sp += _global_pos.alt - _global_pos.baro_alt; - - } else { - /* switch from global to barometric altitude */ - _alt_sp += _global_pos.baro_alt - _global_pos.alt; - } + _pos_sp(2) = _pos(2); + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } @@ -506,7 +511,7 @@ MulticopterPositionControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); - _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); parameters_update(true); @@ -537,8 +542,7 @@ MulticopterPositionControl::task_main() /* wakeup source */ struct pollfd fds[1]; - /* Setup of loop */ - fds[0].fd = _global_pos_sub; + fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; while (!_task_should_exit) { @@ -564,7 +568,7 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ - _reset_lat_lon_sp = true; + _reset_pos_sp = true; _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; @@ -577,23 +581,20 @@ MulticopterPositionControl::task_main() _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - _vel(0) = _global_pos.vel_n; - _vel(1) = _global_pos.vel_e; - _vel(2) = _global_pos.vel_d; + update_ref(); + + _pos(0) = _local_pos.x; + _pos(1) = _local_pos.y; + _pos(2) = _local_pos.z; + + _vel(0) = _local_pos.vx; + _vel(1) = _local_pos.vy; + _vel(2) = _local_pos.vz; sp_move_rate.zero(); - float alt = _global_pos.alt; - /* select control source */ if (_control_mode.flag_control_manual_enabled) { - /* select altitude source and update setpoint */ - select_alt(_global_pos.global_valid); - - if (!_use_global_alt) { - alt = _global_pos.baro_alt; - } - /* manual control */ if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ @@ -604,8 +605,8 @@ MulticopterPositionControl::task_main() } if (_control_mode.flag_control_position_enabled) { - /* reset lat/lon setpoint to current position if needed */ - reset_lat_lon_sp(); + /* reset position setpoint to current position if needed */ + reset_pos_sp(); /* move position setpoint with roll/pitch stick */ sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); @@ -625,58 +626,29 @@ MulticopterPositionControl::task_main() sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); /* move position setpoint */ - add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp); - _alt_sp -= sp_move_rate(2) * dt; + _pos_sp += sp_move_rate * dt; /* check if position setpoint is too far from actual position */ math::Vector<3> pos_sp_offs; pos_sp_offs.zero(); if (_control_mode.flag_control_position_enabled) { - get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]); - pos_sp_offs(0) /= _params.sp_offs_max(0); - pos_sp_offs(1) /= _params.sp_offs_max(1); + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); } if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2); + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; - add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); - _alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2); - } - - /* fill position setpoint triplet */ - _pos_sp_triplet.previous.valid = true; - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.next.valid = true; - - _pos_sp_triplet.nav_state = NAV_STATE_NONE; - _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; - _pos_sp_triplet.current.lat = _lat_sp; - _pos_sp_triplet.current.lon = _lon_sp; - _pos_sp_triplet.current.alt = _alt_sp; - _pos_sp_triplet.current.yaw = _att_sp.yaw_body; - _pos_sp_triplet.current.loiter_radius = 0.0f; - _pos_sp_triplet.current.loiter_direction = 1.0f; - _pos_sp_triplet.current.pitch_min = 0.0f; - - /* publish position setpoint triplet */ - if (_pos_sp_triplet_pub > 0) { - orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); - - } else { - _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); } } else { - /* always use AMSL altitude for AUTO */ - select_alt(true); - /* AUTO */ bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -686,13 +658,14 @@ MulticopterPositionControl::task_main() if (_pos_sp_triplet.current.valid) { /* in case of interrupted mission don't go to waypoint but stay at current position */ - _reset_lat_lon_sp = true; + _reset_pos_sp = true; _reset_alt_sp = true; - /* update position setpoint */ - _lat_sp = _pos_sp_triplet.current.lat; - _lon_sp = _pos_sp_triplet.current.lon; - _alt_sp = _pos_sp_triplet.current.alt; + /* project setpoint to local frame */ + map_projection_project(&_ref_pos, + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); /* update yaw setpoint if needed */ if (isfinite(_pos_sp_triplet.current.yaw)) { @@ -701,11 +674,25 @@ MulticopterPositionControl::task_main() } else { /* no waypoint, loiter, reset position setpoint if needed */ - reset_lat_lon_sp(); + reset_pos_sp(); reset_alt_sp(); } } + /* fill local position setpoint */ + _local_pos_sp.x = _pos_sp(0); + _local_pos_sp.y = _pos_sp(1); + _local_pos_sp.z = _pos_sp(2); + _local_pos_sp.yaw = _att_sp.yaw_body; + + /* publish local position setpoint */ + if (_local_pos_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + + } else { + _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); + } + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); @@ -729,9 +716,7 @@ MulticopterPositionControl::task_main() } else { /* run position & altitude controllers, calculate velocity setpoint */ - math::Vector<3> pos_err; - get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]); - pos_err(2) = -(_alt_sp - alt); + math::Vector<3> pos_err = _pos_sp - _pos; _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); @@ -741,7 +726,7 @@ MulticopterPositionControl::task_main() } if (!_control_mode.flag_control_position_enabled) { - _reset_lat_lon_sp = true; + _reset_pos_sp = true; _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } @@ -1021,7 +1006,7 @@ MulticopterPositionControl::task_main() } else { /* position controller disabled, reset setpoints */ _reset_alt_sp = true; - _reset_lat_lon_sp = true; + _reset_pos_sp = true; reset_int_z = true; reset_int_xy = true; } From 22c8d91389d2adea9595b4ed2f09c6c72035ae6c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Mar 2014 22:46:48 +0400 Subject: [PATCH 04/53] position_estimator_inav: mark local position as valid even if GPS not available (e.g. only FLOW) --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index eddf6e94ea..f3b9b9d859 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -810,7 +810,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t > pub_last + pub_interval) { pub_last = t; /* publish local position */ - local_pos.xy_valid = can_estimate_xy && use_gps_xy; + local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; local_pos.xy_global = local_pos.xy_valid && use_gps_xy; local_pos.z_global = local_pos.z_valid && use_gps_z; From 2f7303f2ddebedacaa92b287ee70ecea0e2d5baf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Mar 2014 23:56:49 +0400 Subject: [PATCH 05/53] geo lib: minor code style fix --- src/lib/geo/geo.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index fc4e5cd1b5..1588a5346a 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -79,7 +79,7 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou double k = (c == 0.0) ? 1.0 : (c / sin(c)); *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; - *y = k * (cos_lat * sin(lon_rad - ref->lon)) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH; } __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) From c266124099fe67dcff5d5f3deeef37acebdc1695 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Mar 2014 23:58:00 +0400 Subject: [PATCH 06/53] vehicle_local_position: use double for ref_lat and ref_lon instead of int32, fix related apps --- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 4 ++-- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- src/modules/sdlog2/sdlog2.c | 4 ++-- src/modules/uORB/topics/vehicle_local_position.h | 4 ++-- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index a4d5560c71..03e6021dc2 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -341,8 +341,8 @@ void KalmanNav::updatePublications() _localPos.xy_global = true; _localPos.z_global = true; _localPos.ref_timestamp = _pubTimeStamp; - _localPos.ref_lat = getLatDegE7(); - _localPos.ref_lon = getLonDegE7(); + _localPos.ref_lat = lat * M_RAD_TO_DEG; + _localPos.ref_lon = lon * M_RAD_TO_DEG; _localPos.ref_alt = 0; _localPos.landed = landed; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 624740237c..d297be10ac 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -670,8 +670,8 @@ handle_message(mavlink_message_t *msg) hil_local_pos.xy_global = true; hil_local_pos.z_global = true; hil_local_pos.ref_timestamp = timestamp; - hil_local_pos.ref_lat = hil_state.lat; - hil_local_pos.ref_lon = hil_state.lon; + hil_local_pos.ref_lat = hil_state.lat / 1e7d; + hil_local_pos.ref_lon = hil_state.lon / 1e7d; hil_local_pos.ref_alt = alt0; hil_local_pos.landed = landed; orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 3d05b37d81..138b9e46ea 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -576,13 +576,13 @@ MulticopterPositionControl::task_main() was_armed = _control_mode.flag_armed; + update_ref(); + if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - update_ref(); - _pos(0) = _local_pos.x; _pos(1) = _local_pos.y; _pos(2) = _local_pos.z; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index f3b9b9d859..7be5ae9794 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -556,8 +556,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; - local_pos.ref_lat = gps.lat; - local_pos.ref_lon = gps.lon; + local_pos.ref_lat = lat; + local_pos.ref_lon = lon; local_pos.ref_alt = alt + z_est[0]; local_pos.ref_timestamp = t; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2514bafeee..24eed228b0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1085,8 +1085,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; - log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; + log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7; + log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index d567f2e020..aeaf1e2449 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -73,8 +73,8 @@ struct vehicle_local_position_s bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ uint64_t ref_timestamp; /**< Time when reference position was set */ - int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ - int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ + double ref_lat; /**< Reference point latitude in degrees */ + double ref_lon; /**< Reference point longitude in degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ bool landed; /**< true if vehicle is landed */ /* Distance to surface */ From 068b7526b74c9bbcc31acc28f0d578ed9c0f97b1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Mar 2014 00:10:38 +0400 Subject: [PATCH 07/53] copyright and code style fixes --- src/lib/geo/geo.c | 76 ++++++++++++------- src/lib/geo/geo.h | 24 +++--- .../mc_pos_control/mc_pos_control_main.cpp | 54 ++++++++----- .../position_estimator_inav_main.c | 22 ++++-- .../uORB/topics/vehicle_local_position.h | 4 +- 5 files changed, 113 insertions(+), 67 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 1588a5346a..abed7eb1f5 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Lorenz Meier + * 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 * @author Julian Oes * @author Lorenz Meier + * @author Anton Babushkin */ #include @@ -142,7 +140,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub 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 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); } -__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 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 -__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 // 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; // 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); @@ -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, - float radius, float arc_start_bearing, float arc_sweep) +__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) { // 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 @@ -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) { @@ -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, - double lat_next, double lon_next, float alt_next, - float *dist_xy, float *dist_z) + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z) { double current_x_rad = lat_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, - float x_next, float y_next, float z_next, - float *dist_xy, float *dist_z) + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z) { float dx = x_now - x_next; float dy = y_now - y_next; @@ -375,17 +373,23 @@ __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,17 +403,23 @@ __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,17 +433,23 @@ __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,17 +463,23 @@ __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; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index a66bd10e4d..0a3f85d970 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Lorenz Meier + * 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 * @author Julian Oes * @author Lorenz Meier + * @author Anton Babushkin * Additional functions - @author Doug Weibel */ @@ -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 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 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, - float radius, float arc_start_bearing, float arc_sweep); +__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); /* * Calculate distance in global frame */ __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, - float *dist_xy, float *dist_z); + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z); /* * Calculate distance in local frame (NED) */ __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 *dist_xy, float *dist_z); + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z); __EXPORT float _wrap_180(float bearing); __EXPORT float _wrap_360(float bearing); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 138b9e46ea..97357d07a9 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin + * 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 */ #include @@ -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, ¶m_upd); + } if (updated || force) { param_get(_params_handles.thr_min, &_params.thr_min); @@ -412,33 +414,39 @@ 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 @@ -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 */ @@ -663,8 +673,8 @@ MulticopterPositionControl::task_main() /* project setpoint to local frame */ map_projection_project(&_ref_pos, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, - &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); /* update yaw setpoint if needed */ @@ -832,8 +842,9 @@ 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 */ @@ -924,8 +935,9 @@ 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 */ @@ -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; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 7be5ae9794..fc394fda66 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin + * 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 */ #include @@ -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; diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index aeaf1e2449..9e9a4519e2 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier * * 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 + * @author Anton Babushkin */ #ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_ From 712c72d25bc595d879c2592e0a750bb0a981120f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 21 Mar 2014 12:52:27 +0400 Subject: [PATCH 08/53] Optical flow fixes --- src/modules/mavlink/mavlink_receiver.cpp | 1 + .../position_estimator_inav_main.c | 11 ++++++++--- .../position_estimator_inav_params.c | 2 +- src/modules/uORB/topics/optical_flow.h | 1 + 4 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0816814a16..6eec25e4bf 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -245,6 +245,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) memset(&f, 0, sizeof(f)); f.timestamp = hrt_absolute_time(); + f.flow_timestamp = flow.time_usec; f.flow_raw_x = flow.flow_x; f.flow_raw_y = flow.flow_y; f.flow_comp_x_m = flow.flow_comp_m_x; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index caf2f840c2..15a88066f8 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -248,6 +248,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_flow = 0.0f; float sonar_prev = 0.0f; + hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) hrt_abstime xy_src_time = 0; // time of last available position data @@ -434,6 +435,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + /* calculate time from previous update */ + float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; + flow_prev = flow.flow_timestamp; + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; @@ -484,10 +489,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; - /* convert raw flow to angular flow */ + /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k; - flow_ang[1] = flow.flow_raw_y * params.flow_k; + flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index b71f9472f5..6892ac4963 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); -PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h index 98f0e3fa2a..0196ae86b0 100644 --- a/src/modules/uORB/topics/optical_flow.h +++ b/src/modules/uORB/topics/optical_flow.h @@ -57,6 +57,7 @@ struct optical_flow_s { uint64_t timestamp; /**< in microseconds since system start */ + uint64_t flow_timestamp; /**< timestamp from flow sensor */ int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ From e2305d93bd52fb86fde24fb331552483bb25dd7b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 24 Mar 2014 13:44:42 +0400 Subject: [PATCH 09/53] position_estimator_inav: use home position as local projection reference --- .../position_estimator_inav_main.c | 72 +++++++++++++++---- 1 file changed, 57 insertions(+), 15 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 15a88066f8..4f71471675 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -215,6 +216,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix struct map_projection_reference_s ref; memset(&ref, 0, sizeof(ref)); + hrt_abstime home_timestamp = 0; uint16_t accel_updates = 0; uint16_t baro_updates = 0; @@ -267,6 +269,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); + struct home_position_s home; + memset(&home, 0, sizeof(home)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; @@ -284,6 +288,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + int home_position_sub = orb_subscribe(ORB_ID(home_position)); /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); @@ -531,29 +536,56 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } + /* home position */ + orb_check(home_position_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(home_position), home_position_sub, &home); + + if (home.timestamp != home_timestamp) { + home_timestamp = home.timestamp; + if (ref_inited) { + ref_inited = true; + + /* reproject position estimate to new reference */ + float dx, dy; + map_projection_project(&ref, home.lat, home.lon, &dx, &dy); + est_x[0] -= dx; + est_y[0] -= dx; + est_z[0] += home.alt - local_pos.ref_alt; + } + + /* update baro offset */ + baro_offset -= home.alt - local_pos.ref_alt; + + /* update reference */ + map_projection_init(&ref, home.lat, home.lon); + + local_pos.ref_lat = home.lat; + local_pos.ref_lon = home.lon; + local_pos.ref_alt = home.alt; + local_pos.ref_timestamp = home.timestamp; + } + } + /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3) { - /* hysteresis for GPS quality */ - if (gps_valid) { - if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) { - gps_valid = false; - mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); - } - - } else { - if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) { - gps_valid = true; - mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); - } + /* hysteresis for GPS quality */ + if (gps_valid) { + if (gps.eph_m > 10.0f || gps.epv_m > 20.0f || gps.fix_type < 3) { + gps_valid = false; + mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - gps_valid = false; + if (gps.eph_m < 5.0f && gps.epv_m < 10.0f && gps.fix_type >= 3) { + gps_valid = true; + mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); + } } if (gps_valid) { @@ -569,9 +601,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; + /* update baro offset */ + baro_offset -= z_est[0]; + + /* set position estimate to (0, 0, 0), use GPS velocity for XY */ + x_est[0] = 0.0f; + x_est[1] = gps.vel_n_m_s; + y_est[0] = 0.0f; + y_est[1] = gps.vel_e_m_s; + z_est[0] = 0.0f; + local_pos.ref_lat = lat; local_pos.ref_lon = lon; - local_pos.ref_alt = alt + z_est[0]; + local_pos.ref_alt = alt; local_pos.ref_timestamp = t; /* initialize projection */ From 83da4ae02dfc61d6a7f80ae40660826fbbca81be Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Mar 2014 00:27:11 +0400 Subject: [PATCH 10/53] 'vehicle_global_position' topic updated: removed baro_alt and XXX_valid flags. --- src/drivers/frsky_telemetry/frsky_data.c | 2 +- .../att_pos_estimator_ekf/KalmanNav.cpp | 1 - .../attitude_estimator_ekf_main.cpp | 2 +- src/modules/commander/commander.cpp | 73 ++++++++----------- src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/navigator/navigator_main.cpp | 17 +++-- .../position_estimator_inav_main.c | 52 ++++++------- src/modules/sdlog2/sdlog2.c | 4 +- src/modules/sdlog2/sdlog2_messages.h | 6 +- .../uORB/topics/vehicle_global_position.h | 7 +- 10 files changed, 77 insertions(+), 90 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index cfcf91e3fb..57a03bc844 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -225,7 +225,7 @@ void frsky_send_frame2(int uart) float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; int sec = 0; - if (global_pos.global_valid) { + if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { time_t time_gps = global_pos.time_gps_usec / 1000000; struct tm *tm_gps = gmtime(&time_gps); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 03e6021dc2..5cf84542b1 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -312,7 +312,6 @@ void KalmanNav::updatePublications() // global position publication _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; - _pos.global_valid = true; _pos.lat = lat * M_RAD_TO_DEG; _pos.lon = lon * M_RAD_TO_DEG; _pos.alt = float(alt); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 10a6cd2c5c..c61b6ff3fe 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -407,7 +407,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + } else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d114a2e5ce..7da062961c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ +#define POSITION_TIMEOUT 20000 /**< consider the local or global position estimate invalid after 20ms */ #define RC_TIMEOUT 100000 #define DIFFPRESS_TIMEOUT 2000000 @@ -919,7 +919,37 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, true, &(status.condition_global_position_valid), &status_changed); + + /* check if GPS fix is ok */ + static float hdop_threshold_m = 4.0f; + static float vdop_threshold_m = 8.0f; + + /* update home position */ + if (!status.condition_home_position_valid && updated && + (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m) && + (hrt_absolute_time() < global_position.timestamp + POSITION_TIMEOUT) && !armed.armed) { + + /* copy position data to uORB home message, store it locally as well */ + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; + + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + status.condition_home_position_valid = true; + tune_positive(true); + } /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -1067,45 +1097,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - /* check if GPS fix is ok */ - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.global_valid) { - - /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - status.condition_home_position_valid = true; - tune_positive(true); - } } /* start RC input check */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6eec25e4bf..2c9cdbf247 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -765,7 +765,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) memset(&hil_global_pos, 0, sizeof(hil_global_pos)); hil_global_pos.timestamp = timestamp; - hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; @@ -773,6 +772,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_e = hil_state.vy / 100.0f; hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.yaw = hil_attitude.yaw; + hil_global_pos.eph = 2.0f; + hil_global_pos.epv = 4.0f; if (_global_pos_pub > 0) { orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1ba..ef7201790e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -177,7 +177,7 @@ private: class Mission _mission; bool _mission_item_valid; /**< current mission item valid */ - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ + bool _global_pos_valid; /**< track changes of global_position */ bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -817,13 +817,11 @@ Navigator::task_main() if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { _pos_sp_triplet_updated = true; - if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { + if (myState == NAV_STATE_LAND && !_global_pos_valid) { /* got global position when landing, update setpoint */ start_land(); } - _global_pos_valid = _global_pos.global_valid; - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { if (check_mission_item_reached()) { @@ -846,8 +844,15 @@ Navigator::task_main() /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } + + _global_pos_valid = true; + + } else { + /* assume that global position is valid if updated in last 20ms */ + _global_pos_valid = _global_pos.timestamp != 0 && hrt_abstime() < _global_pos.timestamp + 20000; } + /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { _pos_sp_triplet_updated = false; @@ -893,9 +898,9 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); + warnx("Global position: %svalid", _global_pos_valid ? "" : "in"); - if (_global_pos.global_valid) { + if (_global_pos_valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 4f71471675..3f1a5d39b3 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -292,7 +292,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); - orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + orb_advert_t vehicle_global_position_pub = -1; struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; @@ -340,7 +340,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; - global_pos.baro_valid = true; } } } @@ -550,9 +549,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* reproject position estimate to new reference */ float dx, dy; map_projection_project(&ref, home.lat, home.lon, &dx, &dy); - est_x[0] -= dx; - est_y[0] -= dx; - est_z[0] += home.alt - local_pos.ref_alt; + x_est[0] -= dx; + y_est[0] -= dx; + z_est[0] += home.alt - local_pos.ref_alt; } /* update baro offset */ @@ -888,40 +887,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); - /* publish global position */ - global_pos.global_valid = local_pos.xy_global; + if (local_pos.xy_global && local_pos.z_global) { + /* publish global position */ + global_pos.timestamp = t; + global_pos.time_gps_usec = gps.time_gps_usec; - if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = est_lat; global_pos.lon = est_lon; - global_pos.time_gps_usec = gps.time_gps_usec; - } + global_pos.alt = local_pos.ref_alt - local_pos.z; - /* set valid values even if position is not valid */ - if (local_pos.v_xy_valid) { global_pos.vel_n = local_pos.vx; global_pos.vel_e = local_pos.vy; - } - - if (local_pos.z_global) { - global_pos.alt = local_pos.ref_alt - local_pos.z; - } - - if (local_pos.z_valid) { - global_pos.baro_alt = baro_offset - local_pos.z; - } - - if (local_pos.v_z_valid) { global_pos.vel_d = local_pos.vz; + + global_pos.yaw = local_pos.yaw; + + // TODO implement dead-reckoning + global_pos.eph = gps.eph_m; + global_pos.epv = gps.epv_m; + + if (vehicle_global_position_pub < 0) { + vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + + } else { + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + } } - - global_pos.yaw = local_pos.yaw; - - global_pos.timestamp = t; - - orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c7073eb944..36d309d6c9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1126,8 +1126,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; - log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; - log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); + log_msg.body.log_GPOS.eph = buf.global_pos.eph; + log_msg.body.log_GPOS.epv = buf.global_pos.epv; LOGBUFFER_WRITE_AND_COUNT(GPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index e27518aa0a..fbfca76f7d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -204,8 +204,8 @@ struct log_GPOS_s { float vel_n; float vel_e; float vel_d; - float baro_alt; - uint8_t flags; + float eph; + float epv; }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ @@ -317,7 +317,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), + LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index ff9e98e1c4..5c54630e29 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -63,9 +63,6 @@ struct vehicle_global_position_s { uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - bool global_valid; /**< true if position satisfies validity criteria of estimator */ - bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ @@ -74,8 +71,8 @@ struct vehicle_global_position_s float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - - float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ + float eph; + float epv; }; /** From fdb17c9776d573c46358684a6c6bd19afd2e1df2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 11:31:30 +0400 Subject: [PATCH 11/53] mc_pos_control: reproject local position setpoint on local reference updates --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 97357d07a9..21070e1e9b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -473,8 +473,15 @@ void MulticopterPositionControl::update_ref() { if (_local_pos.ref_timestamp != _ref_timestamp) { + if (_ref_timestamp != 0) { + /* reproject local position setpoint to new reference */ + float dx, dy; + map_projection_project(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon, &dx, &dy); + _pos_sp(0) -= dx; + _pos_sp(1) -= dy; + } + _ref_timestamp = _local_pos.ref_timestamp; - // TODO mode position setpoint in assisted modes map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); _ref_alt = _local_pos.ref_alt; From 63cd319ff73ddf6bcbf15d3e35afd5df7b58d72e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 11:57:41 +0400 Subject: [PATCH 12/53] commander: set home position on arming --- src/modules/commander/commander.cpp | 36 ++++++++++++++++++++++++----- 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bc63c810b9..410906c26f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -612,6 +612,7 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = false; bool arm_tune_played = false; + bool was_armed = false; /* set parameters */ param_t _param_sys_type = param_find("MAV_TYPE"); @@ -927,17 +928,15 @@ int commander_thread_main(int argc, char *argv[]) static float vdop_threshold_m = 8.0f; /* update home position */ - if (!status.condition_home_position_valid && updated && - (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m) && - (hrt_absolute_time() < global_position.timestamp + POSITION_TIMEOUT) && !armed.armed) { + if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && + (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) { - /* copy position data to uORB home message, store it locally as well */ home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { @@ -1284,7 +1283,32 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); + + /* update home position on arming */ + if (armed.armed && !was_armed && status.condition_global_position_valid && + (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) { + + // TODO remove code duplication + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; + + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + status.condition_home_position_valid = true; + } } + was_armed = armed.armed; if (main_state_changed) { status_changed = true; From b1d39e65a61ec17d2da30ad37068758ab23d3ba3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 15:36:11 +0400 Subject: [PATCH 13/53] commander: position timeout increased to 30ms --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 410906c26f..1e072678be 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 20000 /**< consider the local or global position estimate invalid after 20ms */ +#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */ #define RC_TIMEOUT 100000 #define RC_TIMEOUT_HIL 500000 #define DIFFPRESS_TIMEOUT 2000000 From 553b122830615b1617570900cf5f5d0c04720c8b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 16:53:22 +0400 Subject: [PATCH 14/53] caommander: setting home position by command implemented --- src/modules/commander/commander.cpp | 51 +++++++++++++++++++++++++++-- 1 file changed, 48 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1e072678be..a896f61462 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -195,7 +195,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub); /** * Mainloop of commander. @@ -390,7 +390,7 @@ int disarm() } } -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* result of the command */ enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -580,6 +580,51 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + case VEHICLE_CMD_DO_SET_HOME: { + bool use_current = cmd->param1 > 0.5f; + if (use_current) { + /* use current position */ + if (status->condition_global_position_valid) { + home->lat = global_pos->lat; + home->lon = global_pos->lon; + home->alt = global_pos->alt; + + home->timestamp = hrt_absolute_time(); + + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + } else { + /* use specified position */ + home->lat = cmd->param5; + home->lon = cmd->param6; + home->alt = cmd->param7; + + home->timestamp = hrt_absolute_time(); + + result = VEHICLE_CMD_RESULT_ACCEPTED; + } + + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); + + /* announce new home position */ + if (*home_pub > 0) { + orb_publish(ORB_ID(home_position), *home_pub, &home); + + } else { + *home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + status->condition_home_position_valid = true; + } + } + break; case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -1268,7 +1313,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed)) + if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) status_changed = true; } From 93617c4073d560ec2a804d728a2830534a74a50a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 17:09:18 +0400 Subject: [PATCH 15/53] commander: set home position on arming only if at least 2 s from commander start spent --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a896f61462..7257cb4b31 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1329,8 +1329,8 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); - /* update home position on arming */ - if (armed.armed && !was_armed && status.condition_global_position_valid && + /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) { // TODO remove code duplication From e9f45a82b8ee48caa7eecd2371e8dedda87ec2c4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 17:20:37 +0400 Subject: [PATCH 16/53] fw_att_pos_estimator: map_projection_XXX usage fixed, vehicle_global_position topic publication fixed --- .../fw_att_pos_estimator_main.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index c9d75bce49..22d321907e 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -167,6 +167,8 @@ private: struct sensor_combined_s _sensor_combined; #endif + struct map_projection_reference_s _pos_ref; + float _baro_ref; /**< barometer reference altitude */ float _baro_gps_offset; /**< offset between GPS and baro */ @@ -803,7 +805,7 @@ FixedwingEstimator::task_main() _baro_gps_offset = _baro_ref - _local_pos.ref_alt; // XXX this is not multithreading safe - map_projection_init(lat, lon); + map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); _gps_initialized = true; @@ -1029,18 +1031,14 @@ FixedwingEstimator::task_main() _global_pos.timestamp = _local_pos.timestamp; - _global_pos.baro_valid = true; - _global_pos.global_valid = true; - if (_local_pos.xy_global) { double est_lat, est_lon; - map_projection_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon); + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; } - /* set valid values even if position is not valid */ if (_local_pos.v_xy_valid) { _global_pos.vel_n = _local_pos.vx; _global_pos.vel_e = _local_pos.vy; @@ -1052,16 +1050,15 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); - if (_local_pos.z_valid) { - _global_pos.baro_alt = _local_pos.ref_alt - _baro_gps_offset - _local_pos.z; - } - if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; } _global_pos.yaw = _local_pos.yaw; + _global_pos.eph = _gps.eph_m; + _global_pos.epv = _gps.epv_m; + _global_pos.timestamp = _local_pos.timestamp; /* lazily publish the global position only once available */ From 4e6a5ed1e8563a9fc0ac148adee383ea50e7182a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 21:44:59 +0400 Subject: [PATCH 17/53] navigator: use vehicle_status flag to decide if global position is valid --- src/modules/navigator/navigator_main.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ef7201790e..810ef457f1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -844,14 +844,9 @@ Navigator::task_main() /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } - - _global_pos_valid = true; - - } else { - /* assume that global position is valid if updated in last 20ms */ - _global_pos_valid = _global_pos.timestamp != 0 && hrt_abstime() < _global_pos.timestamp + 20000; } + _global_pos_valid = _vstatus.condition_global_position_valid; /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { From 367ce63b86b863d72a3f6b4250d9da780a85f40b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 11:45:57 +0400 Subject: [PATCH 18/53] 'signal_lost' flag added to manual_control_setpoint and rc_channels topics to indicate signal loss immediately --- src/modules/commander/commander.cpp | 13 +- src/modules/sensors/sensors.cpp | 250 +++++++++--------- .../uORB/topics/manual_control_setpoint.h | 2 + src/modules/uORB/topics/rc_channels.h | 1 + 4 files changed, 136 insertions(+), 130 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e2..edd77231dc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -118,8 +118,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ -#define RC_TIMEOUT 100000 -#define RC_TIMEOUT_HIL 500000 +#define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[]) } } - - /* - * XXX workaround: - * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) - * which can trigger RC loss if the computer/simulator lags. - */ - uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; - /* start RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) { + if (!status.rc_input_blocked && !sp_man.signal_lost && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f890c4c7f8..9f816f5e1b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -474,6 +474,7 @@ Sensors::Sensors() : _battery_discharged(0), _battery_current_timestamp(0) { + memset(&_rc, 0, sizeof(_rc)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1283,12 +1284,6 @@ Sensors::rc_poll() if (rc_updated) { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct rc_input_values rc_input; - - orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - - if (rc_input.rc_lost) - return; - struct manual_control_setpoint_s manual_control; struct actuator_controls_s actuator_group_3; @@ -1311,19 +1306,28 @@ Sensors::rc_poll() manual_control.aux4 = NAN; manual_control.aux5 = NAN; - /* require at least four channels to consider the signal valid */ - if (rc_input.channel_count < 4) - return; + manual_control.signal_lost = true; - /* failsafe check */ - if (_parameters.rc_fs_ch != 0) { - if (_parameters.rc_fs_mode == 0) { - if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) - return; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - } else if (_parameters.rc_fs_mode == 1) { - if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) - return; + /* detect RC signal loss */ + /* check flags and require at least four channels to consider the signal valid */ + if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { + /* signal looks good */ + manual_control.signal_lost = false; + + /* check throttle failsafe */ + if (_parameters.rc_fs_ch != 0) { + if (_parameters.rc_fs_mode == 0) { + if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) { + manual_control.signal_lost = true; + } + + } else if (_parameters.rc_fs_mode == 1) { + if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) { + manual_control.signal_lost = true; + } + } } } @@ -1332,10 +1336,7 @@ Sensors::rc_poll() if (channel_limit > _rc_max_chan_count) channel_limit = _rc_max_chan_count; - /* we are accepting this message */ - _rc_last_valid = rc_input.timestamp_last_signal; - - /* Read out values from raw message */ + /* read out and scale values from raw message even if signal is invalid */ for (unsigned int i = 0; i < channel_limit; i++) { /* @@ -1382,105 +1383,124 @@ Sensors::rc_poll() } _rc.chan_count = rc_input.channel_count; - _rc.timestamp = rc_input.timestamp_last_signal; + _rc.rssi = rc_input.rssi; + _rc.signal_lost = manual_control.signal_lost; - manual_control.timestamp = rc_input.timestamp_last_signal; - - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; - - /* scale output */ - if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { - manual_control.roll *= _parameters.rc_scale_roll; + if (!manual_control.signal_lost) { + _rc_last_valid = rc_input.timestamp_last_signal; } - if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } + _rc.timestamp = _rc_last_valid; - if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } + manual_control.timestamp = _rc_last_valid; - /* flaps */ - if (_rc.function[FLAPS] >= 0) { + if (!manual_control.signal_lost) { + /* fill values in manual_control_setpoint topic only if signal is valid */ - manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); + /* + * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, + * so reverse sign. + */ + manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); + /* throttle input */ + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { - manual_control.flaps *= _parameters.rc_scale_flaps; + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + + /* scale output */ + if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { + manual_control.roll *= _parameters.rc_scale_roll; + } + + if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } + + if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { + manual_control.yaw *= _parameters.rc_scale_yaw; + } + + /* flaps */ + if (_rc.function[FLAPS] >= 0) { + + manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); + + if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { + manual_control.flaps *= _parameters.rc_scale_flaps; + } + } + + /* mode switch input */ + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + } + + /* assisted switch input */ + if (_rc.function[ASSISTED] >= 0) { + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + } + + /* mission switch input */ + if (_rc.function[MISSION] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); + } + + /* return switch input */ + if (_rc.function[RETURN] >= 0) { + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + } + + // if (_rc.function[OFFBOARD_MODE] >= 0) { + // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + // } + + /* aux functions, only assign if valid mapping is present */ + if (_rc.function[AUX_1] >= 0) { + manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); + } + + if (_rc.function[AUX_2] >= 0) { + manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); + } + + if (_rc.function[AUX_3] >= 0) { + manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); + } + + if (_rc.function[AUX_4] >= 0) { + manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); + } + + if (_rc.function[AUX_5] >= 0) { + manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); + } + + /* copy from mapped manual control to control group 3 */ + actuator_group_3.control[0] = manual_control.roll; + actuator_group_3.control[1] = manual_control.pitch; + actuator_group_3.control[2] = manual_control.yaw; + actuator_group_3.control[3] = manual_control.throttle; + actuator_group_3.control[4] = manual_control.flaps; + actuator_group_3.control[5] = manual_control.aux1; + actuator_group_3.control[6] = manual_control.aux2; + actuator_group_3.control[7] = manual_control.aux3; + + /* publish actuator_controls_3 topic only if control signal is valid */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); } } - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } - - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } - - /* mission switch input */ - if (_rc.function[MISSION] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } - - /* return switch input */ - if (_rc.function[RETURN] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } - -// if (_rc.function[OFFBOARD_MODE] >= 0) { -// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); -// } - - /* aux functions, only assign if valid mapping is present */ - if (_rc.function[AUX_1] >= 0) { - manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); - } - - if (_rc.function[AUX_2] >= 0) { - manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); - } - - if (_rc.function[AUX_3] >= 0) { - manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); - } - - if (_rc.function[AUX_4] >= 0) { - manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); - } - - if (_rc.function[AUX_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); - } - - /* copy from mapped manual control to control group 3 */ - actuator_group_3.control[0] = manual_control.roll; - actuator_group_3.control[1] = manual_control.pitch; - actuator_group_3.control[2] = manual_control.yaw; - actuator_group_3.control[3] = manual_control.throttle; - actuator_group_3.control[4] = manual_control.flaps; - actuator_group_3.control[5] = manual_control.aux1; - actuator_group_3.control[6] = manual_control.aux2; - actuator_group_3.control[7] = manual_control.aux3; - - /* check if ready for publishing */ + /* publish rc_channels topic even if signal is invalid, for debug */ if (_rc_pub > 0) { orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); @@ -1489,21 +1509,13 @@ Sensors::rc_poll() _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); } - /* check if ready for publishing */ + /* publish manual_control_setpoint topic even if signal is not valid to update 'signal_lost' flag */ if (_manual_control_pub > 0) { orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); } else { _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); } - - /* check if ready for publishing */ - if (_actuator_group_3_pub > 0) { - orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); - - } else { - _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); - } } } diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e986..5d13843800 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -51,6 +51,8 @@ struct manual_control_setpoint_s { uint64_t timestamp; + bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ + float roll; /**< ailerons roll / roll rate input */ float pitch; /**< elevator / pitch / pitch rate */ float yaw; /**< rudder / yaw rate / yaw */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 6eb20efd1b..3246a39dd6 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -94,6 +94,7 @@ struct rc_channels_s { char function_name[RC_CHANNELS_FUNCTION_MAX][20]; int8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ + bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ }; /**< radio control channels. */ /** From 2c4792d48ee98cf46d9f9cf8ec43a759d6cc15d0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 11:46:21 +0400 Subject: [PATCH 19/53] sdlog2: added 'signal_lost' logging --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 13981ac544..112a4bd3ef 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1176,6 +1176,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); log_msg.body.log_RC.channel_count = buf.rc.chan_count; + log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; LOGBUFFER_WRITE_AND_COUNT(RC); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 2b41755de9..de12e623a6 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -161,6 +161,7 @@ struct log_STAT_s { struct log_RC_s { float channel[8]; uint8_t channel_count; + uint8_t signal_lost; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ @@ -323,7 +324,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), - LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), + LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), From 1d5f62d890d1d85cef5e0f8e282d8e9e70717d46 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 17:26:07 +0400 Subject: [PATCH 20/53] sensors: use enum for switches position and -1..1 for values in 'manual_control_setpoint' topic --- src/modules/sensors/sensors.cpp | 163 ++++++++---------- .../uORB/topics/manual_control_setpoint.h | 37 ++-- 2 files changed, 92 insertions(+), 108 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9f816f5e1b..e08d8618f0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -135,7 +135,7 @@ */ #define PCB_TEMP_ESTIMATE_DEG 5.0f -#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) +#define STICK_ON_OFF_LIMIT 0.75f /** * Sensor app start / stop handling function @@ -169,6 +169,16 @@ private: hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ + /** + * Get and limit value for specified RC function. Returns NAN if not mapped. + */ + float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value); + + /** + * Get switch position for specified function. + */ + switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func); + /** * Gather and publish RC input data. */ @@ -1275,6 +1285,45 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } +float +Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value < min_value) { + return min_value; + + } else if (value > max_value) { + return max_value; + + } else { + return value; + } + } else { + return NAN; + } +} + +switch_pos_t +Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value > STICK_ON_OFF_LIMIT) { + return SWITCH_POS_ON; + + } else if (value < STICK_ON_OFF_LIMIT) { + return SWITCH_POS_OFF; + + } else { + return SWITCH_POS_MIDDLE; + } + + } else { + return SWITCH_POS_NONE; + } +} + void Sensors::rc_poll() { @@ -1292,13 +1341,6 @@ Sensors::rc_poll() manual_control.pitch = NAN; manual_control.yaw = NAN; manual_control.throttle = NAN; - - manual_control.mode_switch = NAN; - manual_control.return_switch = NAN; - manual_control.assisted_switch = NAN; - manual_control.mission_switch = NAN; -// manual_control.auto_offboard_input_switch = NAN; - manual_control.flaps = NAN; manual_control.aux1 = NAN; manual_control.aux2 = NAN; @@ -1306,6 +1348,11 @@ Sensors::rc_poll() manual_control.aux4 = NAN; manual_control.aux5 = NAN; + manual_control.mode_switch = SWITCH_POS_NONE; + manual_control.return_switch = SWITCH_POS_NONE; + manual_control.assisted_switch = SWITCH_POS_NONE; + manual_control.mission_switch = SWITCH_POS_NONE; + manual_control.signal_lost = true; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); @@ -1316,7 +1363,7 @@ Sensors::rc_poll() /* signal looks good */ manual_control.signal_lost = false; - /* check throttle failsafe */ + /* check for throttle failsafe */ if (_parameters.rc_fs_ch != 0) { if (_parameters.rc_fs_mode == 0) { if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) { @@ -1397,89 +1444,23 @@ Sensors::rc_poll() if (!manual_control.signal_lost) { /* fill values in manual_control_setpoint topic only if signal is valid */ - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + /* limit controls */ + manual_control.roll = get_rc_value(ROLL, -1.0, 1.0); + manual_control.pitch = get_rc_value(PITCH, -1.0, 1.0); + manual_control.yaw = get_rc_value(YAW, -1.0, 1.0); + manual_control.throttle = get_rc_value(THROTTLE, 0.0, 1.0); + manual_control.flaps = get_rc_value(FLAPS, -1.0, 1.0); + manual_control.aux1 = get_rc_value(AUX_1, -1.0, 1.0); + manual_control.aux2 = get_rc_value(AUX_2, -1.0, 1.0); + manual_control.aux3 = get_rc_value(AUX_3, -1.0, 1.0); + manual_control.aux4 = get_rc_value(AUX_4, -1.0, 1.0); + manual_control.aux5 = get_rc_value(AUX_5, -1.0, 1.0); - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; - - /* scale output */ - if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { - manual_control.roll *= _parameters.rc_scale_roll; - } - - if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } - - if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - - /* flaps */ - if (_rc.function[FLAPS] >= 0) { - - manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); - - if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { - manual_control.flaps *= _parameters.rc_scale_flaps; - } - } - - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } - - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } - - /* mission switch input */ - if (_rc.function[MISSION] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } - - /* return switch input */ - if (_rc.function[RETURN] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } - - // if (_rc.function[OFFBOARD_MODE] >= 0) { - // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); - // } - - /* aux functions, only assign if valid mapping is present */ - if (_rc.function[AUX_1] >= 0) { - manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); - } - - if (_rc.function[AUX_2] >= 0) { - manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); - } - - if (_rc.function[AUX_3] >= 0) { - manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); - } - - if (_rc.function[AUX_4] >= 0) { - manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); - } - - if (_rc.function[AUX_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); - } + /* mode switches */ + manual_control.mode_switch = get_rc_switch_position(MODE); + manual_control.assisted_switch = get_rc_switch_position(ASSISTED); + manual_control.mission_switch = get_rc_switch_position(MISSION); + manual_control.return_switch = get_rc_switch_position(RETURN); /* copy from mapped manual control to control group 3 */ actuator_group_3.control[0] = manual_control.roll; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 5d13843800..985d3923fb 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -43,6 +43,16 @@ #include #include "../uORB.h" +/** + * Switch position + */ +typedef enum { + SWITCH_POS_NONE = 0, /**< switch is not mapped */ + SWITCH_POS_ON, /**< switch activated (value = 1) */ + SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ + SWITCH_POS_OFF /**< switch not activated (value = -1) */ +} switch_pos_t; + /** * @addtogroup topics * @{ @@ -53,32 +63,25 @@ struct manual_control_setpoint_s { bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ - float roll; /**< ailerons roll / roll rate input */ - float pitch; /**< elevator / pitch / pitch rate */ - float yaw; /**< rudder / yaw rate / yaw */ - float throttle; /**< throttle / collective thrust / altitude */ - - float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - float return_switch; /**< land 2 position switch (mandatory): land, no effect */ - float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ - /** - * Any of the channels below may not be available and be set to NaN + * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. */ - - // XXX needed or parameter? - //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - - float flaps; /**< flap position */ - + float roll; /**< ailerons roll / roll rate input, -1..1 */ + float pitch; /**< elevator / pitch / pitch rate, -1..1 */ + float yaw; /**< rudder / yaw rate / yaw, -1..1 */ + float throttle; /**< throttle / collective thrust / altitude, 0..1 */ + float flaps; /**< flap position */ float aux1; /**< default function: camera yaw / azimuth */ float aux2; /**< default function: camera pitch / tilt */ float aux3; /**< default function: camera trigger */ float aux4; /**< default function: camera roll */ float aux5; /**< default function: payload drop */ + switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ + switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** From 6f38ed3b4b6f266098d0616b6bd3c18ffe082755 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 20:23:34 +0400 Subject: [PATCH 21/53] commander, navigator: use updated manual_control_setpoint --- src/modules/commander/commander.cpp | 136 +++++++++++------------ src/modules/navigator/navigator_main.cpp | 98 ++++++---------- src/modules/uORB/topics/vehicle_status.h | 28 ----- 3 files changed, 94 insertions(+), 168 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index edd77231dc..7e469c9c19 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -112,8 +113,7 @@ extern struct system_load_s system_load; #define MAVLINK_OPEN_INTERVAL 50000 -#define STICK_ON_OFF_LIMIT 0.75f -#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_LIMIT 0.9f #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) @@ -207,7 +207,7 @@ void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t set_main_state_rc(struct vehicle_status_s *status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); void set_control_mode(); @@ -814,6 +814,11 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); + /* Subscribe to position setpoint triplet */ + int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + struct position_setpoint_triplet_s pos_sp_triplet; + memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -1005,6 +1010,13 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } + /* update subsystem */ + orb_check(pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); + } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; @@ -1135,7 +1147,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1153,7 +1165,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); @@ -1193,11 +1205,8 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } - /* fill status according to mode switches */ - check_mode_switches(&sp_man, &status); - /* evaluate the main state machine according to mode switches */ - res = set_main_state_rc(&status); + res = set_main_state_rc(&status, &sp_man); /* play tune on mode change only if armed, blink LED always */ if (res == TRANSITION_CHANGED) { @@ -1208,6 +1217,33 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } + /* set navigation state */ + /* RETURN switch, overrides MISSION switch */ + if (sp_man.return_switch == SWITCH_POS_ON) { + /* switch to RTL if not already landed after RTL and home position set */ + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else { + /* MISSION switch */ + if (sp_man.mission_switch == SWITCH_POS_ON) { + /* stick is in LOITER position */ + status.set_nav_state = NAV_STATE_LOITER; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + /* stick is in MISSION position */ + status.set_nav_state = NAV_STATE_MISSION; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else if (sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE && + pos_sp_triplet.nav_state == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + status.set_nav_state = NAV_STATE_MISSION; + status.set_nav_state_timestamp = hrt_absolute_time(); + } + } + } else { if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); @@ -1255,7 +1291,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1470,76 +1506,26 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a leds_counter++; } -void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) -{ - /* main mode switch */ - if (!isfinite(sp_man->mode_switch)) { - /* default to manual if signal is invalid */ - status->mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - status->mode_switch = MODE_SWITCH_AUTO; - - } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - status->mode_switch = MODE_SWITCH_MANUAL; - - } else { - status->mode_switch = MODE_SWITCH_ASSISTED; - } - - /* return switch */ - if (!isfinite(sp_man->return_switch)) { - status->return_switch = RETURN_SWITCH_NONE; - - } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - status->return_switch = RETURN_SWITCH_RETURN; - - } else { - status->return_switch = RETURN_SWITCH_NORMAL; - } - - /* assisted switch */ - if (!isfinite(sp_man->assisted_switch)) { - status->assisted_switch = ASSISTED_SWITCH_SEATBELT; - - } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - status->assisted_switch = ASSISTED_SWITCH_EASY; - - } else { - status->assisted_switch = ASSISTED_SWITCH_SEATBELT; - } - - /* mission switch */ - if (!isfinite(sp_man->mission_switch)) { - status->mission_switch = MISSION_SWITCH_NONE; - - } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - status->mission_switch = MISSION_SWITCH_LOITER; - - } else { - status->mission_switch = MISSION_SWITCH_MISSION; - } -} - transition_result_t -set_main_state_rc(struct vehicle_status_s *status) +set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; - switch (status->mode_switch) { - case MODE_SWITCH_MANUAL: + switch (sp_man->mode_switch) { + case SWITCH_POS_NONE: + case SWITCH_POS_OFF: // MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case MODE_SWITCH_ASSISTED: - if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + case SWITCH_POS_MIDDLE: // ASSISTED + if (sp_man->assisted_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_EASY); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to SEATBELT print_reject_mode(status, "EASY"); @@ -1547,29 +1533,33 @@ set_main_state_rc(struct vehicle_status_s *status) res = main_state_transition(status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode + } - if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + if (sp_man->assisted_switch != SWITCH_POS_ON) { print_reject_mode(status, "SEATBELT"); + } // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case MODE_SWITCH_AUTO: + case SWITCH_POS_ON: // AUTO res = main_state_transition(status, MAIN_STATE_AUTO); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to SEATBELT (EASY likely will not work too) print_reject_mode(status, "AUTO"); res = main_state_transition(status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1ba..b7f26198b6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -690,84 +690,46 @@ Navigator::task_main() if (fds[6].revents & POLLIN) { vehicle_status_update(); - /* evaluate state machine from commander and set the navigator mode accordingly */ + /* evaluate state requested by commander */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - bool stick_mode = false; + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - /* switch to RTL if not already landed after RTL and home position set */ + case NAV_STATE_LOITER: + request_loiter_or_ready(); + break; + + case NAV_STATE_MISSION: + request_mission_if_available(); + break; + + case NAV_STATE_RTL: if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { + (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } - stick_mode = true; + break; - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - request_loiter_or_ready(); - stick_mode = true; + case NAV_STATE_LAND: + dispatch(EVENT_LAND_REQUESTED); - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - request_mission_if_available(); - stick_mode = true; - } + break; - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - request_mission_if_available(); - stick_mode = true; - } + default: + warnx("ERROR: Requested navigation state not supported"); + break; } - } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - request_loiter_or_ready(); - break; - - case NAV_STATE_MISSION: - request_mission_if_available(); - break; - - case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } - - break; - - case NAV_STATE_LAND: - dispatch(EVENT_LAND_REQUESTED); - - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - request_mission_if_available(); - } + } else { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + request_mission_if_available(); } } @@ -775,6 +737,8 @@ Navigator::task_main() /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } + + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; } /* parameters updated */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 56be4d2412..48af4c9e2d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -93,29 +93,6 @@ typedef enum { FAILSAFE_STATE_MAX } failsafe_state_t; -typedef enum { - MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_ASSISTED, - MODE_SWITCH_AUTO -} mode_switch_pos_t; - -typedef enum { - ASSISTED_SWITCH_SEATBELT = 0, - ASSISTED_SWITCH_EASY -} assisted_switch_pos_t; - -typedef enum { - RETURN_SWITCH_NONE = 0, - RETURN_SWITCH_NORMAL, - RETURN_SWITCH_RETURN -} return_switch_pos_t; - -typedef enum { - MISSION_SWITCH_NONE = 0, - MISSION_SWITCH_LOITER, - MISSION_SWITCH_MISSION -} mission_switch_pos_t; - enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -187,11 +164,6 @@ struct vehicle_status_s { bool is_rotary_wing; - mode_switch_pos_t mode_switch; - return_switch_pos_t return_switch; - assisted_switch_pos_t assisted_switch; - mission_switch_pos_t mission_switch; - bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_sensors_initialized; From e2ac5222d812bdbfaf33fc2d320ee22ab861d433 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 20:54:28 +0400 Subject: [PATCH 22/53] mc_att_control, mc_pos_control: update manual_control_setpoint usage --- .../mc_att_control/mc_att_control_main.cpp | 54 +++++++++---------- .../mc_att_control/mc_att_control_params.c | 29 ++++++++++ .../mc_pos_control/mc_pos_control_main.cpp | 14 +---- src/modules/sensors/sensor_params.c | 22 -------- src/modules/sensors/sensors.cpp | 19 ------- 5 files changed, 57 insertions(+), 81 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 9cb8e83445..01f1631a97 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -157,7 +157,9 @@ private: param_t yaw_rate_d; param_t yaw_ff; - param_t rc_scale_yaw; + param_t man_scale_roll; + param_t man_scale_pitch; + param_t man_scale_yaw; } _params_handles; /**< handles for interesting parameters */ struct { @@ -167,7 +169,9 @@ private: math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ - float rc_scale_yaw; + float man_scale_roll; + float man_scale_pitch; + float man_scale_yaw; } _params; /** @@ -295,7 +299,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); - _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _params_handles.man_scale_roll = param_find("MC_SCALE_ROLL"); + _params_handles.man_scale_pitch = param_find("MC_SCALE_PITCH"); + _params_handles.man_scale_yaw = param_find("MC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(); @@ -330,7 +336,7 @@ MulticopterAttitudeControl::parameters_update() { float v; - /* roll */ + /* roll gains */ param_get(_params_handles.roll_p, &v); _params.att_p(0) = v; param_get(_params_handles.roll_rate_p, &v); @@ -340,7 +346,7 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; - /* pitch */ + /* pitch gains */ param_get(_params_handles.pitch_p, &v); _params.att_p(1) = v; param_get(_params_handles.pitch_rate_p, &v); @@ -350,7 +356,7 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; - /* yaw */ + /* yaw gains */ param_get(_params_handles.yaw_p, &v); _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); @@ -362,7 +368,14 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_ff, &_params.yaw_ff); - param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + /* manual control scale */ + param_get(_params_handles.man_scale_roll, &_params.man_scale_roll); + param_get(_params_handles.man_scale_pitch, &_params.man_scale_pitch); + param_get(_params_handles.man_scale_yaw, &_params.man_scale_yaw); + + _params.man_scale_roll *= M_PI / 180.0; + _params.man_scale_pitch *= M_PI / 180.0; + _params.man_scale_yaw *= M_PI / 180.0; return OK; } @@ -404,7 +417,6 @@ MulticopterAttitudeControl::vehicle_manual_poll() orb_check(_manual_control_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); } } @@ -483,24 +495,10 @@ MulticopterAttitudeControl::control_attitude(float dt) // reset_yaw_sp = true; //} } else { - float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; - - if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; - - if (_manual_control_sp.yaw > 0.0f) { - yaw_sp_move_rate -= YAW_DEADZONE; - - } else { - yaw_sp_move_rate += YAW_DEADZONE; - } - - yaw_sp_move_rate *= _params.rc_scale_yaw; - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); - _v_att_sp.R_valid = false; - publish_att_sp = true; - } + /* move yaw setpoint */ + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_scale_yaw * dt); + _v_att_sp.R_valid = false; + publish_att_sp = true; } /* reset yaw setpint to current position if needed */ @@ -513,8 +511,8 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.roll; - _v_att_sp.pitch_body = _manual_control_sp.pitch; + _v_att_sp.roll_body = _manual_control_sp.roll * _params.man_scale_roll; + _v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_scale_pitch; _v_att_sp.R_valid = false; publish_att_sp = true; } diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 488107d585..9acf8bfa3e 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -173,3 +173,32 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); * @group Multicopter Attitude Control */ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); + +/** + * Manual control scaling factor for roll + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_SCALE_ROLL, 35.0f); + +/** + * Manual control scaling factor for pitch + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_SCALE_PITCH, 35.0f); + +/** + * Manual control scaling factor for yaw + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_SCALE_YAW, 120.0f); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 78d06ba5b5..2bd2d356a0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -148,9 +148,6 @@ private: param_t tilt_max; param_t land_speed; param_t land_tilt_max; - - param_t rc_scale_pitch; - param_t rc_scale_roll; } _params_handles; /**< handles for interesting parameters */ struct { @@ -160,9 +157,6 @@ private: float land_speed; float land_tilt_max; - float rc_scale_pitch; - float rc_scale_roll; - math::Vector<3> pos_p; math::Vector<3> vel_p; math::Vector<3> vel_i; @@ -306,8 +300,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.tilt_max = param_find("MPC_TILT_MAX"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); - _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); - _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); /* fetch initial parameter values */ parameters_update(true); @@ -354,8 +346,6 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.tilt_max, &_params.tilt_max); param_get(_params_handles.land_speed, &_params.land_speed); param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); - param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); - param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); float v; param_get(_params_handles.xy_p, &v); @@ -608,8 +598,8 @@ MulticopterPositionControl::task_main() reset_lat_lon_sp(); /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); - sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz); + sp_move_rate(0) = _manual.pitch; + sp_move_rate(1) = _manual.roll; } /* limit setpoint move rate */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 288c6e00a0..09afaf9498 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -647,28 +647,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); -/** - * Roll scaling factor - * - * @group Radio Calibration - */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); - -/** - * Pitch scaling factor - * - * @group Radio Calibration - */ -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); - -/** - * Yaw scaling factor - * - * @group Radio Calibration - */ -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); - - /** * Failsafe channel mapping. * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e08d8618f0..8f350751c1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -268,11 +268,6 @@ private: int rc_map_aux4; int rc_map_aux5; - float rc_scale_roll; - float rc_scale_pitch; - float rc_scale_yaw; - float rc_scale_flaps; - int rc_fs_ch; int rc_fs_mode; float rc_fs_thr; @@ -318,11 +313,6 @@ private: param_t rc_map_aux4; param_t rc_map_aux5; - param_t rc_scale_roll; - param_t rc_scale_pitch; - param_t rc_scale_yaw; - param_t rc_scale_flaps; - param_t rc_fs_ch; param_t rc_fs_mode; param_t rc_fs_thr; @@ -536,11 +526,6 @@ Sensors::Sensors() : _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); - _parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); - _parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); - _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); - _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); - /* RC failsafe */ _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); @@ -696,10 +681,6 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); - param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); - param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); - param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); - param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); From ef8b97437342401a464bcc844d6c347c33919d73 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 22:58:57 +0400 Subject: [PATCH 23/53] fw_att_control: update manual_control_setpoint usage --- .../fw_att_control/fw_att_control_main.cpp | 17 ++++++++++++++--- .../fw_att_control/fw_att_control_params.c | 10 ++++++++++ 2 files changed, 24 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index f139c25f48..4464b5e012 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -173,6 +173,8 @@ private: float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float roll_max; /**< Max Roll in rad */ + float pitch_max; /**< Max Pitch in rad */ } _parameters; /**< local copies of interesting parameters */ @@ -211,6 +213,8 @@ private: param_t trim_yaw; param_t rollsp_offset_deg; param_t pitchsp_offset_deg; + param_t roll_max; + param_t pitch_max; } _parameter_handles; /**< handles for interesting parameters */ @@ -354,6 +358,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); + _parameter_handles.roll_max = param_find("FW_R_MAX"); + _parameter_handles.pitch_max = param_find("FW_P_MAX"); + /* fetch initial parameter values */ parameters_update(); } @@ -421,6 +428,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); + param_get(_parameter_handles.roll_max, &(_parameters.roll_max)); + param_get(_parameter_handles.pitch_max, &(_parameters.pitch_max)); + _parameters.roll_max = math::radians(_parameters.roll_max); + _parameters.pitch_max = math::radians(_parameters.pitch_max); /* pitch control parameters */ @@ -700,8 +711,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad; - pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.roll * _parameters.roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; + pitch_sp = (-_manual.pitch * _parameters.pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; @@ -809,7 +820,7 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ _actuators.control[0] = _manual.roll; - _actuators.control[1] = _manual.pitch; + _actuators.control[1] = -_manual.pitch; _actuators.control[2] = _manual.yaw; _actuators.control[3] = _manual.throttle; _actuators.control[4] = _manual.flaps; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index c80a44f2a5..14815745cd 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -186,3 +186,13 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); // @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe // @Range -90.0 to 90.0 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); + +// @DisplayName Max Roll +// @Description Max roll for manual control in attitude stabilized mode +// @Range 0.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_R_MAX, 45.0f); + +// @DisplayName Max Pitch +// @Description Max pitch for manual control in attitude stabilized mode +// @Range 0.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_P_MAX, 45.0f); From 3641faed0c696f1a88f88a17b5666ed5c3ba8b1c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 23:18:43 +0400 Subject: [PATCH 24/53] sensors: publish last valid manual control values when signal lost --- src/modules/sensors/sensors.cpp | 111 ++++++++++++++------------------ 1 file changed, 50 insertions(+), 61 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8f350751c1..669f4e174d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -167,8 +167,6 @@ public: private: static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ - hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ - /** * Get and limit value for specified RC function. Returns NAN if not mapped. */ @@ -216,6 +214,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ + struct manual_control_setpoint_s _manual; /**< manual control setpoint */ struct battery_status_s _battery_status; /**< battery status */ struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; @@ -438,8 +437,6 @@ Sensors *g_sensors = nullptr; } Sensors::Sensors() : - _rc_last_valid(0), - _fd_adc(-1), _last_adc(0), @@ -475,6 +472,21 @@ Sensors::Sensors() : _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); + memset(&_manual, 0, sizeof(_manual)); + + /* set NANs instead of zeroes */ + _manual.roll = NAN; + _manual.pitch = NAN; + _manual.yaw = NAN; + _manual.throttle = NAN; + _manual.flaps = NAN; + _manual.aux1 = NAN; + _manual.aux2 = NAN; + _manual.aux3 = NAN; + _manual.aux4 = NAN; + _manual.aux5 = NAN; + + _manual.signal_lost = true; /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1314,27 +1326,8 @@ Sensors::rc_poll() if (rc_updated) { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct rc_input_values rc_input; - struct manual_control_setpoint_s manual_control; - struct actuator_controls_s actuator_group_3; - /* initialize to default values */ - manual_control.roll = NAN; - manual_control.pitch = NAN; - manual_control.yaw = NAN; - manual_control.throttle = NAN; - manual_control.flaps = NAN; - manual_control.aux1 = NAN; - manual_control.aux2 = NAN; - manual_control.aux3 = NAN; - manual_control.aux4 = NAN; - manual_control.aux5 = NAN; - - manual_control.mode_switch = SWITCH_POS_NONE; - manual_control.return_switch = SWITCH_POS_NONE; - manual_control.assisted_switch = SWITCH_POS_NONE; - manual_control.mission_switch = SWITCH_POS_NONE; - - manual_control.signal_lost = true; + _manual.signal_lost = true; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); @@ -1342,18 +1335,18 @@ Sensors::rc_poll() /* check flags and require at least four channels to consider the signal valid */ if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { /* signal looks good */ - manual_control.signal_lost = false; + _manual.signal_lost = false; /* check for throttle failsafe */ if (_parameters.rc_fs_ch != 0) { if (_parameters.rc_fs_mode == 0) { if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) { - manual_control.signal_lost = true; + _manual.signal_lost = true; } } else if (_parameters.rc_fs_mode == 1) { if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) { - manual_control.signal_lost = true; + _manual.signal_lost = true; } } } @@ -1412,46 +1405,42 @@ Sensors::rc_poll() _rc.chan_count = rc_input.channel_count; _rc.rssi = rc_input.rssi; - _rc.signal_lost = manual_control.signal_lost; + _rc.signal_lost = _manual.signal_lost; - if (!manual_control.signal_lost) { - _rc_last_valid = rc_input.timestamp_last_signal; - } - - _rc.timestamp = _rc_last_valid; - - manual_control.timestamp = _rc_last_valid; - - if (!manual_control.signal_lost) { + if (!_manual.signal_lost) { /* fill values in manual_control_setpoint topic only if signal is valid */ + _manual.timestamp = rc_input.timestamp_last_signal; + _rc.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual_control.roll = get_rc_value(ROLL, -1.0, 1.0); - manual_control.pitch = get_rc_value(PITCH, -1.0, 1.0); - manual_control.yaw = get_rc_value(YAW, -1.0, 1.0); - manual_control.throttle = get_rc_value(THROTTLE, 0.0, 1.0); - manual_control.flaps = get_rc_value(FLAPS, -1.0, 1.0); - manual_control.aux1 = get_rc_value(AUX_1, -1.0, 1.0); - manual_control.aux2 = get_rc_value(AUX_2, -1.0, 1.0); - manual_control.aux3 = get_rc_value(AUX_3, -1.0, 1.0); - manual_control.aux4 = get_rc_value(AUX_4, -1.0, 1.0); - manual_control.aux5 = get_rc_value(AUX_5, -1.0, 1.0); + _manual.roll = get_rc_value(ROLL, -1.0, 1.0); + _manual.pitch = get_rc_value(PITCH, -1.0, 1.0); + _manual.yaw = get_rc_value(YAW, -1.0, 1.0); + _manual.throttle = get_rc_value(THROTTLE, 0.0, 1.0); + _manual.flaps = get_rc_value(FLAPS, -1.0, 1.0); + _manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0); + _manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0); + _manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0); + _manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0); + _manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ - manual_control.mode_switch = get_rc_switch_position(MODE); - manual_control.assisted_switch = get_rc_switch_position(ASSISTED); - manual_control.mission_switch = get_rc_switch_position(MISSION); - manual_control.return_switch = get_rc_switch_position(RETURN); + _manual.mode_switch = get_rc_switch_position(MODE); + _manual.assisted_switch = get_rc_switch_position(ASSISTED); + _manual.mission_switch = get_rc_switch_position(MISSION); + _manual.return_switch = get_rc_switch_position(RETURN); /* copy from mapped manual control to control group 3 */ - actuator_group_3.control[0] = manual_control.roll; - actuator_group_3.control[1] = manual_control.pitch; - actuator_group_3.control[2] = manual_control.yaw; - actuator_group_3.control[3] = manual_control.throttle; - actuator_group_3.control[4] = manual_control.flaps; - actuator_group_3.control[5] = manual_control.aux1; - actuator_group_3.control[6] = manual_control.aux2; - actuator_group_3.control[7] = manual_control.aux3; + struct actuator_controls_s actuator_group_3; + + actuator_group_3.control[0] = _manual.roll; + actuator_group_3.control[1] = _manual.pitch; + actuator_group_3.control[2] = _manual.yaw; + actuator_group_3.control[3] = _manual.throttle; + actuator_group_3.control[4] = _manual.flaps; + actuator_group_3.control[5] = _manual.aux1; + actuator_group_3.control[6] = _manual.aux2; + actuator_group_3.control[7] = _manual.aux3; /* publish actuator_controls_3 topic only if control signal is valid */ if (_actuator_group_3_pub > 0) { @@ -1473,10 +1462,10 @@ Sensors::rc_poll() /* publish manual_control_setpoint topic even if signal is not valid to update 'signal_lost' flag */ if (_manual_control_pub > 0) { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &_manual); } else { - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual); } } From 60355b4e6c0a6916084442d0557cacd74b008b82 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 23:47:09 +0400 Subject: [PATCH 25/53] sensors: switch position reading bug fixed --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 669f4e174d..79b53a79ce 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1305,7 +1305,7 @@ Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) if (value > STICK_ON_OFF_LIMIT) { return SWITCH_POS_ON; - } else if (value < STICK_ON_OFF_LIMIT) { + } else if (value < -STICK_ON_OFF_LIMIT) { return SWITCH_POS_OFF; } else { From a6a4ab1dbeded057a72067a50999034ebbc788cd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 4 Apr 2014 21:45:01 +0400 Subject: [PATCH 26/53] position_estimator_inav: reset position estimate when GPS becomes available --- .../position_estimator_inav_main.c | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3f1a5d39b3..b77e51521c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -573,6 +573,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + bool reset_est = false; + /* hysteresis for GPS quality */ if (gps_valid) { if (gps.eph_m > 10.0f || gps.epv_m > 20.0f || gps.fix_type < 3) { @@ -583,6 +585,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { if (gps.eph_m < 5.0f && gps.epv_m < 10.0f && gps.fix_type >= 3) { gps_valid = true; + reset_est = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } } @@ -606,9 +609,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; + x_est[2] = accel_NED[0]; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; z_est[0] = 0.0f; + y_est[2] = accel_NED[1]; local_pos.ref_lat = lat; local_pos.ref_lon = lon; @@ -626,6 +631,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(&ref, gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + + /* reset position estimate when GPS becomes good */ + if (reset_est) { + x_est[0] = gps_proj[0]; + x_est[1] = gps.vel_n_m_s; + x_est[2] = accel_NED[0]; + y_est[0] = gps_proj[1]; + y_est[1] = gps.vel_e_m_s; + y_est[2] = accel_NED[1]; + } + /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - x_est[0]; corr_gps[1][0] = gps_proj[1] - y_est[0]; From 568eb8962d3ab5798a5048047da75b696e0a3af9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 15:42:07 +0400 Subject: [PATCH 27/53] px4io: typos fixed --- src/drivers/px4io/px4io.cpp | 3 +-- src/modules/px4iofirmware/px4io.h | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a30bfb2dec..71aa5a0a4d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -951,8 +951,7 @@ PX4IO::task_main() int32_t failsafe_param_val; param_t failsafe_param = param_find("RC_FAILS_THR"); - if (failsafe_param > 0) - + if (failsafe_param > 0) { param_get(failsafe_param, &failsafe_param_val); uint16_t failsafe_thr = failsafe_param_val; pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 88ad793980..4db9484848 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -108,7 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #endif -#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]; +#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] #define r_control_values (&r_page_controls[0]) From 77190f5052405ba8ef10c89f3193802d3d59e66f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 15:42:23 +0400 Subject: [PATCH 28/53] sensors: bug fixed --- src/modules/sensors/sensors.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2c1b1258c5..37255c4bf3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1327,9 +1327,10 @@ Sensors::rc_poll() /* check flags and require at least four channels to consider the signal valid */ if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { /* signal looks good, but check for throttle failsafe */ - if (_parameters.rc_fs_thr == 0 || - !((_parameters.rc_fs_thr < _parameters.min[i] && rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr) || - (_parameters.rc_fs_thr > _parameters.max[i] && rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { + int8_t thr_ch = _rc.function[THROTTLE]; + if (_parameters.rc_fs_thr == 0 || thr_ch < 0 || + !((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || + (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr))) { /* valid signal, throttle failsafe not configured or not triggered */ signal_lost = false; } From b00b70aeb2b7f20e74ca185e357e796f54031640 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 16:06:10 +0400 Subject: [PATCH 29/53] manual_control_setpoint: signal_lost flag removed, sensors: bugs fixed --- src/modules/sensors/sensors.cpp | 36 ++++++------------- .../uORB/topics/manual_control_setpoint.h | 2 -- 2 files changed, 10 insertions(+), 28 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 37255c4bf3..5b826a9194 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -214,7 +214,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ - struct manual_control_setpoint_s _manual; /**< manual control setpoint */ struct battery_status_s _battery_status; /**< battery status */ struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; @@ -468,21 +467,6 @@ Sensors::Sensors() : _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); - memset(&_manual, 0, sizeof(_manual)); - - /* set NANs instead of zeroes */ - _manual.roll = NAN; - _manual.pitch = NAN; - _manual.yaw = NAN; - _manual.throttle = NAN; - _manual.flaps = NAN; - _manual.aux1 = NAN; - _manual.aux2 = NAN; - _manual.aux3 = NAN; - _manual.aux4 = NAN; - _manual.aux5 = NAN; - - _manual.signal_lost = true; /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1427,10 +1411,10 @@ Sensors::rc_poll() /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &_manual); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual); } else { - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual); + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); } /* copy from mapped manual control to control group 3 */ @@ -1439,14 +1423,14 @@ Sensors::rc_poll() actuator_group_3.timestamp = rc_input.timestamp_publication; - actuator_group_3.control[0] = _manual.roll; - actuator_group_3.control[1] = _manual.pitch; - actuator_group_3.control[2] = _manual.yaw; - actuator_group_3.control[3] = _manual.throttle; - actuator_group_3.control[4] = _manual.flaps; - actuator_group_3.control[5] = _manual.aux1; - actuator_group_3.control[6] = _manual.aux2; - actuator_group_3.control[7] = _manual.aux3; + actuator_group_3.control[0] = manual.roll; + actuator_group_3.control[1] = manual.pitch; + actuator_group_3.control[2] = manual.yaw; + actuator_group_3.control[3] = manual.throttle; + actuator_group_3.control[4] = manual.flaps; + actuator_group_3.control[5] = manual.aux1; + actuator_group_3.control[6] = manual.aux2; + actuator_group_3.control[7] = manual.aux3; /* publish actuator_controls_3 topic */ if (_actuator_group_3_pub > 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 985d3923fb..2b3a337b29 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -61,8 +61,6 @@ typedef enum { struct manual_control_setpoint_s { uint64_t timestamp; - bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ - /** * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. From 97cde3311efab479c43226dea3b1edd93629c33b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 16:59:01 +0400 Subject: [PATCH 30/53] commander: home publication fixed --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 47e630d070..17c0856526 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -613,10 +613,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* announce new home position */ if (*home_pub > 0) { - orb_publish(ORB_ID(home_position), *home_pub, &home); + orb_publish(ORB_ID(home_position), *home_pub, home); } else { - *home_pub = orb_advertise(ORB_ID(home_position), &home); + *home_pub = orb_advertise(ORB_ID(home_position), home); } /* mark home position as set */ From 0fd6fb53f33bbe923973ee519e2464655f2c2bc5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 17:07:15 +0400 Subject: [PATCH 31/53] position_estimator_inav: projection reinitialization on home change fixed --- .../position_estimator_inav/position_estimator_inav_main.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index b77e51521c..5a713d309b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -544,13 +544,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (home.timestamp != home_timestamp) { home_timestamp = home.timestamp; if (ref_inited) { - ref_inited = true; - /* reproject position estimate to new reference */ float dx, dy; map_projection_project(&ref, home.lat, home.lon, &dx, &dy); x_est[0] -= dx; - y_est[0] -= dx; + y_est[0] -= dy; z_est[0] += home.alt - local_pos.ref_alt; } From 79d2247b44aaeb0f2211a1c66c994e0fa7ca4b9f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 18:11:51 +0400 Subject: [PATCH 32/53] position_estimator_inav, mc_pos_control: precise position reprojection on home position changes --- .../mc_pos_control/mc_pos_control_main.cpp | 24 +++++++---- .../position_estimator_inav_main.c | 42 +++++++++++-------- 2 files changed, 41 insertions(+), 25 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 21070e1e9b..dc0aa172a4 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -473,18 +473,26 @@ void MulticopterPositionControl::update_ref() { if (_local_pos.ref_timestamp != _ref_timestamp) { + double lat_sp, lon_sp; + float alt_sp; + if (_ref_timestamp != 0) { - /* reproject local position setpoint to new reference */ - float dx, dy; - map_projection_project(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon, &dx, &dy); - _pos_sp(0) -= dx; - _pos_sp(1) -= dy; + /* calculate current position setpoint in global frame */ + map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + alt_sp = _ref_alt - _pos_sp(2); + } + + /* update local projection reference */ + map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); + _ref_alt = _local_pos.ref_alt; + + if (_ref_timestamp != 0) { + /* reproject position setpoint to new reference */ + map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp(2) = -(alt_sp - _ref_alt); } _ref_timestamp = _local_pos.ref_timestamp; - - map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); - _ref_alt = _local_pos.ref_alt; } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 5a713d309b..40f7069ca0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -543,25 +543,34 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (home.timestamp != home_timestamp) { home_timestamp = home.timestamp; - if (ref_inited) { - /* reproject position estimate to new reference */ - float dx, dy; - map_projection_project(&ref, home.lat, home.lon, &dx, &dy); - x_est[0] -= dx; - y_est[0] -= dy; - z_est[0] += home.alt - local_pos.ref_alt; - } - /* update baro offset */ - baro_offset -= home.alt - local_pos.ref_alt; + double est_lat, est_lon; + float est_alt; + + if (ref_inited) { + /* calculate current estimated position in global frame */ + est_alt = local_pos.ref_alt - local_pos.z; + map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + } /* update reference */ map_projection_init(&ref, home.lat, home.lon); + /* update baro offset */ + baro_offset += home.alt - local_pos.ref_alt; + local_pos.ref_lat = home.lat; local_pos.ref_lon = home.lon; local_pos.ref_alt = home.alt; local_pos.ref_timestamp = home.timestamp; + + if (ref_inited) { + /* reproject position estimate with new reference */ + map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); + z_est[0] = -(est_alt - local_pos.ref_alt); + } + + ref_inited = true; } } @@ -589,6 +598,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (gps_valid) { + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + float alt = gps.alt * 1e-3; + /* initialize reference position if needed */ if (!ref_inited) { if (ref_init_start == 0) { @@ -596,11 +609,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; - /* reference GPS position */ - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; - float alt = gps.alt * 1e-3; - /* update baro offset */ baro_offset -= z_est[0]; @@ -628,7 +636,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; - map_projection_project(&ref, gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); /* reset position estimate when GPS becomes good */ if (reset_est) { @@ -643,7 +651,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - x_est[0]; corr_gps[1][0] = gps_proj[1] - y_est[0]; - corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0]; + corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { From a4ba705e2f64616e25456c71572661f6b5e7cc3b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Apr 2014 20:04:18 +0400 Subject: [PATCH 33/53] commander: don't use mode switch if it's not mapped --- src/modules/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 73e1a17929..aade0d862e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1514,6 +1514,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin switch (sp_man->mode_switch) { case SWITCH_POS_NONE: + res = TRANSITION_NOT_CHANGED; + break; + case SWITCH_POS_OFF: // MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here From c77a7b11628de9ccca20a444bf38582726d1668d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Apr 2014 22:23:33 +0400 Subject: [PATCH 34/53] mavlink_receiver: don't publish rc_channels on manual_control from mavlink, set switches to unmapped state instead of using previous values --- src/modules/mavlink/mavlink_receiver.cpp | 53 +++++------------------- src/modules/mavlink/mavlink_receiver.h | 1 - 2 files changed, 11 insertions(+), 43 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ec40ee0a9..f6f5e4848c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -88,8 +88,6 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), - _manual_sub(-1), - _global_pos_pub(-1), _local_pos_pub(-1), _attitude_pub(-1), @@ -413,47 +411,20 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) mavlink_manual_control_t man; mavlink_msg_manual_control_decode(msg, &man); - /* rc channels */ - { - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); - rc.timestamp = hrt_absolute_time(); - rc.chan_count = 4; + manual.timestamp = hrt_absolute_time(); + manual.roll = man.x / 1000.0f; + manual.pitch = man.y / 1000.0f; + manual.yaw = man.r / 1000.0f; + manual.throttle = man.z / 1000.0f; - rc.chan[0].scaled = man.x / 1000.0f; - rc.chan[1].scaled = man.y / 1000.0f; - rc.chan[2].scaled = man.r / 1000.0f; - rc.chan[3].scaled = man.z / 1000.0f; + if (_manual_pub < 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - if (_rc_pub < 0) { - _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); - - } else { - orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); - } - } - - /* manual control */ - { - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - - /* get a copy first, to prevent altering values that are not sent by the mavlink command */ - orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual); - - manual.timestamp = hrt_absolute_time(); - manual.roll = man.x / 1000.0f; - manual.pitch = man.y / 1000.0f; - manual.yaw = man.r / 1000.0f; - manual.throttle = man.z / 1000.0f; - - if (_manual_pub < 0) { - _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - - } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); - } + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); } } @@ -883,8 +854,6 @@ MavlinkReceiver::receive_thread(void *arg) sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); prctl(PR_SET_NAME, thread_name, getpid()); - _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct pollfd fds[1]; fds[0].fd = uart_fd; fds[0].events = POLLIN; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8ccb2a0354..72ce4560fb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -138,7 +138,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - int _manual_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; From 606660d94a0f2b2968c0bd3a8f035a6db97d9e10 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:07:16 +0400 Subject: [PATCH 35/53] fw_att_control: renamed FW_R_MAX/FW_P_MAX to FW_MAN_R_MAX/FW_MAN_P_MAX --- .../fw_att_control/fw_att_control_main.cpp | 24 +++++++++---------- .../fw_att_control/fw_att_control_params.c | 8 +++---- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 4ab5c19cf8..5276b1c134 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -173,8 +173,8 @@ private: float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float roll_max; /**< Max Roll in rad */ - float pitch_max; /**< Max Pitch in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ } _parameters; /**< local copies of interesting parameters */ @@ -213,8 +213,8 @@ private: param_t trim_yaw; param_t rollsp_offset_deg; param_t pitchsp_offset_deg; - param_t roll_max; - param_t pitch_max; + param_t man_roll_max; + param_t man_pitch_max; } _parameter_handles; /**< handles for interesting parameters */ @@ -358,8 +358,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); - _parameter_handles.roll_max = param_find("FW_R_MAX"); - _parameter_handles.pitch_max = param_find("FW_P_MAX"); + _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); + _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); /* fetch initial parameter values */ parameters_update(); @@ -428,10 +428,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); - param_get(_parameter_handles.roll_max, &(_parameters.roll_max)); - param_get(_parameter_handles.pitch_max, &(_parameters.pitch_max)); - _parameters.roll_max = math::radians(_parameters.roll_max); - _parameters.pitch_max = math::radians(_parameters.pitch_max); + param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); + param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); + _parameters.man_roll_max = math::radians(_parameters.man_roll_max); + _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); /* pitch control parameters */ @@ -717,8 +717,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll * _parameters.roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; - pitch_sp = (-_manual.pitch * _parameters.pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; + pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 14815745cd..aa637e2074 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -187,12 +187,12 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); // @Range -90.0 to 90.0 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); -// @DisplayName Max Roll +// @DisplayName Max Manual Roll // @Description Max roll for manual control in attitude stabilized mode // @Range 0.0 to 90.0 -PARAM_DEFINE_FLOAT(FW_R_MAX, 45.0f); +PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); -// @DisplayName Max Pitch +// @DisplayName Max Manual Pitch // @Description Max pitch for manual control in attitude stabilized mode // @Range 0.0 to 90.0 -PARAM_DEFINE_FLOAT(FW_P_MAX, 45.0f); +PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); From 9a579fa8707361e38c3681b2d23d6bbab1f9c298 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:16:10 +0400 Subject: [PATCH 36/53] mc_att_control: parameters MC_SCALE_XXX renamed to MC_MAN_X_MAX --- .../mc_att_control/mc_att_control_main.cpp | 36 +++++++++---------- .../mc_att_control/mc_att_control_params.c | 12 +++---- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 01f1631a97..6b0c44fb31 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -157,9 +157,9 @@ private: param_t yaw_rate_d; param_t yaw_ff; - param_t man_scale_roll; - param_t man_scale_pitch; - param_t man_scale_yaw; + param_t man_roll_max; + param_t man_pitch_max; + param_t man_yaw_max; } _params_handles; /**< handles for interesting parameters */ struct { @@ -169,9 +169,9 @@ private: math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ - float man_scale_roll; - float man_scale_pitch; - float man_scale_yaw; + float man_roll_max; + float man_pitch_max; + float man_yaw_max; } _params; /** @@ -299,9 +299,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); - _params_handles.man_scale_roll = param_find("MC_SCALE_ROLL"); - _params_handles.man_scale_pitch = param_find("MC_SCALE_PITCH"); - _params_handles.man_scale_yaw = param_find("MC_SCALE_YAW"); + _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); + _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); + _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); /* fetch initial parameter values */ parameters_update(); @@ -369,13 +369,13 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_ff, &_params.yaw_ff); /* manual control scale */ - param_get(_params_handles.man_scale_roll, &_params.man_scale_roll); - param_get(_params_handles.man_scale_pitch, &_params.man_scale_pitch); - param_get(_params_handles.man_scale_yaw, &_params.man_scale_yaw); + param_get(_params_handles.man_roll_max, &_params.man_roll_max); + param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); + param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); - _params.man_scale_roll *= M_PI / 180.0; - _params.man_scale_pitch *= M_PI / 180.0; - _params.man_scale_yaw *= M_PI / 180.0; + _params.man_roll_max *= M_PI / 180.0; + _params.man_pitch_max *= M_PI / 180.0; + _params.man_yaw_max *= M_PI / 180.0; return OK; } @@ -496,7 +496,7 @@ MulticopterAttitudeControl::control_attitude(float dt) //} } else { /* move yaw setpoint */ - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_scale_yaw * dt); + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_yaw_max * dt); _v_att_sp.R_valid = false; publish_att_sp = true; } @@ -511,8 +511,8 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.roll * _params.man_scale_roll; - _v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_scale_pitch; + _v_att_sp.roll_body = _manual_control_sp.roll * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_pitch_max; _v_att_sp.R_valid = false; publish_att_sp = true; } diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 9acf8bfa3e..e52c39368b 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -175,30 +175,30 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); /** - * Manual control scaling factor for roll + * Max manual roll * * @unit deg * @min 0.0 * @max 90.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_SCALE_ROLL, 35.0f); +PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f); /** - * Manual control scaling factor for pitch + * Max manual pitch * * @unit deg * @min 0.0 * @max 90.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_SCALE_PITCH, 35.0f); +PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f); /** - * Manual control scaling factor for yaw + * Max manual yaw rate * * @unit deg/s * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_SCALE_YAW, 120.0f); +PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f); From eb5cd54023c39d3adc266d68e02c1f10e77ac62a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:32:37 +0400 Subject: [PATCH 37/53] sensors: lost signal detection rewritten to be more clear --- src/modules/sensors/sensors.cpp | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f20c328d31..91bf92da64 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1307,17 +1307,26 @@ Sensors::rc_poll() orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); /* detect RC signal loss */ - bool signal_lost = true; + bool signal_lost; /* check flags and require at least four channels to consider the signal valid */ - if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { - /* signal looks good, but check for throttle failsafe */ + if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) { + /* signal is lost or no enough channels */ + signal_lost = true; + + } else { + /* signal looks good */ + signal_lost = false; + + /* check throttle failsafe */ int8_t thr_ch = _rc.function[THROTTLE]; - if (_parameters.rc_fs_thr == 0 || thr_ch < 0 || - !((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || - (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr))) { - /* valid signal, throttle failsafe not configured or not triggered */ - signal_lost = false; + if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) { + /* throttle failsafe configured */ + if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || + (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) { + /* throttle failsafe triggered, signal is lost by receiver */ + signal_lost = true; + } } } From 9f52c4459331bc0fd32764a35387abb2cab88b4a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:34:14 +0400 Subject: [PATCH 38/53] sensors: use timestamp_last_signal for actuator_group_3 timestamping --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 91bf92da64..ba233dfd00 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1431,7 +1431,7 @@ Sensors::rc_poll() struct actuator_controls_s actuator_group_3; memset(&actuator_group_3, 0 , sizeof(actuator_group_3)); - actuator_group_3.timestamp = rc_input.timestamp_publication; + actuator_group_3.timestamp = rc_input.timestamp_last_signal; actuator_group_3.control[0] = manual.roll; actuator_group_3.control[1] = manual.pitch; From b770c9fc1edc570fc216bdf849f84519e4e3513f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 17:16:43 +0400 Subject: [PATCH 39/53] position_estimator_inav: increase acceptable EPH/EPV, in commander use EPH/EPV to decide if global position valid --- src/modules/commander/commander.cpp | 23 ++++++--- .../position_estimator_inav_main.c | 49 ++++++++++++++----- 2 files changed, 53 insertions(+), 19 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17c0856526..2f373c6db8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -153,6 +153,7 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; static int parachute_enabled = 0; +static float eph_epv_threshold = 5.0f; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -965,15 +966,25 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, true, &(status.condition_global_position_valid), &status_changed); + /* hysteresis for EPH/EPV*/ + bool eph_epv_good; + if (status.condition_global_position_valid) { + if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { + eph_epv_good = false; + } + + } else { + if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { + eph_epv_good = true; + } + } + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ - static float hdop_threshold_m = 4.0f; - static float vdop_threshold_m = 8.0f; /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) { + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { home.lat = global_position.lat; home.lon = global_position.lon; @@ -1004,7 +1015,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_local_position_valid and condition_local_altitude_valid */ - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); static bool published_condition_landed_fw = false; @@ -1322,7 +1333,7 @@ int commander_thread_main(int argc, char *argv[]) /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) { + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { // TODO remove code duplication home.lat = global_position.lat; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 40f7069ca0..dc85f74827 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -168,13 +168,13 @@ 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 x_est_prev[3], float y_est_prev[3], float z_est_prev[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]); + 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] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.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], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]); fwrite(s, 1, n, f); n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); fwrite(s, 1, n, f); @@ -199,6 +199,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float x_est_prev[3], y_est_prev[3], z_est_prev[3]; + memset(x_est_prev, 0, sizeof(x_est_prev)); + memset(y_est_prev, 0, sizeof(y_est_prev)); + memset(z_est_prev, 0, sizeof(z_est_prev)); + int baro_init_cnt = 0; int baro_init_num = 200; float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted @@ -249,6 +254,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; + static float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation + static float max_eph_epv = 10.0f; // max EPH/EPV acceptable for estimation + float sonar_prev = 0.0f; hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) @@ -584,13 +592,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > 10.0f || gps.epv_m > 20.0f || gps.fix_type < 3) { + if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < 5.0f && gps.epv_m < 10.0f && gps.fix_type >= 3) { + if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); @@ -665,8 +673,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } - w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m); - w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m); + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); + w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); } } else { @@ -782,23 +790,34 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); + if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + memcpy(z_est, z_est_prev, sizeof(z_est)); + } + /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); - float x_est_prev[3], y_est_prev[3]; + if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + memcpy(z_est, z_est_prev, sizeof(z_est)); + memset(corr_acc, 0, sizeof(corr_acc)); + memset(corr_gps, 0, sizeof(corr_gps)); + corr_baro = 0; - memcpy(x_est_prev, x_est, sizeof(x_est)); - memcpy(y_est_prev, y_est, sizeof(y_est)); + } else { + memcpy(z_est_prev, z_est, sizeof(z_est)); + } if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); - if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { - write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } @@ -822,13 +841,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { - write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_acc, 0, sizeof(corr_acc)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_flow, 0, sizeof(corr_flow)); + + } else { + memcpy(x_est_prev, x_est, sizeof(x_est)); + memcpy(y_est_prev, y_est, sizeof(y_est)); } } From dfd9601b571057e73668d9b39d584bc4eb9cc305 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 17:24:39 +0400 Subject: [PATCH 40/53] commander: minor comment fix --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2f373c6db8..531d17145f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -966,7 +966,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - /* hysteresis for EPH/EPV*/ + /* hysteresis for EPH/EPV */ bool eph_epv_good; if (status.condition_global_position_valid) { if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { From 0b97dd2b776ce61fd53776f036230ea0089e26e9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 9 Apr 2014 18:55:55 +0400 Subject: [PATCH 41/53] commander: brackets added to return switch check --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index aade0d862e..d4567c4f11 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1236,7 +1236,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state = NAV_STATE_MISSION; status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE && + } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && pos_sp_triplet.nav_state == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ status.set_nav_state = NAV_STATE_MISSION; From a1cf8801bb000e38d11b4573d7cde452f02abbc3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 22 Apr 2014 11:36:25 +0200 Subject: [PATCH 42/53] sdlog2: add failsafe state logging --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e026753dce..1ca8cf8c59 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -960,6 +960,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; + log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 9a02fa2a64..753438d7bc 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -154,6 +154,7 @@ struct log_ATTC_s { struct log_STAT_s { uint8_t main_state; uint8_t arming_state; + uint8_t failsafe_state; float battery_remaining; uint8_t battery_warning; uint8_t landed; @@ -350,7 +351,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), + LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), From 60554c8a5682bc5b2edb66e1ca6b7a9163b1dbf9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Apr 2014 14:15:59 +0200 Subject: [PATCH 43/53] navigator: publish global_position_setpoint on vehicle_status updates --- src/modules/navigator/navigator_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d8f7c1273b..37009ff699 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -692,6 +692,9 @@ Navigator::task_main() /* evaluate state machine from commander and set the navigator mode accordingly */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + /* publish position setpoint triplet on each status update if navigator active */ + _pos_sp_triplet_updated = true; + bool stick_mode = false; if (!_vstatus.rc_signal_lost) { @@ -813,8 +816,8 @@ Navigator::task_main() if (fds[1].revents & POLLIN) { global_position_update(); - /* publish position setpoint triplet on each position update if navigator active */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + /* publish position setpoint triplet on each position update if navigator active */ _pos_sp_triplet_updated = true; if (myState == NAV_STATE_LAND && !_global_pos_valid) { From 320c97c498cc6e8f2634f88147f0ef15ca9b24e3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Apr 2014 15:24:45 +0200 Subject: [PATCH 44/53] navigator: check if mission reached on vehicle_status updates --- src/modules/navigator/navigator_main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 37009ff699..d4d23396a1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -774,6 +774,13 @@ Navigator::task_main() } } + /* check if waypoint has been reached in MISSION, RTL and LAND modes */ + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { + if (check_mission_item_reached()) { + on_mission_item_reached(); + } + } + } else { /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); From 34597599fcac2d76e9c8f35b12f18f1bbcb04bbe Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Apr 2014 15:26:24 +0200 Subject: [PATCH 45/53] navigator: merging bug fixed --- src/modules/navigator/navigator_main.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 29b82fe755..12fd35a0a9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -692,6 +692,9 @@ Navigator::task_main() /* evaluate state requested by commander */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + /* publish position setpoint triplet on each status update if navigator active */ + _pos_sp_triplet_updated = true; + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { /* commander requested new navigation mode, try to set it */ switch (_vstatus.set_nav_state) { From 56592ec77d3863f135c10619b15a0591f957fdbf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Apr 2014 19:01:05 +0200 Subject: [PATCH 46/53] commander: don't start RTL on failsafe if landed --- src/modules/commander/commander.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2712094129..ac433b1c77 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1015,7 +1015,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - /* update subsystem */ + /* update position setpoint triplet */ orb_check(pos_sp_triplet_sub, &updated); if (updated) { @@ -1273,10 +1273,15 @@ int commander_thread_main(int argc, char *argv[]) } else { /* failsafe for manual modes */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + transition_result_t res = TRANSITION_DENIED; + + if (!status.condition_landed) { + /* vehicle is not landed, try to perform RTL */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + } if (res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate), try LAND */ + /* RTL not allowed (no global position estimate) or not wanted, try LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); if (res == TRANSITION_DENIED) { From 63c50676f9757f18dfca9ec3735ce59a045cea33 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Apr 2014 22:38:19 +0200 Subject: [PATCH 47/53] MISSION switch renamed to LOITER --- src/modules/commander/commander.cpp | 4 ++-- src/modules/sensors/sensor_params.c | 4 ++-- src/modules/sensors/sensors.cpp | 12 ++++++------ src/modules/uORB/topics/manual_control_setpoint.h | 2 +- src/modules/uORB/topics/rc_channels.h | 2 +- 5 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ac433b1c77..67b9d16e89 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1231,12 +1231,12 @@ int commander_thread_main(int argc, char *argv[]) } else { /* MISSION switch */ - if (sp_man.mission_switch == SWITCH_POS_ON) { + if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { /* stick is in MISSION position */ status.set_nav_state = NAV_STATE_MISSION; status.set_nav_state_timestamp = hrt_absolute_time(); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index a1f2a4ad52..bc49f5c85e 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -594,13 +594,13 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); /** - * Mission switch channel mapping. + * Loiter switch channel mapping. * * @min 0 * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ba233dfd00..41e2148ac3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -254,7 +254,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; int rc_map_assisted_sw; - int rc_map_mission_sw; + int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -297,7 +297,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; param_t rc_map_assisted_sw; - param_t rc_map_mission_sw; + param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -508,7 +508,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); - _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -654,7 +654,7 @@ Sensors::parameters_update() warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx(paramerr); } @@ -682,7 +682,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; - _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1416,7 +1416,7 @@ Sensors::rc_poll() /* mode switches */ manual.mode_switch = get_rc_switch_position(MODE); manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.mission_switch = get_rc_switch_position(MISSION); + manual.loiter_switch = get_rc_switch_position(LOITER); manual.return_switch = get_rc_switch_position(RETURN); /* publish manual_control_setpoint topic */ diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 2b3a337b29..a23d89cd27 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -79,7 +79,7 @@ struct manual_control_setpoint_s { switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 3246a39dd6..c168b2facb 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -65,7 +65,7 @@ enum RC_CHANNELS_FUNCTION { MODE = 4, RETURN = 5, ASSISTED = 6, - MISSION = 7, + LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, AUX_1 = 10, From 2453b354faa5a6ccdcca6afb94b54967679cc9de Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Apr 2014 22:26:51 +0200 Subject: [PATCH 48/53] Failsafe landing without position control fixed --- src/modules/commander/commander.cpp | 7 +++++-- src/modules/mc_pos_control/mc_pos_control_main.cpp | 12 ++++++++++++ 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1827f252cc..0bf7522792 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1747,8 +1747,11 @@ set_control_mode() control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; + + /* in failsafe LAND mode position may be not available */ + control_mode.flag_control_position_enabled = status.condition_local_position_valid; + control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; + control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6b797f222b..5194ef697b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1001,6 +1001,18 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = euler(0); _att_sp.pitch_body = euler(1); /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + + } else if (!_control_mode.flag_control_manual_enabled) { + /* autonomous altitude control without position control (failsafe landing), + * force level attitude, don't change yaw */ + R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; } _att_sp.thrust = thrust_abs; From ac0b50eaa44521998e57d8d1203a25dc997108f5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Apr 2014 22:41:26 +0200 Subject: [PATCH 49/53] rc_mode_switch diagram updated --- Documentation/rc_mode_switch.odg | Bin 22232 -> 22421 bytes Documentation/rc_mode_switch.pdf | Bin 28728 -> 29666 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg index 29d738c39c9598003b84fe5bd1565cfdff644ee0..1ef05458fc1f638b1c190a8054d208c2e238f34e 100644 GIT binary patch delta 20965 zcmZ^|V{qq9(C__^y|J^gla0MGH@0ot{Kdv*V`t+HH@0otw(X~`z0dueSEoKN=Eb+G zr)H|Fr>A?V0xYQt?2Cdl1SAFs1PcNk7Nm*Bf02d!x9SrBtHdawm+zSIt8@pwW{cvn zk81Z*^J*(9C$=F01$7)6f*i-4j19ZxvoWPVno~W^Hrzu==6kEH)vHE4ZO3`KX{6Lf zVCm|YD-tdOw6l&mRq?U5DQ81<+fh_iNimoVbC1J{J?1TACE7^NqRbdDV9?0j*efMr*gT{|Bti(o zRA~+aOtP$^%o8ZWn3>hXrN4OkuwnK#Vr=I&IdFW3kI2L(sUqBmOpBh#wDzsF-hy%_ zsG8h)I@XuVo3a+1KFjXl6ZC4Vvo_ ziY1o%4_-gQ?2yoo5;AJ1RdlA?$HJ4s>v8Oa*-!{(T#F(oV#t{~*rvz1=KFTi{CO!V zv(j&WGC+@n*uRZ*4IYVP*`>^;v9iykjIIhNKBbfyo3tDQKB|Am$k9_MO`ke>bYIxV zeU{DSr!+OOQII^@+C{DGsS=CG#~R?d?o)j?av&gG8ma-af*2De_8*QO`etPA`M}P$j`n6rBmg;HK%T8rgF}9B5{7t;Y^PiO=-jDW?8c_w*QBd@ zAE3eN6&qY-PC%d(=`Q)xMSbGALnfjZR`p&DAy9}RA{d=+fGPvAX_k1_@`U6A&9o?b z>5$<+^*eI^t-*c@rYnrE%SY71jr^oF3YPbWBQz(U%x7Sw1*<2sb23v5>YncI{<58h(QbvNubacy}poy9b9;X)>vpgJNI>* zUmZ)iE<27vW2CP!3YqKey_6xwKssNI>)Ba*zSfl3{Rh|w-Ga-XSMc^nS(-NF`QqfC ztDkI9aA&j7{Vtn*cisHaOeVMo`1R_*FG~+j#R-}OX$j}`C6v*xNM7)7tzEBM*uHI2)`<4f}+KsN8PEI<_in1`72#riGM3`yh+|z&F)Af7xR7=DO;!FRrY2kcG zcz@cf#EG;T&dz?g$O0sIroQG}{7(p175eDVb%?^a93Y|qtg{jh-Y2V%kqgK=sg@$? z^;hvO7lKfIRZ03Tp_Hxq)PJMpZBj{SiGJ10QCvI>XHMKBJhMWoDUTt&*g{L4DqJNy z=^DzX(`~KHA(jm@tZh?Jb?92C6N@&1ej0JbwkQ)M9{CBf+SOBj1%-C$LD|Q6^=Q_A z@fB5ZmB#-q)|UT1YnOLvE(mOqz!kOo`S49{LTsNft~A@xen8?@H)Y-miRz%5x)z;Y zHSsDJ-$C`-!1t{}x^8utID$3~xU^2>WJ%F&IZP*T+GB!?Lb>C&_&3BbB`A~bQbm40 z)|NU9B7W&%(iY&RMt`0ELd&UielD7l3~mn_AOW`_D|3jWiVRN3%mt8o&sWrox6UzA zjO@-~u%vxL{PcL4qgO>Nh`z#wJ#@o>)U>U1BNN`Tm*|^Zj*o7>*kd1L_@9Jm@j?* zqhy{?c5K^Y&dXMye$;o6pdrJrot<#eyZ73MgHYE!E9Iu|TBj@>S>EOfDptKpiT5XT z5pNXVj2l?!vI>{c(JmAo-kfN2%|1$jC zl>l5V6bOVy@&9M||4r~#u!KPDDp&GHr#y*=8@8l<29~S3ZE+wX4+NWnBo+;4k@+Vi zZCVZcH)%Y{O6ktNkG01PDqN*srcyNw48sg5&fqGgG<}K(RTMNJ<1|vrcp8;wd{@AAyjh~Hh9Frgz&b45w-wDL1! zw$JB6%!Kc5d~8N3|7JNW*ZyE|DT4>B{)MLwtue1Z|L~==Go$<$d0YB8AtB*ZP;{UL zcW;s9gp%d&zq$noobfDf=2y{> zV<5-Q?VVQZ(HC1$Ik8?Q&YRO&(3}4 z&*@;q{v|znm2Q-W?U#3)Y{%)3lo2 z<0u0Ks|BML)k{D|Vky}qb*3Uy+D7Z%wtNEG_j-4k+8Ji$lFN}_lsnD2>=0Py^s-=7 zb!8XfbXr(zCy@9+e7%U7;0?~NAyd*(=~~?+DQf0O>9%iU7?+#si1RKjv6fZ&`E;;V zO>3rskqdF}k;Y`S8eH>ebVo*m)tP73ZO@Ci39`5X%Fw{bX5YAU!SKBs+OGgPUOGZ9 z7Zw&*`V1NDzK5OD92OQDLWE`WqQ}8OJ0oS2jz$Z;1x&N^s%&NF{vPuiVxRG-QrhbrHJrXrUC9(a-byQdi(B}{WFk&l z)q@jz5&GnMt1G*1v--`YH?>>K3MxkIcj@FNvAIwf^IjL5d>x$Qxj}F1B zSGM~SXrXEo6cQC_QVyIT&}qpu>>p?rk%&p6Q7{1K9b_aWvqEvqy4B*eMz!xFK(ppMn|b~?^@N*nh5um1g<1M{I?k^g4uPU z;$J1Yx_qrXFMb(aZE`y9@W{^xKbE6CFf6YWzO{Xetsiy4g+iXibWEXxe~;boS2=;4 z(?9?kMlvadhU$7EN9J}Q-GzSrDI+%R zG-=L|oxykS33x)B(liKQvRk*ZF)p2s1Xh6w3L%SSJ7KyQv1 zQ_^$!A`-Faz_8xtz7zN1n!Cq@-wvBETMzvpV<})?pL+G3YlT(XhT-H?(*jJ(0zp zIaj3~y9x^rR0%J$N_G#_W26MjjEVE6#XzXjmW5xa_9-#vU3Z5`GYbQ<1I1(I zpxUfdJ>!WI@3ytb`FP?|$0DW8<1^s@xbWBc$h(&JPxRo-*;kv}(uQ@ZC@HAwR~SqK zc9{mA!a|!js0iLgD$NA!d=qujQLcKU#hcM(cVR5No%lhWwN0LSAwX_{+iBf;_p1Mt zp?==C%0b=zIJadnvuLo~Tbl8*%5l8*ai?OQJNEW@e1n|(iRH%fNBphs%K&h9iH;PN z9#(F5q(0qmtyjW)N)qwyeu%Hx1O)A;bDnxtv<|6f$zln8k?va&&o6d zn%NxWvNTm%m8`!8`LgVvx?yMi&Lo|)SyweXb3WVQIPUCs+tGgiU2cEUe#e~!BYP1g z+>#GEK6AR%Ob`D}qGT_=OaZ{pHCEj=LI3M%o0?T#);<#P*w#S#T{xlhZ@c)pem&;) z@Y(zq(|om>20Z*fzlycH#;g<5=R)$~CovV$?!rD+Lek#g;VKo5w=>+B!_Wr*+FU z5~ePuNK2ILbGiPAbN9B(wEvlS;uu8b10J`A&bTSjyy`(H87woDJ$nOCEf31DnI3v> z%6>e5eyXcv&;^(Iq=(spc`kfS!f2(|YCx*nG}11^2y;WDYw^|EgzERKlN}2Un0&Z$ z%gS7h-ia#Mbr2aY*iD4V5Zn~6?A$lNqN zImtfhw}Kcml4sKM{5W$U2ch56m<@pjJ973Q9IHhp(~PblUZ$XfA@`u~S;u?96-)KXU)Y}_L*?U*m?fk zalqZV1eRWL!AzTRi0AF?CH(?vo${0GbLL3rguPQ)E!!#)r!{tS_A#tscYekKgxVw5G`Cr1xx1}jwtg`75#=cQE}4*gg_(w$aOMs2K6R-u!* z~+S@R= z);BVXCKY_OI^$!3_@AfkWN_!tiB=utu-+!hWy>=T^Oo-773s_fvFGv1o7y5)M-Mgw z9q?`kEckP0W8v1mRJDjX@edavDZ9!Kk8FrWoUMD&lm&QlQZr8h>!Cteg(Y)3Sku*F zMRCefL>GWpZ(cP1-hwgZq&N%2>pM;j=9NY|-Gq<)tAVOju<9COSzPLg(iX+LLGI3O zeqXZZcebR4nQ!S>C#*>mU~jJE>F{A6Xgxa)PoH<&w^c;`VE@BJ09Xr?nq8lUC)a@0 zkP7=3W7h&hUz`D{gj3|k#?G|~fe%D#1Zn_*`ai5oQy-0g4E1?%jeYQZf}^g4V@GEQG*QHEqdm?>riz1S3sB1HVo=tCBmZ$IJ;K=TTXx)rkW-UG?Mm-6XGiogo}zz1)Y&rw zqkyG=rSO2L;P$4g10|W)J-JNdVx_T5b&jDsKQkkIuA{}9gH*R|BY4ls#V~{=4pDSE zviI_^8}^(durU?@?+?KH!~4Te_`|mltJwxGp>p8SqF5)saZRq8JUwkc*n_J6hboX> zy>XBAdJeM#dWxK(z`K+8>uAd4mMyf$fB5ZRor%`{dUepW?=x*jPeCHTT>g^ybS?;m zTL{9(O!I2MZ}L9Xy=n?MnegqBPd-J!2n+wIspbLHnl;){3Ekc06C-*I6r4X4;6KVsBS3vc+T?k^+dqu^YYV?x)MeadT$Mf8ap<0YOqYT^7tQ)R(u=m6#ZHnE3i9iu2 zr}X;-Kl+~2PHJU0d?0&#k&F>vbgYYHNvgW5E`sx?h9j2*;94j};T%N2OLL>Pa@VNP z>Tn1`g7my9Ad6ZIl!TGtF_-~Mh*bv!j&b-_Mn%Q}>`kv97MTv-Znayiv7`M;# z1J1IT5O+!R7fH%p;*CA7ZS+E4R#>@Qeo-ZUh1VCH{$4CYc$RtJSDcF8)CC2A6DE2$ zqf7P&c^h1XtW0pdbeQW_r!CRcAi<!a*x9UQse{uFnj2M zGrV)ux_DlAc@-8VWVNy)Ux&ZqWec@|)TH9=X<1^nR~1p%Kz<}w2%1~iEs&j4qxzAG zP?lej5EVk{m_bEgMPNmpRH^)aHtLu~NWrqO=isT(|KQItNTroy)!**2C-%3%>9Xvy z>^hF>I@XLu9Q|enT)uu(+amKnKgMCI2^{rtGT4Tv{Ng>o@c!BbqFmL1*208{D&ajMOCufTIJH1o*)BocP z*Z-{AS2*>xZIC80NL0GqyzC&oSAsUaF`@$GcOD{RC);(Hi$GN(L9Bl~APPPledLbZ zBd7vDH#~*Z@*5Vtth-!#Nnl1H}6MANTp^Sf%)x0s-Fp>m;e z`55ewi>Fj2w2CY7daM?)%{pw35GWWG8Jja?$dPrY1@90XupO`+ z7MC3sqgrlh%4k%J<2f*lx{HzdeFe=057lEQ-b~+WyBu{gbgCsPJJecSp)^2fgS{6NT|XU2I%f3MTJy6 zGS9O-1(nr11}Bl4ms9X-1ksXl9#pYVX(A(=>|M|23M=Xx>a#32PwQqid2EZT>{l$a zI_%t9+*>Tl8s?|9DqOL}1TipBG$Bpo!Gp`mIrlm;AZf&cOQ_+va|C{6WH?XoJsv#r ztuh`G&U;M8l4LGdkILc;jx(SqVI`q-d=*(Dp7I%hCkb{}03I`%ihb9zB>bVe*t zATimmFPZ3vqaJ0TAE!j9y<-S)t10DiZnW7EwI2VSzfSORos*F&J+amMatHW;d9zUo41VVy1I{ z_4c;@8(~OF%jA}hl6O?c#1F@&YiIwh)dZgJvG?_*X@0UI=KaQg^7t3l6}?(^QEne8 zbA#)gEMlXw7>gEuWvQQW98bKy(j1?k(_iM!kq?&xQ3ikbCc`#B2DI4V^S1yqURI)k zP?%7Q^6h0-dBib`=sZsAak>?Ws9oo4CWcILf_u0Vv-kY=tZ0G3Nf;atnqQ~Q{E8EX4kSTC*dFaS{LqlAFaf&%C5N4$wpU4Yt7O}i)AC~ zRo2z^yXDg$;0!}h!k43eFd_A)emSpR;JrR$=`sV)NAhnqGr+2dNKc!V?Zr|1_AHfs zDa6dO77YUsC=Q7hnH(%FYs9Io2Sq$lKcf_xSkzMrqRaaJMmW5kW{8PjSl5X{cY-d5 zutWLpR5F?^!2?<>sJU*+PG6$rvpui%8Sis_AaxaV6J?F7jZSILmz1FY!ZE?8-W=)Y zw~jS|fj%nqTh6Y4N+$FJGin<88r@y4-T}ed6@VJO_Bb{5ZD~p;s=u!}{eTT?0Xv?T z@)ZSXUv{t2uPE6Jol%(5E%BNDkj^2LyiVdFJEw&->Rcg_)wQ~b5sD*}`#TbScJ+x& zzJsuWI(re$6b!MP&EI2W~z@i#BTXMdF>l$d5nm}k z0jR4GW5M}bkUQnm{IOtnpwa)jIUyZMx0Zv??%mRSK+5T=8e6(G*cQd5E5%u7-Ec`v z9`So^;n(ONedF?|b;*e? zKO-RJTg%$ZX?eN1d3l=huez`g!KCx`Z_faUb(tf@q&LF20w5Fo3lJ zQ{rL&SQtvztM6#=C&O}6$L}kLiXU^vkf9S_l4YcpEOC#g{SJPWoGsVA)Nn}q?kCB8 z3*zSX_Hx%uIykHL;!BucUhX#_WrN%rkYkOKDL8ea5u-feICkn>wL+CP)iGF1#9nk0 zg$vGqF6YIuN9zKKuLoczXA&l78UYZaXOsjvrUk|tjass3z7JQr|qZPM&48gC%Vg=XyNR*wyLXJ~(ERhlkW9-23z9fAN7y zFp^sT5-W3iz>RJ<7n}+$jGc1R(ufmarBRysBX-APCT?O z5X!1xoSW}Eyt6KP3}QEauVsaP%@ctAZnTTQ*b)mBR44*pzFA#1>5l{}(k-08qz)g; zNgLH4I3Pf-M+So3bKCj~el@p4v~%wf1|t5MVZ4+#ivS6V0*20e5gyCiTqQux<&pIad{R0iOjr;|B1c3 z|NLh4?f&Oi^1l}{#vBgR01&(5Orn7xox-k(!Mb*coDQ#v!32rj3FT2i^dBVofyJ;O za9m7Y;s2aPV-b>oU`^_90N?*?St6r5K*T>4O%nfYd<1R{3YTY12lwy)M2X&dAi5$! z1}g{S#n!dG=>dZuJ6{5 zS`pPBm)UWphwZ@p(zl8Ng!?gl&zj~ybwz^94%W2;XvF#g)f5iHvylUNWa(YrL=$Gj z1lG@$y05V>lYT(Qb}!yO-rd#UG~FQ)U{8b=EU(+lgW__wu@1}PyujD6mGwx=S)K#u zF8dWr1Hyv=_wl`nxXbWla+WPa1@A)Ms(>`_<87aGOwf!=VAG#Ij>}7@j@18+Arl0kbko<+;yT)O+ zdh-Zx?&IM&2_A!38^V;|?RFv|js!%PHl->#UARP3r`c;x)BHZ|Ng9*zIgDmrbc^J* zZes>0$oN@=IV4aPMD)+2wVHRNJ}$mWSfOFB5t)vy-;Ll$AgC=d>cPV^UkE6nC!x6> zp8*u`Uwt}kTZOa$d%K}GL z#AHLV907#h+pUB>Di5>Fo?PvD<&vI@q`Je|U&z_`2^bj{VH4976@RT5ug-nv37oYPiFvy#m~*0^I`uC3LbZ@%Ijw9zL$LS z$%}5Ae9W6U$XPE}@%5lQNC5x}~X=AG&EwW_B zp8KENNClanD@PiW5k9JC9-f+p+^QI;krwH2cA)(B^66}TbFauuRb2<3sv0U`TmXmA zj?s39CZ0iEsj`)D;f}=p*ESSWZo5$O4iQ$v+acrRRF7r%@BRDs&sH_@ z83cGajMJYM+zC*y`Eq42frMp6f7=R&A+17~orUcq*6vU&@r|Gvz$<+p*Luh4H4Ww2aLwfR61L3(+e zmi%vf`dU2NtFCGRYSz;@sK5X0*aQKP;|BF)!u@UZbBsDZPElGIs1~$#`nL`T3yq8g zylETLr2}vbk!xVL5Z< z`HQN?Lw16!`U4@6tC9@M_Tt;$UAc>`4y}xS_|@Tn$*~STDQ$uBs+mY z!N7hM3(z!ICgSsfR-{ImTK{cs6ke3@hu*HBT)uKF336j*T-c6E1kPZpCi1NR*ZC7% z)M6_?DQM9;W(0DqRDIWiBP`(b`@H`Mi9+|2Oat7oA*({a2a7=@9l)y@5G{xr9-ia# zjLIJv&e_KQ0XDT!lKwfej%d)LbIkQOLxvv9fshUd)F~gxGHt#Yr?{&|_l=nqiF4+! zw`jTH3W-UYtd+mdI4D=SB8ze0O=jf_lj0C*jz(OmCS$xI2zL1AtXb?O3+geAbiI}^ z{lPDq30=NsP1zs)V?ZYP&FipZqgUh4)uHh_uph0>$D}@@cp%uG3tRbodVct)c=$uh zal_3DuLDUnlA@nH$pMU@C?^d{{Bx{m} z8&~=2nO72j+(gD{2}sTliwq2z5_Mu1{B*SC09o`(;C=<`lG@T*^T6X71hU0Ymu=1E z(BlKwSFH(s&J!B{2>87ek{o<^(gA?Mx^nG?=VZz!0Hv$$6!D8dPQx*n4D+{L{ks9u z7CYC+8v&^i#O{K`z)wu|gUV0hq*9LQ)uei+g4RUztAD_D5FQbZzy znOT4(Hm(KJbPf_k7l3bi5=zCUh5kiQYCsAqJ=u763rQYI^k@SSi$Fw$;!`k)-F#Rx zuAuH;s(VYK!>WP*L?!1U@eb`6gXi*V(l*Wo4CbIf0nx-^{GSAA?*?db6K+)yAYLKR zUo`({44v4EAY{(4tG!2o+oqXz)a_|(5G0EdEavu1gB)Z zOHDsY(1G!>^q#(eP=+d$l^iNnWs|O~Pmx6ZI~&;pKsHc9b}?5ae`!lgsT)ndd|d$j z)b5gYr&czp+DJv}3dIlKaRm2I0{p|aUf`r2&;J%NNTa&jyx&2F4I(&K@|NpokreYl>m7){$!S&gd%Va=ZeitH zL5Bj-flY!*YoRW?n4FTByx)YcuvVOUseip_#9;1~Ly}A)4ktflnD;YUybVmr{=BG! z$;L}s!xNecrQ$pSF!nffXqA>(DH09yJ@P=r?%Aj3+(v(;YB8Yf=NXFw%v`m4BLb!rfNFF)u=>fj1`oM)0%uIWrw}KmW7W7orfE5nA8vlCE+uhqj_-u*iV^53tUy z+=OQu_}vq`6L=er0rAZZAH)YzNF_*wzO#)!x+^(Y zTOgdOm}PAioHdHI8KxUvre1RvW|>@lsc96pcfK&n@nBA}k z$@~sa#(aWF8Ni)#F^`)w>}Kx@cx`GEsDyb*VHMFIMJr zBbd=GJq(WNztnt-7F4{h{w9N98U0*2Zk-RKW2lJIPY$rO3hrBFa_ZK#Co}12%bHgO z^W7{3x^I-NT^YP-%LDCp3C07!z$;ABXA(NE1$Zx z*)Dcq7FwPe>sEzs=9Z8@J~)H__>8r?q6T2Yfe^mSY7v`V#&*2$|%^d8U8$>wFOG3)M=^}4ez8CozSLLe-LDs zmy}zgT%VV&3_yR`zvW{GoH5qYrmI66upl->iXRCkzu72hYq8ygIAf5#64=3r@UFJh z7LL_L)N6mpr`!x;U$(b>bT4|sUZ+f|1GiPBApMw+|J@9V6WlWcaL@23b= ze2E@Z&KpD`VFTbtyaw#4{u$b)-U*;e1jWu%ndo%45y1d&c{oHPMiI&vgQef)z-> z$ZL%aUqIr-oL9zLOQd)9IiDfsoB~c4E47>~jK8l|1*wE`C;@-xO0I#~I0$O{f>ruP_CS{(Md* z0-*{#-;nu0q6*w8kxsO^JNeRWNen^mah_ga$5-9vyqjz5QZRe#ZQXMs6DUBoyp%(d zwwPy!OF`WzWH763BE4CFKWhzg?Gosj7ysC5f>dCLsYeB5Xj%5x*=`b;j7t?Nu%n=e zySD><*~>YFl;pA(U>wke_1jGGneCKLfQ^qCoLQ}*nBN(b35UZyP>pWNN2I@h=9gMG zKc-=SaM<7G@Y8r5R-@u@G;vd|*LJiCty>t~V*W;)+$6W(Gq}#`-G&`;y2vQm(tFRd z-|Yj1wDtnvzR_obCYvbaO&@O}lO`vj0)3@*7#$u5GP>;x_8r1TEaRq%8mAZ7vl`0( zN^fhI9Tnh9Ncu;sdVXDg(qv6<>m&z<+G1Til#znejfNh9yTvTQ!nduC@pZ~m0vGB9 z<0L_2L#GgHZ#B1q+pK=0fU=M%QOZ-I)EL7>fZQQ0d{hs^eYd(z3A8m3_GZr$zdD}l z^z}a4b2m6OqgNY>>EWu*%5YB1!`vVIN|d)&3nxJk%!4gk?OQZoE~pJ!Xb z#M$61jaQvIDvxShT5Kixy5dVCQ|1)Ai~P zfM3Chmui*pw3%LteH#fMiMHTVDrnFaJnQ>WN^WfSMr{+uOezTWL?H61Ze^i5r+#w4 zca?e(wfT;itxeEgCf-cBeN#rHP9+Q`*cK;L7V=x0UPZc>)86)N$Ka`10>YK5ncxe; zZ{}3C#+1B!-dqszmo=L&X-CYT?|m!J0GK@BmFHSL9{FAV;C#7d6eWS#UEJRr=zgTm z1rc)!PWX0`#PGVl(@A|}=vtr|QobqXkDkV>@V^XMK>p97Zyrv(1azwH7Df@@O^ISM z26hM`=z}d!*i57uirp0^#Q^d2n=;9o*DRC9&XPiGw{45%K3WZi;KUN2?S*8n)zsugTL+ z-(8M8@|YgyX;*Uotor0GX%RxdEu7>oG_tfsT#n6XI)UpsXXrtx^tb;iVuq=MESs^^ zGYwo#WlfvpIAlVbc*LH$82}m_LNok#b6XH5dLR;FKFxQhg6u{Uy_5RMlHYBJhtAPf zuZ+*+Y=zk8*VVsBE-`i-n^Y?&bLxv~b~LW9(XD3JGAR(`eF3h$%B1+az(DkV9&)V- z6R@6=*^_|W!BQG}drOu8EyJ-)6muj{sB0*CQLVta=-{l|Id@cE5-`X(OZIl`?dD4s zo^?MY^IB&A6|$=b9&fUNtv0aN7O9v~Q(4PdVCXdaX$B$6t^F>O!%PijdM@t}^6x!G zF+PjRp_Yr!{4v?!q&Z)Mqnly`!^G1%LMsDP_21^%AUB_Q0c*#lZDn0&bJ|Jlxq;$1 zZ+B@Sr$kl!p9c0;aKKN+lj|*%FX*O2ho%w;m_p;5gh#0vff%2E_f@Q2no54RXc7*5 zSB#eX(t|`@u`0;zdm~Y#;ZxWaI%XUETO6%ccn4qhkhzx$oH*(V8KDZ}Vq-B;A1ytd zc&mOc5f7!hq>Q$)qBfb%z)kx&mNHh>+MLcNM$FIA1(S-dd>7Dm3%5B+d<~G8%yhaI zlsr-K+~1czzefZD(X;s4TUUOho+~cbTHc#hP_oNm8;Y7E==RlXWvGZ6aAif)ps}8~ z`1!G`VrGdR1^%!L@0)__`$ed|vW_|j&9|+ARW=A47}RS-RfunW`|gojKhNuHs3xym zs6jcy&YoaSZ3O(uNy4K(tZ%SVPM)y#y|p~eD{cWR8SWgyCX2Bm?JCK4K2HClLwV?W zeC(mSjH%_IL0Vj1oi>3j84$2P_Kim6E!huYh?*%HL=f`?r=l(-7YNd+Vy8uCD$|R} zUnYRAyi|dp4HJagWnHEu4;vT6+#U9(5h;4HMmcUr<^%S4>eB1Xx(-?gR%!?sM@`!Q zXx3SM*Op-}ZMDQf&YBLnZH8sp4G@qKVW#An#q|9E7u=$ZiKiX#!VakpSv_(bB}_k~ zPufkYR*rVzz};{xSHp)ilJmmy9H9jkIs{7ROmIvl#-PBX#mv~h!<;%EjKptYoF4~*xamLIV+i zazi>rA(DaqHAC?(!gVH-@NATYcp)3p1sfjhA2Ka6F>P1xl|6r~katQ$sRkPb&WR5K zUUGY7MuVfH@S}h7!WBekWgH-I&P`(;?zm@__sUNKXFZenP)EA19K0>jASc(^Pl;mu+*BsNL9>9u0MNOxj z;CPalLR9-ssxmez{NB?~)mS2i8c?+Jz>aKg=ld!=l5qUB!YXqs@5gMMx5_s!tJ$Wb zdC4H1!k4>hqAyHnkRL>E1{pL}yi4>EQ7Ar&Q;8fCxoFf3nWbs<^!d-i#wRy0?+m_= zm9rJ!zv=pHubEm#=N0Br&;!37N03eFvY%N`hpabU`{ceHH?OUIONlBOhdL)gxB1Fd zTbNr^aNUiBAT8~pai9D(WvoC7|D2_>WwhpLN?8K=$X}rJPk;Y#baNtp#YG{Qtyc@8(((H8P^V3TBFvEMbMy$*0}wUYJ#+&piW zUWB8sGHB)Pl4pYMwTX0#sQM`BrXt&{3R;%y+{F62-zf?d-50XHaY8|c#3Eya9ZN@b ze0*5+tQ05Hx%NvS2c8L(PWrZb0*5pG)6Wu&&r#QJm1F8Dx+KnM*#tT|OH!5$9HM4R zj2MMgW8Bs625UQMw!k2~$iV^5iH)+T>~m?jON91!u~)9%MG)-No-W#&Ki$z+a3%C5d3HhXUtJV;c83Z9QW|>?=##y~;{a1}Q5utFMs8wc?$>`$As4hCZud#;T zdy<@7SHAG58x#)ptNE&9%b`1Y+b}OX=I-doT6^<}^dYzC{82L4pKSNrp3J~TDN?(k z&9b%nZ?x#Z0vJ=UQ!{E<%r#8E_p9@DpPOKq76h}MenCL>C#kR%6%6(e7T1`T*Kb^8 zb#2SbKVfJgm!jPP7bUmYNWrh`@)BF^Hwc*;Yu>ay&took>8O|u-{s=`F?=``Et`Cd zPLY zF{jwf zIrli;-At}lXU;2vON<8FI30nAoZ0efSad5bimL0@!eJ@xUOKsgp-~8w`!ZseWZ->_}C!yb*IZsO5=ccZw%84IG!IJ-)@@I%8 z?nd<1ykL$7K}Rpa>QpLXQJ(Q_AJ%>7E#VBf`Jw5%#-aI>U_vw+a%Yv(cbsYga^{Sd z1Tu$0IjH(juFs~rhq?hY^wFQEOtDArqz~Mo{d5a=S>(p|>%FS#Y*<8xTw!pTp|K&9 zhLx+>kGi8!?KxCSm{4m!uyg$GM=D{Y+b14qD^Ha)sSWNAAn!c8s{>G%afo7>ntvQQK>>`rht1qqbmG(lKI@)oMT>MV)!iuQgCzO^bt0Z@Zt^n{<-Q~s)qLNvoE&F z@9tky3Xby>&ORvpJ?fCvDC$<7cFJk7XHUNo3Tdb&q!XvtuAiD%UmYC)Ds=&D)$KTa z)xcD5=(KB#7Kci?9r?xI0jM{-s1Re-6 zm0Z##Dbp4WQgehc@4Fr#K$up@waIl6!A>)fjvzo-cC>Ixz+cpw_IZ1DSnXUBjdahY zi^nj4d`{Qu_OQDp#~_L-+p3U@oJC;xO0$@W(zj(Uw+BwnVJx-JSv)Q}^zixIN2=^p zE_*H*-_w3o%fOIsds%&KDcn7s{NO+2SWkr@jKyC=9scO8*%Kn$JPo;$&VlvWdxkqIP_xWp-tcTjn(3f29@3wOEK zqKOR5wIIkbFZlmDxz4Djwr(3b0@AzGpcJKdq$x^5uL+2h(0i58(NHf4NPy5mnn36v zAiYXa1Zj~j(xe0gM0)=QdhgTkjyK+qw}0)m*4*o?)5e(ljI*Xy>R6a@464yG!j6Y} zZ}3IS>WCd{TUHOLNqmAWndGSlFAsxCn)N7iFu>}~Sg9%wzweK2?#x6Fq>7yDL6e_z z4cAQyxJy(2f|{74W{q$%X7?6%&3*TQRjAV10&@TvS5O=I9mE(DrHzhAZ{_3C*70o; zjm>&eyd1NoruA^hryvh#tyw_yyA%COoB{3VhS+ z2g?+Lp0@pT)e}j*-Y5P9hNk|+ci=6!{tYc;K4*r*RD?SYIb39mn0sKx?yJ|$&e^VS zLLA=RVi@j=7{OnE!Owyp{F!#E*5ofLfWx^UhNs-yTGR`wUS&kEga6qPx3P?!bMF?< z3=PknyB`KKrOVY)Q5Hod`lc=RiOOo_j`Es}Ou>ms%pz%)qt-o1XZi)ugGt($TlQ%6ifwLR3|crRzoD!N&slQ)gbGF3S_S z&ca=S>v*4wLj4noOtuQqn&j0%u8JVctFqBr2S#^R7(*9v5P$h;()p}J0qc_3)H^Mm zWCc;1AtJO#M+R+=3z9@|Dv;CJHBK2iRdKk%@1ovMw&{qO1Rw`-lmkd-(#z6qv3p}5 zHcYw^ffq_nAlr`m%M%~G&YnrR^`7e3UG!emO_ze@v`ZtM@ru*q0z-l7RN)G%wmi(P z|FX$_HWf4b{+U-urlFBUMpT@FXXH5BQdQ_v8DZWuyn4LnV0iz}&(PZ^$Yx_`x{qp3{pgvD3i>(qD$)=qD-mH! zYF5n)7(=}Y#~97%dNsaj%_>-;W7RcIvIOlXJ=P0;BFI|5V=FMwH2fFe+CGoFB!xJw z6v2WQLuC%GSbw%6esfhoTYrc zS1r<_rif4XiLAMGMS9){B~V1nX4g@4U}>jaEtz&q=qcb~(@t8FDai4ptJK8I_|>?$ zgq;?}x=9Z`@$z1;R=njx=q}dwU7)g{aGkM3V0seSe1ub<;a!rv?p)ni^?bxb^wP_k z8Un^OxS++%5KTUmE^W5gg*OUQYy&B=c8EB3h{&-U1H)L_RT zVC@at_?Pn;6Ro*tS?2Jwuhxc!Ks?~t^s7LLN7)gReM0)|m0~Ul+h1^-DtKRQ>XK(B zDon29D_nzz-4A45BM$vP|J40>i6>R!7x9s|dJH83qW`JJf>H;`{F-!tI8bsBGO|B4 zTTmS!@!!#be@BJElE0!kU=Gwg2p<&-CjZAF`fD-{*8j)Dfl@ReL0qN(r#&3-#~=X! z0G@C^XB$sJUl-?DW0>bs9Tj35Jb?Uy{nz$0cy(@gs9qN=$=5L?R5hMM%x0ouv6X8GzPd@M)A z=Q-7-CQx$<=gu Q>~IwX*O82UaGm|I-yc}$D>jJqHXb6DKZr^H5~GM7seZ;HeE z%zMPr4V3Pn0t%8xxg4u=7o(;UllOGV=|EqS`Hd)GMj{!bxg5>8Z{L%qrY};l-#$xy z=iR*-#KRQttR4L>wcv09Ri22L`rzT+JPVh~c=Fn#W%&t~*RkAPXV)e?lFpD(TD-8~ z(U!0NP_Zw~?LM9JxCscKK!OBaXf9_0%Qr`Edvbnr!IR~x=Nf6r2zh71fQha`q7`%X zZgWNb0T;ux@Gesu`t<5+@iSTHt#Ac^?)mP*(hyaigZ+pkMiI zk~MR~^^*!&pS_ySR@o12G#YvqRYc$^JZd6|ML8_)^&>eVY3Gw5LD4=|IhLeJaV@QqgsIO}`RYk$jG36SI7m2~2qP)1?R-x!VrOCu8aQJQ# zfqdpPAEP@xz>l_MLe$C;=!dkZv;ZWDZ$+q_*HMU*I8pfIC$Ld>wD_mf9GrY46CIiA zO#{o)akBEvxK*(!ft0G>mMCBQDwu~$5cv@TlCL~tm-1HjOWf{HcDik@EysQsr)FH!Ojk} zH}B3iXEzM&pwVnW4tb4A$#x=s)e`k?oY~7pgkV1>2JyHgvhThf&T=Pl-v+?4g;>H$ zv8_)&cK1iixK*Ph9!fmZJD)Nw5P=d7XgF=1^~+8M%Od;`6AnWIqaouOHx3xgFE;p^ zZC9*ZBI{d@+FZVW>);*oYFD(0*W+c$Ygu6_tntxX1*u*x<|ioDB#u`_yqxBY9?L^Q z244z(?(Y?gmDP+DuW^e5K%4~?eb+M@Qb#RFmWJ4+2mlSWJBF=dx|*0#Yni=x8F{d zM#uVE-~42^EZD)O!%DD%XU??g2+`=+z1UEQPt_VpPjhFnlvTrKS3}B6&vnW36$FH@ z8~r9(?duv_exMtN7shj*EhRLbTL*zBW9#(Tfx8u z5}rE<5^v^5O|NO#-tN7wT5j?xw{lQm>I?r zuzeR8`$^D(?9Ft@#k@Z4o+R!UH@8OfY#DE9j%SmElKY3i6n=lbEL1$@>7``DYprQg z2%?vxjFZYDpgfu$qq5Frj^voMb<)`*bXF~T9K;{?-tqdh4$CrR8tEcF-Hp~-{31AO zQe1~1OB+^Le86q2!ERrcw&_Ln4)QoXa_M$s30SlG^-O^L@f_k(Up7R;X1CN`Z?<&!;(3=rS;dzO3c@7A{5JlJ->PN4~i=Ermeh!^$_Hx zQBT$1w5<0o6)D9Qr<>IrSl$IlJ&0qVW+>Yb56!2IH|~u7I4+nW#ivc-ekj;qZH9!# z06XhjlGdU7`8>h~d>!1m&9tWK-_$rtj@4H4siV|yIDUleOK8-PGZQ^lL=~~*<=62@ zj?29rcIDa|fMHGz*DQ1!hHuVLUq@6uPoriKw-sLppcLU2Q$0E-50-9#z)KH#LMsvI z;O+}CzPST0N%69~zU#>E-$F$X8s(A2^=l3R4eRZ(b)fTvA6FZ`_E#gWk@h|tn-PiI z`0w(HAAG&Fz;E?Ecv<6V)(@aSj8GLz0;Ob#S?`{HTW*HlZ9T%$i)g*sK!gM8$r;1y z+w2^uSmchu)Ua#HhQnTIW}-2#OEB++D@m)lH5PxTN7`KL*vh_fWdY;l=zn&h&z8SE z&%xwE=ly^S6mX*-Hq_J>knoIibZa(NQh)Agn<*FAgmA{Eas4kjN^;tpz8T4B>rPp3LFt{9fw<;|tOll2?YR&`Kh` z=9bF!9SLOD&Ty;F<73<03?DcsHH?EIE81R3)hx1-sxqSI&`9lD1p-NUciCS-LinIg z4I4Kco!D2cz}^IYW=QUSpx`6OzEjF}_8_&Rf|zdbhT{WMqHt%Yl#BDum+?(paHrvp38SaaCl9sRLaeh(6Og+(S zR|NDehQ%23|3;eRlAtNec&0Z~*2%s8Yd94O7-+?w!z1KdZ& zL;o(b1or-u%!wxWMw%BNK6n276h7(#}JD{R)G9B8WI8-EU3RO-?2g;;6Fg%+Fcu^ z#4Rhb8M5sak>~ji-I5KpGN1lP!3@We8u|KH{Op%}yUwK{YMW4hd$db5h;ng%1u4tB zmOQvW`M>he^@JRV6?pOuLvE)N`TWtJLy&^VE@em)bN6Bix3Vdq41efB9}Fc9)>R!! zr%IUeb2$O>UkpNoG6`TogS)JPRMnimfg?4LYHlJttxmn&ZW#S|sF}D}48pA72zOFxzb2Aed;?OW6^i?o!I+#c!8&e#8A%(ftXuGBU4cEgIE(aoZ&`68z08DU>so zl;0p@rol%U*Rw`x9yu%C(N&QWLFdtP<^<2+uc{U z`>ye1&hnCCgp65`V?E4c-j>fH=LGG#a1oAp>9lIHMm7S;G;!moie+hboi1J#s@&Wk z>(vU#U>A8u&k%}g2SH_2z%yk6&S*lZB~b^f*xk%B52@XUwS9bhyb15dnNJmGE9!ZKp;0r5a@pg0u~k;_TPy}2mmJp_I34KcQ`QvF8>H3hHh zq;@T=;kO9Gat4oQRQ*&Tmm`+>CK}*R384edX>7jcQONI9U2w_A%gf8&ayBMsN2S(9qUH?8^A8O~ik4Ju^&Xp_*Subi_uWL|I`IqJvzQCqlH}vA- z7c`;^9F#c4m3Q!~>w`6sEcOr|jybyzAXspxVfEFxwo3ZSrCOWTF5ptqm1Ntsd2Xpq z>|J<-f6&nl3if95r;2HeW_#R8-Pyd>a$`n4U`b}4S6v&DiO1p4FtY5o7Z=!36SwwI z6JPBhJ+i_Dsr7fkv~S#(&oaD8HE@`;%G&76A>5tRk&G--vO~QD(-D49SO{_>1OYAL z8)MRJo)D=P!|oP6iO@IUC%w=%^YW+tk)oaDCvOr+Z*l!6#C0*Irtr-Qt7s1H20AyNpB5L%9G6l+;jKG%byv@yuxpaG9~CvP8kE}0@nj%5{3n_@Xk!-}EJy)M z4!zCDSoRL>Y74}If|f-T=5t97jzPETq(Kq)!SEe`xmEB{#S5U%AiLe-1JMq1&^`zL<{ z65$MX|DAT<-GCr6tJx(qp}*gLS?3V9A1-Z5;&5J`w*y0Hfz*#m@J<@17^y-QAjbi^ z^yr!ssUThtmq&@y51R;w5_+4115LyhE)-_aO^w#fC;?lNX^(tpzzwSt4ol&aem>Wu z3!SYHyZS zp-&=yod>rggS6Hxbbg{6nUecIfJP5KoB+YdOg5N^&M!iHX{$tk$}0W9-BY2e=)cS5 zd4qY8TGMW!vP!n zzZ4v{-Dxw(7k}=}bIx$xy|5V|o1TQ7OE#rj7D|ZqX9upd%#Znl#I}lmG)f5MY1)gT zpv|w(NFJd+?!DD1-o3-Z0{Rr)b(1kgMbCPqgzsXlb4dM$q~ug7zJl;!~Id`%%$9H$N5r(@cyY9 zA$%rPXupQ<*!_brzC@WA_mF92_nj;XwQSdiP^Zn08;A}_R$^{o3e@f zbA)VopZ=(N{x%K2CE22`aAA`*k6v|n7Kh?5!~)~E)qT@9)F2Ublq41Jr}PK<1hq-H z{&*||AJZ|{T7`?%kEtiJ7WoXCfMtjDjp|hBe6;8@954)_?lo_^dQQ(isJCHbZX2Jx zJamin+kR@&Af?N$@#b3oI~Bq*DkYPM$B*L z&t{H=?;hvhuK6EnhIlxy+53)9xUCRI7LE41-TZMB(y(mM(qHuRG6`~YYI$bvC$csk zslAFzhF}>h>_rpFc9=Q>Qv%mIfMUD7vOj`(5|550G{9(u{h9Y!h8h1cJ@-WLUxpW5 z^Leg?0)a3n|NjjCe+k|emKcc7<4*n%QWEj9ZaHLo2Bu9 z_&kRscP3NbgtuEXY|DIO`y5rzZfpJMjS1Rq!fvmDux-x`V5E?*32~~ zxbGQ4S)2`Mbu{N*YnT{7eYxjqt{j=svesEn;P$@pu~hmw+Klm%dN)31PVYV5`axOL zv%ltHCivfjG?mfi%pAvq!KDmdu-X?o4h&DJ2&u_?z?V;lO2+GJh5$c*4h$UBw8<}| z-qeDUpFNsia4N}}fux!Wyn7mEhAiV?OM+!Ag|Uto=j9sN4$F+ zT9$@3zxFvhenKxh6>Gs`+bGVDPy#ZCAz=$0moJ2?QGY+~cgHL0^XXmb&pZ&641^Tr z0oydMeV=x{pkYoo-C0jxPbelm0$#LB#3*bn3pAu3G_?3b$4cPkn5s)wY2|Geu)8G$ z(-LN@z7lD?z)WN}9z?=%yBx^}cS)6+?;wW+(uARtHOsAffljNpc ziS7!;s)zG(A~uMb!B|NqCAM~sRgHZlEJ^2*#-Oy^H~+uJ>55vcr|x!cP}>@`o=G{m z^QN7P*JxNv6Bh)YQ~lwlw9AWLx6JHWocnKMxHQZq00;Azgwt^f3W!WEO@5~(GqaOa z?97$9o1Iel`}4Ku42&*->DqaDjwN{DmGr6c(niR#e^GV@ONN>n6Rv$ zi3t8vFqtmt^0B5*4R%Ono8D1`yL0@BWpIv)^K)PKfc?_iib+CGhhZ~mI1@kel_xJ_@Lc8*?LGhRx)f<5@2i8`z_w>==QR;;>ScyrqZC9;1Z7I z1(w13sUrWW09pAp<5{9}IuvWjB4M>neBXN%Jo4Kk6<_6G(+rAVy3p3s2%WKFipbsv zvC~yndRD6u?(3ub@1}{_$}NGv!Sz^r&5;1t-`#UI1M%)&H<|VRc#&39A67Yca0`YE zUm$Ll0=8LGTeC^GAh?xO=84eWFkLE>q9URp0oA;JkGAbJF}=3Jet6rSVC_9qclbvJ z!w>f!h37U&!8%HwBen!&`|ZMCe4T}HOs-4)L5C{^)Dk+#ua-LAvZ(3F8C9~TDsPvg zgav8X*E4l#@bfRkQ*wK|ck%L#1bEQ%z`t_;O8AkoHp<&oZ()7HRiC<=$ zS0gxwCF5Vhc6RkaX17?a1(7)nx7e{MgD73wM0^}>MNd~}gs%;v=oP05iJq|xQE6`b zFA~rT(C8BiQn&Bj42wTUrtaQjSLL_vuLkUl=Sd2X6#L!bXYbT%xkhFn<}O0rqw`; z-YC5nvLOHftP9}QZ1L3iCU$^Gw%+kZg!zY z|7`vDrT5IF@FbQJ2G*-VHFgh$PJlL!ImP#mRq4X8bLEOh@m=<00ed+8G^@k>e3j!y zKik5f3^%H%O0i-RrCxE;#kRWJ)h3C2wGA8I_qrvaq2*FxZ6T9T{ZTjZu>W*}2mf|? z>RTLPDUZfi>Akbm^8nZ1wuVt}VDuOVb* zf!G(L@%#_5dQ6My^A5STM8(p2tUeov*UoBwy#t3U$b*x#uzppSZ*1m}%lov>?RC#y z|29(4mtdBEu=4F$;fvC*o?Coy=ZZnnc=j8GgZgeDSeCdMz8;KM6HAd3EMxK4@^H&a z*>A5&+l_U!^tYeqUu`~1y@6orS}nfBDBa<7E>2ZSrNgc<+vp|jb2r|F@@JzwjqN(( zp|*?QD2;?sH;M!Ilyfs^zDyyOzpZjo#rxdu(lPfw4!@a;)K8p$P@99rY^X6Cx;N|j z5-S8V#0gnmW}A`J=FZl~JlXO^qy@XdyVl;lcT$)qGZqN<8Qn57`ZDlx(v3_{O2IIy#Bjr>}B(=Qu8yM(E%P2A-SN93$ z+8n~9=8&DdIItE{8PxY(Ly>_hcOSb;9gt(szsD)C^od|pOd8kpqgP7bNmF{O#DAdI zre1ESZ{v|($b3#Xhh{as4fQ4GRjqlCegoT#9<-oCpG@?uWC1S0_gxTxV0XmMEJ4d- z!vn77{pPVkte&=!Im4SW(X-?4(DdozO4i436A?l%enNjp;UUXRK7endQ}W5>A7~2Y z=D1A(E%&GqJ`r7N8l-v$p~ZNenQg8V+nMCGBtPGa3BrnEZ$@EL7kQ(KlTtr&m;9RH z?G~M_FuKG{3@4^Lu3jpG~aMa`sW9ialjI$HG!l|CBGyd<0MDvZ0+Yucoue z7`1g7N=rW0n!6-_0OhR^(njl{`oJ2EK+o-k+Z=PJa>38gMXcpHwZM`*4dNpJahW3V znnUaNAdvkaLT??cS%Rla!a7?ZEdL-P)jufJV<1o=O_?!=l#YGknlupv;-J8H!TXFW zru(t|x%;Zr*#-j*`Uf`*w3pz(NH43V#4pt>Q(Ni&6_!ZKgww~yMxo6Kp*Khxg#W{q zrinixDK7NdDfQj_1CJE@KLFAPL{g7Cf~ogo>` zP1NfXk*uZ0&oe>#3co&UsM-T_RV7q7mlr+Gby%q-;g3~oBeV-7-IEke%&(Zs+->Gr znEql^!NjXx8`m+BNS=^tS)ca8HUdN$rdke|P%6i5{?0n0Jl?MQZ9ux0;v0ia>gJM> zRXB|)))C$j-qEw)@$)lr>@EcGRH#&_ zQ~`)oB0xk-QhaXo*5g6E*T#D%8cSSyzF#>upU~0U6H-?C@lid6n8Tgr%UXb!(C2MP z1`iAPKYH+3L;;+UktiCzo*4)!9sOkqaxxXd&{{K>_wD!dO^_+rKMcVJ=mz_ie4NB2 za|QkU9}jLPZ&3`tk&hsUIYEg2hasUTA6nU-pp_=UADz=+3nCfcWEdH5Z)Hgz^V6vB zh>UtfYso%1{0^}#t_D|m$1mW$U+Bz|u{!jCYl5}}QlA%**kA?Iwq=yn+DuVPz12H2 zwu))}_0tl_lGTEffqV6QEgUr%=7NB?w^h{hlGYk^b?1;@mR5$$}5ovbxy?Y3#ol-KUu%CMDhnK*RHe*Q~%drQ$mUnHG_C z?ntkHuN?%AJfG1D*er6YN85`vFnF}uiI{)+n+YxIq=1-AwE^9X-g%~%QeQ!Jfn8uX zcM+4)Tmd1CQzHhQdjMVa-v7_kLfG$LSEw%l$)Lk;|=?{i3-r_zyw_|O1BR%iixTu z-6-UeWRM_`o55`~;NrW3Kh9_sZev+_Gvq{8u#FmX3QtP{mTi~ixejU_EX=)-bK8Sm*L#e@N*loUrl8&>u=z#xzvjmf)$G z(apMQgc#y0_9M&jm1NUlK7oD(E z^&G&g6n}eF&^^C+fBg5!RD={R@Qqou7^V>XAG{ES_<_}o!BelfJ+kF+rW|twqzR{d z$SRecpjfDX2tdRNc*w)12wuX$?+^c_q%$M+PArby1ws7}eSi$=tWWQ0dUhcYAz69x zPZ`D$P5p@#>3b5?&nganD$J)Ywf|w+r3SI(9?4u;Jn(}fAMuBlzl;3_OyzhP3Ms-r ze4$Bw@4RAvMK*XTC%@_Lh-i!8Lk0&z{0|=(JvZ?c2vKO>XLpfSUK^s<)T)+RG2jkM zJQxv@^-B(24qZ;E+@I1z2)oDbJ-gA#i|Re}4m!QM@dj8VajoQl*F{e!bUZ~e2$uhd zfgFQ?xDOopf7w+``O5K@^%Z3Cj{`^C)WN0Io=ygs1Uhc|nJz2n%`Nm_Dn`0;u(%LY z;8WmJSzuGdyMV1E3l*PS$S?I~xR7mIonMJ&6P`r~-B<;+<4hx6M(}nXV4@&VkZ6;c zC`)XI7%VSYxGEK7PY(8F08aXX3~bSqhL}xJC0!2|8Fyy|b7Y%%1-b)UEXQ14&^mLM z!GBox8ZhHeSe8>=LPyR0K}BHo4?Tw9h2ZUUabuD(91ubv9IQXW0`^`a&VRIa1<+B# zEci&^`Di2lS-L#^1pQ~Cus+;kK%jv@7+7HcXOsWG_a>Vxe9n#xAdq81Hl8TJz$qgx zqVDzkJQL0j!~A7%Q-&?U%tawR&t8^U){!PLc?LeQc1@Yag^`plH@B?(@I!0SVDi4t zKmtugWDyEW992Sv=5ak4+`by8{jyFWy*5pT?NdgzDmf2IHY4`&HR#xk?UL_OK5O$; z@RI+wqxX+BzA)(qd{HL?DCiELRtBN|g@y<1%nO4Fx8c)(LbZ0!!JjVni5T(^|hJTp`Tu=3cnyZ;s_g^Redml=L=mMfsw+_&_8!6=Wy+ofE$%}Y9Tf@ zszY8Y^J{=ohCjUt(pVgua@|J<)LhxXJNU-Fv4y?wJKPE&pvx*{vrPe{4Av9|E~F{M zU+7;qQmeS-Gu8`n(Wu>8n7yf1o{56Rtzg7>e(&lNqqn z<6uu$KpjIdowMOF8)gP1K1#%b$x8PkcH4XhZvvj(9f<&WNf%NrQlkXP!7R7i$Bc)f zH7-NAe!pF>HjqtB+hY{(M?#p6qazbSGS=C$aXUSw`zituM@5{7lc=7$sv#q@*FW~- zTy%K5-M#)#3?f4*CdEb_=;PJvtlcn*+2M_i&{pSh1cr(!g&A0AytWj~V$!nJ3qS3y z5*|GWfV5y!ku%&hI|Gb^Ft@w&4&mI(x(A&(=FW3HI`eG#6Im z`C7sC^kFuR#5#aQwv%R)q2}{g>+X$$?5#%jU**MzTD~NW(g9#1zy2SM_Vvm7YHz@^QCQR*tzL~<69jcWL ziyawr{lKLs$n06Sw?*=|9GW$6*b*RAmNGYxX(^_>@v5Ri!Y7@oLNiQ%t&)A!s&rko zoqd+~K4AGH85N+Ms96TfVZvUdC5LeM=xf7@LF=mj?GACAjqmc(ev|fy$F<1tygKU2 zGg<>%|X$r9Odd>Y|&Jx=+c#LWr=yKo^dX8Cy!Ss7lv;h0|WbVHd%QO3HQ=Ii} zw}>g!6Ke>V1_u@g(85-*(bZ77t=WvZ1>r#vn(ItXUV&8BM(G{R8RLWJg=iSvTXqO}zrGP(MU{_sg)nrg2g#~y~9wnlsT4m1@ z=aJ2}Uv)=DWI}7$d~4)wGW&IPX9Cz$=1;E?!SsvDc&SX8eoZQX7q4Rq!KJY1eS14H zC!iCVN-N;Oz`YGGp=rRt<2On|XJ9$l#>rh7O9TTAGvb7=ydSuZ=Vj{%%34E#uUU_` z+yK44db>C=*Zb)3OduS^{pSHGwv;7@nFiCo0^~rrZpc7~(P0N^JieywH@FJJ--tL@ zpaKy%+Gz0KR;mPYF@O$!14kx{j&!2TUK1?M$&!@4nYxHH));n$W*OhTy}Q+~Vs3hV z!J4StXY~v?Y1rS*`g1>L`~O`3!4AKCg9Vy-v%UnL25uvV3^?OA?ad^Q(j#a0a8dN! z4>yY!z&B$K`EGZ)WiXOb$|~QjwKIzsNstJ<={;Rd%X-vOawH$8kZ@<1CJ$~wV;t%? z3>$TseVo@bn&L3xp>?k`Y|q}_TYN`_qZ}dOtca48vsw0!Eot}1g0Jn5=LVAY2LUAl z$R4#cjmNf#OT9;%1~_wPFjmt1gQqVlPQ?p$Dcd)FtPpayr*tkc+uhE2FoD0q-+IS_ zq#HCW5s>Q>>x=mL)s+3Jtu{Sv8LLOLwUc+Qg>LP8*((uuUp$XNO$~RZaUim=P>tg; zN#bm!l_k|qBk+2NjGxx9xw~Gr0Dxwy(Q++j7f_p`1L$V|3{##8XTh)gjy78e3xAI; zw|#7bk+5LLxh$@-yIXDv*!F^Cc=6G@FkJk_U|tYnEuN>YgSyC2vjm*dkKuAShZAYH zy``90{GsVb%o~h2F%o(zx%vv2Yl+re)Ccu35UqJ?y^HwyA=QxKd30hefkHL$Dx@ap ztS^CO9>6PK;7`eZ>e)nET5~e%TwKRKse-9_o;5d4{HJTc;=N;c1jhEGwoq8`OOF=3 zG6$I|G1{=50+Cc2YT_}ca;2oanSSTC)~x{~y6@)=)mH)EUxCZq&N40ZG1YLiHuXP` z;hYFhOBCF7t2>1^*Q%k)0p0T@>*3tednsy0*%aN?RKYMv^6|Z+l2k(7PxmWCuqMVx zD2O70!agEJp1h#4bBfPdjK7A|6{n_Wbs?J>2fF=9TEoy#$aQA( zgoE&s+@0OkLIEhv=>Liu82C{|5iw~j{WSN6XKX)O&x?P zy$Sj!RQV^wyPyHV=1)5Dfpwjb%%n7Bf=FXKApfhcO)39kOxNTP389l-6t0Mc(rn4rG0V;C^u zPnaFSwMY=>-cstmSQyvuhh4kVA8=Qur~vS_i8DJwB@0n(LOy-qlbpy95K% zl?oI`$|4*6bQl^GFc^Lv_(d*75&g?LE{y%8aWaF)PDRRs!>FV-Tw~Vc_%uDvo1%TA z;|$bD>b)1e!eYQf8zqbU-$o}$W9f07h`>{lBelg5HJV-+>X5AUk0hvm5EmI$`H7pq z2}jYMzxeo#Znkm-zg>5&z3^+|LU?~m_PW zC9qOlX@97cSxZsx^xAqK#6D993#?zkQ4-MIAZ?KXsj*#=RL!#5X`bCZd2FLU;exm{ z`bMFS-dYbXH%17sI~`{?xHf#A*0M5rUWzpwTQ$L(o6L?!ftDlfm`oPUF062t$l@^} zXXUd|2)i^$O7%F6O?7b8#dDRcZ~m8f@HpV~N<+zCw9Ak!2^1LOf*m-2Md2RWiknc; zF8HYfeUEV^W?!uFb`(x0f5bVn@f0qEnfl`c+{S2eI@JAf@(7@pD)e&~(oQ@CF`JRe z7k$489&C`S-KR0(93PaUybp0>ZDAa_wzyO zo+9d6c6y}329gDUh@Jdr=CY_Nc*M^_VZI33H2LBZ#^w9@1&nzZYG(iQ7fR!kv-?Y+ zLofi{@%Eu@C^$BK_HfKtBxvo|o6pn=l`L%SK6V2np1j3MxEw5aEoTyzW}?--;50{p zeT_7D^ZhvCb=pDfVS-C%eOXI;X(QIKW9QptFr&B8>h#$A$IX>U#U_ip)WLxLV^p&q z1@<*sYyFF;W=G6l)o(Yp8*yUjaiIeMWk7UdaqRLoazlWVKbU@fi-}KlBuO?mBN6C; zL>N?{R%W_Gt$lBUj!K4ht70qVj4ewMiE+eWjSfzd-gB19Kb-d4TG*!mTmSt;3>{LX z+|T!@J*S}1df?R6t4m$;OuITmo*Vi1=@)+C^*r(CBf~|hGTQjQ9m%MiNZe{bIohzG z<1%vHBfOt@cy4V{YKcKNO5mIct-zke#ggs`*6MFX-i$gKi!&#vL5SJg9Rt|5GB8OIPTLfZ;jrtSB)a!!dl zWHd}7K@?IRj+C>Wk%v+R0%BFCdMVyX13zvc*TMx8g0u_{mt;Is00yl4={L#s0{MOc9gavBcCyjt_~ZxvEe^t=vS#(*f$rIm@HwABY)6cA6Ij(d zFw+!2Vc++yVhB4w&_H+JDozwX!2_eZ+ymK~c+f4Bd_HR@>7k?bu!xa;rXZlJpqwm$97KmiwazTVs~Ns? zz76KNnY=`0kSC-vv;eh7lgqmv^GaNh5~H&(4y-Wqkz^p-5nPZMmHZq|tng5wj~fbR z7j{=|AX}4QnxS8id^3=mN`IsS0}=-FB2$K56s4H+Z`8gmw*%P|%Z2M)?Qd88%eQJ7ikGG8@-lGy4-&_| zF|8ko9l#^9;!`38_ybs5%;*oKO2a?u*zSnP4-y0&@9u!3f&I}?|6c9o9MEE}7!XAH zTkviH2kSue<#|zH?1ix&<|6x5b?f)=5T3iX=x@Xgs2fqAh1e7QA3vx$EVA>-^IIFf zwS87hZXCYVUCW8x-5euiz1(+DQ~xzMbQTM02HQ3C&wE={i}Eh`X))-2h5Fy0EOz`O zLy8=a5C}MrF77LR>>5JA3%)D%cRp&myRGAIEf^*Ksa5fNzpx#RGJlJ-(0&6e_M z_yZ1P924L46GZAiyp$BfWvhS*>Q(!8l~B`_8uG~&Om;vZmwsf;o;ZeF5`k^(FX%4u z*Qj#j`ND{GOESMp5VWUj#QmoyOp!pDHV8b>a||HcTKZ$Dgx*SWsCdX(%p)t~T@dte ze@*yrzdd#YOuxfO{7Q04cZpt5+V8;z?evi&Ha5bhRhi90f;2aT|JU3}f(L#>-NWX& zo}T@*y%J$MvnTmjf&oU4hBNiK&_R$c5xCufNo86mKs<2;(};CVjA~7b!p=8vATSEF zr-=(Z7JF`Zhdq$+2CU5W5o-%MasAgM_Ixl3QLo*4MW&wSsoqW1`~i`AB9r}b=p8@` zYML<}fY67#`|g8tiHD+G1!%$XvcztBiIuD=f?~fN%C0glcX~fM>L%jw(6wy~N3S@5 zvC5zkn#-;jW!swXeAKI+1up}FP(?6O>H+P8;$2{@v8T#9&tlaqoT2Uw(OIZlX-uer-=@w(OiMmq+BH7cxo94Zh1OZ) zltamJV`A=`R@}5C3lwO2ZD`xS%7c;oypr|drWxyt3fQ?eXZOqA@X80itjnrSL{SGU z)y$%1vf1Z4#uTYLaYdgC1XRc8C6Y!#^DfmA|N3!9w$7QAaG}$O>)jK+2xCJ_>ud;H zOS?6UG_6YxM?q}+zNP4ci|ix9HG(uA?{XYux%gv>$*Evu*C1yrP9|U+YLq06czG90 zz<~}VJsE@nX>c&K$TtC;`oWwA2=bHqi3Wy9eK#hbVie%;zr;g9<(H0soveu3#cnJ8 z&e%nyFlp-_!?L!q^Tu3M`1Za(&Guz=>EIpxUtX}N84pJ>jt2WV6;!b zoYMKPR6!IJsh)#v6q5w_VA}heyil*6nI$E(?Mtwz{ej!pPA>YBp!5JoG(KaGv#6r) z;743J$E^^=R@i&l{~Gciw#RTbJh#rWkpPAG<*x`&i+UN7L^8CZ8sm?mjdte{+DlRt%sTS zuh1Xnszyp|X62jaFGY(TI>y!N8fB{zA83Lmu4)_G(4YGWyHddj9h?45qJs~I=}|BE z&vQxL{X7AAyD^tf6204`+RD1YFQcWQcx^WOL#&nMR@9A;qIbjvMM}@nxU}Hot%9!% zMUqayBl+XJ`30S=l@&~vjnr8_(e9V&FtmbYEKDldPEQ>aOJgT4elbvr;fE{Zji=*2 zQpDt1ll_~3ps1i4)E|8&aOFSJ3_B|Lipt;7MG{Lq?O*co^~FG1?NGyWYu+J`9P{%H z${Zo!HG!dtUTfdbyNV(~RjYMWnZt$lEWuEK%%tT<=>ePtC>kZ`D}G<~&H%|RBV>}l zKI$_mg7FO)u?91n`tRYx7-_oAH^=p>PhV%}V<`IZ&m{h zR|_TIl^d0TaYrEOaRI{l-C!@Hn;jBVdC57I#5V~k)Os-8b3aMwV3F@bb`*~m#03ni;u7Gzf>O>bVw zcU?a)7%?k3#RetZ)n-)xjaL@Zju6?>I5g?NHMSy~=b~qvAciZwIY1ToSZhsH z>dk6yw+xVT!~RTK3|h!ue?uZl|9$oTYKWglP$NFAf9q|*r&gqF#uq`KG90Xcbsv%a zd?+_7^tNirV@q=Ub3%2~RSFigg??xy2-%-`$qR>O@Un9=s^}-?^Ws;Ipp`ujwpuOx z9Q^(BwUJ|s1`|1`xst^6J*5*6^$Omcx1hAx+tWd7ucptAvYx4-2(RNXC`c9{7q@0p z=+$}hAH~8I^zg4sCmSwqsumQ*w%jvy6t{V8ZQ~lw{1Uh-+DH^bFka!BbwT>$6bHID zED&`uBM}m9qLVxNhn#!3>OIV&qA;qK*%qExKFDX3qqV+g)wnRl&}JRz#BVA+eq8?P zPQtGW>dIrf);Kn?@%QFkbo|c$-m^iIMHk2yII19$O*HM_#{38dpObxT61}{@KIQ_M z?i97|n94v$9mh`C6g0b^B@EVt7J1()FOQT0jDHInQ?{znUKYJTv%=K-(o<%Zckve=vw7oTyM$kVzs9r5Bro z7k#8u206f}SwiTJKK3#Zj#iW}!ckyXlPuVDQL087tUy}TT;=$5Z(eeeyk48NpG{E< zUGIrm1%~fiWqd4fFK?U7<^ZhQTb*87`}@9H(umG=gM=Bgob%;HjVDZIqP_8c~kPxO2u(Jj2J%2 zck{O9f$GV8L_wf7M!%bcEA5%XO{Ft4I0Ou8OR~wQOsl5{u9aln`VDmQ*J=i<&=>o6mQ#m0pt}tIW~5b^Kq|t>$c+A*U-fT`}P^ zG;OV37>nSMBEpb`6)G7S)ct9)h1~Z7zdg%1WCwXf^XuQ!WC_k)P#vkXcRX!a(oGWE zt1mX(7ES2H>o&cwzjWbQs|9yDJO#%CNLbI%PN?$IMv@8ol;vZsM%yZB`#r(JmI$Dh zPAKF3SC`so!jfWJY~XXaQDebL=xqy)NYH5Qq+olM)6b`4g8>c1Pm#XqRJ~4(sE$eK zcfx%Y0X%0PZ@%q1GD4{OH8bMK9t(#}Twu&<`^`|B1s3L>b^Sp=5hOOW+r|X^Y8K*t z%n7N^2ke1Yh_CqP^`1c!X^Ds4q7X4`MUpV_Ih_T|&-uK0DHyE{iO_WyKOc{TmSBJz z4m;LGV*h^iTXoEDHb5($nk$aDDU~YtS&W1}r-K5s7x(zguJWM$C)F2XgB;NAipIR27@!82{QuWMbEIrC_b>vMB?Ctiu()0XIYh zYUveduCbaB=o!2n?TL_@EG8?MB7eS>^90~KQQ9T;q4HH-`};$H@6&WaBf<|ePgk#& zQkc|gpiS=U-jCNMa`fjj*APC*D6y|gjU@f(Q;m21BZ3IR|4L~}=D8Zp4r^b|v=|<^ zGy=g1xoblBeB}*<3*G>l%SYY!RLKX@Dy^|%JR8LMjoM9J;ZDtjm`D%g^cgGtE@Az$ zU-XFwa>=AvCBCZQMuEpX&v%pfS1^55KaYFtP#FZ+SWnN#&6Q#p`gQhKnV>uBwRH{D z>TeBO5B;CacEjE*vS)NhYHX=($4{BxZw}P-^mypC+BNh)kNW}r5YWro9wcrUx7s*m>{t}e zh0N_DKErl3Mv*LrUuuWNti*8Oo(2Lshs{?Yx~F_095Z-5zkWA(xw-%P%+ zFxKB@oA^opiuBjN>McWSG2CSKwe^Whk_+6Zr3%IUMH)gq--#b4A`DY|a4=dA1=jG( zwx`0yiO}5|QJ<5jbCe#QzG%1kM-KcUGI!(cfZ}i(<>w<_+*E#vX=OUkcXar{jwbrX z(GseS&J*C~Lv}F}d%su%febP9=Ce z34vhYUu4)`9y7U{eAI#BME^+1UqB6cJeF^ckO=^bcJBTGyRH`3ai-_l(fyr75n9YK zj0T@17T4*fyPY4~D>sfxFVntraKAjxG`!{~ksypw{}#e9#janw!AGp~iDjKP*agqq zPQHlx;Bb?b)Bjh2vsu@^@nq3+p|l8Z{AzsX5O4`9XC(N%+=rNC9N@aBNE!NZmCOe5 z3y?xYPfp@Yvx?Sd8shtn6N@{qs~{;=65Noy10BV0Jkk@lW>0c%-K!i-z|Hfmy_v!V z{%`4YdVz2(6Ia%Gx+&n>c|8tH%vjNig4{5ttYqM#lL$8mSlr{g!ZDhfSsH1KwD}jy z|6QPq3Ud?5K15 z`y8q>ebOY+qTOuPBj<=>;DVjHmDst!iao}kJOk#Vf&Bul=txP`-Hmw@|a z#n3a>_G7hpn7Aa{RMki2hIc&Ik!{D(iUiv(aT|1+>TyQVSWGHeFtw|c?Z*O?t)oBK zBwyGJb$_Q-)lsJT0^>C?-VVkbuF7%Wr^@ONZW1)Wb`v19D@|D7R1lYxzbja$QH~x- zTX9*h@)Y`SYCXf16qn=)kkKt(x{-qldn`^q-V8RE>)kDjt3{3W;g;DTZRfY`^1>+N zr^W}W(02Wzq{k@w(__M)1lfQj1`0;8HtTn97B)qy!8u;%a;04qY6dd4(|qom?Xq_| z9OT34^c#{#n+6BRP&Ccj-I;_w%Ixz$=;GsyA;{X?CTT!UwmhrtQQ+ja=c&H^oBl#g zQ{~9c%3KA)989Z@XGbDshR@g1-M<2lAzf8njinRR%2wsvn@?F2RX%_Yd`|tL9Mad2 z287^~->NDmh@(~*)~9+lHt(y5CM5-J-QVe|T7KIsua6$ELnZW0=(37=LI(UlvB8mz(YTV61~w+Lm}z&P#CqHp z<$c(L9qoxlZUbVbE^BVDOubX7(W8SYq{1ei!0s;?B*TirF4e%f8&cO3Arhbh!RPq4 zID(;0`L*fSx%5wsuilbjh}tM-vxh)Ixe(l(ir8k*d>KpDTS5oSSkkQNnZCe|t*C2i ztK`nViq9rC43oj_EpWN^S*D9+DUG~8l+NZ)-w%!ypz!CsY&OgoeQVb#wP{r@R!W4r zoy09r6%Ua~x{>pIpzVBu3($u!T&cPx%@{oo1*>{OFVXInYJvX6Dx5B};=nd2kL$=x znMwU44*Lg^k$xl)!6qfiq94c+YY{vg0ZCrSSo!&!mtA_sXFfWFiP290iTQ&L?6iL7Ao$v1PMSAVPml zkKAg3jF|6-glqOR1?r$4~LW$7U5!u*Y@l+Mf8LT zYK;H9bKni@4RKKHq&LgsV7QJBaN?Yt$n4A$}A#{<;1XV z$dk|5ujvwNJ;4HM73Ji z`WH~7LQbk<61#;}U032IjpU0{Q74B_F*%`woke^TI`2s5-6XjJRi*HhN#kZHo)_~v zEYRgXIB*|G5niM6bFEqOiBGJQW2;0xrAWk7j?Y34;%PsSexv%(U4(g`S%p`$4BWE+ zC*LC2mu>?!CfX0~+nEAorUUq+7d{j}b0gf=ouh6UjNd?koaVUm#ob_PrD-(vq0bP@ z;hhUnx9RW^rK`bkB@v1R+BUg+EXS7Zk0| z(0&S(yWq%jcB6*wie9X-+yFFq&0Y%G=dvkbFG7?q<99IlBx&Wo>+;Wn>u{WCVt^}m z=ZlPilu#*EA9-P!c$2;goyBlcU~YVwR1=3yh5DB%D_Y;#=2@1W{jYaJP$v~!P|Y>q zT8JXKv~s52X_7g>!QaED01>uNRC*$dNui{_YQdrP75S48D1GhscfNwX@e&QCJ`v&e zq_0rH~ganQGu zhl|%d;Fyw4ZNUzZQ(TVl;o(RUQ&X1zPZw7n4rTj=$E55svX8MOI}?Mf6WK;&WM4v& zLiRoD8?yQ4h+a$PJ`!-{wvfY;*#b;Im;_4L#I36)ZB|)k zU878HX%6QN*^>o+G?QpqoF~1OPprB6T>bfzXApSl+3Osq1~uUl&wES(a|Nd&tnH?p z;%%EdC7hhI>`#?PSbu~wukvpcfkfuei1k;r2w~RI4d*Y_LXux<|p9Qe!@Qc3VDIZ&{5wC>r z*QGZH>EE%{IAI&9xYq&I;>>&_rDt_*aGBgnf=$;)dt!3%kPScotWV!qc0X1pXdtDQ zI_$Nb#>uy#&o>2^+}= z+LS&?EKm76h1iVNhb&RzEdwUzl#FrpPi8CQtYltu6%%_*3e*628_`9Xd^TBCb*KsT3t~!)++mBcXK{)zHMQn6B?h>C5U8X%;J)`l!;7d(&8$uNhsWj*Al9&Mew*y&q5x*WtHU>e%i#W{;&ey5zBT*_r;UaO z&T+=xD$rZ+e5!F`J1-VS^I~5Dw-w7Ntal1UuVMs-r2De=d z!@V^NQ3isZNMD{7h=^X|f8P>n%*+)I|40-;Xu=X{b*Q`&C0B`Q&FOzbfhSIsy@GsK zW8GjUmra;3(&43?XEpn>t=0SXlm#Sgw%qRU5z;)z@HF|$&aCWU3iY~_p{cBlqVrLa z50mf`SMa4VO}_4O`N6!{3+ix)3kYG^Gk44`F9lGG$d{;fC>2HXNiwbo^H;XRCcKN> z@lcY1QPH(*Kewz2OzS$mv#>^vXa?G5gq^(T6+iOr* zt_cEtXGnCqtkiw=N(dLRLKbcjT<|~!&X`mpb0v-~wIB;*`&uf?BkTmESKxgx>PyVS z0Sv8>p$sC^Tb3~&Ho*9Ifp~E?9c67% zT2mA%BA-X1SdZh8gf1Fxw4ZHfKI=?R3T!`*jph$zX6$}X*CH?Va3H8T()E%ewv>|o z=|?L})feS}IhYvAX}a%XposG%TUps2kn}Fv(LtJilrfpHY}rt zctc!aZ}+hp(QWgtaOd@H--76!n8&CU!%w2An?p}7pqqiT{JQ0^T=YYG7vRa{#fbBs zDt80C60LiLG4zatc<5h8CGW!3TI-GO0(juD*RANLGp&m580W1Ojmt70x> zy$vtGS(xMB2qgp=)SEDfTMy}|XlPep^-VqvXdvS=SDJGQ>@4owDVLSdXMW&Y>n`E{ zP}ttCJn6FoC|7(u&a+kB~Aqc-P?E{Yy-Y5a#|tnP-A@pv}2$M6<9I9R}^tN>?4ANr(@ zDRJ;Zg@@0MM_n4LS@jwuDeGldr%TTBZ<&aQ+>kDzDi61Uf!O_q?J1D%>EpdMc!||@ z5BSTC8}v8L(Vl~9$atnt7F8RFGr}K)!2V>_v8}bLx|*A&{S2ge(#fI`P)? z{O9n+D<*9_8GK1%dmR2vOWf6eIlT4s&28RMFj_w!gf2eJx8amMWYZ`011>(O-Jr-U zm*rcimF=R{W-K=*Imb~an7q(|O*Xi5pt`d7tRN+y(|PK2mh;7o_c`V_(z`|Xueq0w zi%CG7(|E)!uRPQ?FuR*#IvNYxi)=H)Pivqp95^)JfN3JfVUq?b)W((I`alOR6?8Im{`kFs>GwBV;BtbEjJa7(7^80BIg(e{X>+2rW>M7Hw)RX@FnVr96;y=F49jPQz< z3|6ngOQ=Tb`eH%V481Ty@Jc82N_xcdTJYFGw*WntJat<{kmTkYVtjr>3YwusCj zo|R^~FKVF3Uq60v2a&+H(R5vD*W;1PdvLb+FtAVZ!oqq#lo;Vfpu?|a^;NV+ z`{(;oGiNKhUJ14@r&PzhC&lQ$dT>iRIL!T(=YEz2L`b4^;`GO%yjhd+QzOw_4_EV; z$dV_6#ruNJP0()A4bLrVd!uHraX;=_jUloz%Yc$z&{k3|?+nd#5Xq~K&hQ&Y-6k~V zY=B}VN{LUzyPEBTz~mb}H;%qan6xRFkk2MQ@I>^G|{nypoWyF9^+fW)tdgzCSlb0H?J^{xY~o z@Zcr$kEX-Me!VEx5s3x}jNlG-Ou**R1L)BqTLuj%R)>GpLp`Mm%^7}f;%P)qk>qQG z+e@Pw4w_DdykbuzG^noM9K#t2YO4XnjgEh8+%%-#qV=-q(I#@8gd}l6SNmw6;gUi^;)8qdNnq+64M@;fq zS}xYMszbMBY`yu8xyz5qQOs+R>& zVx<6{>dRi3)>%=Dy~~rM$WA}bfvw53^&I>+KRi0knR$JB@e_1cS*oThO+xe* z*}~EHYz5WCpyh4bFR7%{Cp`<5u+%}XSbXR$v*_(Fi=^q_g*F{EPBsM-rT~^JH|c~K z?hdou8e4dn@FQhQh$;VArmg8X&EV@kqrc~XMI3@f{xJSN?H8()J!i@uk)-H;TWA(x z#aL%(FYx2lt#Kbe?uxJOD`|Yie&u~1NMyK7n|WTah1>Ml=y&&)-@O>Ih2k#{F=t`= z2=yLnG8w;yQ&KFYC`mc?TV(;BD@iwv`!W`Kw^sXX3r~c;j$~Mt;JQ6>gLC{2ZhHFb z=9BlaeNbthgN`K-`Gt!a2?fh`Pbe63aQ$)P2UzN*YX(t#HF6zsAJWBXSToKMc-WL@!d2OaY46rJ3_s$wifo^2h|iF$HK*bNa^nX?@skaY zNYI>QsjYR^-?*jcJq`sXi2&;@I_ANXyfzgsQXT#ZE?Q06ruPN7dVhIJzNpGj7pM3D zgcE!tjsREinzd^fLKP9-Ox>S&Hvm1EPhUGdcQzY#4tJ-dfD{Ch6aTNk&i@%tCAp3U z(Jw}1tcxH!V?#f6*Dx=2h1t)%-!*zD|DW~KoG>%G(4Uy!N6;w#z?foK^jLmk{uxFS z0)d$Rz$9UG^>}|BAwZ7>j8w&>=n4PwyY*Ozd@xIT{QtTY7`Xm#?kdJppP%EuB#!lu z4KAZFjrsz=O7s}8fK5@DtNIKWb_1G!o)N`i&<4Ce4|*$ToX6|+e4S?lg<*DqAwdVQ)Fof3=V;rld|4B6@9`rr z^sbG!k4rnMo9*YBpWk8xFUp^u6*$_31QnRPTKgrn+7%3D{a~+9A|e?hS2Fzk-T|F9 z{qctWyi4)Q((__y6yu+r{vqqP8OMQnF@FwM(~@Q`7ww)u|2cSCETrS`)=68HAo|GC zf*Xkc&u-d!A7HU~BYxiRpcS{5%YLvd`O`1YzQ6hjzX#S#<|lzUS7Two=HbpztzD2( z^HyZ++!k?ew`4J##9*ZlYuY(-jfF`F+>rqy>N|12YVCZf7Vii=?&@lS+#9t+VYSCg zG%Ay;X40$m&{fjrVR`RSTPKe!%p;?K>xF)zOJZgypj^DPtz6{xR=P84X}cfrM|xIo z*HnXdT_)z%UjXRVpr8$-%*UdZ><$IHNxGUEs#9V?w*G=oMiLEG9kq*2aSKls#k<&> z5~#i7VN{1bBDAlD?GxYZm{;3TIkZ2$^L^=Nxoy#?S>tC*=C33MYklP0j_%C$hlw|r zUe9;|;zywEo{?A#(Vsg~rt>Piw;{Mv+G@wK@xHMavC2v|k-_L~Wz}k}ZJ%42o)mSe zj01@27fbi8?iaIij3f_7!&9o2xqW;lGSlWMa8xnzMS`m(Qhwx9QK3p>N* zYA_G!IcKDMvW~wWKNiOzTiPy9X^=mOtLHIMthX_sn?1r}LS~{1Su@SAQRjpN$__b{mR5_9mhQ;uhCkDghFK>7dznZ3S4_ ziaw4P)TeGTJ@t#?w!m~oA1h^FV@pm@+{m`60w;@)iY;6suU|FT$>yhAZ)NG6xH35- z342|m>Zv!VcxifEs7P$-lKz^jK>#(FL&sxqEN73xz<(t}lPf-YY}4U4RDhE8G7Q5h zGn!prVRD-I#%?=ACMTA4s>$xb$To0GsuIh_KH6L$NyUcbD8pw|5;r6~GPz)0vR^xMg!lW5tPfoExE(Z*zAz!PQ*N+^1V0 zvGA?{%-gT)PWAw4RBZFOZJSL$Ju~_6@R|=U_haL_7MA|5$Kpoi+kVKw;WvP^cb5Km zeU-M0$OASvvW|_w0$cBi04nEk&B9H7Cp@kG$e&tGde10&;9dHm=`0&TO|}$=AQOT; z(!?Jgt1{u*>QyC|SiSiS-I$Ux3l4b+PE6;Y6&(5zotV`1278Cn1T4Y!CZV1d9?Y^B z_;LFF>^|^P&wB4`6m>Qq_;%ZScu?I&631wBGgz1I=saeqmZ0}?*8ALIOxn$4T4Q@# zN(*_(Du4dbX7Dq|Y(p(c{p^f?VRDZro{lXyKbv-MAkxLM$5}KvDG=^3A(V$!Q>< z%Xq6*NY}Dh%J{2;Ia*m<_^MULu4cK!<7Fr=ZVU^V1Pr^nszx97Dezs+AkV z4+%}@jSvH^UPfz0gs%&V{ zVMJgj2z}olvL^y`@|A`zX_+&-yVw$fN;UgE@jCJn5uRpsCyuc3MP>DU|uidZdRoE@7lp_v|+NRQdHR6U<3OT!*^5_Oj zO98kx^EbcEMX%k@H#xvUw9239=u3?o%4;uK8I8sB-AtJhTwY$utCa6P_|dZHOs#H1 z${gJGDXT2hw0=hZUD&dyJ43KWz@%Fhhtqy6_H1#!)EnfM-2a|i*Rw07IC-}jOUe;Z zm@GaP+f)U@S`8i{FYg)$KK&ufdi_?-cg_F~o>ebI{>BJZqF3Su4fo29xqsL={<(Hy za%_jLcUEFGVfvXGN@OEkhl^+#?n##DyP%W4EUBg0^J6G6aX-4R{>)%ZyK9V1tNpKx zU1E-q$fp$&)5nZE`XwUCywA{ODI|gGHR7`oUp{dF5Ai{{Eb(W>i`gRORk{pPL4eAp zG=!-w+wudAbuJ`aC#vb@J~yj=6lOzvn|15d+a5^>Q(Fa%7q&N2@h894skAF68WUD_A)yY*0!J}6Kx_&j2FJU0N|v29`<|qbYpfBoUeiv@lX)IDyUm!i zs}lf7-Gf^_!sg5__7gF9K=xebCSS^vobD!L?9Uy@uKI+5*=!35)R&1((x%n}%BM8(!wCh_T_Clk z`r>&b>YLPol-A9=!rPoVLXGzAdhD?)8cYDEo8%RV@u9dC_se@{hN+S5hab~!>)WjR z;mGoPpiFoOqprnKx$T659T8KN3S*HSatiCz#PW~Jb@VbQY~MoXqeL&hT`%u&|4rf9 z_caNrkQUr2YW`*DWmPrl;E=Qy#M!D?*Q!n6+qu^A+>Ph-L4cftzqqC9jMr8c&I3rh zuy6R*1S+OS_Z`_iuT?Z%j4XD1WS6$9lpBBI2<`AS=JYupm~F5ZpYE6vD&1YSFg>X* z+b(td3 z-S5$`(_}Q0%2ts4jQff5E?vhSTPGmucS$_>;)MBpn0Cm^CTD@lSIwNfoPw5%$&0?h zna!-j6ADnT07~V?ldPA7qAqiF&|IVoR+h4?6Q%AniHKtoFnt}~+tz&vv%KZc&h=nl zyRvn;`JGR9IY$Hgn;Lqag6cvQHle3p}PR&C&sRKhU1= ztlQJeIve7g$KlZY8*-w`Ib5{F(<%GLw&KKx75q$GDl(9_gFa~&vdd?V81GozU$5su zsAcqJGHxqHI5Es&@b#0Tl4}9gaqd>|*vf@9y=V}tTB?}JlUW94jagyTeD7>rPgd`* zD=Zb_B9f}%$z}J_dBiDYP+tJcu1U`JsXo!mRpM#iKBBpncy&r5&6 zq;KVuUP1Uj;tO+LUNx0eNWK|Q3AeW0K$NATwnh3|w96qDS7We&cwL~j()>@WQTd2} zf-{lkZ*5D~Q3&VH^9SF2tyFHwH}{-VU!*DULW#e*rxrgkZjn6HD54068~z2>)Tb#bn0tSB~8QO zA~xQX?#Bkc-|?0Y?g#@i-Q!i3p|7b^Uxs!v0!XQuj%mj*Cg`rODB^xfckv~|Bb^)(ZQG=u8 zma~q`q*a?l&#IQU+cEs~-!8O6w&y?R%$3_4oOiH|#gk`y#V>A!gxc_Iy5{m9Fhi&p z%??L#gAY4b^V37~B7~zLGax~>073+&m#-gw#lBHd+`(EK zw9tD#+VVDs8?{>q{KPi4%#p4cu1Ub_3^)Ui=NqwAMA^&C6Uiyge3^m<{9h$fYTTaR z1?y)tWVz(uqw5zPgr;4S=DzuKTX&oCy{W_eLt~b0wZz%R)xn_2Daw}@>sb0^tT{5i zEMcTc6Hp)xT{Qq3j)*114Nqv?Wh?viah9b?hLmry(Zlhk_Q#w$t8D8*$gjxw36=A` zo~7Bl))TKkx#LGg)%yrxyuxf0dff@YQl#f&>vC(}!egk`Q&yn zPj70j%8B+rERWLySXJ*EXGUvWWtRGTNQlkGUhET|Lg z1_W@Fm8I^Umk~72%QH?~jE|RlRlYRABe*%OFd20fYkAgusP(~D%oN$V>CN?ShP!%N z7M{mMa*HN-R&cV&YGC71mF~{~TTLg_Z#@BFGf#&?*8vBYNO0n?LfRjUDbCGl^uT1tmQa$O_5aovJ^ zZYcvZ7Es9a{X4Z-azIf&zZ66>Si@uf3ZuP`b=_h+Mp~GdQP}Zn)_z~5uDDEu=ITu* zPj2>~p|H_nDMs+dhTtt0A9Vsm)m>4GK#I|gWcF43j0+WVJI{OnVn#t(Fm$#S=-9i! z0Nsn=3jG*-qOz5yzC(YO95|Y7DT(<2+4@-Q$##_U0QE@7Yvh>ErA1Cr_>>l2wznWM zdSL5nTrmE@Nz?bkOX*A%%+T;V0T#kaAX{r_pRACxv>G~zB|jKWseLDaX1*gFZegEf z%hI;P_oVV|rS=S6fxs4r^x4OXAFKGSmh0=4G#Dsj0mSwA;rI0U8T3*QCE!vp(=o*oXiNqk@sySxxo$j$CP2mw_<*QBzK+OQ#@2)F_qj({TMll)v! z%xEYamh{H;3V?ti&_dAvKA{Ifp#S+m5ilg?A104L{>9*E7*gRc21UrD|Ms9rBogtr zheV?PI}7}uU?ds||4W3Ukx9 delta 3746 zcmaizX*ASrAIFoW?8!11lbuTVo82%Y#xA6ElO=nJ#=eukk|k@l5J{1dU6Xa}dsK|> z7Hh~_(@d5uS$fWWzj~f}@m#O2^EsdE_x+vkIoI40OXCd7J!84QyeE$A{Spvf7J-^xx{D3`YoMcYv;mBMz4yK7IB}fMHJ;Jc>2S)WGysh0tK~vWU6aVxLZ+E;Z%*8$viYv40SRWLr-|z3v zhN2TuuNT>={qHthDu?;rJLQYG@vJpOuI|nBG6B=YmXG$0) z4}NmIuZiI)Y73}9dM%gwu?#EOGE74#T<+b^mah7#o^c_%RQO8CfY<45 zwI4#;sTw;)q4NfkSAui8UqAwLE+s{-fVXA0!Jf9m`K*5n<#ZRWx$#Ax7N}_~bDBF{ zEt)5h+1{zWndvCH`qYI}g)O|ELb(xvj)waac@>1GiMEQ(HhfbDk)P;Ftih( zUwp%kJE3h(#7aO_R?IYE@!T96HNLT4=t?oG)7e)G1LJ_1z3fy$j^`=ZOMT~Z(&{|7 zZTJ1dEz=~uNzY|8c>SuJnyT;&V$L6j6>f#bkowuYc`FU3 z?0iwj|?i*H{VqcE;*X44>BY0D+B@#mKvhlj$Goe49S$3=s zrckpeHluX=8fupR{OH{}0{{2qvL>lYbt1eWLB@ze9NZt!y1t+gUu2Dpe=vL`1@oz- z4f4!tE+(QN9b))o;{HLW&-n~HACSP-FHbfVcot$G6JBLXsxTJCV#v1}Arc0}@*iIe zn;K00j}BIvQW7k7pX}-vezZZ(A0v%4cI0L%*^q|wyG9G@Frx)^!7trAKRh0pKvWmC z2AsJb{vLU$DQYQ;xBA7k=%>C%j-*OjeqQII)((uE40{m)N#rs4n$|_sj{`STo^)Lc zoi|rsEvn2y4^Szi=P|9%-x~_CdrfG(Ewc`2@y6!waO9O$=@(s0z}ePlayDe_nOooU(vwdlO@!S~+-PK8tt}mEcC2n{UIDK-VeDWdWGnES zhviD^orbGcrPTtS=ZGkP7mO~YiPJ48O84D{F+2>R1D@an*ADz7>sJ=ZoLIl_80#>v zUciHYXT)IVG1-_yvzk68utuHYu9uxBGS|7N!kdX4AGsAn8Ib~?mC&C@9mZ%5<*~|! zlfy)ZZ#G<~3_Q)5beglb?QlBbp$>N3QTB@5-qld;n5YGv4(9;SEv(q4GqOf9_k6CF z0CnNg{1LH`L(XY?u3C@$G;!T^9IhG@{lR=loy+dU11-!si?gA8D_&TxnaRFcj-_a$ zGGJO%?UbnUX*A0P9fK;V_r>|V7Om;$4J@1-g$v)mF;-exJzz<)6<-v{Xj-wIi4r8U4~^Gjk@U+HZb{<2kvyvxDbcZ#WP!soO1XJn zu1CF_t}i77(ro1a{WzqF>F4`f>id#kV$*(}tSsik zVVHPq&dlniBk_VuyHnNH)gkLabEBldTvch^M^eU4rO7f4Lpm{eDoS=`-uOq*HcD%= z{q^)waPftS_?h%fHQb6cr?o=2LEZR50Xk5`<&jeu{khqAM$PC8x7WEVT~92k?}aAY zU+**8-1#tcN;KZvOs5gW8X?@7&A;MNIv$s9StX5TW4JWB%!{Ai9jbIkwA zmqX0vJmd4}>E5kRrLG-zRi8JoM^iW8xAmB~cChQ7Cch}3nove0G8li!FI{kjR)Ck@ ztegPX8#}ANP4~YGaNB>QbVH*jHVPVhKGrk2R(!-?4KKAjprzawTf+=Us_vLygU{+z zeEH_qtch9eTW)og&D=Gav^%+Y4-RRz-^%WlO1`6lQL2B|VYuw$^GV2OK(DTR%kNu?_{SI^tbGOc)^qhVpk^cn=#M7tUi6Oee3e3S76HVM`4-PkZiC# zElmE$ZbP`>!mGx&f(}Q$tlOwL66l{Yp4ZgwCNmm@w?BIrGPw7!iqf-J-`WNK9lu;| zw7kA6ccld@uJgbrb;_Z8mNw@W>pE4z_sn%gbMxfxty4YIA)~BvTNV%U0v4=lSsrm# zlI}I~E}G-rloJCr1-d;#yEe2`5E9~`zwF$W`B2#Xr}+l43p}RwjciOt|Dwhi8(eZU zA&aJx%5`Sjtrbf--LRXW6zgJ8I{J|7ep00CJJz@h!n(uGO+1Dznu)XCi$IZD9w1Hka&9B$s3g&G;)0ENx9WQ z$YkNV(54E#Fqxu#ZR#m3Z+&j64wkpJmjw$N;*+fUu^Z9U?e#&KAY#-L>z&r>3l+01 zOM6g@Z@MrWT_=~7VAMFBrVDCaV0-~q1;`QKI?@-)VLJ~Ti7_z%OL=Ln_+oK_*vUMu z&tf<4wVm@ieUlNt;Ym;~%tCa>&TR9?f9^B2hG{|44lNOTubnkpuB?So6qJ;t4D&UT z_+!rf#hb~WaJNGW(i^==UEER@3tzwC?>0|3K^q>-wuQCPN};Wie`K<1BLCDC!HGN*+*D%x^;gK})7~A2waG?~Av)jf!s5_{wu9JmF^N+lWCIS&x zPxdQ}nD%gKri#!VK<|N>UC$b^=2U|p$hFl9XwsNwn(A|W6G%a-$ooKG&6P9&sV%A*7BW52>gERSfVTrIDPadyYClCn1l7> z%y>2|e;D&h5*Vh$tm?QrrNSqMd*61TgK$Q0g+;vB{58!{$^Y@Px$?4FL)7D&-+8;z z1}0}20?a%EyXz-dbWiw}Nc(A2#OoQc>$5L#=cRPWo(0-!IP}!x0N(r~t-;k)t=6pI z5B3x6a+k;XtDmJPGy}(= z{%8h{fnfgJ5QD{FGDQ6GAPRxRVgNV-g$4AGXd*^ipNLl1Mj~|peIgdXUUszS6423x>!P*cNIi8d9MD6fF$gRcz+iOMxCjI;{{Sby XfRJN;ZdbYBSOAISl9$&r*XQ~VR|}*< From a432ed49003d419414c076cfa324743fadb92b6f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Apr 2014 22:53:48 +0200 Subject: [PATCH 50/53] mc_pos_control: convert tilt_max to degrees --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 ++ src/modules/mc_pos_control/mc_pos_control_params.c | 10 ++++++---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 5194ef697b..65f4cbeaa1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -356,8 +356,10 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max, &_params.tilt_max); + _params.tilt_max = math::radians(_params.tilt_max); param_get(_params_handles.land_speed, &_params.land_speed); param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); + _params.land_tilt_max = math::radians(_params.land_tilt_max); float v; param_get(_params_handles.xy_p, &v); diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 0082a5e6a6..609ff626a7 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -176,11 +176,12 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); * * Limits maximum tilt in AUTO and EASY modes. * + * @unit deg * @min 0.0 - * @max 1.57 + * @max 90.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 45.0f); /** * Landing descend rate @@ -195,8 +196,9 @@ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); * * Limits maximum tilt on landing. * + * @unit deg * @min 0.0 - * @max 1.57 + * @max 90.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f); +PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 15.0f); From 84943644d77ce21e91fa60a326ab333069333e74 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Apr 2014 23:06:32 +0200 Subject: [PATCH 51/53] mc_pos_control: parameters comments minor fixes --- src/modules/mc_pos_control/mc_pos_control_params.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 609ff626a7..fe0004ea8a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -100,6 +100,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); * * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). * + * @unit m/s * @min 0.0 * @group Multicopter Position Control */ @@ -155,6 +156,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); * * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). * + * @unit m/s * @min 0.0 * @group Multicopter Position Control */ @@ -186,6 +188,7 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 45.0f); /** * Landing descend rate * + * @unit m/s * @min 0.0 * @group Multicopter Position Control */ From e6d332aa7cd038b00643a70d99e82d60b7ffbcf0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Apr 2014 15:50:53 +0200 Subject: [PATCH 52/53] Make commander less pedantic about position status --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3527117341..53ed34f46b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */ +#define POSITION_TIMEOUT 100000 /**< consider the local or global position estimate invalid after 100ms */ #define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 From 08408594ec3f136bdcf554a2a75c2e0af8e03c8c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Apr 2014 16:06:34 +0200 Subject: [PATCH 53/53] Renamed parameters which changed from RAD to DEGREES to avoid user confusion. Also made naming of the two parameters more consistent. --- .../mc_pos_control/mc_pos_control_params.c | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index fe0004ea8a..104df4e59b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -174,16 +174,28 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** - * Maximum tilt + * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and EASY modes. + * Limits maximum tilt in AUTO and EASY modes during flight. * * @unit deg * @min 0.0 * @max 90.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 45.0f); +PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); + +/** + * Maximum tilt during landing + * + * Limits maximum tilt angle on landing. + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f); /** * Landing descend rate @@ -194,14 +206,3 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 45.0f); */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); -/** - * Maximum landing tilt - * - * Limits maximum tilt on landing. - * - * @unit deg - * @min 0.0 - * @max 90.0 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 15.0f);