From 3a898e54adbaac7fda28e0b9079076d9cdf8028c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 14:30:29 +0200 Subject: [PATCH 001/600] towards a global map projection instance, WIP: need to remove local updates of the reference and add a global map update --- src/lib/geo/geo.c | 46 ++++++++++++------- src/lib/geo/geo.h | 9 ++-- .../fw_att_pos_estimator_main.cpp | 6 +-- src/modules/mavlink/mavlink_receiver.cpp | 4 +- .../mc_pos_control/mc_pos_control_main.cpp | 9 ++-- .../position_estimator_inav_main.c | 20 +++----- 6 files changed, 49 insertions(+), 45 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9a24ff50e2..32d52aadc5 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -55,33 +55,47 @@ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -__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 -{ - ref->lat = lat_0 / 180.0 * M_PI; - ref->lon = lon_0 / 180.0 * M_PI; +struct map_projection_reference_s mp_ref; - ref->sin_lat = sin(ref->lat); - ref->cos_lat = cos(ref->lat); +__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 +{ + mp_ref.lat = lat_0 / 180.0 * M_PI; + mp_ref.lon = lon_0 / 180.0 * M_PI; + + mp_ref.sin_lat = sin(mp_ref.lat); + mp_ref.cos_lat = cos(mp_ref.lat); + + mp_ref.init_done = true; } -__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) { + if (!mp_ref.init_done) { + return false; + } + double lat_rad = lat / 180.0 * M_PI; double lon_rad = lon / 180.0 * M_PI; double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); - double cos_d_lon = cos(lon_rad - ref->lon); + double cos_d_lon = cos(lon_rad - mp_ref.lon); - double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double c = acos(mp_ref.sin_lat * sin_lat + mp_ref.cos_lat * cos_lat * cos_d_lon); 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; + *x = k * (mp_ref.cos_lat * sin_lat - mp_ref.sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - mp_ref.lon) * CONSTANTS_RADIUS_OF_EARTH; + + return true; } -__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon) { + if (!mp_ref.init_done) { + return false; + } + 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); @@ -92,12 +106,12 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f double lon_rad; 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)); + lat_rad = asin(cos_c * mp_ref.sin_lat + (x_rad * sin_c * mp_ref.cos_lat) / c); + lon_rad = (mp_ref.lon + atan2(y_rad * sin_c, c * mp_ref.cos_lat * cos_c - x_rad * mp_ref.sin_lat * sin_c)); } else { - lat_rad = ref->lat; - lon_rad = ref->lon; + lat_rad = mp_ref.lat; + lon_rad = mp_ref.lon; } *lat = lat_rad * 180.0 / M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 0a3f85d970..b48d26a66f 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -71,6 +71,7 @@ struct map_projection_reference_s { double lon; double sin_lat; double cos_lat; + bool init_done; }; /** @@ -80,7 +81,7 @@ struct map_projection_reference_s { * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0); +__EXPORT void map_projection_init(double lat_0, double lon_0); /** * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane @@ -88,8 +89,9 @@ __EXPORT void map_projection_init(struct map_projection_reference_s *ref, double * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) + * @return true if map_projection_init was called before, false else */ -__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); +__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y); /** * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system @@ -98,8 +100,9 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) + * @return true if map_projection_init was called before, false else */ -__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon); /** * Returns the distance to the next waypoint in meters. 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 9d2d9ed5f3..420a03644e 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 @@ -177,8 +177,6 @@ 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 */ @@ -822,8 +820,6 @@ FixedwingEstimator::task_main() _ekf->baroHgt = _baro.altitude - _baro_ref; _baro_gps_offset = _baro_ref - _local_pos.ref_alt; - // XXX this is not multithreading safe - 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; @@ -1046,7 +1042,7 @@ FixedwingEstimator::task_main() if (_local_pos.xy_global) { double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + map_projection_reproject(_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; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index adafdd1395..df61c3c156 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -795,7 +795,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); +// map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); //XXX fix reference update hil_local_pos.ref_timestamp = timestamp; hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; @@ -804,7 +804,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) float x; float y; - map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); + map_projection_project(lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = 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 dc0aa172a4..963349d42e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -478,17 +478,17 @@ MulticopterPositionControl::update_ref() if (_ref_timestamp != 0) { /* calculate current position setpoint in global frame */ - map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + map_projection_reproject(_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); +// map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); //XXX fix reference update _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]); + map_projection_project(lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(alt_sp - _ref_alt); } @@ -687,8 +687,7 @@ MulticopterPositionControl::task_main() _reset_alt_sp = true; /* project setpoint to local frame */ - map_projection_project(&_ref_pos, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + map_projection_project(_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); 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 368424853c..b803dfb581 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -558,12 +558,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) 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); + map_projection_reproject(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; @@ -572,9 +569,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_alt = home.alt; local_pos.ref_timestamp = home.timestamp; - if (ref_inited) { + if (ref_inited) { //XXX fix reference update /* reproject position estimate with new reference */ - map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); + map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]); z_est[0] = -(est_alt - local_pos.ref_alt); } @@ -633,18 +630,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_lon = lon; local_pos.ref_alt = alt; local_pos.ref_timestamp = t; - - /* initialize projection */ - 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); } } - if (ref_inited) { + if (ref_inited) { //XXX fix reference update /* project GPS lat lon to plane */ float gps_proj[2]; - map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); + map_projection_project(lat, lon, &(gps_proj[0]), &(gps_proj[1])); /* reset position estimate when GPS becomes good */ if (reset_est) { @@ -938,7 +930,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.time_gps_usec = gps.time_gps_usec; double est_lat, est_lon; - map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = est_lat; global_pos.lon = est_lon; From 6517c201f701da97acde1d7e3cc72712b9a1613c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 15:59:13 +0200 Subject: [PATCH 002/600] geo: add missing return, make map projection reference static --- src/lib/geo/geo.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 32d52aadc5..60994940f9 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -55,7 +55,7 @@ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -struct map_projection_reference_s mp_ref; +static struct map_projection_reference_s mp_ref; __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 { @@ -116,6 +116,8 @@ __EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lo *lat = lat_rad * 180.0 / M_PI; *lon = lon_rad * 180.0 / M_PI; + + return true; } From fbcb248bc3a91d1c26f6f47a2a46badd2d8a58f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 15:59:49 +0200 Subject: [PATCH 003/600] navigator: init map projection on home pos update --- src/modules/navigator/navigator_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d4d23396a1..b7cf49cfa9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -625,6 +625,7 @@ Navigator::task_main() parameters_update(); global_position_update(); home_position_update(); + map_projection_init(_home_pos.lat, _home_pos.lon); navigation_capabilities_update(); offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); @@ -816,6 +817,7 @@ Navigator::task_main() if (fds[2].revents & POLLIN) { home_position_update(); // XXX check if home position really changed + map_projection_init(_home_pos.lat, _home_pos.lon); dispatch(EVENT_HOME_POSITION_CHANGED); } From 8bb1b39399eb09ddf93c2682713d80be4f776c5d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 16:12:28 +0200 Subject: [PATCH 004/600] mavlink receiver: hil: local pos reference lat lon from first data --- src/modules/mavlink/mavlink_receiver.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index df61c3c156..40121efe4f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -795,7 +795,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; -// map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); //XXX fix reference update hil_local_pos.ref_timestamp = timestamp; hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; From 35d15720ef79dae4d6f6202092d4869a455fc7d2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:04:27 +0200 Subject: [PATCH 005/600] geo: add timestamp and function to get init state --- src/lib/geo/geo.c | 10 ++++++++-- src/lib/geo/geo.h | 7 +++++++ 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 60994940f9..dbbf1f80cc 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -57,6 +57,11 @@ static struct map_projection_reference_s mp_ref; +__EXPORT bool map_projection_inited() +{ + return mp_ref.init_done; +} + __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 { mp_ref.lat = lat_0 / 180.0 * M_PI; @@ -65,12 +70,13 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are mp_ref.sin_lat = sin(mp_ref.lat); mp_ref.cos_lat = cos(mp_ref.lat); + mp_ref.timestamp = hrt_absolute_time(); mp_ref.init_done = true; } __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) { - if (!mp_ref.init_done) { + if (!map_projection_inited()) { return false; } @@ -92,7 +98,7 @@ __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) __EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon) { - if (!mp_ref.init_done) { + if (!map_projection_inited()) { return false; } diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b48d26a66f..b19bfe4625 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -72,8 +72,15 @@ struct map_projection_reference_s { double sin_lat; double cos_lat; bool init_done; + uint64_t timestamp; }; +/** + * Initializes the map transformation. + * @return true if map_projection_init was called before, false else + */ +__EXPORT bool map_projection_inited(); + /** * Initializes the map transformation. * From 9f2da53f08f5d186d95653bad9afa04e8c7276d2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:05:30 +0200 Subject: [PATCH 006/600] mc pos control: map projection init not needed anymore --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- 1 file changed, 2 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 963349d42e..fa3438ea82 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -482,8 +482,6 @@ MulticopterPositionControl::update_ref() alt_sp = _ref_alt - _pos_sp(2); } - /* update local projection reference */ -// map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); //XXX fix reference update _ref_alt = _local_pos.ref_alt; if (_ref_timestamp != 0) { From 618ac319e63a6597cc62df9c810d76cdc094012b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:06:10 +0200 Subject: [PATCH 007/600] pos estimator inav: check if map projection is initialized --- .../position_estimator_inav_main.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 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 b803dfb581..d01bf488f9 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -549,7 +549,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(home_position), home_position_sub, &home); - if (home.timestamp != home_timestamp) { + if (home.timestamp != home_timestamp && map_projection_inited()) { home_timestamp = home.timestamp; double est_lat, est_lon; @@ -569,7 +569,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_alt = home.alt; local_pos.ref_timestamp = home.timestamp; - if (ref_inited) { //XXX fix reference update + if (ref_inited) { /* reproject position estimate with new reference */ map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]); z_est[0] = -(est_alt - local_pos.ref_alt); @@ -602,7 +602,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (gps_valid) { + if (gps_valid && map_projection_inited()) { double lat = gps.lat * 1e-7; double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; @@ -633,7 +633,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (ref_inited) { //XXX fix reference update + if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(lat, lon, &(gps_proj[0]), &(gps_proj[1])); From b249d04e52ae34d43d9d80638d6f1ead476f39ea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:13:06 +0200 Subject: [PATCH 008/600] geo: get function for timestamp of map projection --- src/lib/geo/geo.c | 5 +++++ src/lib/geo/geo.h | 7 +++++++ 2 files changed, 12 insertions(+) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index dbbf1f80cc..1e3ef56aef 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -62,6 +62,11 @@ __EXPORT bool map_projection_inited() return mp_ref.init_done; } +__EXPORT uint64_t map_projection_timestamp() +{ + return mp_ref.timestamp; +} + __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 { mp_ref.lat = lat_0 / 180.0 * M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b19bfe4625..913e83b6e1 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -81,6 +81,13 @@ struct map_projection_reference_s { */ __EXPORT bool map_projection_inited(); + +/** + * Initializes the map transformation. + * @return the timestamp of the map_projection + */ +__EXPORT uint64_t map_projection_timestamp(); + /** * Initializes the map transformation. * From 4e30784d6c506cdfdf58c623da20cffcb799e496 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Apr 2014 17:16:20 +0200 Subject: [PATCH 009/600] rename map_projection_inited to map_projection_initialized --- src/lib/geo/geo.c | 6 +++--- src/lib/geo/geo.h | 2 +- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 1e3ef56aef..725a04a298 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -57,7 +57,7 @@ static struct map_projection_reference_s mp_ref; -__EXPORT bool map_projection_inited() +__EXPORT bool map_projection_initialized() { return mp_ref.init_done; } @@ -81,7 +81,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) { - if (!map_projection_inited()) { + if (!map_projection_initialized()) { return false; } @@ -103,7 +103,7 @@ __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) __EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon) { - if (!map_projection_inited()) { + if (!map_projection_initialized()) { return false; } diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 913e83b6e1..66e4dc75e8 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -79,7 +79,7 @@ struct map_projection_reference_s { * Initializes the map transformation. * @return true if map_projection_init was called before, false else */ -__EXPORT bool map_projection_inited(); +__EXPORT bool map_projection_initialized(); /** 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 d01bf488f9..e88493a5e2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -549,7 +549,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(home_position), home_position_sub, &home); - if (home.timestamp != home_timestamp && map_projection_inited()) { + if (home.timestamp != home_timestamp && map_projection_initialized()) { home_timestamp = home.timestamp; double est_lat, est_lon; @@ -602,7 +602,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (gps_valid && map_projection_inited()) { + if (gps_valid && map_projection_initialized()) { double lat = gps.lat * 1e-7; double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; From 9ebd6b348687d5563b87c5c872d432adbfad696f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:41:46 +0200 Subject: [PATCH 010/600] geo: interface to get reference lat lon, option to set reference timestamp on initialization --- src/lib/geo/geo.c | 16 ++++++++++++++-- src/lib/geo/geo.h | 8 +++++++- 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 725a04a298..a2117ecd01 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -67,7 +67,7 @@ __EXPORT uint64_t map_projection_timestamp() return mp_ref.timestamp; } -__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(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { mp_ref.lat = lat_0 / 180.0 * M_PI; mp_ref.lon = lon_0 / 180.0 * M_PI; @@ -75,10 +75,22 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are mp_ref.sin_lat = sin(mp_ref.lat); mp_ref.cos_lat = cos(mp_ref.lat); - mp_ref.timestamp = hrt_absolute_time(); + mp_ref.timestamp = timestamp; mp_ref.init_done = true; } +__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon) +{ + if (!map_projection_initialized()) { + return false; + } + + *ref_lat = mp_ref.lat; + *ref_lon = mp_ref.lon; + + return true; +} + __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) { if (!map_projection_initialized()) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 66e4dc75e8..b5e6596855 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -88,6 +88,12 @@ __EXPORT bool map_projection_initialized(); */ __EXPORT uint64_t map_projection_timestamp(); +/** + * Writes the reference values to ref_lat and ref_lon + * @return true if map_projection_init was called before, false else + */ +__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon); + /** * Initializes the map transformation. * @@ -95,7 +101,7 @@ __EXPORT uint64_t map_projection_timestamp(); * @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(double lat_0, double lon_0, uint64_t timestamp); /** * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane From f5d9023b1060f10c14d016492c88375c6fdc22aa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:42:28 +0200 Subject: [PATCH 011/600] navigator: set home_pos timestamp as map projection reference timestamp --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b7cf49cfa9..228324789b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -625,7 +625,7 @@ Navigator::task_main() parameters_update(); global_position_update(); home_position_update(); - map_projection_init(_home_pos.lat, _home_pos.lon); + map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); navigation_capabilities_update(); offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); @@ -817,7 +817,7 @@ Navigator::task_main() if (fds[2].revents & POLLIN) { home_position_update(); // XXX check if home position really changed - map_projection_init(_home_pos.lat, _home_pos.lon); + map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); dispatch(EVENT_HOME_POSITION_CHANGED); } From 7e1ea35571149a9d766cc7122738f206f1ed3427 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:43:19 +0200 Subject: [PATCH 012/600] mavlink receiver: in hil use map_projection_timestamp for hil_local_pos.ref_timestamp --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 40121efe4f..e9b58bccf2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -795,7 +795,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - hil_local_pos.ref_timestamp = timestamp; + hil_local_pos.ref_timestamp = map_projection_timestamp(); hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; hil_local_pos.ref_alt = _hil_local_alt0; From 5285eb2f997b590071186a8f695af4375f96e779 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 09:44:40 +0200 Subject: [PATCH 013/600] fw att pos estimator: use map projection reference values for local pos --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 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 420a03644e..45ca13a7fb 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 @@ -797,7 +797,7 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + if (!_gps_initialized && (_ekf->GPSstatus == 3) && map_projection_initialized()) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; @@ -809,10 +809,9 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.lon; + map_projection_reference(&_local_pos.ref_lat, &_local_pos.ref_lon); _local_pos.ref_alt = alt; - _local_pos.ref_timestamp = _gps.timestamp_position; + _local_pos.ref_timestamp = map_projection_timestamp(); // Store orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); From 08bc777208a8510da7cd91fab5059ffa8bd14150 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 13:57:06 +0200 Subject: [PATCH 014/600] pos estimator inav: use map_projection_reference to set local pos reference lat lon --- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 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 e88493a5e2..91642e5b92 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -626,8 +626,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) z_est[0] = 0.0f; y_est[2] = accel_NED[1]; - local_pos.ref_lat = lat; - local_pos.ref_lon = lon; + map_projection_reference(&local_pos.ref_lat, &local_pos.ref_lon); + local_pos.ref_alt = alt; local_pos.ref_timestamp = t; } From 241a99fc28992694f807595ce6bb48f460c9a28a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 14:45:21 +0200 Subject: [PATCH 015/600] position estimator mc: do not initialize map projection because this is now handled globally --- .../position_estimator_mc/position_estimator_mc_main.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index 3639618193..ee6457aded 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -322,11 +322,6 @@ int position_estimator_mc_thread_main(int argc, char *argv[]) lon_current = ((double)(gps.lon)) * 1e-7d; alt_current = gps.alt * 1e-3f; gps_origin_altitude = alt_current; - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - /* publish global position messages only after first GPS message */ - printf("[pos_est_mc] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); - } else { mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON"); /* onboard calculated position estimations */ From aab64af8844e03b04f5616c1beecf18c9f6a7d81 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 14:47:37 +0200 Subject: [PATCH 016/600] geo: map projection: safer initialization, only accept init from navigator, return int instead of bool --- src/lib/geo/geo.c | 41 ++++++++++++++++++++++++----------------- src/lib/geo/geo.h | 16 ++++++++-------- 2 files changed, 32 insertions(+), 25 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index a2117ecd01..6cbf74e466 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -55,7 +55,7 @@ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -static struct map_projection_reference_s mp_ref; +static struct map_projection_reference_s mp_ref = {0}; __EXPORT bool map_projection_initialized() { @@ -67,34 +67,41 @@ __EXPORT uint64_t map_projection_timestamp() return mp_ref.timestamp; } -__EXPORT void map_projection_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - mp_ref.lat = lat_0 / 180.0 * M_PI; - mp_ref.lon = lon_0 / 180.0 * M_PI; + if (strcmp("navigator", getprogname() == 0)) { - mp_ref.sin_lat = sin(mp_ref.lat); - mp_ref.cos_lat = cos(mp_ref.lat); + mp_ref.lat = lat_0 / 180.0 * M_PI; + mp_ref.lon = lon_0 / 180.0 * M_PI; - mp_ref.timestamp = timestamp; - mp_ref.init_done = true; + mp_ref.sin_lat = sin(mp_ref.lat); + mp_ref.cos_lat = cos(mp_ref.lat); + + mp_ref.timestamp = timestamp; + mp_ref.init_done = true; + + return 0; + } else { + return -1; + } } -__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon) +__EXPORT int map_projection_reference(double *ref_lat, double *ref_lon) { if (!map_projection_initialized()) { - return false; + return -1; } *ref_lat = mp_ref.lat; *ref_lon = mp_ref.lon; - return true; + return 0; } -__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) +__EXPORT int map_projection_project(double lat, double lon, float *x, float *y) { if (!map_projection_initialized()) { - return false; + return -1; } double lat_rad = lat / 180.0 * M_PI; @@ -110,13 +117,13 @@ __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) *x = k * (mp_ref.cos_lat * sin_lat - mp_ref.sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; *y = k * cos_lat * sin(lon_rad - mp_ref.lon) * CONSTANTS_RADIUS_OF_EARTH; - return true; + return 0; } -__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon) +__EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon) { if (!map_projection_initialized()) { - return false; + return -1; } float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; @@ -140,7 +147,7 @@ __EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lo *lat = lat_rad * 180.0 / M_PI; *lon = lon_rad * 180.0 / M_PI; - return true; + return 0; } diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b5e6596855..b0f8d6bc40 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -77,7 +77,7 @@ struct map_projection_reference_s { /** * Initializes the map transformation. - * @return true if map_projection_init was called before, false else + * @return true if map was initialized before, false else */ __EXPORT bool map_projection_initialized(); @@ -90,9 +90,9 @@ __EXPORT uint64_t map_projection_timestamp(); /** * Writes the reference values to ref_lat and ref_lon - * @return true if map_projection_init was called before, false else + * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon); +__EXPORT int map_projection_reference(double *ref_lat, double *ref_lon); /** * Initializes the map transformation. @@ -101,7 +101,7 @@ __EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon); * @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, uint64_t timestamp); +__EXPORT int map_projection_init(double lat_0, double lon_0, uint64_t timestamp); /** * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane @@ -109,9 +109,9 @@ __EXPORT void map_projection_init(double lat_0, double lon_0, uint64_t timestamp * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) - * @return true if map_projection_init was called before, false else + * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT bool map_projection_project(double lat, double lon, float *x, float *y); +__EXPORT int map_projection_project(double lat, double lon, float *x, float *y); /** * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system @@ -120,9 +120,9 @@ __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y) * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) - * @return true if map_projection_init was called before, false else + * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT bool map_projection_reproject(float x, float y, double *lat, double *lon); +__EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon); /** * Returns the distance to the next waypoint in meters. From f7e18eaa5809580878b377dfa78ce0ece96d55a7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 15:08:06 +0200 Subject: [PATCH 017/600] uorb home position topic: add valid flag --- src/modules/uORB/topics/home_position.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 08d11abaea..73c8dc50d1 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -63,6 +63,7 @@ struct home_position_s double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude in meters */ + bool valid; /**< True if home position is valid */ }; /** From 06dd10cb018d48f60abd1a5e9f83fbdc67867d8c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 15:08:38 +0200 Subject: [PATCH 018/600] commander: set home position valid flag --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1827f252cc..23fe647695 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -617,6 +617,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe 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); + home->valid = true; + /* announce new home position */ if (*home_pub > 0) { orb_publish(ORB_ID(home_position), *home_pub, home); @@ -999,6 +1001,7 @@ int commander_thread_main(int argc, char *argv[]) home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; + home.valid = true; 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); @@ -1385,6 +1388,7 @@ int commander_thread_main(int argc, char *argv[]) home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; + home.valid = true; 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); From 35b0c294568ca916b344e6a9e6a8e27cefb6f8ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Apr 2014 15:09:11 +0200 Subject: [PATCH 019/600] navigator: set reference for map projection only once --- src/modules/navigator/navigator_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e430284156..2f6bf49f3a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -625,7 +625,6 @@ Navigator::task_main() parameters_update(); global_position_update(); home_position_update(); - map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); navigation_capabilities_update(); offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); @@ -781,7 +780,6 @@ Navigator::task_main() if (fds[2].revents & POLLIN) { home_position_update(); // XXX check if home position really changed - map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); dispatch(EVENT_HOME_POSITION_CHANGED); } @@ -824,6 +822,11 @@ Navigator::task_main() _global_pos_valid = _vstatus.condition_global_position_valid; + /* set reference for map _projection if global pos is valid and home position is valid and we have not done so already */ + if (!map_projection_initialized() && _global_pos_valid && _home_pos.valid) { + map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); + } + /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { _pos_sp_triplet_updated = false; From 3ec818ce1ed764aeb5f3e8c7371b53b87c2498c3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 14:39:36 +0200 Subject: [PATCH 020/600] rerwite projection interrface to not break the current implementation --- src/lib/geo/geo.c | 98 +++++++++++++++++++++++++++++++---------------- src/lib/geo/geo.h | 81 ++++++++++++++++++++++++++++++++------- 2 files changed, 132 insertions(+), 47 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 6cbf74e466..53e601fadb 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -57,50 +57,77 @@ static struct map_projection_reference_s mp_ref = {0}; -__EXPORT bool map_projection_initialized() +__EXPORT bool map_projection_global_initialized() { - return mp_ref.init_done; + return map_projection_initialized(&mp_ref); } -__EXPORT uint64_t map_projection_timestamp() +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref) { - return mp_ref.timestamp; + return ref->init_done; } -__EXPORT int map_projection_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT uint64_t map_projection_global_timestamp() +{ + return map_projection_timestamp(&mp_ref); +} + +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref) +{ + return ref->timestamp; +} + +__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { if (strcmp("navigator", getprogname() == 0)) { - mp_ref.lat = lat_0 / 180.0 * M_PI; - mp_ref.lon = lon_0 / 180.0 * M_PI; - - mp_ref.sin_lat = sin(mp_ref.lat); - mp_ref.cos_lat = cos(mp_ref.lat); - - mp_ref.timestamp = timestamp; - mp_ref.init_done = true; - - return 0; + return map_projection_init(&mp_ref, lat_0, lon_0, timestamp); } else { return -1; } } -__EXPORT int map_projection_reference(double *ref_lat, double *ref_lon) +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - if (!map_projection_initialized()) { - return -1; - } - *ref_lat = mp_ref.lat; - *ref_lon = mp_ref.lon; + 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); + + ref->timestamp = timestamp; + ref->init_done = true; return 0; } -__EXPORT int map_projection_project(double lat, double lon, float *x, float *y) +__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon) { - if (!map_projection_initialized()) { + return map_projection_reference_s(&mp_ref, ref_lat, ref_lon); +} + +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + *ref_lat = ref->lat; + *ref_lon = ref->lon; + + return 0; +} + +__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y) +{ + return map_projection_project(&mp_ref, lat, lon, x, y); + +} + +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +{ + if (!map_projection_initialized(ref)) { return -1; } @@ -109,20 +136,25 @@ __EXPORT int map_projection_project(double lat, double lon, float *x, float *y) double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); - double cos_d_lon = cos(lon_rad - mp_ref.lon); + double cos_d_lon = cos(lon_rad - ref->lon); - double c = acos(mp_ref.sin_lat * sin_lat + mp_ref.cos_lat * cos_lat * cos_d_lon); + 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)); - *x = k * (mp_ref.cos_lat * sin_lat - mp_ref.sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; - *y = k * cos_lat * sin(lon_rad - mp_ref.lon) * CONSTANTS_RADIUS_OF_EARTH; + *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; return 0; } -__EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon) +__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon) { - if (!map_projection_initialized()) { + map_projection_project(&mp_ref, x, y, lat, lon); +} + +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +{ + if (!map_projection_initialized(ref)) { return -1; } @@ -136,12 +168,12 @@ __EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon double lon_rad; if (c != 0.0) { - lat_rad = asin(cos_c * mp_ref.sin_lat + (x_rad * sin_c * mp_ref.cos_lat) / c); - lon_rad = (mp_ref.lon + atan2(y_rad * sin_c, c * mp_ref.cos_lat * cos_c - x_rad * mp_ref.sin_lat * sin_c)); + 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 { - lat_rad = mp_ref.lat; - lon_rad = mp_ref.lon; + lat_rad = ref->lat; + lon_rad = ref->lon; } *lat = lat_rad * 180.0 / M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b0f8d6bc40..990f791948 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -76,45 +76,86 @@ struct map_projection_reference_s { }; /** - * Initializes the map transformation. + * Checks if global projection was initialized * @return true if map was initialized before, false else */ -__EXPORT bool map_projection_initialized(); - +__EXPORT bool map_projection_global_initialized(); /** - * Initializes the map transformation. + * Checks if projection given as argument was initialized + * @return true if map was initialized before, false else + */ +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref); + +/** + * Get the timestamp of the global map projection * @return the timestamp of the map_projection */ -__EXPORT uint64_t map_projection_timestamp(); +__EXPORT uint64_t map_projection_global_timestamp(); /** - * Writes the reference values to ref_lat and ref_lon + * Get the timestamp of the map projection given by the argument + * @return the timestamp of the map_projection + */ +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref); + +/** + * Writes the reference values of the global projection to ref_lat and ref_lon * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reference(double *ref_lat, double *ref_lon); +__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon); /** - * Initializes the map transformation. + * Writes the reference values of the projection given by the argument to ref_lat and ref_lon + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon); + +/** + * Initializes the global map transformation. * - * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT int map_projection_init(double lat_0, double lon_0, uint64_t timestamp); +__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp); /** - * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * Initializes the map transformation given by the argument. + * + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp); + +/** + * Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the global projection * @param x north * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_project(double lat, double lon, float *x, float *y); +__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y); + + + /* Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the projection given by the argument + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_project(const 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 + * Transforms a point in the local azimuthal equidistant plane to the + * geographic coordinate system using the global projection * * @param x north * @param y east @@ -122,7 +163,19 @@ __EXPORT int map_projection_project(double lat, double lon, float *x, float *y); * @param lon in degrees (8.1234567°, not 81234567°) * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reproject(float x, float y, double *lat, double *lon); +__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon); + +/** + * Transforms a point in the local azimuthal equidistant plane to the + * geographic coordinate system using the projection given by the argument + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); /** * Returns the distance to the next waypoint in meters. From 4f84cdc8b871b6c8da7f05a58e96f97b65f290c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 14:40:01 +0200 Subject: [PATCH 021/600] fw_att_pos_estimator: use new global map projection --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 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 45ca13a7fb..79328d5161 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 @@ -797,7 +797,7 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3) && map_projection_initialized()) { + if (!_gps_initialized && (_ekf->GPSstatus == 3) && map_projection_global_initialized()) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; @@ -809,9 +809,9 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - map_projection_reference(&_local_pos.ref_lat, &_local_pos.ref_lon); + map_projection_global_reference(&_local_pos.ref_lat, &_local_pos.ref_lon); _local_pos.ref_alt = alt; - _local_pos.ref_timestamp = map_projection_timestamp(); + _local_pos.ref_timestamp = map_projection_global_timestamp(); // Store orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); @@ -1041,7 +1041,7 @@ FixedwingEstimator::task_main() 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_global_reproject(_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 a7289a32664e3118bf553d23cfd91ed5c86caa84 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:39:26 +0200 Subject: [PATCH 022/600] mavlink interface: update mapprojection usage --- src/lib/geo/geo.c | 11 ++++++++--- src/lib/geo/geo.h | 13 ++++++++++++- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 3 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 53e601fadb..438e354dff 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -81,13 +81,13 @@ __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t tim { if (strcmp("navigator", getprogname() == 0)) { - return map_projection_init(&mp_ref, lat_0, lon_0, timestamp); + return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } else { return -1; } } -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { ref->lat = lat_0 / 180.0 * M_PI; @@ -102,9 +102,14 @@ __EXPORT int map_projection_init(struct map_projection_reference_s *ref, double return 0; } +__EXPORT int 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 +{ + map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); +} + __EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon) { - return map_projection_reference_s(&mp_ref, ref_lat, ref_lon); + return map_projection_reference(&mp_ref, ref_lat, ref_lon); } __EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 990f791948..3d7ba050f9 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -129,7 +129,18 @@ __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t tim * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp); +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, + double lat_0, double lon_0, uint64_t timestamp); + +/** + * Initializes the map transformation given by the argument and sets the timestamp to now. + * + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT int 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 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b2c5e0079a..db8f00e8f7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -766,7 +766,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - hil_local_pos.ref_timestamp = map_projection_timestamp(); + hil_local_pos.ref_timestamp = map_projection_global_timestamp(); hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; hil_local_pos.ref_alt = _hil_local_alt0; @@ -774,7 +774,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) float x; float y; - map_projection_project(lat, lon, &x, &y); + map_projection_global_project(lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; From 5a868751b58ebb6b169beafd6258cd8874440483 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:40:04 +0200 Subject: [PATCH 023/600] navigator: update mapprojection usage --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2f6bf49f3a..14c568c46d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -823,8 +823,8 @@ Navigator::task_main() _global_pos_valid = _vstatus.condition_global_position_valid; /* set reference for map _projection if global pos is valid and home position is valid and we have not done so already */ - if (!map_projection_initialized() && _global_pos_valid && _home_pos.valid) { - map_projection_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); + if (!map_projection_global_initialized() && _global_pos_valid && _home_pos.valid) { + map_projection_global_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); } /* publish position setpoint triplet if updated */ From 53d23c67d72a6395cef509817df25264f05fbd85 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:40:34 +0200 Subject: [PATCH 024/600] mc pos ctrl: revert to local map projection --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 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 bf7b6a19cd..6b797f222b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -468,15 +468,17 @@ MulticopterPositionControl::update_ref() if (_ref_timestamp != 0) { /* calculate current position setpoint in global frame */ - map_projection_reproject(_pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + 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(lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(alt_sp - _ref_alt); } @@ -675,7 +677,8 @@ MulticopterPositionControl::task_main() _reset_alt_sp = true; /* project setpoint to local frame */ - map_projection_project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + 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); From 510678bdae82a04151b6d5dcd5b02bee6f96abfd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:40:54 +0200 Subject: [PATCH 025/600] pos estimator inav: revert to local map projection --- .../position_estimator_inav_main.c | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 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 91642e5b92..368424853c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -549,7 +549,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(home_position), home_position_sub, &home); - if (home.timestamp != home_timestamp && map_projection_initialized()) { + if (home.timestamp != home_timestamp) { home_timestamp = home.timestamp; double est_lat, est_lon; @@ -558,9 +558,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* calculate current estimated position in global frame */ est_alt = local_pos.ref_alt - local_pos.z; - 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); } + /* update reference */ + map_projection_init(&ref, home.lat, home.lon); + /* update baro offset */ baro_offset += home.alt - local_pos.ref_alt; @@ -571,7 +574,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* reproject position estimate with new reference */ - map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]); + map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); z_est[0] = -(est_alt - local_pos.ref_alt); } @@ -602,7 +605,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (gps_valid && map_projection_initialized()) { + if (gps_valid) { double lat = gps.lat * 1e-7; double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; @@ -626,17 +629,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) z_est[0] = 0.0f; y_est[2] = accel_NED[1]; - map_projection_reference(&local_pos.ref_lat, &local_pos.ref_lon); - + local_pos.ref_lat = lat; + local_pos.ref_lon = lon; local_pos.ref_alt = alt; local_pos.ref_timestamp = t; + + /* initialize projection */ + 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); } } if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; - map_projection_project(lat, lon, &(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) { @@ -930,7 +938,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.time_gps_usec = gps.time_gps_usec; 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; From 606f3cba5c932d6aba0e383e915b02b78009fd9f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 29 Apr 2014 15:41:12 +0200 Subject: [PATCH 026/600] pos estimator mc: revert to local map projection --- .../position_estimator_mc/position_estimator_mc_main.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index ee6457aded..3639618193 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -322,6 +322,11 @@ int position_estimator_mc_thread_main(int argc, char *argv[]) lon_current = ((double)(gps.lon)) * 1e-7d; alt_current = gps.alt * 1e-3f; gps_origin_altitude = alt_current; + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + /* publish global position messages only after first GPS message */ + printf("[pos_est_mc] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); + } else { mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON"); /* onboard calculated position estimations */ From f4c36f8c5cf69bd3af55a1065ab7a18c999c054c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 1 May 2014 20:57:36 +0200 Subject: [PATCH 027/600] mc_pos_control: use current velocity to calculate position setpoint on reset to make transition to stabilized modes more smooth --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 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 7c625a0c53..fe0e8fe1ae 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -493,8 +493,8 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); + _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -504,7 +504,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2); + _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } From 7232c0354bee1f8f572bdb2f53f9969ba9778a6e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 1 May 2014 23:01:30 +0200 Subject: [PATCH 028/600] mc_pos_control: use MPC_XXX_FF to adjust setpoint on reset --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 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 fe0e8fe1ae..356f3c15e8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -388,9 +388,11 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.z_vel_max, &v); _params.vel_max(2) = v; param_get(_params_handles.xy_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(0) = v; _params.vel_ff(1) = v; param_get(_params_handles.z_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; @@ -493,8 +495,8 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0); - _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1); + _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0) * (1.0f - _params.vel_ff(0)); + _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1) * (1.0f - _params.vel_ff(1)); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -504,7 +506,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2); + _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2) * (1.0f - _params.vel_ff(2)); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } From 60ccbaa8bb59f4d510b74f7599ef60ef45837180 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 15:37:53 +0200 Subject: [PATCH 029/600] init global map reference in commander and not in navigator --- src/modules/commander/commander.cpp | 14 ++++++++++++-- src/modules/navigator/navigator_main.cpp | 5 ----- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2db30b0664..54834cf9bb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -87,6 +87,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -366,7 +367,7 @@ static orb_advert_t status_pub; transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; - + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); @@ -375,7 +376,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armed } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); } - + return arming_res; } @@ -571,6 +572,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->valid = true; + /* set reference for map _projection */ + map_projection_global_init(home->lat, home->lon, hrt_absolute_time()); + /* announce new home position */ if (*home_pub > 0) { orb_publish(ORB_ID(home_position), *home_pub, home); @@ -958,6 +962,9 @@ int commander_thread_main(int argc, char *argv[]) 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); + /* set reference for map _projection */ + map_projection_global_init(home.lat, home.lon, hrt_absolute_time()); + /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); @@ -1345,6 +1352,9 @@ int commander_thread_main(int argc, char *argv[]) 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); + /* set reference for map _projection */ + map_projection_global_init(home.lat, home.lon, hrt_absolute_time()); + /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 14c568c46d..12fd35a0a9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -822,11 +822,6 @@ Navigator::task_main() _global_pos_valid = _vstatus.condition_global_position_valid; - /* set reference for map _projection if global pos is valid and home position is valid and we have not done so already */ - if (!map_projection_global_initialized() && _global_pos_valid && _home_pos.valid) { - map_projection_global_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); - } - /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { _pos_sp_triplet_updated = false; From 67e3c808d24773c7c4bd4608866de1edb617cb04 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 15:40:02 +0200 Subject: [PATCH 030/600] remove home position valid flag --- src/modules/commander/commander.cpp | 4 ---- src/modules/uORB/topics/home_position.h | 1 - 2 files changed, 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 54834cf9bb..bc3bdb7822 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -570,8 +570,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe 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); - home->valid = true; - /* set reference for map _projection */ map_projection_global_init(home->lat, home->lon, hrt_absolute_time()); @@ -957,7 +955,6 @@ int commander_thread_main(int argc, char *argv[]) home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; - home.valid = true; 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); @@ -1347,7 +1344,6 @@ int commander_thread_main(int argc, char *argv[]) home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; - home.valid = true; 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); diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 73c8dc50d1..08d11abaea 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -63,7 +63,6 @@ struct home_position_s double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude in meters */ - bool valid; /**< True if home position is valid */ }; /** From 6e0690fde1073649a4ef201671ca947cb83edc37 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 16:42:52 +0200 Subject: [PATCH 031/600] init global map projection when gps is valid --- src/lib/geo/geo.c | 2 +- src/modules/commander/commander.cpp | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 438e354dff..10a518908d 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -79,7 +79,7 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - if (strcmp("navigator", getprogname() == 0)) { + if (strcmp("commander", getprogname() == 0)) { return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } else { diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bc3bdb7822..f8f215cd7f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -570,9 +570,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe 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); - /* set reference for map _projection */ - map_projection_global_init(home->lat, home->lon, hrt_absolute_time()); - /* announce new home position */ if (*home_pub > 0) { orb_publish(ORB_ID(home_position), *home_pub, home); @@ -959,9 +956,6 @@ int commander_thread_main(int argc, char *argv[]) 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); - /* set reference for map _projection */ - map_projection_global_init(home.lat, home.lon, hrt_absolute_time()); - /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); @@ -1130,6 +1124,15 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); } + /* Initialize map projection if gps is valid */ + if (!map_projection_global_initialized() + && (gps_position.eph < eph_epv_threshold) + && (gps_position.epv < eph_epv_threshold)) { + /* set reference for map _projection */ + map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time()); + + } + /* start RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ @@ -1348,9 +1351,6 @@ int commander_thread_main(int argc, char *argv[]) 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); - /* set reference for map _projection */ - map_projection_global_init(home.lat, home.lon, hrt_absolute_time()); - /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); From 7ab428976e703284037eb2f24f68ba105db9d6b8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 16:49:21 +0200 Subject: [PATCH 032/600] fix wrong variable name --- 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 f8f215cd7f..c15651ed54 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1126,8 +1126,8 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() - && (gps_position.eph < eph_epv_threshold) - && (gps_position.epv < eph_epv_threshold)) { + && (gps_position.eph_m < eph_epv_threshold) + && (gps_position.epv_m < eph_epv_threshold)) { /* set reference for map _projection */ map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time()); From f24a6187b60f5b9890d9645881b3642be420f5d6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 11:16:15 +0200 Subject: [PATCH 033/600] geo: map projection: fix variable name to highlight the unit --- src/lib/geo/geo.c | 30 ++++++++++++++---------------- src/lib/geo/geo.h | 8 ++++---- 2 files changed, 18 insertions(+), 20 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 10a518908d..ff07014aa6 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -80,7 +80,6 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { if (strcmp("commander", getprogname() == 0)) { - return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } else { return -1; @@ -90,11 +89,10 @@ __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t tim __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - 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); + ref->lat_rad = lat_0 * M_DEG_TO_RAD; + ref->lon_rad = lon_0 * M_DEG_TO_RAD; + ref->sin_lat = sin(ref->lat_rad); + ref->cos_lat = cos(ref->lat_rad); ref->timestamp = timestamp; ref->init_done = true; @@ -107,19 +105,19 @@ __EXPORT int map_projection_init(struct map_projection_reference_s *ref, double map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); } -__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon) +__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad) { - return map_projection_reference(&mp_ref, ref_lat, ref_lon); + return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad); } -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon) +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) { if (!map_projection_initialized(ref)) { return -1; } - *ref_lat = ref->lat; - *ref_lon = ref->lon; + *ref_lat_rad = ref->lat_rad; + *ref_lon_rad = ref->lon_rad; return 0; } @@ -141,13 +139,13 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); - double cos_d_lon = cos(lon_rad - ref->lon); + double cos_d_lon = cos(lon_rad - ref->lon_rad); 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)); *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_rad) * CONSTANTS_RADIUS_OF_EARTH; return 0; } @@ -174,11 +172,11 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r 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)); + lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); } else { - lat_rad = ref->lat; - lon_rad = ref->lon; + lat_rad = ref->lat_rad; + lon_rad = ref->lon_rad; } *lat = lat_rad * 180.0 / M_PI; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 1d0cd524a7..20fb34ca35 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -69,8 +69,8 @@ struct crosstrack_error_s { /* lat/lon are in radians */ struct map_projection_reference_s { - double lat; - double lon; + double lat_rad; + double lon_rad; double sin_lat; double cos_lat; bool init_done; @@ -105,13 +105,13 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference * Writes the reference values of the global projection to ref_lat and ref_lon * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_global_reference(double *ref_lat, double *ref_lon); +__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad); /** * Writes the reference values of the projection given by the argument to ref_lat and ref_lon * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat, double *ref_lon); +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad); /** * Initializes the global map transformation. From 596b06ff2e13b170545b4f13da1c64e66088aedc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 11:22:18 +0200 Subject: [PATCH 034/600] commander: init gps eph and epv to large values, safer map projection initialization --- src/modules/commander/commander.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c15651ed54..71de33bccc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -788,6 +789,8 @@ int commander_thread_main(int argc, char *argv[]) int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); + gps_position.eph_m = FLT_MAX; + gps_position.epv_m = FLT_MAX; /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -1127,7 +1130,8 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() && (gps_position.eph_m < eph_epv_threshold) - && (gps_position.epv_m < eph_epv_threshold)) { + && (gps_position.epv_m < eph_epv_threshold) + && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { /* set reference for map _projection */ map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time()); From fc204a18902b5d623fff1e541a3212502295ed82 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 13:13:35 +0200 Subject: [PATCH 035/600] geo: map projection: fix stupid typo and use constants for deg to rad conversion --- src/lib/geo/geo.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index ff07014aa6..220ea149d3 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -134,8 +134,8 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref return -1; } - double lat_rad = lat / 180.0 * M_PI; - double lon_rad = lon / 180.0 * M_PI; + double lat_rad = lat * M_DEG_TO_RAD; + double lon_rad = lon * M_DEG_TO_RAD; double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); @@ -152,7 +152,7 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref __EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon) { - map_projection_project(&mp_ref, x, y, lat, lon); + map_projection_reproject(&mp_ref, x, y, lat, lon); } __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) From 548c7f4aaf93bddeb05053cd4dede945fede22ef Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 14:43:23 +0200 Subject: [PATCH 036/600] geo: introduce global/local coordinate frame converter which uses the map projection but also converts altitude --- src/lib/geo/geo.c | 36 +++++++++++++++++++++++++++++ src/lib/geo/geo.h | 26 +++++++++++++++++++++ src/modules/commander/commander.cpp | 5 ++-- 3 files changed, 64 insertions(+), 3 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 220ea149d3..c9d48491ee 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -56,6 +56,7 @@ */ static struct map_projection_reference_s mp_ref = {0}; +static struct globallocal_converter_reference_s gl_ref = {0}; __EXPORT bool map_projection_global_initialized() { @@ -185,6 +186,41 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r return 0; } +__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp) +{ + if (strcmp("commander", getprogname() == 0)) { + gl_ref.alt = alt_0; + gl_ref.init_done = true; + return map_projection_global_init(lat_0, lon_0, timestamp); + } else { + return -1; + } +} + +__EXPORT bool globallocalconverter_initialized() +{ + return gl_ref.init_done && map_projection_global_initialized(); +} + +__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + map_projection_global_project(lat, lon, x, y); + *z = gl_ref.alt - alt; +} + +__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + map_projection_global_reproject(x, y, lat, lon); + *alt = gl_ref.alt - z; +} __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 20fb34ca35..98dfbd14b9 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -77,6 +77,11 @@ struct map_projection_reference_s { uint64_t timestamp; }; +struct globallocal_converter_reference_s { + float alt; + bool init_done; +}; + /** * Checks if global projection was initialized * @return true if map was initialized before, false else @@ -190,6 +195,27 @@ __EXPORT int map_projection_global_reproject(float x, float y, double *lat, doub */ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +/** + * Initialize the global mapping between global position (spherical) and local position (NED). + */ +__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp); + +/** + * Checks if globallocalconverter was initialized + * @return true if map was initialized before, false else + */ +__EXPORT bool globallocalconverter_initialized(); + +/** + * Convert from global position coordinates to local position coordinates using the global reference + */ +__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z); + +/** + * Convert from local position coordinates to global position coordinates using the global reference + */ +__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt); + /** * Returns the distance to the next waypoint in meters. * diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 71de33bccc..98887a0e44 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1132,9 +1132,8 @@ int commander_thread_main(int argc, char *argv[]) && (gps_position.eph_m < eph_epv_threshold) && (gps_position.epv_m < eph_epv_threshold) && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { - /* set reference for map _projection */ - map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time()); - + /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ + globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } /* start RC input check */ From a6a2efb651d875171aaec698a836f26b548ca0d6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 May 2014 16:59:09 +0200 Subject: [PATCH 037/600] mavlink receiver: switch back to use local and not systemwide map projection --- src/modules/mavlink/mavlink_receiver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1cfbde1d29..7c93c1c009 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -778,7 +778,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - hil_local_pos.ref_timestamp = map_projection_global_timestamp(); + map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); + hil_local_pos.ref_timestamp = timestamp; hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; hil_local_pos.ref_alt = _hil_local_alt0; @@ -786,7 +787,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) float x; float y; - map_projection_global_project(lat, lon, &x, &y); + map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; From 7813566e6680f4940989fb91760ddb0782b05858 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 6 May 2014 19:30:45 +0400 Subject: [PATCH 038/600] Initial UAVCAN integration. The library compiles successfully, CAN driver appears to be working properly. There is one hardcoded path in the module makefile that needs to be fixed; plus the compilation will likely fail unless arch/math.h contains log2l() --- .gitignore | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + makefiles/setup.mk | 1 + makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/modules/uavcan/.gitignore | 1 + src/modules/uavcan/module.mk | 74 +++++++++++++++++++ src/modules/uavcan/uavcan_clock.cpp | 65 +++++++++++++++++ src/modules/uavcan/uavcan_main.cpp | 100 ++++++++++++++++++++++++++ src/modules/uavcan/uavcan_main.hpp | 39 ++++++++++ 9 files changed, 283 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/.gitignore create mode 100644 src/modules/uavcan/module.mk create mode 100644 src/modules/uavcan/uavcan_clock.cpp create mode 100644 src/modules/uavcan/uavcan_main.cpp create mode 100644 src/modules/uavcan/uavcan_main.hpp diff --git a/.gitignore b/.gitignore index 5b345b34a8..8c3bb1bb55 100644 --- a/.gitignore +++ b/.gitignore @@ -37,3 +37,4 @@ mavlink/include/mavlink/v0.9/ tags .tags_sorted_by_file .pydevproject +/uavcan diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13421acc5..09b5bf7c6e 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -73,6 +73,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led +MODULES += modules/uavcan # # Estimation modules (EKF/ SO3 / other filters) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 183b143d6c..a5271c6569 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -48,6 +48,7 @@ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/ +export UAVCAN_DIR = $(abspath $(PX4_BASE)/uavcan)/ export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/ export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/ export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/ diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index b519e0e7a1..3c00e77f11 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -111,7 +111,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics # Generic warnings # diff --git a/src/modules/uavcan/.gitignore b/src/modules/uavcan/.gitignore new file mode 100644 index 0000000000..24fbf171f0 --- /dev/null +++ b/src/modules/uavcan/.gitignore @@ -0,0 +1 @@ +./dsdlc_generated/ diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk new file mode 100644 index 0000000000..5caa8d401a --- /dev/null +++ b/src/modules/uavcan/module.mk @@ -0,0 +1,74 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# Author: Pavel Kirienko +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# UAVCAN <--> uORB bridge +# + +MODULE_COMMAND = uavcan + +MAXOPTIMIZATION = -Os + +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp + +# +# libuavcan +# +include $(UAVCAN_DIR)/libuavcan/include.mk +SRCS += $(LIBUAVCAN_SRC) +# TODO fix include path +INCLUDE_DIRS += $(LIBUAVCAN_INC) /media/storage/px4/Firmware/Build/px4fmu-v2_default.build/nuttx-export/include/cxx/ +EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ + -DUAVCAN_TOSTRING=0 \ + -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ + -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 + +# +# libuavcan drivers for STM32 +# +include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk +SRCS += $(LIBUAVCAN_STM32_SRC) +INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) +EXTRADEFINES += -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=1 + +# +# Invoke DSDL compiler +# TODO: Add make target for this, or invoke dsdlc manually. +# The second option assumes that the generated headers shall be saved +# under the version control, which may be undesirable. +# The first option requires python3 and python3-mako for the sources to be built. +# +$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) +INCLUDE_DIRS += dsdlc_generated diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp new file mode 100644 index 0000000000..2a5e889890 --- /dev/null +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include + +namespace uavcan_stm32 +{ +namespace clock +{ + +uavcan::MonotonicTime getMonotonic() +{ + return uavcan::MonotonicTime::fromUSec(hrt_absolute_time()); +} + +uavcan::UtcTime getUtc() +{ + return uavcan::UtcTime(); +} + +void adjustUtc(uavcan::UtcDuration adjustment) +{ + (void)adjustment; +} + +uavcan::uint64_t getUtcUSecFromCanInterrupt() +{ + return 0; +} + +} +} + diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp new file mode 100644 index 0000000000..fbb2e174b8 --- /dev/null +++ b/src/modules/uavcan/uavcan_main.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include "uavcan_main.hpp" + +extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); + +namespace +{ + +uavcan_stm32::CanInitHelper<> can_driver; + +void print_usage() +{ + warnx("usage: uavcan start [can_bitrate]"); +} + +int test_thread(int argc, char *argv[]) +{ + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN2_RX); + stm32_configgpio(GPIO_CAN2_TX); + int res = can_driver.init(1000000); + if (res < 0) + { + errx(res, "CAN driver init failed"); + } + while (true) + { + ::sleep(1); + auto iface = static_cast(can_driver.driver.getIface(0)); + res = iface->send(uavcan::CanFrame(), uavcan_stm32::clock::getMonotonic(), 0); + warnx("published %i, pending %u", res, can_driver.driver.getIface(0)->getRxQueueLength()); + } + return 0; +} + +} + +int uavcan_main(int argc, char *argv[]) +{ + if (argc < 2) { + print_usage(); + ::exit(1); + } + + if (!std::strcmp(argv[1], "start")) { + static bool started = false; + if (started) + { + warnx("already started"); + ::exit(1); + } + started = true; + (void)task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 3000, + static_cast(&test_thread), const_cast(argv)); + return 0; + } else { + print_usage(); + ::exit(1); + } + return 0; +} diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp new file mode 100644 index 0000000000..7fcc992d04 --- /dev/null +++ b/src/modules/uavcan/uavcan_main.hpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +// ... From 7d7a375dd1a69bd286f127de51b11e5ecd390640 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 6 May 2014 19:42:40 +0400 Subject: [PATCH 039/600] Fixed hardcoded include path --- makefiles/nuttx.mk | 1 + src/modules/uavcan/module.mk | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk index d283096b25..bf07441408 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -64,6 +64,7 @@ LDSCRIPT += $(NUTTX_EXPORT_DIR)build/ld.script # Add directories from the NuttX export to the relevant search paths # INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \ + $(NUTTX_EXPORT_DIR)include/cxx \ $(NUTTX_EXPORT_DIR)arch/chip \ $(NUTTX_EXPORT_DIR)arch/common diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 5caa8d401a..6974de8ca0 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -48,8 +48,7 @@ SRCS += uavcan_main.cpp \ # include $(UAVCAN_DIR)/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) -# TODO fix include path -INCLUDE_DIRS += $(LIBUAVCAN_INC) /media/storage/px4/Firmware/Build/px4fmu-v2_default.build/nuttx-export/include/cxx/ +INCLUDE_DIRS += $(LIBUAVCAN_INC) EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ -DUAVCAN_TOSTRING=0 \ -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ @@ -61,7 +60,8 @@ EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) -EXTRADEFINES += -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=1 +EXTRADEFINES += -DUAVCAN_STM32_NUTTX \ + -DUAVCAN_STM32_NUM_IFACES=1 # # Invoke DSDL compiler From 5716dad25db27315fa7cebf8183a71f864860f41 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 6 May 2014 20:14:07 +0400 Subject: [PATCH 040/600] Added workaround for hardware issue on Pixhawk v1 --- src/modules/uavcan/module.mk | 2 +- src/modules/uavcan/uavcan_main.cpp | 8 +++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 6974de8ca0..3966210b52 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -61,7 +61,7 @@ include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) EXTRADEFINES += -DUAVCAN_STM32_NUTTX \ - -DUAVCAN_STM32_NUM_IFACES=1 + -DUAVCAN_STM32_NUM_IFACES=2 # # Invoke DSDL compiler diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index fbb2e174b8..474063aa65 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include "uavcan_main.hpp" extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -53,9 +54,14 @@ void print_usage() int test_thread(int argc, char *argv[]) { + /* + * Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver. + * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to + * fail during initialization. + */ stm32_configgpio(GPIO_CAN1_RX); stm32_configgpio(GPIO_CAN1_TX); - stm32_configgpio(GPIO_CAN2_RX); + stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); stm32_configgpio(GPIO_CAN2_TX); int res = can_driver.init(1000000); if (res < 0) From 4b11145797fdc26e5bf29738dd319ab9e8003356 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 7 May 2014 13:42:34 +0400 Subject: [PATCH 041/600] Working UAVCAN node. No application logic is implemented yet; the node just publishes its status once a second (uavcan.protocol.NodeStatus) and responds to basic services (transport stats, node discovery) --- src/modules/uavcan/uavcan_clock.cpp | 9 +- src/modules/uavcan/uavcan_main.cpp | 149 ++++++++++++++++++++++------ src/modules/uavcan/uavcan_main.hpp | 29 +++++- 3 files changed, 152 insertions(+), 35 deletions(-) diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp index 2a5e889890..0e844f3460 100644 --- a/src/modules/uavcan/uavcan_clock.cpp +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -60,6 +60,13 @@ uavcan::uint64_t getUtcUSecFromCanInterrupt() return 0; } -} +} // namespace clock + +SystemClock& SystemClock::instance() +{ + static SystemClock inst; + return inst; +} + } diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 474063aa65..bf6d4fb087 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -40,64 +40,147 @@ #include #include "uavcan_main.hpp" -extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); +/* + * UavcanNode + */ +UavcanNode* UavcanNode::_instance; -namespace +int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { + if (_instance != nullptr) { + warnx("Already started"); + return -1; + } -uavcan_stm32::CanInitHelper<> can_driver; - -void print_usage() -{ - warnx("usage: uavcan start [can_bitrate]"); -} - -int test_thread(int argc, char *argv[]) -{ /* + * GPIO config. * Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver. * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to * fail during initialization. */ - stm32_configgpio(GPIO_CAN1_RX); - stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio (GPIO_CAN1_RX); + stm32_configgpio (GPIO_CAN1_TX); stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); - stm32_configgpio(GPIO_CAN2_TX); - int res = can_driver.init(1000000); - if (res < 0) - { - errx(res, "CAN driver init failed"); + stm32_configgpio (GPIO_CAN2_TX); + + /* + * CAN driver init + */ + static CanInitHelper can; + static bool can_initialized = false; + if (!can_initialized) { + const int can_init_res = can.init(bitrate); + if (can_init_res < 0) { + warnx("CAN driver init failed %i", can_init_res); + return can_init_res; + } + can_initialized = true; } - while (true) - { - ::sleep(1); - auto iface = static_cast(can_driver.driver.getIface(0)); - res = iface->send(uavcan::CanFrame(), uavcan_stm32::clock::getMonotonic(), 0); - warnx("published %i, pending %u", res, can_driver.driver.getIface(0)->getRxQueueLength()); + + /* + * Node init + */ + _instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance()); + if (_instance == nullptr) { + warnx("Out of memory"); + return -1; } - return 0; + const int node_init_res = _instance->init(node_id); + if (node_init_res < 0) { + delete _instance; + _instance = nullptr; + warnx("Node init failed %i", node_init_res); + return node_init_res; + } + + /* + * Start the task. Normally it should never exit. + */ + static auto run_trampoline = [](int, char*[]) {return UavcanNode::_instance->run();}; + return task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, + static_cast(run_trampoline), nullptr); } +int UavcanNode::init(uavcan::NodeID node_id) +{ + uavcan::protocol::SoftwareVersion swver; + swver.major = 12; // TODO fill version info + swver.minor = 34; + _node.setSoftwareVersion(swver); + + uavcan::protocol::HardwareVersion hwver; + hwver.major = 42; // TODO fill version info + hwver.minor = 42; + _node.setHardwareVersion(hwver); + + _node.setName("org.pixhawk"); // Huh? + + _node.setNodeID(node_id); + + return _node.start(); } +int UavcanNode::run() +{ + _node.setStatusOk(); + while (true) { + // TODO: ORB multiplexing + const int res = _node.spin(uavcan::MonotonicDuration::getInfinite()); + if (res < 0) { + warnx("Spin error %i", res); + ::sleep(1); + } + } + return -1; +} + +/* + * App entry point + */ +static void print_usage() +{ + warnx("usage: uavcan start [can_bitrate]"); +} + +extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); + int uavcan_main(int argc, char *argv[]) { + constexpr long DEFAULT_CAN_BITRATE = 1000000; + if (argc < 2) { print_usage(); ::exit(1); } if (!std::strcmp(argv[1], "start")) { - static bool started = false; - if (started) - { - warnx("already started"); + if (argc < 3) { + print_usage(); ::exit(1); } - started = true; - (void)task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 3000, - static_cast(&test_thread), const_cast(argv)); - return 0; + /* + * Node ID + */ + const int node_id = atoi(argv[2]); + if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { + warnx("Invalid Node ID %i", node_id); + ::exit(1); + } + /* + * CAN bitrate + */ + long bitrate = 0; + if (argc > 3) { + bitrate = atol(argv[3]); + } + if (bitrate <= 0) { + bitrate = DEFAULT_CAN_BITRATE; + } + /* + * Start + */ + warnx("Node ID %i, bitrate %li", node_id, bitrate); + return UavcanNode::start(node_id, bitrate); } else { print_usage(); ::exit(1); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 7fcc992d04..8bd660cc53 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -36,4 +36,31 @@ #include -// ... +/** + * Implements basic functinality of UAVCAN node. + */ +class UavcanNode +{ + static constexpr unsigned MemPoolSize = 10752; + static constexpr unsigned RxQueueLenPerIface = 64; + static constexpr unsigned StackSize = 3000; + +public: + typedef uavcan::Node Node; + typedef uavcan_stm32::CanInitHelper CanInitHelper; + + UavcanNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& system_clock) + : _node(can_driver, system_clock) + { } + + static int start(uavcan::NodeID node_id, uint32_t bitrate); + + Node& getNode() { return _node; } + +private: + int init(uavcan::NodeID node_id); + int run(); + + static UavcanNode* _instance; + Node _node; +}; From 04df4270f0ac6aec360076185363338366475166 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 7 May 2014 13:56:05 +0400 Subject: [PATCH 042/600] Removed the placement new workaround. It seems like we can pull from the toolchain's standard includes with no harm. --- src/modules/uavcan/module.mk | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 3966210b52..c91208342a 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -51,8 +51,7 @@ SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ -DUAVCAN_TOSTRING=0 \ - -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ - -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 + -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 # # libuavcan drivers for STM32 From be728d189e4b1b04de5a0b45710e08effcf50b8b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 7 May 2014 14:24:40 +0400 Subject: [PATCH 043/600] Catching up with libuavcan - some preprocessor symbols are no longer required to be defined explicitly --- src/modules/uavcan/module.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index c91208342a..0985438796 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -49,9 +49,9 @@ SRCS += uavcan_main.cpp \ include $(UAVCAN_DIR)/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) -EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ - -DUAVCAN_TOSTRING=0 \ - -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 +# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile +# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. +EXTRADEFINES += -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 # # libuavcan drivers for STM32 From 973b193261fab69320f25fae68345a60d7678dc8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 14:29:30 +0200 Subject: [PATCH 044/600] Fixed comments and code style of UAVCAN node --- src/modules/uavcan/uavcan_clock.cpp | 13 +++++++--- src/modules/uavcan/uavcan_main.cpp | 40 +++++++++++++++++++++++------ src/modules/uavcan/uavcan_main.hpp | 19 +++++++++----- 3 files changed, 55 insertions(+), 17 deletions(-) diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp index 0e844f3460..e41d5f953b 100644 --- a/src/modules/uavcan/uavcan_clock.cpp +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko + * Copyright (c) 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,14 @@ #include #include +/** + * @file uavcan_clock.cpp + * + * Implements a clock for the CAN node. + * + * @author Pavel Kirienko + */ + namespace uavcan_stm32 { namespace clock @@ -62,7 +69,7 @@ uavcan::uint64_t getUtcUSecFromCanInterrupt() } // namespace clock -SystemClock& SystemClock::instance() +SystemClock &SystemClock::instance() { static SystemClock inst; return inst; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index bf6d4fb087..8d715e3b1d 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko + * Copyright (c) 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,10 +39,18 @@ #include #include "uavcan_main.hpp" +/** + * @file uavcan_main.cpp + * + * Implements basic functinality of UAVCAN node. + * + * @author Pavel Kirienko + */ + /* * UavcanNode */ -UavcanNode* UavcanNode::_instance; +UavcanNode *UavcanNode::_instance; int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { @@ -58,22 +65,25 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to * fail during initialization. */ - stm32_configgpio (GPIO_CAN1_RX); - stm32_configgpio (GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); - stm32_configgpio (GPIO_CAN2_TX); + stm32_configgpio(GPIO_CAN2_TX); /* * CAN driver init */ static CanInitHelper can; static bool can_initialized = false; + if (!can_initialized) { const int can_init_res = can.init(bitrate); + if (can_init_res < 0) { warnx("CAN driver init failed %i", can_init_res); return can_init_res; } + can_initialized = true; } @@ -81,11 +91,14 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * Node init */ _instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance()); + if (_instance == nullptr) { warnx("Out of memory"); return -1; } + const int node_init_res = _instance->init(node_id); + if (node_init_res < 0) { delete _instance; _instance = nullptr; @@ -96,9 +109,9 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) /* * Start the task. Normally it should never exit. */ - static auto run_trampoline = [](int, char*[]) {return UavcanNode::_instance->run();}; + static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; return task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, - static_cast(run_trampoline), nullptr); + static_cast(run_trampoline), nullptr); } int UavcanNode::init(uavcan::NodeID node_id) @@ -123,14 +136,17 @@ int UavcanNode::init(uavcan::NodeID node_id) int UavcanNode::run() { _node.setStatusOk(); + while (true) { // TODO: ORB multiplexing const int res = _node.spin(uavcan::MonotonicDuration::getInfinite()); + if (res < 0) { warnx("Spin error %i", res); ::sleep(1); } } + return -1; } @@ -158,32 +174,40 @@ int uavcan_main(int argc, char *argv[]) print_usage(); ::exit(1); } + /* * Node ID */ const int node_id = atoi(argv[2]); + if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { warnx("Invalid Node ID %i", node_id); ::exit(1); } + /* * CAN bitrate */ long bitrate = 0; + if (argc > 3) { bitrate = atol(argv[3]); } + if (bitrate <= 0) { bitrate = DEFAULT_CAN_BITRATE; } + /* * Start */ warnx("Node ID %i, bitrate %li", node_id, bitrate); return UavcanNode::start(node_id, bitrate); + } else { print_usage(); ::exit(1); } + return 0; } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 8bd660cc53..b27449f1f4 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko + * Copyright (c) 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 @@ -37,7 +36,15 @@ #include /** - * Implements basic functinality of UAVCAN node. + * @file uavcan_main.hpp + * + * Defines basic functinality of UAVCAN node. + * + * @author Pavel Kirienko + */ + +/** + * A UAVCAN node. */ class UavcanNode { @@ -49,18 +56,18 @@ public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; - UavcanNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& system_clock) + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : _node(can_driver, system_clock) { } static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node& getNode() { return _node; } + Node &getNode() { return _node; } private: int init(uavcan::NodeID node_id); int run(); - static UavcanNode* _instance; + static UavcanNode *_instance; ///< pointer to the library instance Node _node; }; From ab5e76e3d9da5a76d6520e166d5370c6bdfc4a53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 15:03:08 +0200 Subject: [PATCH 045/600] Fixed printing of bit rate --- src/modules/uavcan/uavcan_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8d715e3b1d..ca4da1c2dc 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -162,7 +162,7 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); int uavcan_main(int argc, char *argv[]) { - constexpr long DEFAULT_CAN_BITRATE = 1000000; + constexpr unsigned DEFAULT_CAN_BITRATE = 1000000; if (argc < 2) { print_usage(); @@ -188,7 +188,7 @@ int uavcan_main(int argc, char *argv[]) /* * CAN bitrate */ - long bitrate = 0; + unsigned bitrate = 0; if (argc > 3) { bitrate = atol(argv[3]); @@ -201,7 +201,7 @@ int uavcan_main(int argc, char *argv[]) /* * Start */ - warnx("Node ID %i, bitrate %li", node_id, bitrate); + warnx("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); } else { From d62f3b8aa7a1ba932b932b26068b79bd14e205dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 09:14:23 +0200 Subject: [PATCH 046/600] Added mixing code, not complete, not compiliing --- src/modules/uavcan/uavcan_main.cpp | 254 ++++++++++++++++++++++++++++- src/modules/uavcan/uavcan_main.hpp | 27 ++- 2 files changed, 273 insertions(+), 8 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ca4da1c2dc..94fb15544f 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -37,6 +37,9 @@ #include #include #include + +#include + #include "uavcan_main.hpp" /** @@ -52,6 +55,20 @@ */ UavcanNode *UavcanNode::_instance; +UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : + _node(can_driver, system_clock), + _controls({}), + _poll_fds({}) +{ + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); + + memset(_controls, 0, sizeof(_controls)); + memset(_poll_fds, 0, sizeof(_poll_fds)); +} + int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { if (_instance != nullptr) { @@ -106,6 +123,14 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return node_init_res; } + int ret; + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) + return ret; + /* * Start the task. Normally it should never exit. */ @@ -137,8 +162,94 @@ int UavcanNode::run() { _node.setStatusOk(); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + while (true) { - // TODO: ORB multiplexing + + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + /* force setting update rate */ + _current_update_rate = 0; + } + + int ret = ::poll(_poll_fds, _poll_fds_num, 5/* 5 ms wait time */); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + + } else if (ret == 0) { + /* timeout: no control data, switch to failsafe values */ +// warnx("no PWM: failsafe"); + + } else { + + /* get controls for required topics */ + unsigned poll_id = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + } + poll_id++; + } + } + + /* can we mix? */ + if (_mixers != nullptr) { + + // XXX one output group has 8 outputs max, + // but this driver could well serve multiple groups. + unsigned num_outputs_max = 8; + + // Do mixing + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < outputs.noutputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (!isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = -1.0f; + } + } + + printf("CAN out: ") + /* output to the bus */ + for (unsigned i = 0; i < outputs.noutputs; i++) { + printf("%u: %8.4f ", i, outputs.output[i]) + // can_send_xxx + } + printf("\n"); + + } + } + + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + + /* update the armed status and check that we're not locked down */ + bool set_armed = _armed.armed && !_armed.lockdown; + + arm_actuators(set_armed); + } + + // Output commands and fetch data + const int res = _node.spin(uavcan::MonotonicDuration::getInfinite()); if (res < 0) { @@ -147,7 +258,146 @@ int UavcanNode::run() } } - return -1; + teardown(); + + exit(0); +} + +int +UavcanNode::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = _controls[control_group].control[control_index]; + return 0; +} + +int +UavcanNode::teardown() +{ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs > 0) { + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + } + ::close(_armed_sub); +} + +void +UavcanNode::arm_actuators(bool arm) +{ + bool changed = (_armed != arm); + + _armed = arm; + + if (changed) { + // Propagate immediately to CAN bus + } +} + +void +UavcanNode::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + warnx("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + if (unsub_groups & (1 << i)) { + warnx("unsubscribe from actuator_controls_%d", i); + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + +int +PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + arm_actuators(true); + break; + + case PWM_SERVO_SET_ARM_OK: + case PWM_SERVO_CLEAR_ARM_OK: + case PWM_SERVO_SET_FORCE_SAFETY_OFF: + // these are no-ops, as no safety switch + break; + + case PWM_SERVO_DISARM: + arm_actuators(false); + break; + + case MIXERIOCGETOUTPUTCOUNT: + *(unsigned *)arg = _output_count; + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + } + + break; + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); + + if (_mixers == nullptr) { + _groups_required = 0; + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + ret = -EINVAL; + } else { + + _mixers->groups_required(_groups_required); + } + } + + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; } /* diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index b27449f1f4..213cb42061 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -34,6 +34,7 @@ #pragma once #include +#include /** * @file uavcan_main.hpp @@ -46,7 +47,7 @@ /** * A UAVCAN node. */ -class UavcanNode +class UavcanNode : public device::CDev { static constexpr unsigned MemPoolSize = 10752; static constexpr unsigned RxQueueLenPerIface = 64; @@ -56,18 +57,32 @@ public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; - UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) - : _node(can_driver, system_clock) - { } + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); static int start(uavcan::NodeID node_id, uint32_t bitrate); Node &getNode() { return _node; } + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + void subscribe(); + private: int init(uavcan::NodeID node_id); int run(); - static UavcanNode *_instance; ///< pointer to the library instance - Node _node; + static UavcanNode *_instance; ///< pointer to the library instance + Node _node; + + MixerGroup *_mixers; + + uint32_t _groups_required; + uint32_t _groups_subscribed; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + unsigned _poll_fds_num; }; From 517f2df0d1de008c795061badc57fbfdbb68d0d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 13:28:41 +0200 Subject: [PATCH 047/600] UAVCAN: Fixed all compile errors --- src/modules/uavcan/uavcan_main.cpp | 126 ++++++++++++++++++++--------- src/modules/uavcan/uavcan_main.hpp | 46 ++++++++--- 2 files changed, 124 insertions(+), 48 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 94fb15544f..332c3a5e87 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -31,14 +31,18 @@ * ****************************************************************************/ +#include + #include #include #include #include +#include #include #include -#include +#include +#include #include "uavcan_main.hpp" @@ -55,7 +59,13 @@ */ UavcanNode *UavcanNode::_instance; -UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : +UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : + CDev("uavcan", UAVCAN_DEVICE_PATH), + _task(0), + _task_should_exit(false), + _armed_sub(-1), + _is_armed(false), + _output_count(0), _node(can_driver, system_clock), _controls({}), _poll_fds({}) @@ -65,8 +75,37 @@ UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); - memset(_controls, 0, sizeof(_controls)); - memset(_poll_fds, 0, sizeof(_poll_fds)); + // memset(_controls, 0, sizeof(_controls)); + // memset(_poll_fds, 0, sizeof(_poll_fds)); +} + +UavcanNode::~UavcanNode() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + /* clean up the alternate device node */ + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + ::close(_armed_sub); + + _instance = nullptr; } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -123,6 +162,24 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return node_init_res; } + /* + * Start the task. Normally it should never exit. + */ + static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; + _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, + static_cast(run_trampoline), nullptr); + + if (_instance->_task < 0) { + warnx("start failed: %d", errno); + return -errno; + } + + return OK; +} + +int UavcanNode::init(uavcan::NodeID node_id) +{ + int ret; /* do regular cdev init */ @@ -131,16 +188,6 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) if (ret != OK) return ret; - /* - * Start the task. Normally it should never exit. - */ - static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - return task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, - static_cast(run_trampoline), nullptr); -} - -int UavcanNode::init(uavcan::NodeID node_id) -{ uavcan::protocol::SoftwareVersion swver; swver.major = 12; // TODO fill version info swver.minor = 34; @@ -162,32 +209,35 @@ int UavcanNode::run() { _node.setStatusOk(); + // XXX figure out the output count + _output_count = 2; + + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - while (true) { - + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + + while (!_task_should_exit) { + if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; - /* force setting update rate */ - _current_update_rate = 0; } int ret = ::poll(_poll_fds, _poll_fds_num, 5/* 5 ms wait time */); - /* this would be bad... */ + // this would be bad... if (ret < 0) { log("poll error %d", errno); - usleep(1000000); continue; } else if (ret == 0) { - /* timeout: no control data, switch to failsafe values */ -// warnx("no PWM: failsafe"); + // timeout: no control data, switch to failsafe values } else { - /* get controls for required topics */ + // get controls for required topics unsigned poll_id = 0; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { @@ -198,7 +248,7 @@ int UavcanNode::run() } } - /* can we mix? */ + //can we mix? if (_mixers != nullptr) { // XXX one output group has 8 outputs max, @@ -209,9 +259,9 @@ int UavcanNode::run() outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); outputs.timestamp = hrt_absolute_time(); - /* iterate actuators */ + // iterate actuators for (unsigned i = 0; i < outputs.noutputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ + // last resort: catch NaN, INF and out-of-band errors if (!isfinite(outputs.output[i]) || outputs.output[i] < -1.0f || outputs.output[i] > 1.0f) { @@ -224,25 +274,25 @@ int UavcanNode::run() } } - printf("CAN out: ") + printf("CAN out: "); /* output to the bus */ for (unsigned i = 0; i < outputs.noutputs; i++) { - printf("%u: %8.4f ", i, outputs.output[i]) + printf("%u: %8.4f ", i, outputs.output[i]); // can_send_xxx } - printf("\n"); + printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); } } - /* check arming state */ + // Check arming state bool updated = false; orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - /* update the armed status and check that we're not locked down */ + // Update the armed status and check that we're not locked down bool set_armed = _armed.armed && !_armed.lockdown; arm_actuators(set_armed); @@ -271,7 +321,7 @@ UavcanNode::control_callback(uintptr_t handle, { const actuator_controls_s *controls = (actuator_controls_s *)handle; - input = _controls[control_group].control[control_index]; + input = controls[control_group].control[control_index]; return 0; } @@ -287,22 +337,24 @@ UavcanNode::teardown() ::close(_armed_sub); } -void +int UavcanNode::arm_actuators(bool arm) { - bool changed = (_armed != arm); + bool changed = (_is_armed != arm); - _armed = arm; + _is_armed = arm; if (changed) { // Propagate immediately to CAN bus } + + return OK; } void UavcanNode::subscribe() { - /* subscribe/unsubscribe to required actuator control groups */ + // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; _poll_fds_num = 0; @@ -326,7 +378,7 @@ UavcanNode::subscribe() } int -PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) +UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 213cb42061..857c1dc662 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -33,8 +33,14 @@ #pragma once +#include + #include -#include +#include + +#include +#include +#include /** * @file uavcan_main.hpp @@ -44,6 +50,9 @@ * @author Pavel Kirienko */ +#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 +#define UAVCAN_DEVICE_PATH "/dev/uavcan" + /** * A UAVCAN node. */ @@ -57,21 +66,36 @@ public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; - UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); - static int start(uavcan::NodeID node_id, uint32_t bitrate); + virtual ~UavcanNode(); - Node &getNode() { return _node; } + static int start(uavcan::NodeID node_id, uint32_t bitrate); + + Node& getNode() { return _node; } static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); - void subscribe(); + + void subscribe(); + + int teardown(); + int arm_actuators(bool arm); private: - int init(uavcan::NodeID node_id); - int run(); + int init(uavcan::NodeID node_id); + int run(); + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + + int _task; ///< handle to the OS task + bool _task_should_exit; ///< flag to indicate to tear down the CAN driver + int _armed_sub; ///< uORB subscription of the arming status + actuator_armed_s _armed; ///< the arming request of the system + bool _is_armed; ///< the arming status of the actuators on the bus + + unsigned _output_count; ///< number of actuators currently available static UavcanNode *_instance; ///< pointer to the library instance Node _node; @@ -80,9 +104,9 @@ private: uint32_t _groups_required; uint32_t _groups_subscribed; - int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; - actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; - orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; unsigned _poll_fds_num; }; From 185c95fda6acac869c1821846d44359faeef22d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 13:57:23 +0200 Subject: [PATCH 048/600] UAVCAN: improve printing, ready for full closed loop test --- src/modules/uavcan/uavcan_main.cpp | 46 ++++++++++++++++++++++++++---- src/modules/uavcan/uavcan_main.hpp | 9 ++++-- 2 files changed, 47 insertions(+), 8 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 332c3a5e87..a86314852e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -68,7 +68,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _output_count(0), _node(can_driver, system_clock), _controls({}), - _poll_fds({}) + _poll_fds({}), + _mixers(nullptr), + _groups_required(0), + _groups_subscribed(0), + _poll_fds_num(0) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -225,7 +229,7 @@ int UavcanNode::run() _groups_subscribed = _groups_required; } - int ret = ::poll(_poll_fds, _poll_fds_num, 5/* 5 ms wait time */); + int ret = ::poll(_poll_fds, _poll_fds_num, 50/* 50 ms wait time */); // this would be bad... if (ret < 0) { @@ -234,6 +238,7 @@ int UavcanNode::run() } else if (ret == 0) { // timeout: no control data, switch to failsafe values + // XXX trigger failsafe } else { @@ -278,11 +283,12 @@ int UavcanNode::run() /* output to the bus */ for (unsigned i = 0; i < outputs.noutputs; i++) { printf("%u: %8.4f ", i, outputs.output[i]); - // can_send_xxx + // XXX send out via CAN here } printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); } + } // Check arming state @@ -300,7 +306,7 @@ int UavcanNode::run() // Output commands and fetch data - const int res = _node.spin(uavcan::MonotonicDuration::getInfinite()); + const int res = _node.spin(uavcan::MonotonicDuration::fromUSec(5000)); if (res < 0) { warnx("Spin error %i", res); @@ -309,6 +315,7 @@ int UavcanNode::run() } teardown(); + warnx("exiting."); exit(0); } @@ -378,7 +385,7 @@ UavcanNode::subscribe() } int -UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg) +UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; @@ -428,7 +435,7 @@ UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - debug("mixer load failed with %d", ret); + warnx("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; _groups_required = 0; @@ -449,9 +456,24 @@ UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg) unlock(); + if (ret == -ENOTTY) { + ret = CDev::ioctl(filp, cmd, arg); + } + return ret; } +void +UavcanNode::print_info() +{ + if (!_instance) { + warnx("not running, start first"); + } + + warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK"); +} + /* * App entry point */ @@ -511,5 +533,17 @@ int uavcan_main(int argc, char *argv[]) ::exit(1); } + /* commands below require the app to be started */ + UavcanNode *inst = UavcanNode::instance(); + + if (!inst) { + errx(1, "application not running"); + } + + if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { + + inst->print_info(); + } + return 0; } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 857c1dc662..97598ddf3c 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -66,10 +66,12 @@ public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; - UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); virtual ~UavcanNode(); + virtual int ioctl(file *filp, int cmd, unsigned long arg); + static int start(uavcan::NodeID node_id, uint32_t bitrate); Node& getNode() { return _node; } @@ -84,10 +86,13 @@ public: int teardown(); int arm_actuators(bool arm); + void print_info(); + + static UavcanNode* instance() { return _instance; } + private: int init(uavcan::NodeID node_id); int run(); - int pwm_ioctl(file *filp, int cmd, unsigned long arg); int _task; ///< handle to the OS task bool _task_should_exit; ///< flag to indicate to tear down the CAN driver From f70db56e90972cf0492ac9d295c3d0f5df87aa66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 14:14:52 +0200 Subject: [PATCH 049/600] UAVCAN: Fix start / stop commands --- src/modules/uavcan/uavcan_main.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a86314852e..9cd486bf5d 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -92,8 +92,8 @@ UavcanNode::~UavcanNode() unsigned i = 10; do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); + /* wait 5ms - it should wake every 10ms or so worst-case */ + usleep(5000); /* if we have given up, kill it */ if (--i == 0) { @@ -522,15 +522,16 @@ int uavcan_main(int argc, char *argv[]) bitrate = DEFAULT_CAN_BITRATE; } + if (UavcanNode::instance()) { + errx(1, "already started"); + } + /* * Start */ warnx("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); - } else { - print_usage(); - ::exit(1); } /* commands below require the app to be started */ @@ -543,7 +544,16 @@ int uavcan_main(int argc, char *argv[]) if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { inst->print_info(); + return OK; } - return 0; + if (!std::strcmp(argv[1], "stop")) { + + delete inst; + inst = nullptr; + return OK; + } + + print_usage(); + ::exit(1); } From ec5602e45d51e500327bd3aa08d4a3d678573936 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 14:23:33 +0200 Subject: [PATCH 050/600] UAVCAN quad X autostart setup --- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 27 ++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 ++++ ROMFS/px4fmu_common/init.d/rc.interface | 5 ++++ ROMFS/px4fmu_common/init.d/rcS | 12 +++++++++- 4 files changed, 48 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/init.d/4012_quad_x_can diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can new file mode 100644 index 0000000000..471ac54b4d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -0,0 +1,27 @@ +#!nsh +# +# F450-sized quadrotor with CAN +# +# Lorenz Meier +# + +sh /etc/init.d/4001_quad_x + +if [ $DO_AUTOCONFIG == yes ] +then + # TODO REVIEW + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 +fi + +set OUTPUT_MODE can diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 5ec735d393..3d0de950d2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -136,6 +136,11 @@ then sh /etc/init.d/4011_dji_f450 fi +if param compare SYS_AUTOSTART 4012 +then + sh /etc/init.d/4012_quad_x_can +fi + # # Quad + # diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 7f793b2198..d6f1b54bc1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -24,6 +24,11 @@ then else set OUTPUT_DEV /dev/pwm_output fi + + if [ $OUTPUT_MODE == can ] + then + set OUTPUT_DEV /dev/uavcan + fi if mixer load $OUTPUT_DEV $MIXER_FILE then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 756ee8ef88..5d76e4283e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -284,7 +284,17 @@ then # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output if [ $OUTPUT_MODE != none ] then - if [ $OUTPUT_MODE == io ] + if [ $OUTPUT_MODE == can ] + then + if uavcan start 1 + then + echo "CAN UP" + else + echo "CAN ERR" + fi + fi + + if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == can ] then echo "[init] Use PX4IO PWM as primary output" if px4io start From 4055833c9e6b6ecadab44d231047c44796ff17bc Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 8 May 2014 17:03:40 +0400 Subject: [PATCH 051/600] UAVCAN mixer renamed to /dev/uavcan/esc --- ROMFS/px4fmu_common/init.d/rc.interface | 2 +- src/modules/uavcan/uavcan_main.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index d6f1b54bc1..da4c734704 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -27,7 +27,7 @@ then if [ $OUTPUT_MODE == can ] then - set OUTPUT_DEV /dev/uavcan + set OUTPUT_DEV /dev/uavcan/esc fi if mixer load $OUTPUT_DEV $MIXER_FILE diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 97598ddf3c..beaa5e1d42 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -51,7 +51,7 @@ */ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 -#define UAVCAN_DEVICE_PATH "/dev/uavcan" +#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" /** * A UAVCAN node. From f4c28473f9038875a56eace4b9b7364694bb03df Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 8 May 2014 17:12:05 +0400 Subject: [PATCH 052/600] Warning fixes --- src/modules/uavcan/uavcan_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 9cd486bf5d..859db93c78 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -336,12 +336,12 @@ int UavcanNode::teardown() { for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs > 0) { + if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } } - ::close(_armed_sub); + return ::close(_armed_sub); } int From 4a98dae227f3e60f1a220164bce0b995ce303f3d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 8 May 2014 19:42:20 +0400 Subject: [PATCH 053/600] UAVCAN ESC controller - proof of concept state --- src/modules/uavcan/esc_controller.cpp | 124 ++++++++++++++++++++++++++ src/modules/uavcan/esc_controller.hpp | 99 ++++++++++++++++++++ src/modules/uavcan/module.mk | 5 +- src/modules/uavcan/uavcan_main.cpp | 47 +++++----- src/modules/uavcan/uavcan_main.hpp | 35 ++++---- 5 files changed, 265 insertions(+), 45 deletions(-) create mode 100644 src/modules/uavcan/esc_controller.cpp create mode 100644 src/modules/uavcan/esc_controller.hpp diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp new file mode 100644 index 0000000000..bde4d2a7fa --- /dev/null +++ b/src/modules/uavcan/esc_controller.cpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file esc_controller.cpp + * + * @author Pavel Kirienko + */ + +#include "esc_controller.hpp" +#include + +UavcanEscController::UavcanEscController(uavcan::INode &node) : + _node(node), + _uavcan_pub_raw_cmd(node), + _uavcan_sub_status(node), + _orb_timer(node) +{ +} + +int UavcanEscController::init() +{ + int res = -1; + + // ESC status subscription + res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); + if (res < 0) + { + warnx("ESC status sub failed %i", res); + return res; + } + + // ESC status will be relayed from UAVCAN bus into ORB at this rate + _orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb)); + _orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ)); + + return res; +} + +void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) +{ + assert(outputs != nullptr); + assert(num_outputs <= MAX_ESCS); + + /* + * Rate limiting - we don't want to congest the bus + */ + const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { + return; + } + _prev_cmd_pub = timestamp; + + /* + * Fill the command message + * If unarmed, we publish an empty message anyway + */ + uavcan::equipment::esc::RawCommand msg; + + if (_armed) { + for (unsigned i = 0; i < num_outputs; i++) { + + float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; + if (scaled < 1.0F) + scaled = 1.0F; // Since we're armed, we don't want to stop it completely + + assert(scaled >= uavcan::equipment::esc::RawCommand::CMD_MIN); + assert(scaled <= uavcan::equipment::esc::RawCommand::CMD_MAX); + + msg.cmd.push_back(scaled); + } + } + + /* + * Publish the command message to the bus + * Note that for a quadrotor it takes one CAN frame + */ + (void)_uavcan_pub_raw_cmd.broadcast(msg); +} + +void UavcanEscController::arm_esc(bool arm) +{ + _armed = arm; +} + +void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() +} + +void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) +{ + // TODO publish to ORB +} diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp new file mode 100644 index 0000000000..0ed0c59b56 --- /dev/null +++ b/src/modules/uavcan/esc_controller.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file esc_controller.hpp + * + * UAVCAN <--> ORB bridge for ESC messages: + * uavcan.equipment.esc.RawCommand + * uavcan.equipment.esc.RPMCommand + * uavcan.equipment.esc.Status + * + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +class UavcanEscController +{ +public: + UavcanEscController(uavcan::INode& node); + + int init(); + + void update_outputs(float *outputs, unsigned num_outputs); + + void arm_esc(bool arm); + +private: + /** + * ESC status message reception will be reported via this callback. + */ + void esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg); + + /** + * ESC status will be published to ORB from this callback (fixed rate). + */ + void orb_pub_timer_cb(const uavcan::TimerEvent &event); + + + static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable + static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; + static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; + + typedef uavcan::MethodBinder&)> + StatusCbBinder; + + typedef uavcan::MethodBinder + TimerCbBinder; + + /* + * libuavcan related things + */ + uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting + uavcan::INode &_node; + uavcan::Publisher _uavcan_pub_raw_cmd; + uavcan::Subscriber _uavcan_sub_status; + uavcan::TimerEventForwarder _orb_timer; + + /* + * ESC states + */ + bool _armed = false; + uavcan::equipment::esc::Status _states[MAX_ESCS]; +}; diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 0985438796..ce9f981a88 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,8 +40,9 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os -SRCS += uavcan_main.cpp \ - uavcan_clock.cpp +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp \ + esc_controller.cpp # # libuavcan diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 859db93c78..e7829cbba9 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -61,18 +61,8 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), - _task(0), - _task_should_exit(false), - _armed_sub(-1), - _is_armed(false), - _output_count(0), _node(can_driver, system_clock), - _controls({}), - _poll_fds({}), - _mixers(nullptr), - _groups_required(0), - _groups_subscribed(0), - _poll_fds_num(0) + _esc_controller(_node) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -183,8 +173,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) int UavcanNode::init(uavcan::NodeID node_id) { - - int ret; + int ret = -1; /* do regular cdev init */ ret = CDev::init(); @@ -192,6 +181,10 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret != OK) return ret; + ret = _esc_controller.init(); + if (ret < 0) + return ret; + uavcan::protocol::SoftwareVersion swver; swver.major = 12; // TODO fill version info swver.minor = 34; @@ -222,6 +215,11 @@ int UavcanNode::run() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); + /* + * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update(); + * IO multiplexing shall be done here. + */ + while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { @@ -279,14 +277,16 @@ int UavcanNode::run() } } + /* + * Output to the bus + */ printf("CAN out: "); - /* output to the bus */ for (unsigned i = 0; i < outputs.noutputs; i++) { printf("%u: %8.4f ", i, outputs.output[i]); - // XXX send out via CAN here } printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); + _esc_controller.update_outputs(outputs.output, outputs.noutputs); } } @@ -304,13 +304,12 @@ int UavcanNode::run() arm_actuators(set_armed); } - // Output commands and fetch data + // Output commands and fetch data TODO ORB multiplexing - const int res = _node.spin(uavcan::MonotonicDuration::fromUSec(5000)); + const int spin_res = _node.spin(uavcan::MonotonicDuration::fromMSec(1)); - if (res < 0) { - warnx("Spin error %i", res); - ::sleep(1); + if (spin_res < 0) { + warnx("node spin error %i", spin_res); } } @@ -347,14 +346,8 @@ UavcanNode::teardown() int UavcanNode::arm_actuators(bool arm) { - bool changed = (_is_armed != arm); - _is_armed = arm; - - if (changed) { - // Propagate immediately to CAN bus - } - + _esc_controller.arm_esc(arm); return OK; } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index beaa5e1d42..f4a709c793 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -42,6 +42,8 @@ #include #include +#include "esc_controller.hpp" + /** * @file uavcan_main.hpp * @@ -94,24 +96,25 @@ private: int init(uavcan::NodeID node_id); int run(); - int _task; ///< handle to the OS task - bool _task_should_exit; ///< flag to indicate to tear down the CAN driver - int _armed_sub; ///< uORB subscription of the arming status - actuator_armed_s _armed; ///< the arming request of the system - bool _is_armed; ///< the arming status of the actuators on the bus + int _task = -1; ///< handle to the OS task + bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver + int _armed_sub = -1; ///< uORB subscription of the arming status + actuator_armed_s _armed; ///< the arming request of the system + bool _is_armed = false; ///< the arming status of the actuators on the bus - unsigned _output_count; ///< number of actuators currently available + unsigned _output_count = 0; ///< number of actuators currently available - static UavcanNode *_instance; ///< pointer to the library instance - Node _node; + static UavcanNode *_instance; ///< singleton pointer + Node _node; ///< library instance + UavcanEscController _esc_controller; - MixerGroup *_mixers; + MixerGroup *_mixers = nullptr; - uint32_t _groups_required; - uint32_t _groups_subscribed; - int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; - actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; - orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; - unsigned _poll_fds_num; + uint32_t _groups_required = 0; + uint32_t _groups_subscribed = 0; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + unsigned _poll_fds_num = 0; }; From c697aae17a32f25b2f163282b9cb18efedb14d77 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 8 May 2014 23:34:23 +0400 Subject: [PATCH 054/600] Proper IO miltiplexing libuavcan + ORB --- src/modules/uavcan/uavcan_main.cpp | 68 ++++++++++++++++++------------ src/modules/uavcan/uavcan_main.hpp | 3 +- 2 files changed, 44 insertions(+), 27 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index e7829cbba9..6ba596ad05 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -202,9 +203,17 @@ int UavcanNode::init(uavcan::NodeID node_id) return _node.start(); } +void UavcanNode::node_spin_once() +{ + const int spin_res = _node.spin(uavcan::MonotonicTime()); + if (spin_res < 0) { + warnx("node spin error %i", spin_res); + } +} + int UavcanNode::run() { - _node.setStatusOk(); + const unsigned PollTimeoutMs = 50; // XXX figure out the output count _output_count = 2; @@ -215,42 +224,65 @@ int UavcanNode::run() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); + const int busevent_fd = ::open(uavcan_stm32::Event::DevName, 0); + if (busevent_fd < 0) + { + warnx("Failed to open %s", uavcan_stm32::Event::DevName); + _task_should_exit = true; + } + /* * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update(); * IO multiplexing shall be done here. */ + _node.setStatusOk(); + while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Actual event type (POLLIN/POLLOUT/...) doesn't matter here. + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN | POLLOUT; + _poll_fds_num += 1; } - int ret = ::poll(_poll_fds, _poll_fds_num, 50/* 50 ms wait time */); + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + + node_spin_once(); // Non-blocking // this would be bad... - if (ret < 0) { + if (poll_ret < 0) { log("poll error %d", errno); continue; - - } else if (ret == 0) { - // timeout: no control data, switch to failsafe values - // XXX trigger failsafe - } else { - // get controls for required topics + bool controls_updated = false; unsigned poll_id = 0; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { + controls_updated = true; orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } poll_id++; } } + if (!controls_updated) { + // timeout: no control data, switch to failsafe values + // XXX trigger failsafe + } + //can we mix? if (_mixers != nullptr) { @@ -277,15 +309,7 @@ int UavcanNode::run() } } - /* - * Output to the bus - */ - printf("CAN out: "); - for (unsigned i = 0; i < outputs.noutputs; i++) { - printf("%u: %8.4f ", i, outputs.output[i]); - } - printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); - + // Output to the bus _esc_controller.update_outputs(outputs.output, outputs.noutputs); } @@ -303,14 +327,6 @@ int UavcanNode::run() arm_actuators(set_armed); } - - // Output commands and fetch data TODO ORB multiplexing - - const int spin_res = _node.spin(uavcan::MonotonicDuration::fromMSec(1)); - - if (spin_res < 0) { - warnx("node spin error %i", spin_res); - } } teardown(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index f4a709c793..751a94a8a7 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -94,6 +94,7 @@ public: private: int init(uavcan::NodeID node_id); + void node_spin_once(); int run(); int _task = -1; ///< handle to the OS task @@ -115,6 +116,6 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent unsigned _poll_fds_num = 0; }; From 5a905825675a33b210469eff96f0b82a8bd70eb9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 9 May 2014 02:18:45 +0400 Subject: [PATCH 055/600] Catching up with STM32 driver optimizations in libuavcan --- src/modules/uavcan/uavcan_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 6ba596ad05..d4a0d894a2 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -224,10 +224,10 @@ int UavcanNode::run() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); - const int busevent_fd = ::open(uavcan_stm32::Event::DevName, 0); + const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) { - warnx("Failed to open %s", uavcan_stm32::Event::DevName); + warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); _task_should_exit = true; } @@ -252,7 +252,7 @@ int UavcanNode::run() */ _poll_fds[_poll_fds_num] = ::pollfd(); _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN | POLLOUT; + _poll_fds[_poll_fds_num].events = POLLIN; _poll_fds_num += 1; } From 8501158427d7cf96b125eafe48193f654c7fb2f0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 9 May 2014 02:23:52 +0400 Subject: [PATCH 056/600] Micro optimization in UAVCAN polling loop --- src/modules/uavcan/uavcan_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index d4a0d894a2..73f519fa1f 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -284,7 +284,7 @@ int UavcanNode::run() } //can we mix? - if (_mixers != nullptr) { + if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. From 4edc432f399297ed6e74685408e80c0640873099 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 9 May 2014 02:24:46 +0400 Subject: [PATCH 057/600] Removed misleading comment --- src/modules/uavcan/uavcan_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 73f519fa1f..ab687a6b9d 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -245,7 +245,6 @@ int UavcanNode::run() _groups_subscribed = _groups_required; /* * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Actual event type (POLLIN/POLLOUT/...) doesn't matter here. * Please note that with such multiplexing it is no longer possible to rely only on * the value returned from poll() to detect whether actuator control has timed out or not. * Instead, all ORB events need to be checked individually (see below). From e0042ec12cdccd157d181d5b9410fbeedfe3ce72 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 May 2014 13:15:29 +0200 Subject: [PATCH 058/600] geo: add functions to get global projection/transformation reference values --- src/lib/geo/geo.c | 39 +++++++++++++++++++++++++++++++++++++++ src/lib/geo/geo.h | 10 ++++++++++ 2 files changed, 49 insertions(+) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index c9d48491ee..b1e0383233 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -186,6 +186,23 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r return 0; } +__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + if (lat_0 != NULL) { + *lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad; + } + + if (lon_0 != NULL) { + *lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad; + } + + return 0; + +} __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp) { if (strcmp("commander", getprogname() == 0)) { @@ -210,6 +227,8 @@ __EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, flo map_projection_global_project(lat, lon, x, y); *z = gl_ref.alt - alt; + + return 0; } __EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt) @@ -220,6 +239,26 @@ __EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *l map_projection_global_reproject(x, y, lat, lon); *alt = gl_ref.alt - z; + + return 0; +} + +__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + if (map_projection_global_getref(lat_0, lon_0)) + { + return -1; + } + + if (alt_0 != NULL) { + *alt_0 = gl_ref.alt; + } + + return 0; } __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 98dfbd14b9..4bc3cc17c3 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -195,6 +195,11 @@ __EXPORT int map_projection_global_reproject(float x, float y, double *lat, doub */ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +/** + * Get reference position of the global map projection + */ +__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0); + /** * Initialize the global mapping between global position (spherical) and local position (NED). */ @@ -216,6 +221,11 @@ __EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, flo */ __EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt); +/** + * Get reference position of the global to local converter + */ +__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0); + /** * Returns the distance to the next waypoint in meters. * From a43e963bdb73e6fa0480d442e72e1d764418cad4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 16:09:05 +0200 Subject: [PATCH 059/600] geo: fix logic of globallocal converter initialization to depend on map projection initialization --- src/lib/geo/geo.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index b1e0383233..235da5b394 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -207,8 +207,14 @@ __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, { if (strcmp("commander", getprogname() == 0)) { gl_ref.alt = alt_0; - gl_ref.init_done = true; - return map_projection_global_init(lat_0, lon_0, timestamp); + if (!map_projection_global_init(lat_0, lon_0, timestamp)) + { + gl_ref.init_done = true; + return 0; + } else { + gl_ref.init_done = false; + return -1; + } } else { return -1; } From 882076a0d48d465becff9c62db0096e76dd685b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Jun 2014 16:38:45 +0200 Subject: [PATCH 060/600] Better defaults for min values for some of the multicopters --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 3 +++ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 ++-- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 4 ++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 084dff140f..3f47390c15 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -30,3 +30,6 @@ fi set MIXER FMU_quad_w set PWM_OUTPUTS 1234 + +set PWM_MIN 1200 +set PWM_MAX 1950 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index cd4480c3eb..aa43940155 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -23,5 +23,5 @@ then param set MC_YAWRATE_D 0.0 fi -set PWM_MIN 1175 -set PWM_MAX 1900 +set PWM_MIN 1200 +set PWM_MAX 1950 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index ac2ecc70aa..4d055b4217 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -24,5 +24,5 @@ then param set MC_YAWRATE_D 0.0 fi -set PWM_MIN 1175 -set PWM_MAX 1900 +set PWM_MIN 1230 +set PWM_MAX 1950 From cb246a79cad96c469f29042dd33658d63cbcc743 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Jun 2014 16:47:16 +0200 Subject: [PATCH 061/600] Added vision position estimate topic --- src/modules/uORB/objects_common.cpp | 3 + .../uORB/topics/vision_position_estimate.h | 82 +++++++++++++++++++ 2 files changed, 85 insertions(+) create mode 100644 src/modules/uORB/topics/vision_position_estimate.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e9..fc9920a418 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -199,3 +199,6 @@ ORB_DEFINE(encoders, struct encoders_s); #include "topics/estimator_status.h" ORB_DEFINE(estimator_status, struct estimator_status_report); + +#include "topics/vision_position_estimate.h" +ORB_DEFINE(vision_position_estimate, struct vision_position_estimate); diff --git a/src/modules/uORB/topics/vision_position_estimate.h b/src/modules/uORB/topics/vision_position_estimate.h new file mode 100644 index 0000000000..74c96cf2e0 --- /dev/null +++ b/src/modules/uORB/topics/vision_position_estimate.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vision_position_estimate.h + * Vision based position estimate + */ + +#ifndef TOPIC_VISION_POSITION_ESTIMATE_H_ +#define TOPIC_VISION_POSITION_ESTIMATE_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Vision based position estimate in NED frame + */ +struct vision_position_estimate { + + unsigned id; /**< ID of the estimator, commonly the component ID of the incoming message */ + + uint64_t timestamp_boot; /**< time of this estimate, in microseconds since system start */ + uint64_t timestamp_computer; /**< timestamp provided by the companion computer, in us */ + + float x; /**< X position in meters in NED earth-fixed frame */ + float y; /**< Y position in meters in NED earth-fixed frame */ + float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ + + float vx; /**< X velocity in meters per second in NED earth-fixed frame */ + float vy; /**< Y velocity in meters per second in NED earth-fixed frame */ + float vz; /**< Z velocity in meters per second in NED earth-fixed frame */ + + float q[4]; /**< Estimated attitude as quaternion */ + + // XXX Add covariances here + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vision_position_estimate); + +#endif /* TOPIC_VISION_POSITION_ESTIMATE_H_ */ From 7a776eacb11f30a007812449a02916c6f00d2ad0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Jun 2014 16:50:09 +0200 Subject: [PATCH 062/600] MAVLink app: Publish vision position estimate --- src/modules/mavlink/mavlink_receiver.cpp | 44 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 3 ++ 2 files changed, 47 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 53769e0cfe..6b73a4bfa2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -103,6 +103,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _flow_pub(-1), _offboard_control_sp_pub(-1), _vicon_position_pub(-1), + _vision_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), @@ -138,6 +139,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vicon_position_estimate(msg); break; + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + handle_message_vision_position_estimate(msg); + break; + case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); break; @@ -339,6 +344,45 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) +{ + mavlink_vision_position_estimate_t pos; + mavlink_msg_vision_position_estimate_decode(msg, &pos); + + struct vision_position_estimate vision_position; + memset(&vision_position, 0, sizeof(vision_position)); + + // Use the component ID to identify the vision sensor + vision_position.id = msg->compid; + + vision_position.timestamp_boot = hrt_absolute_time(); + vision_position.timestamp_computer = pos.usec; + vision_position.x = pos.x; + vision_position.y = pos.y; + vision_position.z = pos.z; + + // XXX fis this + vision_position.vx = 0.0f; + vision_position.vy = 0.0f; + vision_position.vz = 0.0f; + + math::Quaternion q; + q.from_euler(pos.roll, pos.pitch, pos.yaw); + + vision_position.q[0] = q(0); + vision_position.q[1] = q(1); + vision_position.q[2] = q(2); + vision_position.q[3] = q(3); + + if (_vision_position_pub < 0) { + _vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position); + + } else { + orb_publish(ORB_ID(vision_position_estimate), _vision_position_pub, &vision_position); + } +} + void MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a3..b5e6b1b1a3 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -109,6 +110,7 @@ private: void handle_message_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); + void handle_message_vision_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); @@ -135,6 +137,7 @@ private: orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; orb_advert_t _vicon_position_pub; + orb_advert_t _vision_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; From 57a7e764068178e365b05b7baaa39041762f2e96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Jun 2014 16:51:09 +0200 Subject: [PATCH 063/600] INAV: Added vision position estimate input / topic --- .../position_estimator_inav_main.c | 62 ++++++++++++++++++- .../position_estimator_inav_params.c | 3 + .../position_estimator_inav_params.h | 4 ++ 3 files changed, 68 insertions(+), 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 d7503e42da..9ab21ee38c 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 @@ -78,6 +79,7 @@ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s +static const hrt_abstime vision_topic_timeout = 1000000; // Vision topic timeout = 1s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss @@ -270,6 +272,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool sonar_valid = false; // sonar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) + bool vision_valid = false; /* declare and safely initialize all structs */ struct actuator_controls_s actuator; @@ -288,6 +291,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&local_pos, 0, sizeof(local_pos)); struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); + struct vision_position_estimate vision; + memset(&vision, 0, sizeof(vision)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); @@ -299,6 +304,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 vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int home_position_sub = orb_subscribe(ORB_ID(home_position)); /* advertise */ @@ -587,6 +593,53 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + + /* check no vision circuit breaker is set */ + if (params.no_vision != CBRK_NO_VISION) { + /* vehicle vision position */ + orb_check(vision_position_estimate_sub, &updated); + + + + // XXX THIS IS EVIL UNTESTED CODE + // set CBRK_NO_VISION to 0 to disengage the circuit breaker + // and activate this section + + + if (updated) { + orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); + + /* reset position estimate on first vision update */ + if (!vision_valid) { + x_est[0] = vision.x; + x_est[1] = vision.vx; + y_est[0] = vision.y; + y_est[1] = vision.vy; + z_est[0] = vision.z; + z_est[1] = vision.vz; + + vision_valid = true; + } + + /* calculate correction for position */ + corr_gps[0][0] = vision.x - x_est[0]; + corr_gps[1][0] = vision.y - y_est[0]; + corr_gps[2][0] = vision.z - z_est[0]; + + /* calculate correction for velocity */ + corr_gps[0][1] = vision.vx - x_est[1]; + corr_gps[1][1] = vision.vy - y_est[1]; + corr_gps[2][1] = vision.vz - z_est[1]; + + eph = 0.05f; + epv = 0.05f; + + 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); + + } + } + /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); @@ -706,6 +759,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); } + /* check for timeout on vision topic */ + if (vision_valid && t > vision.timestamp_boot + vision_topic_timeout) { + vision_valid = false; + warnx("VISION timeout"); + mavlink_log_info(mavlink_fd, "[inav] VISION timeout"); + } + /* check for sonar measurement timeout */ if (sonar_valid && t > sonar_time + sonar_timeout) { corr_sonar = 0.0f; @@ -731,7 +791,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) xy_src_time = t; } - bool can_estimate_xy = eph < max_eph_epv * 1.5; + bool can_estimate_xy = (eph < max_eph_epv * 1.5) || vision_valid; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); 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 8aa08b6f2c..c7c1c070ce 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -55,6 +55,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); +PARAM_DEFINE_INT32(CBRK_NO_VISION, 328754); int parameters_init(struct position_estimator_inav_param_handles *h) { @@ -73,6 +74,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->land_t = param_find("INAV_LAND_T"); h->land_disp = param_find("INAV_LAND_DISP"); h->land_thr = param_find("INAV_LAND_THR"); + h->no_vision = param_find("CBRK_NO_VISION"); return OK; } @@ -94,6 +96,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->land_t, &(p->land_t)); param_get(h->land_disp, &(p->land_disp)); param_get(h->land_thr, &(p->land_thr)); + param_get(h->no_vision, &(p->no_vision)); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 08ec996a13..b7826793aa 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -56,6 +56,7 @@ struct position_estimator_inav_params { float land_t; float land_disp; float land_thr; + int32_t no_vision; }; struct position_estimator_inav_param_handles { @@ -74,8 +75,11 @@ struct position_estimator_inav_param_handles { param_t land_t; param_t land_disp; param_t land_thr; + param_t no_vision; }; +#define CBRK_NO_VISION 328754 + /** * Initialize all parameter handles and values * From c4280a7cd83f7793d4773ffedd36e0fa220e2c07 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 6 Jun 2014 17:47:25 +0200 Subject: [PATCH 064/600] matlab logging: Initial CSV example --- .../matlab_csv_serial/matlab_csv_serial.c | 246 ++++++++++++++++++ src/examples/matlab_csv_serial/module.mk | 41 +++ 2 files changed, 287 insertions(+) create mode 100644 src/examples/matlab_csv_serial/matlab_csv_serial.c create mode 100644 src/examples/matlab_csv_serial/module.mk diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c new file mode 100644 index 0000000000..7c8bcc31e8 --- /dev/null +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -0,0 +1,246 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file matlab_csv_serial_main.c + * + * Matlab CSV / ASCII format interface at 921600 baud, 8 data bits, + * 1 stop bit, no parity + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +__EXPORT int matlab_csv_serial_main(int argc, char *argv[]); +static bool thread_should_exit = false; /**< Daemon exit flag */ +static bool thread_running = false; /**< Daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +int matlab_csv_serial_thread_main(int argc, char *argv[]); +static void usage(const char *reason); + +static void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The daemon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_spawn_cmd(). + */ +int matlab_csv_serial_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) + { + if (thread_running) + { + printf("flow position estimator already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn_cmd("matlab_csv_serial", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + matlab_csv_serial_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) + { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) + { + if (thread_running) { + warnx("running"); + } else { + warnx("stopped"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int matlab_csv_serial_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + thread_running = true; + + if (argc < 2) { + errx(1, "need a serial port name as argument"); + } + + int serial_fd = open(argv[1], O_RDWR | O_NOCTTY); + + unsigned speed = 921600; + + if (serial_fd < 0) { + err(1, "failed to open port: %s", argv[1]); + } + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + bool is_usb = false; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { + warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); + close(_uart_fd); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(_uart_fd, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* USB serial is indicated by /dev/ttyACM0*/ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERR SET BAUD %s: %d\n", argv[1], termios_state); + close(serial_fd); + return -1; + } + + } + + if ((termios_state = tcsetattr(serial_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR SET CONF %s\n", argv[1]); + close(serial_fd); + return -1; + } + + /* subscribe to vehicle status, attitude, sensors and flow*/ + struct accel_report accel0; + struct accel_report accel1; + struct gyro_report gyro0; + struct gyro_report gyro1; + + /* subscribe to parameter changes */ + int accel0_sub = orb_subscribe(ORB_ID(sensor_accel)); + int accel1_sub; + int gyro0_sub; + int gyro1_sub; + + while (!thread_should_exit) + { + + /*This runs at the rate of the sensors */ + struct pollfd fds[] = { + { .fd = accel0_sub, .events = POLLIN }, + { .fd = accel1_sub, .events = POLLIN }, + { .fd = gyro0_sub, .events = POLLIN }, + { .fd = gyro1_sub, .events = POLLIN } + }; + + /* wait for a sensor update, check for exit condition every 500 ms */ + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + + } + else if (ret == 0) + { + /* no return value, ignore */ + warnx("no sensor data"); + } + else + { + + /* parameter update available? */ + if (fds[0].revents & POLLIN) + { + orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0); + + // write out on accel 0, but collect for all other sensors as they have updates + vdprintf(serial_fd, "%llu,%d\n", accel0.timestamp, (int)accel0.x_raw); + } + + } + } + + warnx("exiting"); + thread_running = false; + + fflush(stdout); + return 0; +} + + diff --git a/src/examples/matlab_csv_serial/module.mk b/src/examples/matlab_csv_serial/module.mk new file mode 100644 index 0000000000..c83d1fefd6 --- /dev/null +++ b/src/examples/matlab_csv_serial/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build position estimator +# + +MODULE_COMMAND = matlab_csv_serial + +SRCS = matlab_csv_serial.c \ + matlab_csv_serial.c From a6cf04a6ff623b7d39a97c70f837198b6c064f5b Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 8 Jun 2014 14:08:22 -0400 Subject: [PATCH 065/600] Create system state entry in dataman - ATTN Anton Create persistent system state id for data manager to store system state that will persist across resets. --- ROMFS/px4fmu_common/init.d/rcS | 10 ++--- src/modules/dataman/dataman.c | 50 +++++++++++++++++++--- src/modules/dataman/dataman.h | 26 +++++++++++- src/modules/mavlink/mavlink_main.cpp | 54 +++++++++++++++++++++++- src/modules/navigator/navigator_main.cpp | 9 ++++ 5 files changed, 136 insertions(+), 13 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6d06f897a3..13c08f405f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -402,6 +402,11 @@ then fi fi + # + # Start the datamanager + # + dataman start + # # MAVLink # @@ -544,11 +549,6 @@ then fi fi - # - # Start the datamanager - # - dataman start - # # Start the navigator # diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 1a65313e89..96dc98f2a1 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -61,6 +61,8 @@ __EXPORT int dataman_main(int argc, char *argv[]); __EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen); __EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen); __EXPORT int dm_clear(dm_item_t item); +__EXPORT void dm_lock(dm_item_t item); +__EXPORT void dm_unlock(dm_item_t item); __EXPORT int dm_restart(dm_reset_reason restart_type); /** Types of function calls supported by the worker task */ @@ -113,12 +115,17 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_FENCE_POINTS_MAX, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, - DM_KEY_WAYPOINTS_ONBOARD_MAX + DM_KEY_WAYPOINTS_ONBOARD_MAX, + DM_KEY_SYSTEM_STATE_MAX }; /* Table of offset for index 0 of each item type */ static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; +/* Item type lock mutexes */ +static sem_t *g_item_locks[DM_KEY_NUM_KEYS]; +static sem_t g_sys_state_mutex; + /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; static const char *k_data_manager_device_path = "/fs/microsd/dataman"; @@ -138,22 +145,22 @@ static work_q_t g_work_q; /* pending work items. To be consumed by worker thread sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ sem_t g_init_sema; -static bool g_task_should_exit; /**< if true, dataman task should exit */ +static bool g_task_should_exit; /**< if true, dataman task should exit */ #define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */ static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */ static void init_q(work_q_t *q) { - sq_init(&(q->q)); /* Initialize the NuttX queue structure */ + sq_init(&(q->q)); /* Initialize the NuttX queue structure */ sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ - q->size = q->max_size = 0; /* Queue is initially empty */ + q->size = q->max_size = 0; /* Queue is initially empty */ } static inline void destroy_q(work_q_t *q) { - sem_destroy(&(q->mutex)); /* Destroy the queue lock */ + sem_destroy(&(q->mutex)); /* Destroy the queue lock */ } static inline void @@ -567,6 +574,32 @@ dm_clear(dm_item_t item) return enqueue_work_item_and_wait_for_result(work); } +__EXPORT void +dm_lock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if ((g_fd < 0) || g_task_should_exit) + return; + if (item >= DM_KEY_NUM_KEYS) + return; + if (g_item_locks[item]) { + sem_wait(g_item_locks[item]); + } +} + +__EXPORT void +dm_unlock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if ((g_fd < 0) || g_task_should_exit) + return; + if (item >= DM_KEY_NUM_KEYS) + return; + if (g_item_locks[item]) { + sem_post(g_item_locks[item]); + } +} + /* Tell the data manager about the type of the last reset */ __EXPORT int dm_restart(dm_reset_reason reason) @@ -607,6 +640,12 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < dm_number_of_funcs; i++) g_func_counts[i] = 0; + /* Initialize the item type locks, for now only DM_KEY_SYSTEM_STATE supports locking */ + sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */ + for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) + g_item_locks[i] = NULL; + g_item_locks[DM_KEY_SYSTEM_STATE] = &g_sys_state_mutex; + g_task_should_exit = false; init_q(&g_work_q); @@ -742,6 +781,7 @@ task_main(int argc, char *argv[]) destroy_q(&g_work_q); destroy_q(&g_free_q); sem_destroy(&g_work_queued_sema); + sem_destroy(&g_sys_state_mutex); return 0; } diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 1dfb26f73d..f2b6cd4d3c 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -53,6 +53,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ + DM_KEY_SYSTEM_STATE, /* Persistent system state storage */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -62,7 +63,8 @@ extern "C" { DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED + DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_SYSTEM_STATE_MAX = 1 }; /** Data persistence levels */ @@ -101,6 +103,18 @@ extern "C" { size_t buflen /* Length in bytes of data to retrieve */ ); + /** Lock all items of this type */ + __EXPORT void + dm_lock( + dm_item_t item /* The item type to clear */ + ); + + /** Unlock all items of this type */ + __EXPORT void + dm_unlock( + dm_item_t item /* The item type to clear */ + ); + /** Erase all items of this type */ __EXPORT int dm_clear( @@ -113,6 +127,16 @@ extern "C" { dm_reset_reason restart_type /* The last reset type */ ); + /* NOTE: The following structure defines the persistent system state data stored in the single + entry DM_KEY_SYSTEM_STATE_KEY item type. It contains global system state information that + needs to survive restarts. This definition is application specific so it doesn't really belong + in this header, but till I find it a better home here it is */ + + typedef struct { + char current_offboard_waypoint_id; /* the index of the active offboard waypoint data */ + /* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */ + } persistent_system_state_t; + #ifdef __cplusplus } #endif diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e300be0747..cb1b762a72 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -952,6 +952,8 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { + persistent_system_state_t sys_state; + state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; state->current_state = MAVLINK_WPM_STATE_IDLE; @@ -962,6 +964,10 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->timestamp_last_send_request = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->current_dataman_id = 0; + if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { + if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1)) + state->current_dataman_id = sys_state.current_offboard_waypoint_id; + } } /* @@ -1428,12 +1434,38 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mission.dataman_id = 0; } - if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + if (dm_write(dm_next, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, len) != len) { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } + // offboard mission data saved correctly, now update the persistent system state + { + persistent_system_state_t state; + bool dm_result; + // Since we are doing a read-modify-write we must lock the item type + dm_lock(DM_KEY_SYSTEM_STATE); + // first read in the current state data. There may eventually be data other than the offboard index + // and we must preserve it + if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) { + // Not sure how to handle this? It means that either the item was never + // written, or fields have been added to the system state struct. In any case + // fields that may not be ours need to be initialized to sane values. + // For now the offboard index is the only field, so for now there + // is nothing to do here. + } + state.current_offboard_waypoint_id = mission.dataman_id; + dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); + dm_unlock(DM_KEY_SYSTEM_STATE); + if (dm_result) { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; + } + } + // if (wp.current) { // warnx("current is: %d", wp.seq); @@ -1485,7 +1517,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) publish_mission(); if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + persistent_system_state_t state; + bool dm_result; + dm_lock(DM_KEY_SYSTEM_STATE); + // first read in the current state data. There may eventually be data other than the offboard index + // and we must preserve it + if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) { + // Not sure how to handle this? It means that either the item was never + // written, or fields have been added to the system state struct. In any case + // fields that may not be ours need to be initialized to sane values. + // For now the offboard index is the only field, so we can deal with it here. + } + state.current_offboard_waypoint_id = -1; + dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) == sizeof(state); + dm_unlock(DM_KEY_SYSTEM_STATE); + if (dm_result) { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + } else { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + } } else { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 06e0c59046..04ea7da0eb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -603,6 +603,15 @@ Navigator::task_main() warnx("Could not clear geofence"); } +#if 0 // *** UNTESTED... Anton, this is for you + /* Get the last offboard mission id */ + persistent_system_state_t sys_state; + if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { + if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1)) + _mission.set_offboard_dataman_id(sys_state.current_offboard_waypoint_id); + } +#endif + /* * do subscriptions */ From 4ad435b483510158ea8a5b303cd680f9e982df84 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 9 Jun 2014 16:09:03 +0200 Subject: [PATCH 066/600] dataman: allow writing empty items with nullptr pointer to data --- src/modules/dataman/dataman.c | 4 +++- src/modules/dataman/dataman.h | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 96dc98f2a1..14fee7c9a5 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -324,7 +324,9 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v buffer[1] = persistence; buffer[2] = 0; buffer[3] = 0; - memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + if (count > 0) { + memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + } count += DM_SECTOR_HDR_SIZE; len = -1; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index f2b6cd4d3c..b25a2a5efc 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -133,7 +133,7 @@ extern "C" { in this header, but till I find it a better home here it is */ typedef struct { - char current_offboard_waypoint_id; /* the index of the active offboard waypoint data */ + char offboard_waypoint_id; /* the index of the active offboard waypoint data */ /* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */ } persistent_system_state_t; From cad0877f67c393b698b8fc4f242944c9e1ba1bc5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 9 Jun 2014 16:10:24 +0200 Subject: [PATCH 067/600] mavlink: waypoint manager fixes, mission saving on reboot --- src/modules/mavlink/mavlink_main.cpp | 444 +++++++++++------------ src/modules/mavlink/mavlink_main.h | 13 +- src/modules/navigator/navigator_main.cpp | 6 +- src/modules/uORB/topics/mission.h | 4 +- 4 files changed, 233 insertions(+), 234 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cb1b762a72..4e0597e80d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -954,20 +954,53 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { persistent_system_state_t sys_state; + /* init WPM state */ state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; + state->dataman_id = 0; state->current_state = MAVLINK_WPM_STATE_IDLE; state->current_partner_sysid = 0; state->current_partner_compid = 0; + state->current_dataman_id = 0; state->timestamp_lastaction = 0; state->timestamp_last_send_setpoint = 0; state->timestamp_last_send_request = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->current_dataman_id = 0; - if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { - if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1)) - state->current_dataman_id = sys_state.current_offboard_waypoint_id; + + int sys_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)); + if (sys_state_size == sizeof(sys_state)) { + if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) { + state->dataman_id = sys_state.offboard_waypoint_id; + + if (_verbose) { warnx("WPM init: using dataman ID %d", state->dataman_id); } + + /* count waypoints in current waypoints storage */ + dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + struct mission_item_s mission_item; + + int seq = 0; + while (seq < state->max_size && dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + seq++; + } + + if (_verbose) { warnx("WPM init: found %d items", seq); } + + state->size = seq; + + } else { + if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in SYSTEM_STATE", sys_state.offboard_waypoint_id); } + } + + } else { + if (_verbose) { warnx("WPM init: dataman SYSTEM_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } } + + /* init mission topic */ + mission.count = state->size; + mission.dataman_id = state->dataman_id; + mission.current_index = -1; // TODO store current index in dataman? + + publish_mission(); } /* @@ -985,7 +1018,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa); mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } + if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } } /* @@ -1009,13 +1042,12 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) mavlink_missionlib_send_message(&msg); } else if (seq == 0 && _wpm->size == 0) { - /* don't broadcast if no WPs */ } else { - mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); + if (_verbose) { warnx("WPM: Send WAYPOINT_CURRENT ERROR: seq %u out of bounds", seq); } - if (_verbose) { warnx("ERROR: index out of bounds"); } + mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); } } @@ -1031,25 +1063,15 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } + if (_verbose) { warnx("WPM: Send WAYPOINT_COUNT %u to ID %u", wpc.count, wpc.target_system); } } void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) { - + dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; struct mission_item_s mission_item; - ssize_t len = sizeof(struct mission_item_s); - dm_item_t dm_current; - - if (_wpm->current_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - if (dm_read(dm_current, seq, &mission_item, len) == len) { + if (dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { /* create mission_item_s from mavlink_mission_item_t */ mavlink_mission_item_t wp; @@ -1062,13 +1084,13 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } + if (_verbose) { warnx("WPM: Send WAYPOINT seq %u to ID %u", wp.seq, wp.target_system); } } else { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD"); - if (_verbose) { warnx("ERROR: could not read WP%u", seq); } + if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, dm_id); } } } @@ -1084,12 +1106,12 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u mavlink_missionlib_send_message(&msg); _wpm->timestamp_last_send_request = hrt_absolute_time(); - if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } + if (_verbose) { warnx("WPM: Send WAYPOINT_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } } else { mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); } + if (_verbose) { warnx("WPM: Send WAYPINT_REQUEST ERROR: seq %u exceeds list capacity", seq); } } } @@ -1110,7 +1132,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); - if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); } + if (_verbose) { warnx("WPM: Send WAYPOINT_REACHED seq %u", wp_reached.seq); } } void Mavlink::mavlink_waypoint_eventloop(uint64_t now) @@ -1120,7 +1142,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) mavlink_missionlib_send_gcs_string("Operation timeout"); - if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } + if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } _wpm->current_state = MAVLINK_WPM_STATE_IDLE; _wpm->current_partner_sysid = 0; @@ -1143,23 +1165,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); - if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { - _wpm->timestamp_lastaction = now; + if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { + _wpm->timestamp_lastaction = now; - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (_wpm->current_wp_id == _wpm->size - 1) { + if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (_wpm->current_wp_id == _wpm->size - 1) { + if (_verbose) { warnx("WPM: MISSION_ACK all items sent, switch to MAVLINK_WPM_STATE_IDLE"); } - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_wp_id = 0; + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _wpm->current_wp_id = 0; + } } + + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); } } - break; } @@ -1172,6 +1196,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.seq < _wpm->size) { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT OK (%d)", wpc.seq); } mission.current_index = wpc.seq; publish_mission(); @@ -1180,16 +1205,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) // mavlink_wpm_send_waypoint_current(wpc.seq); } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); } - if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); } + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); } } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - - if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } - } } @@ -1205,23 +1229,22 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (_wpm->size > 0) { - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; _wpm->current_wp_id = 0; _wpm->current_partner_sysid = msg->sysid; _wpm->current_partner_compid = msg->compid; } else { - if (_verbose) { warnx("No waypoints send"); } + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST nothing to send, mission is empty"); } } _wpm->current_count = _wpm->size; mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count); } else { - mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } - if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } + mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); } } @@ -1232,88 +1255,73 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); - if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; + if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) { + _wpm->timestamp_lastaction = now; - if (wpr.seq >= _wpm->size) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); } - - break; - } - - /* - * Ensure that we are in the correct state and that the first request has id 0 - * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - - if (wpr.seq == 0) { - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - - if (_verbose) { warnx("REJ. WP CMD: First id != 0"); } + if (wpr.seq >= _wpm->size) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: ignored, requested seq %u was out of bounds [0 %d]", wpr.seq, _wpm->size - 1); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); break; } - } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + /* + * Ensure that we are in the correct state and that the first request has id 0 + * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + */ + if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (wpr.seq == 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - if (wpr.seq == _wpm->current_wp_id) { + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: first id != 0 (%d)", wpr.seq); } - } else if (wpr.seq == _wpm->current_wp_id + 1) { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); + break; + } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (wpr.seq == _wpm->current_wp_id) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u (again) from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + + } else if (wpr.seq == _wpm->current_wp_id + 1) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, should be %i or %i", wpr.seq, msg->sysid, _wpm->current_wp_id, _wpm->current_wp_id + 1); } + + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); + break; + } } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); } + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _wpm->current_state); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); break; } - } else { + _wpm->current_wp_id = wpr.seq; + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + if (wpr.seq < _wpm->size) { + mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); } + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bounds", wpr.seq); } - break; - } - - _wpm->current_wp_id = wpr.seq; - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - if (wpr.seq < _wpm->size) { - - mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + } } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } - } - - - } else { - //we we're target but already communicating with someone else - if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } } } - break; } @@ -1327,48 +1335,51 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); break; } if (wpc.count == 0) { - mavlink_missionlib_send_gcs_string("COUNT 0"); - - if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); } + if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } + mavlink_missionlib_send_gcs_string("WP COUNT 0: CLEAR MISSION"); break; } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - _wpm->current_count = wpc.count; - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { if (_wpm->current_wp_id == 0) { + /* looks like our WAYPOINT_REQUEST was lost, try again */ + if (_verbose) { warnx("WPM: MISSION_COUNT %u (again) from ID %u", wpc.count, msg->sysid); } + mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _wpm->current_wp_id); } - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + break; } } else { - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _wpm->current_state); } - if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } + mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); + break; } + + _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; + _wpm->current_wp_id = 0; + _wpm->current_partner_sysid = msg->sysid; + _wpm->current_partner_compid = msg->compid; + _wpm->current_count = wpc.count; + _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } break; @@ -1378,32 +1389,28 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_msg_mission_item_decode(msg, &wp); if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { - _wpm->timestamp_lastaction = now; - /* - * ensure that we are in the correct state and that the first waypoint has id 0 + /* Ensure that we are in the correct state and that the first waypoint has id 0 * and the following waypoints have the correct ids */ - if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (wp.seq != 0) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq != 0 (%d)", wp.seq); } break; } } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (wp.seq >= _wpm->current_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq %d out of bounds [0 %d]", wp.seq, _wpm->current_count - 1); } + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); break; } if (wp.seq != _wpm->current_wp_id) { - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id); + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _wpm->current_wp_id); } + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); break; } @@ -1412,86 +1419,92 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; struct mission_item_s mission_item; - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); if (ret != OK) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } - ssize_t len = sizeof(struct mission_item_s); + dm_item_t dm_id = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - dm_item_t dm_next; + if (dm_write(dm_id, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, dm_id); } - if (_wpm->current_dataman_id == 0) { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; - mission.dataman_id = 1; - - } else { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.dataman_id = 0; - } - - if (dm_write(dm_next, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, len) != len) { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } - // offboard mission data saved correctly, now update the persistent system state - { - persistent_system_state_t state; - bool dm_result; - // Since we are doing a read-modify-write we must lock the item type - dm_lock(DM_KEY_SYSTEM_STATE); - // first read in the current state data. There may eventually be data other than the offboard index - // and we must preserve it - if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) { - // Not sure how to handle this? It means that either the item was never - // written, or fields have been added to the system state struct. In any case - // fields that may not be ours need to be initialized to sane values. - // For now the offboard index is the only field, so for now there - // is nothing to do here. - } - state.current_offboard_waypoint_id = mission.dataman_id; - dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); - dm_unlock(DM_KEY_SYSTEM_STATE); - if (dm_result) { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - } - -// if (wp.current) { -// warnx("current is: %d", wp.seq); -// mission.current_index = wp.seq; -// } - // XXX ignore current set - mission.current_index = -1; + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } _wpm->current_wp_id = wp.seq + 1; if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - - if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - mission.count = _wpm->current_count; - - publish_mission(); - - _wpm->current_dataman_id = mission.dataman_id; - _wpm->size = _wpm->current_count; + /* got all new mission items successfully */ + if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + /* write terminator item */ + if (_wpm->current_count < _wpm->max_size) { + if (dm_write(dm_id, _wpm->current_count, DM_PERSIST_POWER_ON_RESET, nullptr, 0) != 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR writing terminator item"); } + + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + break; + } + } + + /* offboard mission data saved correctly, now update the persistent system state */ + persistent_system_state_t state; + bool dm_result; + + /* since we are doing a read-modify-write we must lock the item type */ + dm_lock(DM_KEY_SYSTEM_STATE); + + /* first read in the current state data, there may eventually be data other than the offboard index + * and we must preserve it */ + ssize_t dm_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)); + if (dm_state_size != sizeof(state)) { + warnx("dataman SYSTEM_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); + } + + /* set new dataman storage ID */ + state.offboard_waypoint_id = _wpm->current_dataman_id; + + /* write back to dataman */ + dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); + + dm_unlock(DM_KEY_SYSTEM_STATE); + + if (dm_result) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman SYSTEM_STATE", state.offboard_waypoint_id); } + + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); + + } else { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + + /* update WPM state */ + _wpm->dataman_id = _wpm->current_dataman_id; + _wpm->size = _wpm->current_count; + + /* update mission topic */ + mission.dataman_id = _wpm->dataman_id; + mission.count = _wpm->current_count; + mission.current_index = 0; + + publish_mission(); + } + } else { + /* request next item */ mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } @@ -1506,46 +1519,34 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - _wpm->timestamp_lastaction = now; + /* clear only active dataman storage, don't change storage id */ + dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - _wpm->size = 0; + if (dm_clear(dm_id) == OK) { + if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } - /* prepare mission topic */ - mission.dataman_id = -1; - mission.count = 0; - mission.current_index = -1; - publish_mission(); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { - persistent_system_state_t state; - bool dm_result; - dm_lock(DM_KEY_SYSTEM_STATE); - // first read in the current state data. There may eventually be data other than the offboard index - // and we must preserve it - if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) { - // Not sure how to handle this? It means that either the item was never - // written, or fields have been added to the system state struct. In any case - // fields that may not be ours need to be initialized to sane values. - // For now the offboard index is the only field, so we can deal with it here. - } - state.current_offboard_waypoint_id = -1; - dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) == sizeof(state); - dm_unlock(DM_KEY_SYSTEM_STATE); - if (dm_result) { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - } + /* update WPM state */ + _wpm->timestamp_lastaction = now; + _wpm->size = 0; + + /* update mission topic */ + mission.count = 0; + mission.current_index = -1; + + publish_mission(); } else { + if (_verbose) { warnx("WPM: CLEAR_ALL ERROR: can't clear dataman mission storage"); } + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); } - } else { mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); } + if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } } } @@ -1570,7 +1571,6 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) } - int Mavlink::mavlink_missionlib_send_gcs_string(const char *string) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 40edc4b851..fbb25029d7 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -84,18 +84,19 @@ enum MAVLINK_WPM_CODES { struct mavlink_wpm_storage { - uint16_t size; - uint16_t max_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_count; + uint16_t size; ///< Count of waypoints in active mission + uint16_t max_size; ///< Maximal count of waypoints + int dataman_id; ///< Dataman storage ID for active mission + enum MAVLINK_WPM_STATES current_state; ///< Current waypoint manager state + int16_t current_wp_id; ///< Waypoint ID in current transmission + uint16_t current_count; ///< Waypoints count in current transmission uint8_t current_partner_sysid; uint8_t current_partner_compid; + int current_dataman_id; ///< Dataman storage ID for current transmission uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; uint64_t timestamp_last_send_request; uint32_t timeout; - int current_dataman_id; }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 04ea7da0eb..a3426e65ea 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -603,14 +603,12 @@ Navigator::task_main() warnx("Could not clear geofence"); } -#if 0 // *** UNTESTED... Anton, this is for you /* Get the last offboard mission id */ persistent_system_state_t sys_state; if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { - if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1)) - _mission.set_offboard_dataman_id(sys_state.current_offboard_waypoint_id); + if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) + _mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id); } -#endif /* * do subscriptions diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index ef4bc1defd..5f3cbcb1d4 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -95,8 +95,8 @@ struct mission_item_s { struct mission_s { - int dataman_id; /**< default -1, there are two offboard storage places in the dataman: 0 or 1 */ - unsigned count; /**< count of the missions stored in the datamanager */ + int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ + unsigned count; /**< count of the missions stored in the dataman */ int current_index; /**< default -1, start at the one changed latest */ }; From 3015f2e7af94e684c666689aa70c602f79810218 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 10 Jun 2014 14:32:37 +0200 Subject: [PATCH 068/600] mavlink: retry timeout moved to define --- src/modules/mavlink/mavlink_main.cpp | 7 ++++--- src/modules/mavlink/mavlink_main.h | 8 +++++--- src/modules/mavlink/waypoints.h | 2 +- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4e0597e80d..52cae2085d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -965,7 +965,8 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->timestamp_lastaction = 0; state->timestamp_last_send_setpoint = 0; state->timestamp_last_send_request = 0; - state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; int sys_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)); if (sys_state_size == sizeof(sys_state)) { @@ -1138,7 +1139,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) void Mavlink::mavlink_waypoint_eventloop(uint64_t now) { /* check for timed-out operations */ - if (now - _wpm->timestamp_lastaction > _wpm->timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + if (now - _wpm->timestamp_lastaction > _wpm->action_timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { mavlink_missionlib_send_gcs_string("Operation timeout"); @@ -1148,7 +1149,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) _wpm->current_partner_sysid = 0; _wpm->current_partner_compid = 0; - } else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + } else if (now - _wpm->timestamp_last_send_request > _wpm->retry_timeout && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { /* try to get WP again after short timeout */ mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index fbb25029d7..e31bbbb318 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -78,8 +78,9 @@ enum MAVLINK_WPM_CODES { #define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds +#define MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint #define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 @@ -96,7 +97,8 @@ struct mavlink_wpm_storage { uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; uint64_t timestamp_last_send_request; - uint32_t timeout; + uint32_t action_timeout; + uint32_t retry_timeout; }; diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index 532eff7aa1..a2109f2d85 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -88,7 +88,7 @@ struct mavlink_wpm_storage { uint8_t current_partner_compid; uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; - uint32_t timeout; + uint32_t action_timeout; int current_dataman_id; }; From 578680135e6813151a791dbcc3c31b1ba9c31a97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:03:58 +0200 Subject: [PATCH 069/600] introduce multi device support on uORB --- src/drivers/drv_accel.h | 3 ++- src/drivers/drv_baro.h | 3 ++- src/drivers/drv_gyro.h | 3 ++- src/drivers/drv_mag.h | 3 ++- src/modules/uORB/objects_common.cpp | 12 ++++++++---- 5 files changed, 16 insertions(+), 8 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 7d93ed9383..a6abaec70f 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -81,7 +81,8 @@ struct accel_scale { /* * ObjDev tag for raw accelerometer data. */ -ORB_DECLARE(sensor_accel); +ORB_DECLARE(sensor_accel0); +ORB_DECLARE(sensor_accel1); /* * ioctl() definitions diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index e2f0137ae2..248b9a73dc 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -63,7 +63,8 @@ struct baro_report { /* * ObjDev tag for raw barometer data. */ -ORB_DECLARE(sensor_baro); +ORB_DECLARE(sensor_baro0); +ORB_DECLARE(sensor_baro1); /* * ioctl() definitions diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 2ae8c70582..3635cbce14 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -81,7 +81,8 @@ struct gyro_scale { /* * ObjDev tag for raw gyro data. */ -ORB_DECLARE(sensor_gyro); +ORB_DECLARE(sensor_gyro0); +ORB_DECLARE(sensor_gyro1); /* * ioctl() definitions diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 06107bd3d1..32e0e48b3b 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -79,7 +79,8 @@ struct mag_scale { /* * ObjDev tag for raw magnetometer data. */ -ORB_DECLARE(sensor_mag); +ORB_DECLARE(sensor_mag0); +ORB_DECLARE(sensor_mag1); /* * ioctl() definitions diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e9..8340ca28a8 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -46,16 +46,20 @@ #include #include -ORB_DEFINE(sensor_mag, struct mag_report); +ORB_DEFINE(sensor_mag0, struct mag_report); +ORB_DEFINE(sensor_mag1, struct mag_report); #include -ORB_DEFINE(sensor_accel, struct accel_report); +ORB_DEFINE(sensor_accel0, struct accel_report); +ORB_DEFINE(sensor_accel1, struct accel_report); #include -ORB_DEFINE(sensor_gyro, struct gyro_report); +ORB_DEFINE(sensor_gyro0, struct gyro_report); +ORB_DEFINE(sensor_gyro1, struct gyro_report); #include -ORB_DEFINE(sensor_baro, struct baro_report); +ORB_DEFINE(sensor_baro0, struct baro_report); +ORB_DEFINE(sensor_baro1, struct baro_report); #include ORB_DEFINE(sensor_range_finder, struct range_finder_report); From 96accbf96cbc0262e4bb815d722282a318e8b8c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:04:18 +0200 Subject: [PATCH 070/600] Make sensors app multi-device aware --- src/modules/sensors/sensors.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b268b1b36b..0ae93d7f01 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -967,7 +967,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -993,7 +993,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1019,7 +1019,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report); math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); @@ -1050,7 +1050,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); + orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer); raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar raw.baro_alt_meter = _barometer.altitude; // Altitude in meters @@ -1579,11 +1579,11 @@ Sensors::task_main() /* * do subscriptions */ - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag0)); _rc_sub = orb_subscribe(ORB_ID(input_rc)); - _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); From 07fb1e089d1d15d512a8a68b03020b1ffd877ed3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:04:56 +0200 Subject: [PATCH 071/600] Make commander multi-device aware --- src/modules/commander/gyro_calibration.cpp | 4 ++-- src/modules/commander/mag_calibration.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index cbc2844c11..d89c67c2be 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -92,7 +92,7 @@ int do_gyro_calibration(int mavlink_fd) unsigned poll_errcount = 0; /* subscribe to gyro sensor topic */ - int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0)); struct gyro_report gyro_report; while (calibration_counter < calibration_count) { @@ -104,7 +104,7 @@ int do_gyro_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); + orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report); gyro_scale.x_offset += gyro_report.x; gyro_scale.y_offset += gyro_report.y; gyro_scale.z_offset += gyro_report.z; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 0ead22f773..23900f3868 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -145,7 +145,7 @@ int do_mag_calibration(int mavlink_fd) } if (res == OK) { - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + int sub_mag = orb_subscribe(ORB_ID(sensor_mag0)); struct mag_report mag; /* limit update rate to get equally spaced measurements over time (in ms) */ @@ -170,7 +170,7 @@ int do_mag_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); x[calibration_counter] = mag.x; y[calibration_counter] = mag.y; From a2ef04146aa5eb233fd564f38e447e983e5985a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:05:22 +0200 Subject: [PATCH 072/600] Introduce enum / define for multiple devices --- src/drivers/device/device.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index d99f229220..be6f6f4ec2 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -510,6 +510,9 @@ private: } // namespace device // class instance for primary driver of each class -#define CLASS_DEVICE_PRIMARY 0 +enum CLASS_DEVICE { + CLASS_DEVICE_PRIMARY=0, + CLASS_DEVICE_SECONDARY=1 +}; #endif /* _DEVICE_DEVICE_H */ From dc2df309cba57a754b291b528f92d252fc271f57 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:05:50 +0200 Subject: [PATCH 073/600] Introduce HMC5883 multi-device support --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index fddba806e3..be8e148078 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -866,9 +866,9 @@ HMC5883::collect() if (_mag_topic != -1) { /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + orb_publish(ORB_ID(sensor_mag0), _mag_topic, &new_report); } else { - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report); + _mag_topic = orb_advertise(ORB_ID(sensor_mag0), &new_report); if (_mag_topic < 0) debug("failed to create sensor_mag publication"); From 506f900513c1ce319a0160f8ac82a399274ac66f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:06:05 +0200 Subject: [PATCH 074/600] Introduce MPU6K multi-device uORB support --- src/drivers/mpu6000/mpu6000.cpp | 71 +++++++++++++++++++++++---------- 1 file changed, 50 insertions(+), 21 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 321fdd1733..10966e2d0f 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -491,31 +491,44 @@ MPU6000::init() measure(); - if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); - /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; - _accel_reports->get(&arp); - - /* measurement will have generated a report, publish */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); - - if (_accel_topic < 0) - debug("failed to create sensor_accel publication"); + /* measurement will have generated a report, publish */ + switch (_accel_class_instance) { + case CLASS_DEVICE_PRIMARY: + _accel_topic = orb_advertise(ORB_ID(sensor_accel0), &arp); + break; + + case CLASS_DEVICE_SECONDARY: + _accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp); + break; } - if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) { + if (_accel_topic < 0) { + warnx("failed to create sensor_accel publication"); + } - /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _gyro_reports->get(&grp); - _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _gyro_reports->get(&grp); - if (_gyro->_gyro_topic < 0) - debug("failed to create sensor_gyro publication"); + switch (_gyro->_gyro_class_instance) { + case CLASS_DEVICE_PRIMARY: + _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp); + break; + case CLASS_DEVICE_SECONDARY: + _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp); + break; + + } + + if (_gyro->_gyro_topic < 0) { + warnx("failed to create sensor_gyro publication"); } out: @@ -1326,14 +1339,30 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - if (_accel_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + switch (_accel_class_instance) { + case CLASS_DEVICE_PRIMARY: + orb_publish(ORB_ID(sensor_accel0), _accel_topic, &arb); + break; + + case CLASS_DEVICE_SECONDARY: + orb_publish(ORB_ID(sensor_accel1), _accel_topic, &arb); + break; + } } - if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + switch (_gyro->_gyro_class_instance) { + case CLASS_DEVICE_PRIMARY: + orb_publish(ORB_ID(sensor_gyro0), _gyro->_gyro_topic, &grb); + break; + + case CLASS_DEVICE_SECONDARY: + orb_publish(ORB_ID(sensor_gyro1), _gyro->_gyro_topic, &grb); + break; + } } /* stop measuring */ From 795f3693f26f1671a417cb7b6e36d743cab72304 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:07:42 +0200 Subject: [PATCH 075/600] LSM303D: Add support for multi-uORB devices --- src/drivers/lsm303d/lsm303d.cpp | 69 +++++++++++++++++++++++---------- 1 file changed, 48 insertions(+), 21 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8bf76dcc33..8a14aca4bc 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -600,34 +600,45 @@ LSM303D::init() /* fill report structures */ measure(); - if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise sensor topic, measure manually to initialize valid report */ + struct mag_report mrp; + _mag_reports->get(&mrp); - /* advertise sensor topic, measure manually to initialize valid report */ - struct mag_report mrp; - _mag_reports->get(&mrp); + /* measurement will have generated a report, publish */ + switch (_mag->_mag_class_instance) { + case CLASS_DEVICE_PRIMARY: + _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag0), &mrp); + break; - /* measurement will have generated a report, publish */ - _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp); - - if (_mag->_mag_topic < 0) - debug("failed to create sensor_mag publication"); + case CLASS_DEVICE_SECONDARY: + _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag1), &mrp); + break; + } + if (_mag->_mag_topic < 0) { + warnx("failed to create sensor_mag publication"); } _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); - if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); - /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; - _accel_reports->get(&arp); + /* measurement will have generated a report, publish */ + switch (_accel_class_instance) { + case CLASS_DEVICE_PRIMARY: + _accel_topic = orb_advertise(ORB_ID(sensor_accel0), &arp); + break; - /* measurement will have generated a report, publish */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + case CLASS_DEVICE_SECONDARY: + _accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp); + break; - if (_accel_topic < 0) - debug("failed to create sensor_accel publication"); + } + if (_accel_topic < 0) { + warnx("failed to create sensor_accel publication"); } out: @@ -1541,9 +1552,17 @@ LSM303D::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - if (_accel_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + switch (_accel_class_instance) { + case CLASS_DEVICE_PRIMARY: + orb_publish(ORB_ID(sensor_accel0), _accel_topic, &accel_report); + break; + + case CLASS_DEVICE_SECONDARY: + orb_publish(ORB_ID(sensor_accel1), _accel_topic, &accel_report); + break; + } } _accel_read++; @@ -1615,9 +1634,17 @@ LSM303D::mag_measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - if (_mag->_mag_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); + switch (_mag->_mag_class_instance) { + case CLASS_DEVICE_PRIMARY: + orb_publish(ORB_ID(sensor_mag0), _mag->_mag_topic, &mag_report); + break; + + case CLASS_DEVICE_SECONDARY: + orb_publish(ORB_ID(sensor_mag1), _mag->_mag_topic, &mag_report); + break; + } } _mag_read++; From c35a25e70a57229b58fe065fda23003de856e9f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:08:21 +0200 Subject: [PATCH 076/600] L3GD20: Add support for multi uORB topics --- src/drivers/l3gd20/l3gd20.cpp | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 37e72388be..2fcbdc0ade 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -398,17 +398,19 @@ L3GD20::init() measure(); + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _reports->get(&grp); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp); - /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _reports->get(&grp); - - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); - - if (_gyro_topic < 0) - debug("failed to create sensor_gyro publication"); + } else if (_class_instance == CLASS_DEVICE_SECONDARY) { + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp); + } + if (_gyro_topic < 0) { + debug("failed to create sensor_gyro publication"); } ret = OK; @@ -923,9 +925,17 @@ L3GD20::measure() poll_notify(POLLIN); /* publish for subscribers */ - if (_gyro_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: + orb_publish(ORB_ID(sensor_gyro0), _gyro_topic, &report); + break; + + case CLASS_DEVICE_SECONDARY: + orb_publish(ORB_ID(sensor_gyro1), _gyro_topic, &report); + break; + } } _read++; From becfed9fbd93204134c44f6da09d954151dff10d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:08:47 +0200 Subject: [PATCH 077/600] MS5611: Add support for multi uORB devices --- src/drivers/ms5611/ms5611.cpp | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 1ce93aeeaf..2d85a5e0dc 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -299,12 +299,17 @@ MS5611::init() ret = OK; - if (_class_instance == CLASS_DEVICE_PRIMARY) { + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: + _baro_topic = orb_advertise(ORB_ID(sensor_baro0), &brp); + break; + case CLASS_DEVICE_SECONDARY: + _baro_topic = orb_advertise(ORB_ID(sensor_baro1), &brp); + break; + } - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp); - - if (_baro_topic < 0) - debug("failed to create sensor_baro publication"); + if (_baro_topic < 0) { + warnx("failed to create sensor_baro publication"); } } while (0); @@ -721,9 +726,17 @@ MS5611::collect() report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; /* publish it */ - if (_baro_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: + orb_publish(ORB_ID(sensor_baro0), _baro_topic, &report); + break; + + case CLASS_DEVICE_SECONDARY: + orb_publish(ORB_ID(sensor_baro1), _baro_topic, &report); + break; + } } if (_reports->force(&report)) { From 699ad1512cb55db004ab7d7143dd1ef8741fb9ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:09:16 +0200 Subject: [PATCH 078/600] EKF estimator: Add support for multi uORB sensor topics --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 0a6d3fa8fe..d806b02417 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -522,7 +522,7 @@ FixedwingEstimator::task_main() /* * do subscriptions */ - _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -861,7 +861,7 @@ FixedwingEstimator::task_main() orb_check(_baro_sub, &baro_updated); if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); _ekf->baroHgt = _baro.altitude; @@ -1040,7 +1040,7 @@ FixedwingEstimator::task_main() float gps_alt = _gps.alt / 1e3f; // Set up height correctly - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); _baro_gps_offset = _baro_ref - _baro.altitude; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); From e3e8bf25befa23edd6e2963760794abf4c8d6e2e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:09:41 +0200 Subject: [PATCH 079/600] FW att control: Add support for multi uORB topics --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 178b590ae5..c026e29a72 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -520,7 +520,7 @@ FixedwingAttitudeControl::vehicle_accel_poll() orb_check(_accel_sub, &accel_updated); if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel); } } @@ -568,7 +568,7 @@ FixedwingAttitudeControl::task_main() */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); From 0031713004dc9dcc071e13ee388af8ad028d3978 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:10:12 +0200 Subject: [PATCH 080/600] mavlink: Add support for multi uORB sensor interface --- src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 53769e0cfe..98a7cef5a3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -495,10 +495,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) gyro.temperature = imu.temperature; if (_gyro_pub < 0) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro); } else { - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro); } } @@ -517,10 +517,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) accel.temperature = imu.temperature; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); } } @@ -538,10 +538,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mag.z = imu.zmag; if (_mag_pub < 0) { - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + _mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag); } else { - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag); } } @@ -556,10 +556,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) baro.temperature = imu.temperature; if (_baro_pub < 0) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + _baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro); } else { - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro); } } @@ -835,10 +835,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) accel.temperature = 25.0f; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); } } From d0f4232ac6e2ff9d796df9d995e749734edc32ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:12:11 +0200 Subject: [PATCH 081/600] Build and runtime fixes for matlab csv serial bridge --- .../matlab_csv_serial/matlab_csv_serial.c | 51 ++++++++++--------- src/examples/matlab_csv_serial/module.mk | 3 +- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 7c8bcc31e8..1327d1a232 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -95,7 +95,7 @@ int matlab_csv_serial_main(int argc, char *argv[]) { if (thread_running) { - printf("flow position estimator already running\n"); + warnx("already running\n"); /* this is not an error */ exit(0); } @@ -133,19 +133,21 @@ int matlab_csv_serial_main(int argc, char *argv[]) int matlab_csv_serial_thread_main(int argc, char *argv[]) { - /* welcome user */ - thread_running = true; if (argc < 2) { errx(1, "need a serial port name as argument"); } - int serial_fd = open(argv[1], O_RDWR | O_NOCTTY); + const char* uart_name = argv[1]; + + warnx("opening port %s", uart_name); + + int serial_fd = open(uart_name, O_RDWR | O_NOCTTY); unsigned speed = 921600; if (serial_fd < 0) { - err(1, "failed to open port: %s", argv[1]); + err(1, "failed to open port: %s", uart_name); } /* Try to set baud rate */ @@ -154,15 +156,12 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) bool is_usb = false; /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { + if ((termios_state = tcgetattr(serial_fd, &uart_config)) < 0) { warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); - close(_uart_fd); + close(serial_fd); return -1; } - /* Fill the struct for the new configuration */ - tcgetattr(_uart_fd, &uart_config); - /* Clear ONLCR flag (which appends a CR for every LF) */ uart_config.c_oflag &= ~ONLCR; @@ -171,7 +170,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERR SET BAUD %s: %d\n", argv[1], termios_state); + warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state); close(serial_fd); return -1; } @@ -179,7 +178,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) } if ((termios_state = tcsetattr(serial_fd, TCSANOW, &uart_config)) < 0) { - warnx("ERR SET CONF %s\n", argv[1]); + warnx("ERR SET CONF %s\n", uart_name); close(serial_fd); return -1; } @@ -191,20 +190,19 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) struct gyro_report gyro1; /* subscribe to parameter changes */ - int accel0_sub = orb_subscribe(ORB_ID(sensor_accel)); - int accel1_sub; - int gyro0_sub; - int gyro1_sub; + int accel0_sub = orb_subscribe(ORB_ID(sensor_accel0)); + int accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); + int gyro0_sub = orb_subscribe(ORB_ID(sensor_gyro0)); + int gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); + + thread_running = true; while (!thread_should_exit) { /*This runs at the rate of the sensors */ struct pollfd fds[] = { - { .fd = accel0_sub, .events = POLLIN }, - { .fd = accel1_sub, .events = POLLIN }, - { .fd = gyro0_sub, .events = POLLIN }, - { .fd = gyro1_sub, .events = POLLIN } + { .fd = accel0_sub, .events = POLLIN } }; /* wait for a sensor update, check for exit condition every 500 ms */ @@ -212,8 +210,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) if (ret < 0) { - /* poll error, count it in perf */ - perf_count(mc_err_perf); + /* poll error, ignore */ } else if (ret == 0) @@ -224,13 +221,17 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) else { - /* parameter update available? */ + /* accel0 update available? */ if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0); + orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0); + orb_copy(ORB_ID(sensor_accel1), accel0_sub, &accel1); + orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0); + orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1); // write out on accel 0, but collect for all other sensors as they have updates - vdprintf(serial_fd, "%llu,%d\n", accel0.timestamp, (int)accel0.x_raw); + dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw, + (int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw); } } diff --git a/src/examples/matlab_csv_serial/module.mk b/src/examples/matlab_csv_serial/module.mk index c83d1fefd6..1629c2ce46 100644 --- a/src/examples/matlab_csv_serial/module.mk +++ b/src/examples/matlab_csv_serial/module.mk @@ -37,5 +37,4 @@ MODULE_COMMAND = matlab_csv_serial -SRCS = matlab_csv_serial.c \ - matlab_csv_serial.c +SRCS = matlab_csv_serial.c From 474de62ebb825de8d3757730fc3bdf8af250e173 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:12:23 +0200 Subject: [PATCH 082/600] Enable matlab bridge in test setup --- makefiles/config_px4fmu-v2_test.mk | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 66b2157edb..119056a399 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -32,6 +32,11 @@ MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/ver +# +# Testing modules +# +MODULES += examples/matlab_csv_serial + # # Library modules # From 2951962c2104a0b2a284a7c5208171b257ed9734 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 12 Jun 2014 13:11:45 +0200 Subject: [PATCH 083/600] dataman: rename SYSTEM_STATE section to MISSION_STATE --- src/modules/dataman/dataman.c | 6 +++--- src/modules/dataman/dataman.h | 4 ++-- src/modules/mavlink/mavlink_main.cpp | 18 +++++++++--------- src/modules/navigator/navigator_main.cpp | 2 +- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 14fee7c9a5..4a89c7d41c 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -116,7 +116,7 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, DM_KEY_WAYPOINTS_ONBOARD_MAX, - DM_KEY_SYSTEM_STATE_MAX + DM_KEY_MISSION_STATE_MAX }; /* Table of offset for index 0 of each item type */ @@ -642,11 +642,11 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < dm_number_of_funcs; i++) g_func_counts[i] = 0; - /* Initialize the item type locks, for now only DM_KEY_SYSTEM_STATE supports locking */ + /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */ sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */ for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) g_item_locks[i] = NULL; - g_item_locks[DM_KEY_SYSTEM_STATE] = &g_sys_state_mutex; + g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex; g_task_should_exit = false; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index b25a2a5efc..215ec4c764 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -53,7 +53,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ - DM_KEY_SYSTEM_STATE, /* Persistent system state storage */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -64,7 +64,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_SYSTEM_STATE_MAX = 1 + DM_KEY_MISSION_STATE_MAX = 1 }; /** Data persistence levels */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9159f5d617..be1fc50a62 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -969,7 +969,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - int sys_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)); + int sys_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)); if (sys_state_size == sizeof(sys_state)) { if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) { state->dataman_id = sys_state.offboard_waypoint_id; @@ -990,11 +990,11 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->size = seq; } else { - if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in SYSTEM_STATE", sys_state.offboard_waypoint_id); } + if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in MISSION_STATE", sys_state.offboard_waypoint_id); } } } else { - if (_verbose) { warnx("WPM init: dataman SYSTEM_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } + if (_verbose) { warnx("WPM init: dataman MISSION_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } } /* init mission topic */ @@ -1467,25 +1467,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) bool dm_result; /* since we are doing a read-modify-write we must lock the item type */ - dm_lock(DM_KEY_SYSTEM_STATE); + dm_lock(DM_KEY_MISSION_STATE); /* first read in the current state data, there may eventually be data other than the offboard index * and we must preserve it */ - ssize_t dm_state_size = dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)); + ssize_t dm_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &state, sizeof(state)); if (dm_state_size != sizeof(state)) { - warnx("dataman SYSTEM_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); + warnx("dataman MISSION_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); } /* set new dataman storage ID */ state.offboard_waypoint_id = _wpm->current_dataman_id; /* write back to dataman */ - dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); + dm_result = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); - dm_unlock(DM_KEY_SYSTEM_STATE); + dm_unlock(DM_KEY_MISSION_STATE); if (dm_result) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman SYSTEM_STATE", state.offboard_waypoint_id); } + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman MISSION_STATE", state.offboard_waypoint_id); } mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a3426e65ea..c7b39ba936 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -605,7 +605,7 @@ Navigator::task_main() /* Get the last offboard mission id */ persistent_system_state_t sys_state; - if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { + if (dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) _mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id); } From 91b590ef584cfc67be7555e3d7272bb94bc9b2b4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 13 Jun 2014 23:40:48 +0200 Subject: [PATCH 084/600] Move MISSION_STATE read/write from mavlink to navigator and commander --- src/modules/commander/commander.cpp | 22 ++++- src/modules/dataman/dataman.h | 12 +-- src/modules/mavlink/mavlink_main.cpp | 119 +++++------------------ src/modules/navigator/mission.cpp | 33 +++++++ src/modules/navigator/mission.h | 5 + src/modules/navigator/navigator_main.cpp | 7 -- 6 files changed, 80 insertions(+), 118 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e992706ac3..d2f8c29943 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -89,6 +90,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -691,6 +693,11 @@ int commander_thread_main(int argc, char *argv[]) /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + if (status_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } /* armed topic */ orb_advert_t armed_pub; @@ -708,10 +715,17 @@ int commander_thread_main(int argc, char *argv[]) struct home_position_s home; memset(&home, 0, sizeof(home)); - if (status_pub < 0) { - warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); - warnx("exiting."); - exit(ERROR); + /* init mission state */ + mission_s mission; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { + if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { + mavlink_log_info(mavlink_fd, "[cmd] dataman ID: %i, count: %u, current: %i", + mission.dataman_id, mission.count, mission.current_index); + orb_advert_t mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + close(mission_pub); + } else { + mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + } } mavlink_log_info(mavlink_fd, "[cmd] started"); diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 215ec4c764..dbace21eff 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -53,7 +53,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ - DM_KEY_MISSION_STATE, /* Persistent mission state */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -127,16 +127,6 @@ extern "C" { dm_reset_reason restart_type /* The last reset type */ ); - /* NOTE: The following structure defines the persistent system state data stored in the single - entry DM_KEY_SYSTEM_STATE_KEY item type. It contains global system state information that - needs to survive restarts. This definition is application specific so it doesn't really belong - in this header, but till I find it a better home here it is */ - - typedef struct { - char offboard_waypoint_id; /* the index of the active offboard waypoint data */ - /* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */ - } persistent_system_state_t; - #ifdef __cplusplus } #endif diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5c6ef68eee..bc4376b84a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -18,7 +18,6 @@ * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, @@ -964,8 +963,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { - persistent_system_state_t sys_state; - /* init WPM state */ state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; @@ -980,40 +977,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - int sys_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)); - if (sys_state_size == sizeof(sys_state)) { - if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) { - state->dataman_id = sys_state.offboard_waypoint_id; - - if (_verbose) { warnx("WPM init: using dataman ID %d", state->dataman_id); } - - /* count waypoints in current waypoints storage */ - dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - struct mission_item_s mission_item; - - int seq = 0; - while (seq < state->max_size && dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { - seq++; - } - - if (_verbose) { warnx("WPM init: found %d items", seq); } - - state->size = seq; - - } else { - if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in MISSION_STATE", sys_state.offboard_waypoint_id); } - } - - } else { - if (_verbose) { warnx("WPM init: dataman MISSION_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } - } - - /* init mission topic */ - mission.count = state->size; - mission.dataman_id = state->dataman_id; - mission.current_index = -1; // TODO store current index in dataman? - - publish_mission(); + } /* @@ -1081,10 +1045,10 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) { - dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; struct mission_item_s mission_item; - if (dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { /* create mission_item_s from mavlink_mission_item_t */ mavlink_mission_item_t wp; @@ -1103,7 +1067,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD"); - if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, dm_id); } + if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, _wpm->dataman_id); } } } @@ -1214,9 +1178,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mission.current_index = wpc.seq; publish_mission(); - /* don't answer yet, wait for the navigator to respond, then publish the mission_result */ -// mavlink_wpm_send_waypoint_current(wpc.seq); - } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); } @@ -1392,6 +1353,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_count = wpc.count; _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + mission.current_index = -1; + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } @@ -1442,10 +1405,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - dm_item_t dm_id = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - if (dm_write(dm_id, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, dm_id); } + if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _wpm->current_dataman_id); } mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); @@ -1453,6 +1416,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } + /* waypoint marked as current */ + if (wp.current) { + mission.current_index = wp.seq; + } + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } _wpm->current_wp_id = wp.seq + 1; @@ -1463,65 +1431,23 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - /* write terminator item */ - if (_wpm->current_count < _wpm->max_size) { - if (dm_write(dm_id, _wpm->current_count, DM_PERSIST_POWER_ON_RESET, nullptr, 0) != 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR writing terminator item"); } + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - break; - } - } + /* update WPM state */ + _wpm->dataman_id = _wpm->current_dataman_id; + _wpm->size = _wpm->current_count; - /* offboard mission data saved correctly, now update the persistent system state */ - persistent_system_state_t state; - bool dm_result; + /* update mission topic */ + mission.dataman_id = _wpm->dataman_id; + mission.count = _wpm->current_count; - /* since we are doing a read-modify-write we must lock the item type */ - dm_lock(DM_KEY_MISSION_STATE); - - /* first read in the current state data, there may eventually be data other than the offboard index - * and we must preserve it */ - ssize_t dm_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &state, sizeof(state)); - if (dm_state_size != sizeof(state)) { - warnx("dataman MISSION_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); - } - - /* set new dataman storage ID */ - state.offboard_waypoint_id = _wpm->current_dataman_id; - - /* write back to dataman */ - dm_result = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); - - dm_unlock(DM_KEY_MISSION_STATE); - - if (dm_result) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman MISSION_STATE", state.offboard_waypoint_id); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); - - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - /* update WPM state */ - _wpm->dataman_id = _wpm->current_dataman_id; - _wpm->size = _wpm->current_count; - - /* update mission topic */ - mission.dataman_id = _wpm->dataman_id; - mission.count = _wpm->current_count; - mission.current_index = 0; - - publish_mission(); - } + publish_mission(); } else { /* request next item */ mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } - break; } @@ -1545,6 +1471,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->size = 0; /* update mission topic */ + mission.dataman_id = _wpm->dataman_id; mission.count = 0; mission.current_index = -1; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9244063b19..d9d8353f65 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -430,6 +430,37 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio return false; } +void +Mission::save_offboard_mission_state() +{ + mission_s mission_state; + + /* lock MISSION_STATE item */ + dm_lock(DM_KEY_MISSION_STATE); + + /* read current state */ + int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + + /* check if state actually changed to save flash write cycles */ + if (read_res != sizeof(mission_s) || mission_state.dataman_id != _offboard_mission.dataman_id || + mission_state.count != _offboard_mission.count || + mission_state.current_index != _current_offboard_mission_index) { + + mission_state.dataman_id = _offboard_mission.dataman_id; + mission_state.count = _offboard_mission.count; + mission_state.current_index = _current_offboard_mission_index; + + /* write modifyed state only if changed */ + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + mavlink_log_critical(_navigator->get_mavlink_fd(), "error saving mission state"); + + } + } + + /* unlock MISSION_STATE item */ + dm_unlock(DM_KEY_MISSION_STATE); +} + void Mission::report_mission_item_reached() { @@ -445,6 +476,8 @@ Mission::report_current_offboard_mission_item() { _mission_result.index_current_mission = _current_offboard_mission_index; publish_mission_result(); + + save_offboard_mission_state(); } void diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 322aaf96af..98b11bade2 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -141,6 +141,11 @@ private: bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item); + /** + * Save current offboard mission state to dataman + */ + void save_offboard_mission_state(); + /** * Report that a mission item has been reached */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 888c8e7f43..a3c190c7f2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -227,13 +227,6 @@ Navigator::task_main() warnx("Could not clear geofence"); } - /* Get the last offboard mission id */ - persistent_system_state_t sys_state; - if (dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { - if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) - _mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id); - } - /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); From ffd9ac7e081aebb3d6329a0f6c09812d1c0c4523 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 14 Jun 2014 01:31:23 +0200 Subject: [PATCH 085/600] mavlink: fix WPM initialization --- src/modules/mavlink/mavlink_main.cpp | 59 ++++++++++++++++------------ src/modules/mavlink/mavlink_main.h | 4 +- 2 files changed, 36 insertions(+), 27 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bc4376b84a..126c4dfc6c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -72,8 +72,6 @@ #include #include -#include -#include #include "mavlink_bridge_header.h" #include "mavlink_main.h" @@ -245,7 +243,7 @@ Mavlink::Mavlink() : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { _wpm = &_wpm_s; - mission.count = 0; + fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; _instance_id = Mavlink::instance_count(); @@ -860,10 +858,10 @@ void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + _mission_pub = orb_advertise(ORB_ID(offboard_mission), &_mission); } else { - orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission); + orb_publish(ORB_ID(offboard_mission), _mission_pub, &_mission); } } @@ -963,10 +961,20 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { + /* get offboard_mission topic */ + int mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + if (orb_copy(ORB_ID(offboard_mission), mission_sub, &_mission)) { + /* error getting topic, init to safe values */ + _mission.dataman_id = 0; + _mission.count = 0; + _mission.current_index = -1; + } + close(mission_sub); + /* init WPM state */ - state->size = 0; + state->size = _mission.count; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->dataman_id = 0; + state->dataman_id = _mission.dataman_id; state->current_state = MAVLINK_WPM_STATE_IDLE; state->current_partner_sysid = 0; state->current_partner_compid = 0; @@ -977,7 +985,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - + warnx("wpm init: dataman_id=%d, count=%u, current=%d", state->dataman_id, state->size, _mission.current_index); } /* @@ -1035,7 +1043,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin wpc.target_system = sysid; wpc.target_component = compid; - wpc.count = mission.count; + wpc.count = _wpm->size; mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); mavlink_missionlib_send_message(&msg); @@ -1058,6 +1066,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t wp.target_system = sysid; wp.target_component = compid; wp.seq = seq; + wp.current = (_mission_result.index_current_mission == seq) ? 1 : 0; mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); mavlink_missionlib_send_message(&msg); @@ -1175,7 +1184,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpc.seq < _wpm->size) { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT OK (%d)", wpc.seq); } - mission.current_index = wpc.seq; + _mission.current_index = wpc.seq; publish_mission(); } else { @@ -1353,7 +1362,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_count = wpc.count; _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission - mission.current_index = -1; + _mission.current_index = -1; mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } @@ -1418,7 +1427,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) /* waypoint marked as current */ if (wp.current) { - mission.current_index = wp.seq; + _mission.current_index = wp.seq; } if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } @@ -1438,8 +1447,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->size = _wpm->current_count; /* update mission topic */ - mission.dataman_id = _wpm->dataman_id; - mission.count = _wpm->current_count; + _mission.dataman_id = _wpm->dataman_id; + _mission.count = _wpm->current_count; publish_mission(); @@ -1471,9 +1480,9 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->size = 0; /* update mission topic */ - mission.dataman_id = _wpm->dataman_id; - mission.count = 0; - mission.current_index = -1; + _mission.dataman_id = _wpm->dataman_id; + _mission.count = 0; + _mission.current_index = -1; publish_mission(); @@ -1754,7 +1763,6 @@ Mavlink::pass_message(mavlink_message_t *msg) } - int Mavlink::task_main(int argc, char *argv[]) { @@ -1918,8 +1926,7 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_wpm_init(_wpm); int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - struct mission_result_s mission_result; - memset(&mission_result, 0, sizeof(mission_result)); + memset(&_mission_result, 0, sizeof(_mission_result)); _task_running = true; @@ -2027,19 +2034,19 @@ Mavlink::task_main(int argc, char *argv[]) orb_check(mission_result_sub, &updated); if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + orb_copy(ORB_ID(mission_result), mission_result_sub, &_mission_result); - if (_verbose) { warnx("Got mission result: new current: %d", mission_result.index_current_mission); } + if (_verbose) { warnx("WPM: got mission result, new current: %d", _mission_result.index_current_mission); } - if (mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); + if (_mission_result.mission_reached) { + mavlink_wpm_send_waypoint_reached((uint16_t)_mission_result.mission_index_reached); } - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); } else { if (slow_rate_limiter.check(t)) { - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 61fd7afe2b..5d7273abd2 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -50,6 +50,7 @@ #include #include +#include #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" @@ -238,7 +239,8 @@ private: MavlinkStream *_streams; orb_advert_t _mission_pub; - struct mission_s mission; + struct mission_s _mission; + struct mission_result_s _mission_result; MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; From 5be741607c0d8d477ff30c7639edbd3bce427e7b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 14 Jun 2014 23:57:29 +0200 Subject: [PATCH 086/600] mavlink: mission manager moved to separate class and reworked --- src/modules/commander/commander.cpp | 25 +- src/modules/mavlink/mavlink_main.cpp | 766 ++------------------- src/modules/mavlink/mavlink_main.h | 90 +-- src/modules/mavlink/mavlink_mission.cpp | 823 +++++++++++++++++++++++ src/modules/mavlink/mavlink_mission.h | 176 +++++ src/modules/mavlink/mavlink_receiver.cpp | 13 +- src/modules/mavlink/mavlink_receiver.h | 2 - src/modules/mavlink/module.mk | 1 + src/modules/mavlink/util.h | 53 -- src/modules/mavlink/waypoints.h | 111 --- src/modules/navigator/mission.cpp | 107 +-- src/modules/navigator/mission.h | 2 + src/modules/uORB/topics/mission.h | 6 +- src/modules/uORB/topics/mission_result.h | 8 +- 14 files changed, 1167 insertions(+), 1016 deletions(-) create mode 100644 src/modules/mavlink/mavlink_mission.cpp create mode 100644 src/modules/mavlink/mavlink_mission.h delete mode 100644 src/modules/mavlink/util.h delete mode 100644 src/modules/mavlink/waypoints.h diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d2f8c29943..379ce45fb7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -715,17 +715,27 @@ int commander_thread_main(int argc, char *argv[]) struct home_position_s home; memset(&home, 0, sizeof(home)); - /* init mission state */ + /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ + orb_advert_t mission_pub = -1; mission_s mission; if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { - mavlink_log_info(mavlink_fd, "[cmd] dataman ID: %i, count: %u, current: %i", - mission.dataman_id, mission.count, mission.current_index); - orb_advert_t mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); - close(mission_pub); + warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); + mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", + mission.dataman_id, mission.count, mission.current_seq); } else { + warnx("reading mission state failed"); mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + + /* initialize mission state in dataman */ + mission.dataman_id = 0; + mission.count = 0; + mission.current_seq = 0; + dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); } + + mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + orb_publish(ORB_ID(offboard_mission), mission_pub, &mission); } mavlink_log_info(mavlink_fd, "[cmd] started"); @@ -1310,7 +1320,7 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { + if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.finished)) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { @@ -1327,7 +1337,7 @@ int commander_thread_main(int argc, char *argv[]) /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { + mission_result.finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { status.failsafe_state = FAILSAFE_STATE_RC_LOSS; @@ -1488,6 +1498,7 @@ int commander_thread_main(int argc, char *argv[]) close(diff_pres_sub); close(param_changed_sub); close(battery_sub); + close(mission_pub); thread_running = false; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 126c4dfc6c..f8a31a4ad9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -102,6 +102,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); +extern mavlink_system_t mavlink_system; + static uint64_t last_write_success_times[6] = {0}; static uint64_t last_write_try_times[6] = {0}; @@ -200,9 +202,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } } - - - } static void usage(void); @@ -221,6 +220,7 @@ Mavlink::Mavlink() : _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), + _mission_result_sub(-1), _mission_pub(-1), _mode(MAVLINK_MODE_NORMAL), _total_counter(0), @@ -238,12 +238,11 @@ Mavlink::Mavlink() : _param_component_id(0), _param_system_type(0), _param_use_hil_gps(0), + _mission_manager(nullptr), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { - _wpm = &_wpm_s; - fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; _instance_id = Mavlink::instance_count(); @@ -421,7 +420,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) } void -Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) +Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) { Mavlink *inst; @@ -497,7 +496,6 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) void Mavlink::mavlink_update_system(void) { - if (!_param_initialized) { _param_system_id = param_find("MAV_SYS_ID"); _param_component_id = param_find("MAV_COMP_ID"); @@ -701,9 +699,32 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -extern mavlink_system_t mavlink_system; +void +Mavlink::send_message(const mavlink_message_t *msg) +{ + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; -int Mavlink::mavlink_pm_queued_send() + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); + mavlink_send_uart_bytes(_channel, buf, len); +} + +void +Mavlink::handle_message(const mavlink_message_t *msg) +{ + /* handle packet with mission manager */ + _mission_manager->handle_message(msg); + + /* handle packet with parameter component */ + mavlink_pm_message_handler(_channel, msg); + + if (get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(msg, this); + } +} + +int +Mavlink::mavlink_pm_queued_send() { if (_mavlink_param_queue_index < param_count()) { mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); @@ -780,7 +801,7 @@ int Mavlink::mavlink_pm_send_param(param_t param) mavlink_type, param_count(), param_get_index(param)); - mavlink_missionlib_send_message(&tx_msg); + send_message(&tx_msg); return OK; } @@ -794,7 +815,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + send_statustext("[mavlink pm] sending list"); } } break; @@ -818,7 +839,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown: %s", name); - mavlink_missionlib_send_gcs_string(buf); + send_statustext(buf); } else { /* set and send parameter */ @@ -854,677 +875,18 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav } } -void Mavlink::publish_mission() +int +Mavlink::send_statustext(const char *string) { - /* Initialize mission publication if necessary */ - if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(offboard_mission), &_mission); - - } else { - orb_publish(ORB_ID(offboard_mission), _mission_pub, &_mission); - } + return send_statustext(MAV_SEVERITY_INFO, string); } -int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) -{ - /* only support global waypoints for now */ - switch (mavlink_mission_item->frame) { - case MAV_FRAME_GLOBAL: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = false; - break; - - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = true; - break; - - case MAV_FRAME_LOCAL_NED: - case MAV_FRAME_LOCAL_ENU: - return MAV_MISSION_UNSUPPORTED_FRAME; - - case MAV_FRAME_MISSION: - default: - return MAV_MISSION_ERROR; - } - - switch (mavlink_mission_item->command) { - case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param1; - break; - case MAV_CMD_DO_JUMP: - mission_item->do_jump_mission_index = mavlink_mission_item->param1; - mission_item->do_jump_repeat_count = mavlink_mission_item->param2; - break; - default: - mission_item->acceptance_radius = mavlink_mission_item->param2; - mission_item->time_inside = mavlink_mission_item->param1; - break; - } - - mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); - mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); - mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; - - mission_item->autocontinue = mavlink_mission_item->autocontinue; - // mission_item->index = mavlink_mission_item->seq; - mission_item->origin = ORIGIN_MAVLINK; - - /* reset DO_JUMP count */ - mission_item->do_jump_current_count = 0; - - return OK; -} - -int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) -{ - if (mission_item->altitude_is_relative) { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL; - - } else { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - } - - switch (mission_item->nav_cmd) { - case NAV_CMD_TAKEOFF: - mavlink_mission_item->param1 = mission_item->pitch_min; - break; - - case NAV_CMD_DO_JUMP: - mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; - break; - - default: - mavlink_mission_item->param2 = mission_item->acceptance_radius; - mavlink_mission_item->param1 = mission_item->time_inside; - break; - } - - mavlink_mission_item->x = (float)mission_item->lat; - mavlink_mission_item->y = (float)mission_item->lon; - mavlink_mission_item->z = mission_item->altitude; - - mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; - mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; - mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->autocontinue = mission_item->autocontinue; - // mavlink_mission_item->seq = mission_item->index; - - return OK; -} - -void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) -{ - /* get offboard_mission topic */ - int mission_sub = orb_subscribe(ORB_ID(offboard_mission)); - if (orb_copy(ORB_ID(offboard_mission), mission_sub, &_mission)) { - /* error getting topic, init to safe values */ - _mission.dataman_id = 0; - _mission.count = 0; - _mission.current_index = -1; - } - close(mission_sub); - - /* init WPM state */ - state->size = _mission.count; - state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->dataman_id = _mission.dataman_id; - state->current_state = MAVLINK_WPM_STATE_IDLE; - state->current_partner_sysid = 0; - state->current_partner_compid = 0; - state->current_dataman_id = 0; - state->timestamp_lastaction = 0; - state->timestamp_last_send_setpoint = 0; - state->timestamp_last_send_request = 0; - state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - - warnx("wpm init: dataman_id=%d, count=%u, current=%d", state->dataman_id, state->size, _mission.current_index); -} - -/* - * @brief Sends an waypoint ack message - */ -void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_mission_ack_t wpa; - - wpa.target_system = sysid; - wpa.target_component = compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if (seq < _wpm->size) { - mavlink_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = seq; - - mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - } else if (seq == 0 && _wpm->size == 0) { - /* don't broadcast if no WPs */ - - } else { - if (_verbose) { warnx("WPM: Send WAYPOINT_CURRENT ERROR: seq %u out of bounds", seq); } - - mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - } -} - -void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_mission_count_t wpc; - - wpc.target_system = sysid; - wpc.target_component = compid; - wpc.count = _wpm->size; - - mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send WAYPOINT_COUNT %u to ID %u", wpc.count, wpc.target_system); } -} - -void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - dm_item_t dm_item = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - struct mission_item_s mission_item; - - if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { - - /* create mission_item_s from mavlink_mission_item_t */ - mavlink_mission_item_t wp; - map_mission_item_to_mavlink_mission_item(&mission_item, &wp); - - mavlink_message_t msg; - wp.target_system = sysid; - wp.target_component = compid; - wp.seq = seq; - wp.current = (_mission_result.index_current_mission == seq) ? 1 : 0; - mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send WAYPOINT seq %u to ID %u", wp.seq, wp.target_system); } - - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD"); - - if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, _wpm->dataman_id); } - } -} - -void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < _wpm->max_size) { - mavlink_message_t msg; - mavlink_mission_request_t wpr; - wpr.target_system = sysid; - wpr.target_component = compid; - wpr.seq = seq; - mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr); - mavlink_missionlib_send_message(&msg); - _wpm->timestamp_last_send_request = hrt_absolute_time(); - - if (_verbose) { warnx("WPM: Send WAYPOINT_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } - - } else { - mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - - if (_verbose) { warnx("WPM: Send WAYPINT_REQUEST ERROR: seq %u exceeds list capacity", seq); } - } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_item_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send WAYPOINT_REACHED seq %u", wp_reached.seq); } -} - -void Mavlink::mavlink_waypoint_eventloop(uint64_t now) -{ - /* check for timed-out operations */ - if (now - _wpm->timestamp_lastaction > _wpm->action_timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("Operation timeout"); - - if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_partner_sysid = 0; - _wpm->current_partner_compid = 0; - - } else if (now - _wpm->timestamp_last_send_request > _wpm->retry_timeout && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - /* try to get WP again after short timeout */ - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } -} - - -void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) -{ - uint64_t now = hrt_absolute_time(); - - switch (msg->msgid) { - - case MAVLINK_MSG_ID_MISSION_ACK: { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(msg, &wpa); - - if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { - if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (_wpm->current_wp_id == _wpm->size - 1) { - if (_verbose) { warnx("WPM: MISSION_ACK all items sent, switch to MAVLINK_WPM_STATE_IDLE"); } - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_wp_id = 0; - } - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - - if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { - mavlink_mission_set_current_t wpc; - mavlink_msg_mission_set_current_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.seq < _wpm->size) { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT OK (%d)", wpc.seq); } - - _mission.current_index = wpc.seq; - publish_mission(); - - } else { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); } - - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - } - - } else { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } - - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { - mavlink_mission_request_list_t wprl; - mavlink_msg_mission_request_list_decode(msg, &wprl); - - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (_wpm->size > 0) { - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - - } else { - if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST nothing to send, mission is empty"); } - } - - _wpm->current_count = _wpm->size; - mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count); - - } else { - if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } - - mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - - if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) { - _wpm->timestamp_lastaction = now; - - if (wpr.seq >= _wpm->size) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: ignored, requested seq %u was out of bounds [0 %d]", wpr.seq, _wpm->size - 1); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - break; - } - - /* - * Ensure that we are in the correct state and that the first request has id 0 - * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpr.seq == 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: first id != 0 (%d)", wpr.seq); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - break; - } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq == _wpm->current_wp_id) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u (again) from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - } else if (wpr.seq == _wpm->current_wp_id + 1) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, should be %i or %i", wpr.seq, msg->sysid, _wpm->current_wp_id, _wpm->current_wp_id + 1); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - break; - } - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _wpm->current_state); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - break; - } - - _wpm->current_wp_id = wpr.seq; - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - if (wpr.seq < _wpm->size) { - mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bounds", wpr.seq); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - - if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); - break; - } - - if (wpc.count == 0) { - if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } - - mavlink_missionlib_send_gcs_string("WP COUNT 0: CLEAR MISSION"); - break; - } - - if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - - if (_wpm->current_wp_id == 0) { - /* looks like our WAYPOINT_REQUEST was lost, try again */ - if (_verbose) { warnx("WPM: MISSION_COUNT %u (again) from ID %u", wpc.count, msg->sysid); } - - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - - } else { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _wpm->current_wp_id); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - break; - } - - } else { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _wpm->current_state); } - - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - break; - } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - _wpm->current_count = wpc.count; - _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission - - _mission.current_index = -1; - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } - } - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { - _wpm->timestamp_lastaction = now; - - /* Ensure that we are in the correct state and that the first waypoint has id 0 - * and the following waypoints have the correct ids - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (wp.seq != 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq != 0 (%d)", wp.seq); } - break; - } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (wp.seq >= _wpm->current_count) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq %d out of bounds [0 %d]", wp.seq, _wpm->current_count - 1); } - - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - break; - } - - if (wp.seq != _wpm->current_wp_id) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _wpm->current_wp_id); } - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - break; - } - } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - - struct mission_item_s mission_item; - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); - - if (ret != OK) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - dm_item_t dm_item = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - - if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _wpm->current_dataman_id); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - /* waypoint marked as current */ - if (wp.current) { - _mission.current_index = wp.seq; - } - - if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } - - _wpm->current_wp_id = wp.seq + 1; - - if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - /* got all new mission items successfully */ - if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - /* update WPM state */ - _wpm->dataman_id = _wpm->current_dataman_id; - _wpm->size = _wpm->current_count; - - /* update mission topic */ - _mission.dataman_id = _wpm->dataman_id; - _mission.count = _wpm->current_count; - - publish_mission(); - - } else { - /* request next item */ - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { - mavlink_mission_clear_all_t wpca; - mavlink_msg_mission_clear_all_decode(msg, &wpca); - - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - /* clear only active dataman storage, don't change storage id */ - dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - - if (dm_clear(dm_id) == OK) { - if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - /* update WPM state */ - _wpm->timestamp_lastaction = now; - _wpm->size = 0; - - /* update mission topic */ - _mission.dataman_id = _wpm->dataman_id; - _mission.count = 0; - _mission.current_index = -1; - - publish_mission(); - - } else { - if (_verbose) { warnx("WPM: CLEAR_ALL ERROR: can't clear dataman mission storage"); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - } - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - - if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } - } - } - - break; - } - - default: { - /* other messages might should get caught by mavlink and others */ - break; - } - } -} - -void -Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) -{ - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - mavlink_send_uart_bytes(_channel, buf, len); -} - - int -Mavlink::mavlink_missionlib_send_gcs_string(const char *string) +Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; - statustext.severity = MAV_SEVERITY_INFO; + statustext.severity = severity; int i = 0; @@ -1546,7 +908,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) return OK; } else { - return 1; + return ERROR; } } @@ -1684,7 +1046,7 @@ Mavlink::message_buffer_is_empty() bool -Mavlink::message_buffer_write(void *ptr, int size) +Mavlink::message_buffer_write(const void *ptr, int size) { // bytes available to write int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; @@ -1751,7 +1113,7 @@ Mavlink::message_buffer_mark_read(int n) } void -Mavlink::pass_message(mavlink_message_t *msg) +Mavlink::pass_message(const mavlink_message_t *msg) { if (_passing_on) { /* size is 8 bytes plus variable payload */ @@ -1762,6 +1124,11 @@ Mavlink::pass_message(mavlink_message_t *msg) } } +float +Mavlink::get_rate_mult() +{ + return _datarate / 1000.0f; +} int Mavlink::task_main(int argc, char *argv[]) @@ -1879,8 +1246,6 @@ Mavlink::task_main(int argc, char *argv[]) break; } - _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ @@ -1922,11 +1287,11 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - /* initialize waypoint manager */ - mavlink_wpm_init(_wpm); + _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - memset(&_mission_result, 0, sizeof(_mission_result)); + /* create mission manager */ + _mission_manager = new MavlinkMissionManager(this); + _mission_manager->set_verbose(_verbose); _task_running = true; @@ -1941,7 +1306,7 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkCommandsStream commands_stream(this, _channel); /* add default streams depending on mode and intervals depending on datarate */ - float rate_mult = _datarate / 1000.0f; + float rate_mult = get_rate_mult(); configure_stream("HEARTBEAT", 1.0f); @@ -1976,7 +1341,6 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ @@ -2030,36 +1394,16 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - bool updated; - orb_check(mission_result_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &_mission_result); - - if (_verbose) { warnx("WPM: got mission result, new current: %d", _mission_result.index_current_mission); } - - if (_mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)_mission_result.mission_index_reached); - } - - mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); - - } else { - if (slow_rate_limiter.check(t)) { - mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); - } - } - if (fast_rate_limiter.check(t)) { mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(hrt_absolute_time()); + _mission_manager->eventloop(); if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { - mavlink_missionlib_send_gcs_string(msg.text); + send_statustext(msg.text); } } } @@ -2093,11 +1437,11 @@ Mavlink::task_main(int argc, char *argv[]) } } - - perf_end(_loop_perf); } + delete _mission_manager; + delete _subscribe_to_stream; _subscribe_to_stream = nullptr; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5d7273abd2..085a97d730 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -56,51 +56,7 @@ #include "mavlink_orb_subscription.h" #include "mavlink_stream.h" #include "mavlink_messages.h" - -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -#define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds -#define MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - uint16_t size; ///< Count of waypoints in active mission - uint16_t max_size; ///< Maximal count of waypoints - int dataman_id; ///< Dataman storage ID for active mission - enum MAVLINK_WPM_STATES current_state; ///< Current waypoint manager state - int16_t current_wp_id; ///< Waypoint ID in current transmission - uint16_t current_count; ///< Waypoints count in current transmission - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - int current_dataman_id; ///< Dataman storage ID for current transmission - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_last_send_request; - uint32_t action_timeout; - uint32_t retry_timeout; -}; +#include "mavlink_mission.h" class Mavlink @@ -143,7 +99,7 @@ public: static bool instance_exists(const char *device_name, Mavlink *self); - static void forward_message(mavlink_message_t *msg, Mavlink *self); + static void forward_message(const mavlink_message_t *msg, Mavlink *self); static int get_uart_fd(unsigned index); @@ -168,10 +124,6 @@ public: bool get_forwarding_on() { return _forwarding_on; } - /** - * Handle waypoint related messages. - */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); static int start_helper(int argc, char *argv[]); @@ -192,6 +144,10 @@ public: */ int set_hil_enabled(bool hil_enabled); + void send_message(const mavlink_message_t *msg); + + void handle_message(const mavlink_message_t *msg); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); int get_instance_id(); @@ -209,6 +165,10 @@ public: int get_mavlink_fd() { return _mavlink_fd; } + int send_statustext(const char *string); + int send_statustext(enum MAV_SEVERITY severity, const char *string); + + float get_rate_mult(); /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } @@ -238,12 +198,12 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; + MavlinkMissionManager *_mission_manager; + orb_advert_t _mission_pub; - struct mission_s _mission; - struct mission_result_s _mission_result; + int _mission_result_sub; MAVLINK_MODE _mode; - uint8_t _mavlink_wpm_comp_id; mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; @@ -251,10 +211,6 @@ private: pthread_t _receive_thread; - /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; - bool _verbose; bool _forwarding_on; bool _passing_on; @@ -336,20 +292,9 @@ private: void mavlink_update_system(); - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); - void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); - void mavlink_wpm_send_waypoint_current(uint16_t seq); - void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); - void mavlink_wpm_init(mavlink_wpm_storage *state); - int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); - void publish_mission(); + uint8_t get_system_id(); - void mavlink_missionlib_send_message(mavlink_message_t *msg); - int mavlink_missionlib_send_gcs_string(const char *string); + uint8_t get_component_id(); int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); @@ -364,13 +309,13 @@ private: int message_buffer_is_empty(); - bool message_buffer_write(void *ptr, int size); + bool message_buffer_write(const void *ptr, int size); int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); - void pass_message(mavlink_message_t *msg); + void pass_message(const mavlink_message_t *msg); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); @@ -378,5 +323,4 @@ private: * Main mavlink task. */ int task_main(int argc, char *argv[]); - }; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp new file mode 100644 index 0000000000..1eab7d7615 --- /dev/null +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -0,0 +1,823 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_mission.cpp + * MAVLink mission manager implementation. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "mavlink_mission.h" +#include "mavlink_main.h" + +#include +#include +#include +#include + +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + + +MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : + _mavlink(mavlink), + _channel(mavlink->get_channel()), + _comp_id(MAV_COMP_ID_MISSIONPLANNER), + _state(MAVLINK_WPM_STATE_IDLE), + _time_last_recv(0), + _time_last_sent(0), + _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), + _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), + _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), + _count(0), + _current_seq(0), + _dataman_id(0), + _transfer_dataman_id(0), + _transfer_count(0), + _transfer_seq(0), + _transfer_current_seq(0), + _transfer_partner_sysid(0), + _transfer_partner_compid(0), + _offboard_mission_sub(-1), + _mission_result_sub(-1), + _offboard_mission_pub(-1), + _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), + _verbose(false) +{ + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + + init_offboard_mission(); +} + +MavlinkMissionManager::~MavlinkMissionManager() +{ + close(_offboard_mission_pub); + close(_mission_result_sub); +} + +void +MavlinkMissionManager::init_offboard_mission() +{ + mission_s mission_state; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) { + _dataman_id = mission_state.dataman_id; + _count = mission_state.count; + _current_seq = mission_state.current_seq; + + warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq); + + } else { + _dataman_id = 0; + _count = 0; + _current_seq = 0; + warnx("offboard mission init: ERROR, reading mission state failed"); + } +} + +/** + * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes. + */ +int +MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq) +{ + struct mission_s mission; + + mission.dataman_id = dataman_id; + mission.count = count; + mission.current_seq = seq; + + /* update mission state in dataman */ + /* lock MISSION_STATE item */ + dm_lock(DM_KEY_MISSION_STATE); + + int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + + /* unlock MISSION_STATE item */ + dm_unlock(DM_KEY_MISSION_STATE); + + if (res == sizeof(mission_s)) { + /* update active mission state */ + _dataman_id = dataman_id; + _count = count; + _current_seq = seq; + + /* mission state saved successfully, publish offboard_mission topic */ + if (_offboard_mission_pub < 0) { + _offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + + } else { + orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission); + } + + return OK; + + } else { + warnx("ERROR: can't save mission state"); + _mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state"); + + return ERROR; + } +} + +void +MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_mission_ack_t wpa; + + wpa.target_system = sysid; + wpa.target_component = compid; + wpa.type = type; + + mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } +} + + +void +MavlinkMissionManager::send_mission_current(uint16_t seq) +{ + if (seq < _count) { + mavlink_message_t msg; + mavlink_mission_current_t wpc; + + wpc.seq = seq; + + mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); + _mavlink->send_message(&msg); + + } else if (seq == 0 && _count == 0) { + /* don't broadcast if no WPs */ + + } else { + if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); } + + _mavlink->send_statustext("ERROR: wp index out of bounds"); + } +} + + +void +MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + _time_last_sent = hrt_absolute_time(); + + mavlink_message_t msg; + mavlink_mission_count_t wpc; + + wpc.target_system = sysid; + wpc.target_component = compid; + wpc.count = _count; + + mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } +} + + +void +MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + dm_item_t dm_item = _dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + struct mission_item_s mission_item; + + if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + _time_last_sent = hrt_absolute_time(); + + /* create mission_item_s from mavlink_mission_item_t */ + mavlink_mission_item_t wp; + format_mavlink_mission_item(&mission_item, &wp); + + mavlink_message_t msg; + wp.target_system = sysid; + wp.target_component = compid; + wp.seq = seq; + wp.current = (_current_seq == seq) ? 1 : 0; + mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("#audio: Unable to read from micro SD"); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } + } +} + + +void +MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < _max_count) { + _time_last_sent = hrt_absolute_time(); + + mavlink_message_t msg; + mavlink_mission_request_t wpr; + wpr.target_system = sysid; + wpr.target_component = compid; + wpr.seq = seq; + mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } + + } else { + _mavlink->send_statustext("ERROR: Waypoint index exceeds list capacity"); + + if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); } + } +} + + +void +MavlinkMissionManager::send_mission_item_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_mission_item_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } +} + + +void +MavlinkMissionManager::eventloop() +{ + hrt_abstime now = hrt_absolute_time(); + + bool updated = false; + orb_check(_mission_result_sub, &updated); + + if (updated) { + mission_result_s mission_result; + orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result); + + _current_seq = mission_result.seq_current; + + if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); } + + if (mission_result.reached) { + send_mission_item_reached((uint16_t)mission_result.seq_reached); + } + + send_mission_current(_current_seq); + + } else { + if (_slow_rate_limiter.check(now)) { + send_mission_current(_current_seq); + } + } + + /* check for timed-out operations */ + if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) { + _mavlink->send_statustext("Operation timeout"); + + if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); } + + _state = MAVLINK_WPM_STATE_IDLE; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + /* try to request item again after timeout */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + + } else if (_state == MAVLINK_WPM_STATE_SENDLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + if (_transfer_seq == 0) { + /* try to send items count again after timeout */ + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count); + + } else { + /* try to send item again after timeout */ + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1); + } + } +} + + +void +MavlinkMissionManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_MISSION_ACK: + handle_mission_ack(msg); + break; + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: + handle_mission_set_current(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: + handle_mission_request_list(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST: + handle_mission_request(msg); + break; + + case MAVLINK_MSG_ID_MISSION_COUNT: + handle_mission_count(msg); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: + handle_mission_item(msg); + break; + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: + handle_mission_clear_all(msg); + break; + + default: + break; + } +} + + +void +MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) +{ + mavlink_mission_ack_t wpa; + mavlink_msg_mission_ack_decode(msg, &wpa); + + if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == _count) { + if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); } + + } else { + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); } + } + + _state = MAVLINK_WPM_STATE_IDLE; + } + + } else { + _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } + } + } +} + + +void +MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) +{ + mavlink_mission_set_current_t wpc; + mavlink_msg_mission_set_current_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.seq < _count) { + if (update_active_mission(_dataman_id, _count, wpc.seq) == OK) { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } + } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); } + + _mavlink->send_statustext("IGN WP CURR CMD: Not in list"); + } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } + + _mavlink->send_statustext("IGN WP CURR CMD: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) +{ + mavlink_mission_request_list_t wprl; + mavlink_msg_mission_request_list_decode(msg, &wprl); + + if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_count > 0) { + _state = MAVLINK_WPM_STATE_SENDLIST; + _transfer_seq = 0; + _transfer_count = _count; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } + + } else { + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } + } + + send_mission_count(msg->sysid, msg->compid, _count); + + } else { + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } + + _mavlink->send_statustext("IGN REQUEST LIST: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) +{ + mavlink_mission_request_t wpr; + mavlink_msg_mission_request_decode(msg, &wpr); + + if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + /* _transfer_seq contains sequence of expected request */ + if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u", wpr.seq, msg->sysid); } + + _transfer_seq++; + + } else if (wpr.seq == _transfer_seq - 1 && wpr.seq >= 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); } + + } else { + if (_transfer_seq > 0 && _transfer_seq < _transfer_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, _transfer_seq - 1, _transfer_seq); } + + } else if (_transfer_seq <= 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq); } + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); } + } + + _state = MAVLINK_WPM_STATE_IDLE; + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + return; + } + + /* double check bounds in case of items count changed */ + if (wpr.seq >= 0 && wpr.seq < _count) { + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [0, %u]", wpr.seq, msg->sysid, wpr.seq, _count - 1); } + + _state = MAVLINK_WPM_STATE_IDLE; + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); } + + _mavlink->send_statustext("IGN MISSION_ITEM_REQUEST: No transfer"); + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); } + + _mavlink->send_statustext("REJ. WP CMD: Busy"); + } + + } else { + _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } + } + } +} + + +void +MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) +{ + mavlink_mission_count_t wpc; + mavlink_msg_mission_count_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.count > _max_count) { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE); + return; + } + + if (wpc.count == 0) { + if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } + + /* alternate dataman ID anyway to let navigator know about changes */ + update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); + _mavlink->send_statustext("WP COUNT 0: CLEAR MISSION"); + + // TODO send ACK? + return; + } + + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } + + _state = MAVLINK_WPM_STATE_GETLIST; + _transfer_seq = 0; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + _transfer_count = wpc.count; + _transfer_dataman_id = _dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + _transfer_current_seq = -1; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == 0) { + /* looks like our MISSION_REQUEST was lost, try again */ + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); } + + _mavlink->send_statustext("WP CMD OK AGAIN"); + + } else { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); } + + _mavlink->send_statustext("REJ. WP CMD: Busy"); + return; + } + + } else { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); } + + _mavlink->send_statustext("IGN MISSION_COUNT: Busy"); + return; + } + + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } +} + + +void +MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) +{ + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); + + if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) { + if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (wp.seq != _transfer_seq) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); } + + /* don't send request here, it will be performed in eventloop after timeout */ + return; + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); } + + _mavlink->send_statustext("IGN MISSION_ITEM: No transfer"); + return; + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } + + _mavlink->send_statustext("IGN MISSION_ITEM: Busy"); + return; + } + + struct mission_item_s mission_item; + int ret = parse_mavlink_mission_item(&wp, &mission_item); + + if (ret != OK) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); + _state = MAVLINK_WPM_STATE_IDLE; + return; + } + + dm_item_t dm_item = _transfer_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + + if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("#audio: Unable to write on micro SD"); + _state = MAVLINK_WPM_STATE_IDLE; + return; + } + + /* waypoint marked as current */ + if (wp.current) { + _transfer_current_seq = wp.seq; + } + + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } + + _transfer_seq = wp.seq + 1; + + if (_transfer_seq == _transfer_count) { + /* got all new mission items successfully */ + if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } + + _state = MAVLINK_WPM_STATE_IDLE; + + if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + /* request next item */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } + } +} + + +void +MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) +{ + mavlink_mission_clear_all_t wpca; + mavlink_msg_mission_clear_all_decode(msg, &wpca); + + if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { + + if (_state == MAVLINK_WPM_STATE_IDLE) { + /* don't touch mission items storage itself, but only items count in mission state */ + _time_last_recv = hrt_absolute_time(); + + if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) { + if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + _mavlink->send_statustext("IGN WP CLEAR CMD: Busy"); + + if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } + } + } +} + +int +MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +{ + /* only support global waypoints for now */ + switch (mavlink_mission_item->frame) { + case MAV_FRAME_GLOBAL: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = false; + break; + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = true; + break; + + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_ENU: + return MAV_MISSION_UNSUPPORTED_FRAME; + + case MAV_FRAME_MISSION: + default: + return MAV_MISSION_ERROR; + } + + switch (mavlink_mission_item->command) { + case MAV_CMD_NAV_TAKEOFF: + mission_item->pitch_min = mavlink_mission_item->param1; + break; + case MAV_CMD_DO_JUMP: + mission_item->do_jump_mission_index = mavlink_mission_item->param1; + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; + break; + default: + mission_item->acceptance_radius = mavlink_mission_item->param2; + mission_item->time_inside = mavlink_mission_item->param1; + break; + } + + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + + mission_item->autocontinue = mavlink_mission_item->autocontinue; + // mission_item->index = mavlink_mission_item->seq; + mission_item->origin = ORIGIN_MAVLINK; + + /* reset DO_JUMP count */ + mission_item->do_jump_current_count = 0; + + return MAV_MISSION_ACCEPTED; +} + + +int +MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) +{ + if (mission_item->altitude_is_relative) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + + switch (mission_item->nav_cmd) { + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param1 = mission_item->pitch_min; + break; + + case NAV_CMD_DO_JUMP: + mavlink_mission_item->param1 = mission_item->do_jump_mission_index; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + break; + + default: + mavlink_mission_item->param2 = mission_item->acceptance_radius; + mavlink_mission_item->param1 = mission_item->time_inside; + break; + } + + mavlink_mission_item->x = (float)mission_item->lat; + mavlink_mission_item->y = (float)mission_item->lon; + mavlink_mission_item->z = mission_item->altitude; + + mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; + mavlink_mission_item->command = mission_item->nav_cmd; + mavlink_mission_item->autocontinue = mission_item->autocontinue; + // mavlink_mission_item->seq = mission_item->index; + + return OK; +} diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h new file mode 100644 index 0000000000..d8695cf83d --- /dev/null +++ b/src/modules/mavlink/mavlink_mission.h @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_mission.h + * MAVLink mission manager interface definition. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "mavlink_bridge_header.h" +#include "mavlink_rate_limiter.h" +#include + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + +#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds +#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds + +class Mavlink; + +class MavlinkMissionManager { +public: + MavlinkMissionManager(Mavlink *parent); + + ~MavlinkMissionManager(); + + void init_offboard_mission(); + + int update_active_mission(int dataman_id, unsigned count, int seq); + + void send_message(mavlink_message_t *msg); + + /** + * @brief Sends an waypoint ack message + */ + void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type); + + /** + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ + void send_mission_current(uint16_t seq); + + void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count); + + void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq); + + void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq); + + /** + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ + void send_mission_item_reached(uint16_t seq); + + void eventloop(); + + void handle_message(const mavlink_message_t *msg); + + void handle_mission_ack(const mavlink_message_t *msg); + + void handle_mission_set_current(const mavlink_message_t *msg); + + void handle_mission_request_list(const mavlink_message_t *msg); + + void handle_mission_request(const mavlink_message_t *msg); + + void handle_mission_count(const mavlink_message_t *msg); + + void handle_mission_item(const mavlink_message_t *msg); + + void handle_mission_clear_all(const mavlink_message_t *msg); + + /** + * Parse mavlink MISSION_ITEM message to get mission_item_s. + */ + int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); + + /** + * Format mission_item_s as mavlink MISSION_ITEM message. + */ + int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); + + void set_verbose(bool v) { _verbose = v; } + +private: + Mavlink* _mavlink; + mavlink_channel_t _channel; + uint8_t _comp_id; + + enum MAVLINK_WPM_STATES _state; ///< Current state + + uint64_t _time_last_recv; + uint64_t _time_last_sent; + + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximal count of mission items + + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission + + int _transfer_dataman_id; ///< Dataman storage ID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + int _transfer_seq; ///< Item sequence in current transmission + int _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission + uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission + + int _offboard_mission_sub; + int _mission_result_sub; + orb_advert_t _offboard_mission_pub; + + MavlinkRateLimiter _slow_rate_limiter; + + bool _verbose; +}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1e217ec703..834852d6f8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -79,7 +79,6 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" #include "mavlink_receiver.h" #include "mavlink_main.h" -#include "util.h" __END_DECLS @@ -902,16 +901,8 @@ MavlinkReceiver::receive_thread(void *arg) /* handle generic messages and commands */ handle_message(&msg); - /* handle packet with waypoint component */ - _mavlink->mavlink_wpm_message_handler(&msg); - - /* handle packet with parameter component */ - _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); - - if (_mavlink->get_forwarding_on()) { - /* forward any messages to other mavlink instances */ - Mavlink::forward_message(&msg, _mavlink); - } + /* handle packet with parent object */ + _mavlink->handle_message(&msg); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a3..df5d037f81 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -100,8 +100,6 @@ public: static void *start_helper(void *context); private: - perf_counter_t _loop_perf; /**< loop performance counter */ - Mavlink *_mavlink; void handle_message(mavlink_message_t *msg); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f532e26fea..725a9e184d 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,6 +39,7 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ + mavlink_mission.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h deleted file mode 100644 index 5ca9a085d5..0000000000 --- a/src/modules/mavlink/util.h +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file util.h - * Utility and helper functions and data. - */ - -#pragma once - -/** MAVLink communications channel */ -extern uint8_t chan; - -/** Shutdown marker */ -extern volatile bool thread_should_exit; - -/** Waypoint storage */ -extern mavlink_wpm_storage *wpm; - -/** - * Translate the custom state into standard mavlink modes and state. - */ -extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h deleted file mode 100644 index a2109f2d85..0000000000 --- a/src/modules/mavlink/waypoints.h +++ /dev/null @@ -1,111 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2009-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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file waypoints.h - * MAVLink waypoint protocol definition (BSD-relicensed). - * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - */ - -#ifndef WAYPOINTS_H_ -#define WAYPOINTS_H_ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - -#include -#include "mavlink_bridge_header.h" -#include -#include - -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -#define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - uint16_t size; - uint16_t max_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint32_t action_timeout; - int current_dataman_id; -}; - -typedef struct mavlink_wpm_storage mavlink_wpm_storage; - -int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); -int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - - -void mavlink_wpm_init(mavlink_wpm_storage *state); -void mavlink_waypoint_eventloop(uint64_t now); -void mavlink_wpm_message_handler(const mavlink_message_t *msg); - -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); - -static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; - -#endif /* WAYPOINTS_H_ */ diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index d9d8353f65..f6d304c373 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -67,13 +67,11 @@ Mission::Mission(Navigator *navigator, const char *name) : _current_offboard_mission_index(-1), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE) + _mission_type(MISSION_TYPE_NONE), + _inited(false) { /* load initial params */ updateParams(); - /* set initial mission items */ - on_inactive(); - } Mission::~Mission() @@ -85,16 +83,25 @@ Mission::on_inactive() { _first_run = true; - /* check anyway if missions have changed so that feedback to groundstation is given */ - bool onboard_updated; - orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); - if (onboard_updated) { - update_onboard_mission(); - } + if (_inited) { + /* check if missions have changed so that feedback to ground station is given */ + bool onboard_updated = false; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } - bool offboard_updated; - orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); - if (offboard_updated) { + bool offboard_updated = false; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } + + } else { + /* read mission topics on initialization */ + _inited = true; + + update_onboard_mission(); update_offboard_mission(); } } @@ -105,13 +112,13 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) bool updated = false; /* check if anything has changed */ - bool onboard_updated; + bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); if (onboard_updated) { update_onboard_mission(); } - bool offboard_updated; + bool offboard_updated = false; orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); if (offboard_updated) { update_offboard_mission(); @@ -139,9 +146,9 @@ Mission::update_onboard_mission() { if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { /* accept the current index set by the onboard mission if it is within bounds */ - if (_onboard_mission.current_index >=0 - && _onboard_mission.current_index < (int)_onboard_mission.count) { - _current_onboard_mission_index = _onboard_mission.current_index; + if (_onboard_mission.current_seq >=0 + && _onboard_mission.current_seq < (int)_onboard_mission.count) { + _current_onboard_mission_index = _onboard_mission.current_seq; } else { /* if less WPs available, reset to first WP */ if (_current_onboard_mission_index >= (int)_onboard_mission.count) { @@ -154,7 +161,7 @@ Mission::update_onboard_mission() } } else { _onboard_mission.count = 0; - _onboard_mission.current_index = 0; + _onboard_mission.current_seq = 0; _current_onboard_mission_index = 0; } } @@ -163,13 +170,12 @@ void Mission::update_offboard_mission() { if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { - + warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq); /* determine current index */ - if (_offboard_mission.current_index >= 0 - && _offboard_mission.current_index < (int)_offboard_mission.count) { - _current_offboard_mission_index = _offboard_mission.current_index; + if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) { + _current_offboard_mission_index = _offboard_mission.current_seq; } else { - /* if less WPs available, reset to first WP */ + /* if less items available, reset to first item */ if (_current_offboard_mission_index >= (int)_offboard_mission.count) { _current_offboard_mission_index = 0; /* if not initialized, set it to 0 */ @@ -194,10 +200,12 @@ Mission::update_offboard_mission() _navigator->get_geofence(), _navigator->get_home_position()->alt); } else { + warnx("offboard mission update failed"); _offboard_mission.count = 0; - _offboard_mission.current_index = 0; + _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; } + report_current_offboard_mission_item(); } @@ -326,10 +334,10 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren void Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) { - int next_temp_mission_index = _onboard_mission.current_index + 1; + int next_temp_mission_index = _onboard_mission.current_seq + 1; /* try if there is a next onboard mission */ - if (_onboard_mission.current_index >= 0 && + if (_onboard_mission.current_seq >= 0 && next_temp_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { @@ -349,9 +357,9 @@ void Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) { /* try if there is a next offboard mission */ - int next_temp_mission_index = _offboard_mission.current_index + 1; + int next_temp_mission_index = _offboard_mission.current_seq + 1; warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); - if (_offboard_mission.current_index >= 0 && + if (_offboard_mission.current_seq >= 0 && next_temp_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { @@ -441,19 +449,31 @@ Mission::save_offboard_mission_state() /* read current state */ int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); - /* check if state actually changed to save flash write cycles */ - if (read_res != sizeof(mission_s) || mission_state.dataman_id != _offboard_mission.dataman_id || - mission_state.count != _offboard_mission.count || - mission_state.current_index != _current_offboard_mission_index) { + if (read_res == sizeof(mission_s)) { + /* data read successfully, check dataman ID and items count */ + if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) { + /* navigator may modify only sequence, write modified state only if it changed */ + if (mission_state.current_seq != _current_offboard_mission_index) { + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + warnx("ERROR: can't save mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state"); + } + } + } + } else { + /* invalid data, this must not happen and indicates error in offboard_mission publisher */ mission_state.dataman_id = _offboard_mission.dataman_id; mission_state.count = _offboard_mission.count; - mission_state.current_index = _current_offboard_mission_index; + mission_state.current_seq = _current_offboard_mission_index; - /* write modifyed state only if changed */ + warnx("ERROR: invalid mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state"); + + /* write modified state only if changed */ if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "error saving mission state"); - + warnx("ERROR: can't save mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state"); } } @@ -465,8 +485,8 @@ void Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; + _mission_result.reached = true; + _mission_result.seq_reached = _current_offboard_mission_index; } publish_mission_result(); } @@ -474,7 +494,8 @@ Mission::report_mission_item_reached() void Mission::report_current_offboard_mission_item() { - _mission_result.index_current_mission = _current_offboard_mission_index; + warnx("current offboard mission index: %d", _current_offboard_mission_index); + _mission_result.seq_current = _current_offboard_mission_index; publish_mission_result(); save_offboard_mission_state(); @@ -483,7 +504,7 @@ Mission::report_current_offboard_mission_item() void Mission::report_mission_finished() { - _mission_result.mission_finished = true; + _mission_result.finished = true; publish_mission_result(); } @@ -500,6 +521,6 @@ Mission::publish_mission_result() _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } /* reset reached bool */ - _mission_result.mission_reached = false; - _mission_result.mission_finished = false; + _mission_result.reached = false; + _mission_result.finished = false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 98b11bade2..1310671b06 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -183,6 +183,8 @@ private: MISSION_TYPE_OFFBOARD } _mission_type; + bool _inited; + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 4bcaaaa5a0..499b2e1e54 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -97,11 +97,15 @@ struct mission_item_s { unsigned do_jump_current_count; /**< count how many times the jump has been done */ }; +/** + * This topic used to notify navigator about mission changes, mission itself and new mission state + * must be stored in dataman before publication. + */ struct mission_s { int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ unsigned count; /**< count of the missions stored in the dataman */ - int current_index; /**< default -1, start at the one changed latest */ + int current_seq; /**< default -1, start at the one changed latest */ }; /** diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index ad654a9ff3..beb797e624 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -53,10 +53,10 @@ struct mission_result_s { - bool mission_reached; /**< true if mission has been reached */ - unsigned mission_index_reached; /**< index of the mission which has been reached */ - unsigned index_current_mission; /**< index of the current mission */ - bool mission_finished; /**< true if mission has been completed */ + unsigned seq_reached; /**< Sequence of the mission item which has been reached */ + unsigned seq_current; /**< Sequence of the current mission item */ + bool reached; /**< true if mission has been reached */ + bool finished; /**< true if mission has been completed */ }; /** From da2f68a6a09395a8d02a5d39bd3e92d7d0d79911 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Jun 2014 14:25:29 +0200 Subject: [PATCH 087/600] mavlink: don't lock dataman when updating mission state --- src/modules/mavlink/mavlink_mission.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 9bb956281e..b2288469c9 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -128,14 +128,8 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int mission.current_seq = seq; /* update mission state in dataman */ - /* lock MISSION_STATE item */ - dm_lock(DM_KEY_MISSION_STATE); - int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); - /* unlock MISSION_STATE item */ - dm_unlock(DM_KEY_MISSION_STATE); - if (res == sizeof(mission_s)) { /* update active mission state */ _dataman_id = dataman_id; From 78b7ba33a5f04a11ac859aecfb72ff34d75cb7ff Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Sun, 22 Jun 2014 09:53:33 +0800 Subject: [PATCH 088/600] Fix to allow filter correction with vision position estimate --- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 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 9ab21ee38c..a6a4db3eb2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -781,8 +781,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) /* use GPS if it's valid and reference position initialized */ - bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; - bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; + bool use_gps_xy = (ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W) || vision_valid; + bool use_gps_z = (ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W) || vision_valid; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); From be33b4b6a56402135e5cfb8b38e0cc0ae8b7d673 Mon Sep 17 00:00:00 2001 From: holger Date: Tue, 24 Jun 2014 19:28:39 +0200 Subject: [PATCH 089/600] UAVCAN: append to EXTRADEFINES to those given by make cmd line --- src/modules/uavcan/module.mk | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index ce9f981a88..5ac7019e3b 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -52,7 +52,7 @@ SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. -EXTRADEFINES += -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 # # libuavcan drivers for STM32 @@ -60,8 +60,7 @@ EXTRADEFINES += -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) -EXTRADEFINES += -DUAVCAN_STM32_NUTTX \ - -DUAVCAN_STM32_NUM_IFACES=2 +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 # # Invoke DSDL compiler From 72071cdcd39cae6be8533e67d01cbc7533ca6cb1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 19:22:22 +0200 Subject: [PATCH 090/600] Fix failed merge --- src/modules/mavlink/mavlink_main.h | 54 ------------------------------ 1 file changed, 54 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 18141972a0..91d79057c3 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -138,14 +138,6 @@ public: bool get_forwarding_on() { return _forwarding_on; } -<<<<<<< HEAD -======= - /** - * Handle waypoint related messages. - */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 - static int start_helper(int argc, char *argv[]); /** @@ -165,15 +157,11 @@ public: */ int set_hil_enabled(bool hil_enabled); -<<<<<<< HEAD void send_message(const mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg); MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); -======= - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 int get_instance_id(); @@ -228,7 +216,6 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; -<<<<<<< HEAD MavlinkMissionManager *_mission_manager; orb_advert_t _mission_pub; @@ -236,32 +223,12 @@ private: MAVLINK_MODE _mode; mavlink_channel_t _channel; -======= - orb_advert_t _mission_pub; - struct mission_s mission; - MAVLINK_MODE _mode; - - uint8_t _mavlink_wpm_comp_id; - mavlink_channel_t _channel; ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; pthread_t _receive_thread; -<<<<<<< HEAD - bool _verbose; - bool _forwarding_on; - bool _passing_on; - int _uart_fd; - int _baudrate; - int _datarate; -======= - /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; - bool _verbose; bool _forwarding_on; bool _passing_on; @@ -269,7 +236,6 @@ private: int _uart_fd; int _baudrate; int _datarate; ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 /** * If the queue index is not at 0, the queue sending @@ -347,26 +313,9 @@ private: void mavlink_update_system(); -<<<<<<< HEAD uint8_t get_system_id(); uint8_t get_component_id(); -======= - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); - void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); - void mavlink_wpm_send_waypoint_current(uint16_t seq); - void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); - void mavlink_wpm_init(mavlink_wpm_storage *state); - int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); - void publish_mission(); - - void mavlink_missionlib_send_message(mavlink_message_t *msg); - int mavlink_missionlib_send_gcs_string(const char *string); ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); @@ -381,11 +330,8 @@ private: int message_buffer_is_empty(); -<<<<<<< HEAD bool message_buffer_write(const void *ptr, int size); -======= ->>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); From 0a159e1a2490e5ec7f368d938e2b0fd365d2731e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 12:31:29 +0200 Subject: [PATCH 091/600] mavlink, commander: bugs fixed --- src/modules/commander/commander.cpp | 2 +- src/modules/mavlink/mavlink_main.h | 10 ++-------- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0ff1195c94..bba14ea27d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1407,7 +1407,7 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, - mission_result.mission_finished); + mission_result.finished); // TODO handle mode changes by commands if (main_state_changed) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b4184b21c2..67ef8d00f5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -182,7 +182,7 @@ public: int send_statustext(const char *string); int send_statustext(enum MAV_SEVERITY severity, const char *string); - MavlinkStream * get_streams() { return _streams; } const + MavlinkStream * get_streams() const { return _streams; } float get_rate_mult(); @@ -193,7 +193,7 @@ public: bool get_wait_to_transmit() { return _wait_to_transmit; } bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } - bool message_buffer_write(void *ptr, int size); + bool message_buffer_write(const void *ptr, int size); void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } @@ -322,10 +322,6 @@ private: void mavlink_update_system(); - uint8_t get_system_id(); - - uint8_t get_component_id(); - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int configure_stream(const char *stream_name, const float rate); @@ -338,8 +334,6 @@ private: int message_buffer_is_empty(); - bool message_buffer_write(const void *ptr, int size); - int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); From a1655bb8c4a7f2201887a52de60a519c029a7505 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 12:42:26 +0200 Subject: [PATCH 092/600] rcS: start dataman before commander --- ROMFS/px4fmu_common/init.d/rcS | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 983a6a38f5..b4e90c9602 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -275,6 +275,11 @@ then # Try to get an USB console nshterm /dev/ttyACM0 & + # + # Start the datamanager + # + dataman start + # # Start the Commander (needs to be this early for in-air-restarts) # @@ -402,11 +407,6 @@ then fi fi - # - # Start the datamanager - # - dataman start - # # MAVLink # From f0cfa04b852212cd0be4bc1bffbc4eece54ce659 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 20:55:02 +0200 Subject: [PATCH 093/600] mtecs blocks: fix warning --- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index e4e405227e..4f18d36e86 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -72,8 +72,8 @@ public: * @return: true if the limit is applied, false otherwise */ bool limit(float& value, float& difference) { - float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); - float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); + float minimum = getIsAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); + float maximum = getIsAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); if (value < minimum) { difference = value - minimum; value = minimum; @@ -86,7 +86,7 @@ public: return false; } //accessor: - bool isAngularLimit() {return _isAngularLimit ;} + bool getIsAngularLimit() {return _isAngularLimit ;} float getMin() { return _min.get(); } float getMax() { return _max.get(); } void setMin(float value) { _min.set(value); } From 57827563b9b3b8771be28d580842d4fbcf084918 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:11:05 +0200 Subject: [PATCH 094/600] fw pos ctrl l1 main: fix warning --- .../fw_pos_control_l1_main.cpp | 54 +++++++++---------- 1 file changed, 26 insertions(+), 28 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 000c02e3da..e877f0e842 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -414,25 +414,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _attitude_sp_pub(-1), _nav_capabilities_pub(-1), -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), - /* states */ - _loiter_hold(false), - land_noreturn_horizontal(false), - land_noreturn_vertical(false), - land_stayonground(false), - land_motor_lim(false), - land_onslope(false), - launch_detected(false), - usePreTakeoffThrust(false), - last_manual(false), - flare_curve_alt_rel_last(0.0f), - launchDetector(), - _airspeed_error(0.0f), - _airspeed_valid(false), - _groundspeed_undershoot(0.0f), - _global_pos_valid(false), _att(), _att_sp(), _nav_capabilities(), @@ -442,9 +424,32 @@ FixedwingPositionControl::FixedwingPositionControl() : _global_pos(), _pos_sp_triplet(), _sensor_combined(), + _range_finder(), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), + + _loiter_hold(false), + _launch_valid(false), + land_noreturn_horizontal(false), + land_noreturn_vertical(false), + land_stayonground(false), + land_motor_lim(false), + land_onslope(false), + launch_detected(false), + usePreTakeoffThrust(false), + last_manual(false), + landingslope(), + flare_curve_alt_rel_last(0.0f), + launchDetector(), + _airspeed_error(0.0f), + _airspeed_valid(false), + _airspeed_last_valid(0), + _groundspeed_undershoot(0.0f), + _global_pos_valid(false), + _l1_control(), _mTecs(), - _was_pos_control_mode(false), - _range_finder() + _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; @@ -806,13 +811,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements - // XXX re-visit - float baro_altitude = _global_pos.alt; - - /* filter speed and altitude for controller */ - math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); - math::Vector<3> accel_earth = _R_nb * accel_body; - + /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ @@ -945,7 +944,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_approach = 1.3f * _parameters.airspeed_min; /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); From 02db53cea57d1e2c643b6efc1c357afa3e6be20e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:13:29 +0200 Subject: [PATCH 095/600] fw pos ctrl l1 main: initialize all subscriptions --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e877f0e842..5ec49b6f83 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -129,7 +129,6 @@ private: int _global_pos_sub; int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ - int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _control_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -408,6 +407,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _sensor_combined_sub(-1), _range_finder_sub(-1), /* publications */ From 173da25c9a55185377870f703cd751fd44b902f0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:16:32 +0200 Subject: [PATCH 096/600] fw pos ctrl l1 main: remove unused modes --- .../fw_pos_control_l1_main.cpp | 83 ------------------- 1 file changed, 83 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5ec49b6f83..fc0d588d2a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1133,89 +1133,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* posctrl mode enabled */) { - - _was_pos_control_mode = false; - - /** POSCTRL FLIGHT **/ - - if (0/* switched from another mode to posctrl */) { - _altctrl_hold_heading = _att.yaw; - } - - if (0/* posctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.r; - } - - //XXX not used - - /* climb out control */ -// bool climb_out = false; -// -// /* user wants to climb out */ -// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { -// climb_out = true; -// } - - /* if in altctrl mode, set airspeed based on manual control */ - - // XXX check if ground speed undershoot should be applied here - float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; - - _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - - } else if (0/* altctrl mode enabled */) { - - _was_pos_control_mode = false; - - /** ALTCTRL FLIGHT **/ - - if (0/* switched from another mode to altctrl */) { - _altctrl_hold_heading = _att.yaw; - } - - if (0/* altctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.r; - } - - /* if in altctrl mode, set airspeed based on manual control */ - - // XXX check if ground speed undershoot should be applied here - float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; - - /* user switched off throttle */ - if (_manual.z < 0.1f) { - throttle_max = 0.0f; - } - - /* climb out control */ - bool climb_out = false; - - /* user wants to climb out */ - if (_manual.x > 0.3f && _manual.z > 0.8f) { - climb_out = true; - } - - _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); - _att_sp.roll_body = _manual.y; - _att_sp.yaw_body = _manual.r; - tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - climb_out, math::radians(_parameters.pitch_limit_min), - _global_pos.alt, ground_speed); - } else { _was_pos_control_mode = false; From 62a92542891d32d88c1993e03da9a17d59f70d04 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:22:56 +0200 Subject: [PATCH 097/600] fw pos ctrl l1 main: remove several unused vars --- .../fw_pos_control_l1_main.cpp | 30 ------------------- 1 file changed, 30 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index fc0d588d2a..445abf23e3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -152,18 +152,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - /** manual control states */ - float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ - double _loiter_hold_lat; - double _loiter_hold_lon; - float _loiter_hold_alt; - bool _loiter_hold; - - double _launch_lat; - double _launch_lon; - float _launch_alt; - bool _launch_valid; - /* land states */ /* not in non-abort mode for landing yet */ bool land_noreturn_horizontal; @@ -429,8 +417,6 @@ FixedwingPositionControl::FixedwingPositionControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), - _loiter_hold(false), - _launch_valid(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), @@ -609,13 +595,6 @@ FixedwingPositionControl::vehicle_control_mode_poll() bool was_armed = _control_mode.flag_armed; orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); - - if (!was_armed && _control_mode.flag_armed) { - _launch_lat = _global_pos.lat; - _launch_lon = _global_pos.lon; - _launch_alt = _global_pos.alt; - _launch_valid = true; - } } } @@ -1109,15 +1088,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), - // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); - // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1), - // (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid"); - - // XXX at this point we always want no loiter hold if a - // mission is active - _loiter_hold = false; - /* reset landing state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { reset_landing_state(); From 527ce1048a27f0d15e99a4ba899897c06a8cb3f2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:25:18 +0200 Subject: [PATCH 098/600] fw pos ctrl l1 main: init target_bearing --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 445abf23e3..56fe65f9f2 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -168,8 +168,8 @@ private: /* Landingslope object */ Landingslope landingslope; - float flare_curve_alt_rel_last; + /* heading hold */ float target_bearing; @@ -427,6 +427,7 @@ FixedwingPositionControl::FixedwingPositionControl() : last_manual(false), landingslope(), flare_curve_alt_rel_last(0.0f), + target_bearing(0.0f), launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), From c782d5d3796f9f936c30de2b0395ba76b42b8795 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:27:05 +0200 Subject: [PATCH 099/600] fw pos ctrl l1 main: remove never used var --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 56fe65f9f2..38d275eb06 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -593,8 +593,6 @@ FixedwingPositionControl::vehicle_control_mode_poll() if (vstatus_updated) { - bool was_armed = _control_mode.flag_armed; - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } } From acca14673c4934ea8cdca46286db6d769c4dca40 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Jun 2014 21:31:50 +0200 Subject: [PATCH 100/600] fw pos ctrl l1 main: remove code with no effect --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 38d275eb06..d072580943 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -592,7 +592,6 @@ FixedwingPositionControl::vehicle_control_mode_poll() orb_check(_control_mode_sub, &vstatus_updated); if (vstatus_updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } } @@ -1217,14 +1216,6 @@ FixedwingPositionControl::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } - static uint64_t last_run = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* guard against too large deltaT's */ - if (deltaT > 1.0f) - deltaT = 0.01f; - /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); From 34c13df3dd6edb68921b9cdc34f2d87ac4251e12 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:50:48 +0200 Subject: [PATCH 101/600] geo: Fix a series of warnings, some which actually pointed to real issues --- src/lib/geo/geo.c | 23 +++++++++++------------ src/lib/geo/geo.h | 6 +++--- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index e03ec65c4d..36be32158f 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved. + * 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 @@ -49,14 +49,18 @@ #include #include #include +#include + +#include +#include /* * Azimuthal Equidistant Projection * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -static struct map_projection_reference_s mp_ref = {0}; -static struct globallocal_converter_reference_s gl_ref = {0}; +static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0}; +static struct globallocal_converter_reference_s gl_ref = {0.0f, false}; __EXPORT bool map_projection_global_initialized() { @@ -80,7 +84,7 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - if (strcmp("commander", getprogname() == 0)) { + if (strcmp("commander", getprogname()) == 0) { return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } else { return -1; @@ -103,7 +107,7 @@ __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s * __EXPORT int 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 { - map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); + return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); } __EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad) @@ -153,22 +157,17 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref __EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon) { - map_projection_reproject(&mp_ref, x, y, lat, lon); + return map_projection_reproject(&mp_ref, x, y, lat, lon); } __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { -<<<<<<< HEAD if (!map_projection_initialized(ref)) { return -1; } - float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; - float y_rad = y / CONSTANTS_RADIUS_OF_EARTH; -======= double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; ->>>>>>> 48f4a1e5cd6ef653b466eb68c1073fb47cbefbd7 double c = sqrtf(x_rad * x_rad + y_rad * y_rad); double sin_c = sin(c); double cos_c = cos(c); @@ -210,7 +209,7 @@ __EXPORT int map_projection_global_getref(double *lat_0, double *lon_0) } __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp) { - if (strcmp("commander", getprogname() == 0)) { + if (strcmp("commander", getprogname()) == 0) { gl_ref.alt = alt_0; if (!map_projection_global_init(lat_0, lon_0, timestamp)) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index fbcfe1c2dc..2311e0a7c9 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -86,7 +86,7 @@ struct globallocal_converter_reference_s { * Checks if global projection was initialized * @return true if map was initialized before, false else */ -__EXPORT bool map_projection_global_initialized(); +__EXPORT bool map_projection_global_initialized(void); /** * Checks if projection given as argument was initialized @@ -98,7 +98,7 @@ __EXPORT bool map_projection_initialized(const struct map_projection_reference_s * Get the timestamp of the global map projection * @return the timestamp of the map_projection */ -__EXPORT uint64_t map_projection_global_timestamp(); +__EXPORT uint64_t map_projection_global_timestamp(void); /** * Get the timestamp of the map projection given by the argument @@ -209,7 +209,7 @@ __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, * Checks if globallocalconverter was initialized * @return true if map was initialized before, false else */ -__EXPORT bool globallocalconverter_initialized(); +__EXPORT bool globallocalconverter_initialized(void); /** * Convert from global position coordinates to local position coordinates using the global reference From 801d1d31983d404d5ebe8f5750359f2d8c7fdf43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:51:35 +0200 Subject: [PATCH 102/600] commander: Update after merge --- src/modules/commander/commander.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 326a3e1f35..d7425cbb47 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -826,8 +826,8 @@ int commander_thread_main(int argc, char *argv[]) int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); - gps_position.eph_m = FLT_MAX; - gps_position.epv_m = FLT_MAX; + gps_position.eph = FLT_MAX; + gps_position.epv = FLT_MAX; /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -1261,8 +1261,8 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() - && (gps_position.eph_m < eph_epv_threshold) - && (gps_position.epv_m < eph_epv_threshold) + && (gps_position.eph < eph_epv_threshold) + && (gps_position.epv < eph_epv_threshold) && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); From 8acbe6d5b6770e92fdcb86ba268492217d3e26bd Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Tue, 1 Jul 2014 14:08:59 -0700 Subject: [PATCH 103/600] Added class to convert gnss message from uavcan to uorb --- src/modules/uavcan/gnss_receiver.cpp | 122 +++++++++++++++++++++++++++ src/modules/uavcan/gnss_receiver.hpp | 86 +++++++++++++++++++ src/modules/uavcan/uavcan_main.cpp | 7 +- src/modules/uavcan/uavcan_main.hpp | 2 + 4 files changed, 216 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/gnss_receiver.cpp create mode 100644 src/modules/uavcan/gnss_receiver.hpp diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp new file mode 100644 index 0000000000..e924683330 --- /dev/null +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gnss_receiver.cpp + * + * @author Pavel Kirienko + * @author Andrew Chambers + * + */ + +#include "gnss_receiver.hpp" +#include + +#define MM_PER_CM 10 // Millimeters per centimeter + +UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : + _node(node), + _uavcan_sub_status(node), + _report_pub(-1) +{ +} + +int UavcanGnssReceiver::init() +{ + int res = -1; + + // GNSS fix subscription + res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb)); + if (res < 0) + { + warnx("GNSS fix sub failed %i", res); + return res; + } + + // Clear the uORB GPS report + memset(&_report, 0, sizeof(_report)); + + return res; +} + +void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + _report.timestamp_position = hrt_absolute_time(); + _report.lat = msg.lat_1e7; + _report.lon = msg.lon_1e7; + _report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) + + _report.timestamp_variance = _report.timestamp_position; + _report.s_variance_m_s = msg.velocity_covariance[0] + msg.velocity_covariance[4] + msg.velocity_covariance[8]; + _report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4]; + + /* Use Jacobian to transform velocity covariance to heading covariance + * heading = atan2(vel_e_m_s, vel_n_m_s) + * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative + * + * To calculate the variance of heading from the variance of velocity, + * var(heading) = J(velocity)*var(velocity)*J(velocity)^T + */ + _report.c_variance_rad = + msg.ned_velocity[1] * msg.ned_velocity[1] * msg.velocity_covariance[0] + + -2*msg.ned_velocity[1] * msg.ned_velocity[0] * msg.velocity_covariance[1] + + msg.ned_velocity[0] * msg.ned_velocity[0] * msg.velocity_covariance[4]; + + _report.fix_type = msg.status; + + _report.eph_m = sqrtf(_report.p_variance_m); + _report.epv_m = sqrtf(msg.position_covariance[8]); + + _report.timestamp_velocity = _report.timestamp_position; + _report.vel_n_m_s = msg.ned_velocity[0]; + _report.vel_e_m_s = msg.ned_velocity[1]; + _report.vel_d_m_s = msg.ned_velocity[2]; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s); + _report.vel_ned_valid = true; + + _report.timestamp_time = _report.timestamp_position; + _report.time_gps_usec = msg.timestamp.husec * msg.timestamp.USEC_PER_LSB; // Convert to microseconds + + _report.timestamp_satellites = _report.timestamp_position; + _report.satellites_visible = msg.sats_used; + _report.satellite_info_available = 0; // Set to 0 for no info available + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + +} diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/gnss_receiver.hpp new file mode 100644 index 0000000000..abb8a821af --- /dev/null +++ b/src/modules/uavcan/gnss_receiver.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gnss_receiver.hpp + * + * UAVCAN <--> ORB bridge for ESC messages: + * uavcan.equipment.esc.RawCommand + * uavcan.equipment.esc.RPMCommand + * uavcan.equipment.esc.Status + * + * @author Pavel Kirienko + * @author Andrew Chambers + */ + +#pragma once + +#include + +#include +#include + +#include +#include + +class UavcanGnssReceiver +{ +public: + UavcanGnssReceiver(uavcan::INode& node); + + int init(); + +private: + /** + * GNSS fix message will be reported via this callback. + */ + void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); + + + typedef uavcan::MethodBinder&)> + FixCbBinder; + + /* + * libuavcan related things + */ + uavcan::INode &_node; + uavcan::Subscriber _uavcan_sub_status; + + /* + * uORB + */ + struct vehicle_gps_position_s _report; ///< uORB topic for gnss position + orb_advert_t _report_pub; ///< uORB pub for gnss position + +}; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ab687a6b9d..5bc4376709 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -63,7 +63,8 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), - _esc_controller(_node) + _esc_controller(_node), + _gnss_receiver(_node) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -186,6 +187,10 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret < 0) return ret; + ret = _gnss_receiver.init(); + if (ret < 0) + return ret; + uavcan::protocol::SoftwareVersion swver; swver.major = 12; // TODO fill version info swver.minor = 34; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 751a94a8a7..126d441377 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -43,6 +43,7 @@ #include #include "esc_controller.hpp" +#include "gnss_receiver.hpp" /** * @file uavcan_main.hpp @@ -108,6 +109,7 @@ private: static UavcanNode *_instance; ///< singleton pointer Node _node; ///< library instance UavcanEscController _esc_controller; + UavcanGnssReceiver _gnss_receiver; MixerGroup *_mixers = nullptr; From 6c6de9395818717916bbc1077fecd51c4db87936 Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Wed, 2 Jul 2014 10:04:07 -0700 Subject: [PATCH 104/600] Fixed heading covariance calculation and build errors. --- src/modules/uavcan/gnss_receiver.cpp | 17 ++++++++++++----- src/modules/uavcan/module.mk | 3 ++- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index e924683330..490e35bd17 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -80,17 +80,24 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Wed, 2 Jul 2014 11:18:30 -0700 Subject: [PATCH 105/600] Fixed bug with zero-sized covariance arrays --- src/modules/uavcan/gnss_receiver.cpp | 75 ++++++++++++++++++---------- src/modules/uavcan/uavcan_main.cpp | 23 +++++---- 2 files changed, 60 insertions(+), 38 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 490e35bd17..23c2215652 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -42,12 +42,13 @@ #include "gnss_receiver.hpp" #include -#define MM_PER_CM 10 // Millimeters per centimeter +#define MM_PER_CM 10 // Millimeters per centimeter +#define EXPECTED_COV_SIZE 9 // Expect a 3x3 matrix for position and velocity covariance UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : - _node(node), - _uavcan_sub_status(node), - _report_pub(-1) +_node(node), +_uavcan_sub_status(node), +_report_pub(-1) { } @@ -77,33 +78,53 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0); + bool valid_velocity_covariance = (msg.velocity_covariance.size() == EXPECTED_COV_SIZE && + msg.velocity_covariance[0] > 0); + + if (valid_position_covariance) { + _report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4]; + _report.eph_m = sqrtf(_report.p_variance_m); + } else { + _report.p_variance_m = -1.0; + _report.eph_m = -1.0; + } + + if (valid_velocity_covariance) { + _report.s_variance_m_s = msg.velocity_covariance[0] + msg.velocity_covariance[4] + msg.velocity_covariance[8]; + + /* There is a nonlinear relationship between the velocity vector and the heading. + * Use Jacobian to transform velocity covariance to heading covariance + * + * Nonlinear equation: + * heading = atan2(vel_e_m_s, vel_n_m_s) + * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative + * + * To calculate the variance of heading from the variance of velocity, + * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T + */ + float vel_n = msg.ned_velocity[0]; + float vel_e = msg.ned_velocity[1]; + float vel_n_sq = vel_n * vel_n; + float vel_e_sq = vel_e * vel_e; + _report.c_variance_rad = + (vel_e_sq * msg.velocity_covariance[0] + + -2 * vel_n * vel_e * msg.velocity_covariance[1] + // Covariance matrix is symmetric + vel_n_sq* msg.velocity_covariance[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); + + _report.epv_m = sqrtf(msg.position_covariance[8]); + + } else { + _report.s_variance_m_s = -1.0; + _report.c_variance_rad = -1.0; + _report.epv_m = -1.0; + } _report.fix_type = msg.status; - _report.eph_m = sqrtf(_report.p_variance_m); - _report.epv_m = sqrtf(msg.position_covariance[8]); - _report.timestamp_velocity = _report.timestamp_position; _report.vel_n_m_s = msg.ned_velocity[0]; _report.vel_e_m_s = msg.ned_velocity[1]; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5bc4376709..c0c07be536 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -243,21 +243,22 @@ int UavcanNode::run() _node.setStatusOk(); + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; } const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); @@ -271,7 +272,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 0; + unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { From 607b6511a413b5bc2b2b0ae350a9451e83da9803 Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Wed, 2 Jul 2014 11:27:49 -0700 Subject: [PATCH 106/600] Fixed comments --- src/modules/uavcan/gnss_receiver.hpp | 6 ++---- src/modules/uavcan/uavcan_main.hpp | 2 +- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/gnss_receiver.hpp index abb8a821af..18df8da2f5 100644 --- a/src/modules/uavcan/gnss_receiver.hpp +++ b/src/modules/uavcan/gnss_receiver.hpp @@ -34,10 +34,8 @@ /** * @file gnss_receiver.hpp * - * UAVCAN <--> ORB bridge for ESC messages: - * uavcan.equipment.esc.RawCommand - * uavcan.equipment.esc.RPMCommand - * uavcan.equipment.esc.Status + * UAVCAN --> ORB bridge for GNSS messages: + * uavcan.equipment.gnss.Fix * * @author Pavel Kirienko * @author Andrew Chambers diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 126d441377..443525379e 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -109,7 +109,7 @@ private: static UavcanNode *_instance; ///< singleton pointer Node _node; ///< library instance UavcanEscController _esc_controller; - UavcanGnssReceiver _gnss_receiver; + UavcanGnssReceiver _gnss_receiver; MixerGroup *_mixers = nullptr; From 6c5e3d53412fa1cdad687818328b3bfc1a83e9ca Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Wed, 2 Jul 2014 19:06:30 -0700 Subject: [PATCH 107/600] Address Paval's comments regarding extracting matrix from uavcan msg, position covariance calculation, and _poll_fds_num --- src/modules/uavcan/gnss_receiver.cpp | 28 +++++++++++++++------------- src/modules/uavcan/uavcan_main.cpp | 23 +++++++++++------------ 2 files changed, 26 insertions(+), 25 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 23c2215652..65a7b4a2a3 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -43,7 +43,6 @@ #include #define MM_PER_CM 10 // Millimeters per centimeter -#define EXPECTED_COV_SIZE 9 // Expect a 3x3 matrix for position and velocity covariance UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : _node(node), @@ -75,18 +74,19 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0); - bool valid_velocity_covariance = (msg.velocity_covariance.size() == EXPECTED_COV_SIZE && - msg.velocity_covariance[0] > 0); + + // Check if the msg contains valid covariance information + const bool valid_position_covariance = !msg.position_covariance.empty(); + const bool valid_velocity_covariance = !msg.velocity_covariance.empty(); if (valid_position_covariance) { - _report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4]; + float pos_cov[9]; + msg.position_covariance.unpackSquareMatrix(pos_cov); + _report.p_variance_m = std::max(pos_cov[0], pos_cov[4]); _report.eph_m = sqrtf(_report.p_variance_m); } else { _report.p_variance_m = -1.0; @@ -94,7 +94,9 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index c0c07be536..5bc4376709 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -243,22 +243,21 @@ int UavcanNode::run() _node.setStatusOk(); - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; - while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; } const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); @@ -272,7 +271,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 1; + unsigned poll_id = 0; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { From d7394c7ef973e34d87187420444baad6fcf9854b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Jul 2014 19:00:22 +0200 Subject: [PATCH 108/600] mavlink: stack size increased --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cf6265f097..f73a58fa2f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1601,7 +1601,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1950, + 2700, (main_t)&Mavlink::start_helper, (const char **)argv); From c6c33142ceb6bf59b8c9b8e32e94ae5ea7959dbd Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Thu, 3 Jul 2014 11:32:27 -0700 Subject: [PATCH 109/600] Using proper math library. Corrected speed variance calculation --- src/modules/uavcan/gnss_receiver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 65a7b4a2a3..3e98bdf144 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -41,6 +41,7 @@ #include "gnss_receiver.hpp" #include +#include #define MM_PER_CM 10 // Millimeters per centimeter @@ -86,7 +87,7 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Sun, 6 Jul 2014 15:33:54 +0200 Subject: [PATCH 110/600] Code style improvement, fix linter warning --- src/modules/uavcan/esc_controller.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp index bde4d2a7fa..f603d94482 100644 --- a/src/modules/uavcan/esc_controller.cpp +++ b/src/modules/uavcan/esc_controller.cpp @@ -50,10 +50,8 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : int UavcanEscController::init() { - int res = -1; - // ESC status subscription - res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); + int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); if (res < 0) { warnx("ESC status sub failed %i", res); From 2669f7f3af65921d4abbf3850cd62e48f2eeeec7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Jul 2014 15:34:50 +0200 Subject: [PATCH 111/600] Fix mixer limiter to never output min for an input of max + 1 quantum --- src/modules/uavcan/uavcan_main.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5bc4376709..27e77e9c5a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -301,9 +301,7 @@ int UavcanNode::run() // iterate actuators for (unsigned i = 0; i < outputs.noutputs; i++) { // last resort: catch NaN, INF and out-of-band errors - if (!isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { + if (!isfinite(outputs.output[i])) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally @@ -311,6 +309,18 @@ int UavcanNode::run() */ outputs.output[i] = -1.0f; } + + // limit outputs to valid range + + // never go below min + if (outputs.output[i] < -1.0f) { + outputs.output[i] = -1.0f; + } + + // never go below max + if (outputs.output[i] > 1.0f) { + outputs.output[i] = 1.0f; + } } // Output to the bus From 324322cb29720dd78b6eb534bb679532d5ed83f2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 7 Jul 2014 02:10:09 +0400 Subject: [PATCH 112/600] UAVCAN ESC perf counters --- src/modules/uavcan/esc_controller.cpp | 28 +++++++++++++++++++++------ src/modules/uavcan/esc_controller.hpp | 8 ++++++++ 2 files changed, 30 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp index f603d94482..406eba88c4 100644 --- a/src/modules/uavcan/esc_controller.cpp +++ b/src/modules/uavcan/esc_controller.cpp @@ -48,6 +48,12 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : { } +UavcanEscController::~UavcanEscController() +{ + perf_free(_perfcnt_invalid_input); + perf_free(_perfcnt_scaling_error); +} + int UavcanEscController::init() { // ESC status subscription @@ -67,8 +73,10 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - assert(outputs != nullptr); - assert(num_outputs <= MAX_ESCS); + if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { + perf_count(_perfcnt_invalid_input); + return; + } /* * Rate limiting - we don't want to congest the bus @@ -89,13 +97,21 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) for (unsigned i = 0; i < num_outputs; i++) { float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; - if (scaled < 1.0F) + if (scaled < 1.0F) { scaled = 1.0F; // Since we're armed, we don't want to stop it completely + } - assert(scaled >= uavcan::equipment::esc::RawCommand::CMD_MIN); - assert(scaled <= uavcan::equipment::esc::RawCommand::CMD_MAX); + if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; + perf_count(_perfcnt_scaling_error); + } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; + perf_count(_perfcnt_scaling_error); + } else { + ; // Correct value + } - msg.cmd.push_back(scaled); + msg.cmd.push_back(static_cast(scaled)); } } diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp index 0ed0c59b56..559ede561e 100644 --- a/src/modules/uavcan/esc_controller.hpp +++ b/src/modules/uavcan/esc_controller.hpp @@ -47,11 +47,13 @@ #include #include #include +#include class UavcanEscController { public: UavcanEscController(uavcan::INode& node); + ~UavcanEscController(); int init(); @@ -96,4 +98,10 @@ private: */ bool _armed = false; uavcan::equipment::esc::Status _states[MAX_ESCS]; + + /* + * Perf counters + */ + perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); + perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); }; From dae9b48462bc851ee61d6d18ff2b5697dddf620b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 7 Jul 2014 02:31:05 +0400 Subject: [PATCH 113/600] Renamed OUTPUT_MODE: can --> uavcan_esc --- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 2 +- ROMFS/px4fmu_common/init.d/rc.interface | 2 +- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index 471ac54b4d..8c5a4fbf2d 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -24,4 +24,4 @@ then param set MC_YAWRATE_D 0.0 fi -set OUTPUT_MODE can +set OUTPUT_MODE uavcan_esc diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index da4c734704..1de0abb58d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -25,7 +25,7 @@ then set OUTPUT_DEV /dev/pwm_output fi - if [ $OUTPUT_MODE == can ] + if [ $OUTPUT_MODE == uavcan_esc ] then set OUTPUT_DEV /dev/uavcan/esc fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c95016acee..975cb6d1d4 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -292,7 +292,7 @@ then # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output if [ $OUTPUT_MODE != none ] then - if [ $OUTPUT_MODE == can ] + if [ $OUTPUT_MODE == uavcan_esc ] then if uavcan start 1 then @@ -302,7 +302,7 @@ then fi fi - if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == can ] + if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ] then echo "[init] Use PX4IO PWM as primary output" if px4io start From 72a531b018f0089c89ed40261969555fd282e459 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 7 Jul 2014 15:07:37 +0400 Subject: [PATCH 114/600] Fixed UAVCAN GNSS bridge - EPV computation, catching up with the new GPS ORB topic --- src/modules/uavcan/gnss_receiver.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 3e98bdf144..debba9fee3 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -85,13 +85,18 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) ? sqrtf(horizontal_pos_variance) : -1.0F; + + // Vertical position uncertainty + _report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; } else { - _report.p_variance_m = -1.0; - _report.eph_m = -1.0; + _report.eph = -1.0F; + _report.epv = -1.0F; } if (valid_velocity_covariance) { @@ -118,12 +123,9 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); From 664795c9db9a0d938cbe7221aed87755ca8de7bf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 7 Jul 2014 15:47:40 +0400 Subject: [PATCH 115/600] UAVCAN GNSS - using GNSS time to initialize the field time_gps_usec --- src/modules/uavcan/gnss_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index debba9fee3..ba1fe5e499 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -139,7 +139,7 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Mon, 7 Jul 2014 15:18:12 +0200 Subject: [PATCH 116/600] dataman: added macro for offboard storage selection --- src/modules/dataman/dataman.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index dbace21eff..d625bf9853 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -57,6 +57,8 @@ extern "C" { DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; + #define DM_KEY_WAYPOINTS_OFFBOARD(_id) (_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1) + /** The maximum number of instances for each item type */ enum { DM_KEY_SAFE_POINTS_MAX = 8, From a29f7cad395ce53b74500a0dc03214186c679378 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Jul 2014 15:18:54 +0200 Subject: [PATCH 117/600] navigator: reject mission if the first waypoint is too far from home --- src/modules/navigator/mission.cpp | 69 +++++++++++++++++++++++++- src/modules/navigator/mission.h | 10 +++- src/modules/navigator/mission_params.c | 13 ++++- 3 files changed, 89 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 291e2edeba..53f0724cdc 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -61,6 +61,7 @@ Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _param_onboard_enabled(this, "ONBOARD_EN"), _param_takeoff_alt(this, "TAKEOFF_ALT"), + _param_dist_1wp(this, "DIST_1WP"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -69,8 +70,10 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), + _dist_1wp_ok(false), _need_takeoff(true), _takeoff(false) + { /* load initial params */ updateParams(); @@ -246,6 +249,70 @@ Mission::advance_mission() } } +bool +Mission::check_dist_1wp() +{ + if (_dist_1wp_ok) { + /* always return true after at least one successful check */ + return true; + } + + /* check if first waypoint is not too far from home */ + bool mission_valid = false; + if (_param_dist_1wp.get() > 0.0f) { + if (_navigator->get_vstatus()->condition_home_position_valid) { + struct mission_item_s mission_item; + + /* find first waypoint (with lat/lon) item in datamanager */ + for (int i = 0; i < _offboard_mission.count; i++) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i, + &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { + + /* check only items with valid lat/lon */ + if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + mission_item.nav_cmd == NAV_CMD_TAKEOFF || + mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { + + /* check distance from home to item */ + float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); + + if (dist_to_1wp < _param_dist_1wp.get()) { + _dist_1wp_ok = true; + return true; + + } else { + /* item is too far from home */ + mavlink_log_info(_navigator->get_mavlink_fd(), "first wp is too far from home: %.1fm > %.1fm", dist_to_1wp, _param_dist_1wp.get()); + return false; + } + } + + } else { + /* error reading, mission is invalid */ + mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission"); + return false; + } + } + + /* no waypoints found in mission, then we will not fly far away */ + _dist_1wp_ok = true; + return true; + + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), "no home position"); + return false; + } + + } else { + return true; + } +} + void Mission::set_mission_items() { @@ -266,7 +333,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (read_mission_item(false, true, &_mission_item)) { + } else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 40629b1b21..4da6a11553 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -91,6 +91,12 @@ private: */ void advance_mission(); + /** + * Check distance to first waypoint (with lat/lon) + * @return true only if it's not too far from home (< MIS_DIST_1WP) + */ + bool check_dist_1wp(); + /** * Set new mission items */ @@ -127,8 +133,9 @@ private: */ void publish_mission_result(); - control::BlockParamFloat _param_onboard_enabled; + control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; + control::BlockParamFloat _param_dist_1wp; struct mission_s _onboard_mission; struct mission_s _offboard_mission; @@ -148,6 +155,7 @@ private: } _mission_type; bool _inited; + bool _dist_1wp_ok; MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 8692328db1..5cb3782c93 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -58,7 +58,6 @@ */ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); - /** * Enable onboard mission * @@ -67,3 +66,15 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); * @group Mission */ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0); + +/** + * Maximal horizontal distance from home to first waypoint + * + * Failsafe check to prevent running mission stored from previous flight on new place. + * Value < 0 means that check disabled. + * + * @min -1 + * @max 200 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 50); From 37b4cdfce21ff6a7374599a6706ad387bd359515 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 25 Mar 2014 10:56:33 +1100 Subject: [PATCH 118/600] board_serial: use a uint8_t buffer we should not be using 'char' for binary APIs, as the C standard does not specify if it is signed or unsigned, so results may not be consistent --- src/drivers/px4fmu/fmu.cpp | 2 +- src/modules/systemlib/board_serial.c | 8 ++++---- src/modules/systemlib/board_serial.h | 2 +- src/modules/systemlib/otp.h | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0a4635728e..39cc329673 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1784,7 +1784,7 @@ fmu_main(int argc, char *argv[]) } if (!strcmp(verb, "id")) { - char id[12]; + uint8_t id[12]; (void)get_board_serial(id); errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X", diff --git a/src/modules/systemlib/board_serial.c b/src/modules/systemlib/board_serial.c index ad8c2bf83e..182fd15c63 100644 --- a/src/modules/systemlib/board_serial.c +++ b/src/modules/systemlib/board_serial.c @@ -44,11 +44,11 @@ #include "board_config.h" #include "board_serial.h" -int get_board_serial(char *serialid) +int get_board_serial(uint8_t *serialid) { - const volatile unsigned *udid_ptr = (const unsigned *)UDID_START; + const volatile uint32_t *udid_ptr = (const uint32_t *)UDID_START; union udid id; - val_read((unsigned *)&id, udid_ptr, sizeof(id)); + val_read((uint32_t *)&id, udid_ptr, sizeof(id)); /* Copy the serial from the chips non-write memory and swap endianess */ @@ -57,4 +57,4 @@ int get_board_serial(char *serialid) serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8]; return 0; -} \ No newline at end of file +} diff --git a/src/modules/systemlib/board_serial.h b/src/modules/systemlib/board_serial.h index b14bb4376b..873d9657ba 100644 --- a/src/modules/systemlib/board_serial.h +++ b/src/modules/systemlib/board_serial.h @@ -44,6 +44,6 @@ __BEGIN_DECLS -__EXPORT int get_board_serial(char *serialid); +__EXPORT int get_board_serial(uint8_t *serialid); __END_DECLS diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h index f10e129d8c..273b064f0e 100644 --- a/src/modules/systemlib/otp.h +++ b/src/modules/systemlib/otp.h @@ -125,7 +125,7 @@ struct otp_lock { #pragma pack(push, 1) union udid { uint32_t serial[3]; - char data[12]; + uint8_t data[12]; }; #pragma pack(pop) From ef79d032760e83850a9dbcbe5ae34c8b72f5fb4f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 14 May 2014 16:01:10 +1000 Subject: [PATCH 119/600] mpu6000: allow disabling of on-sensor low pass filter used for vibration testing --- src/drivers/mpu6000/mpu6000.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 0edec3d0ee..fb4acc3601 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -666,7 +666,9 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) /* choose next highest filter frequency available */ - if (frequency_hz <= 5) { + if (frequency_hz == 0) { + filter = BITS_DLPF_CFG_2100HZ_NOLPF; + } else if (frequency_hz <= 5) { filter = BITS_DLPF_CFG_5HZ; } else if (frequency_hz <= 10) { filter = BITS_DLPF_CFG_10HZ; @@ -922,10 +924,11 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSLOWPASS: - - // XXX decide on relationship of both filters - // i.e. disable the on-chip filter - //_set_dlpf_filter((uint16_t)arg); + if (arg == 0) { + // allow disabling of on-chip filter using + // zero as desired filter frequency + _set_dlpf_filter(0); + } _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); @@ -1009,8 +1012,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); - // XXX check relation to the internal lowpass - //_set_dlpf_filter((uint16_t)arg); + if (arg == 0) { + // allow disabling of on-chip filter using 0 + // as desired frequency + _set_dlpf_filter(0); + } return OK; case GYROIOCSSCALE: From 07855120268cef79e9d23d7cd091c526fa4622af Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 19 May 2014 16:49:41 +1000 Subject: [PATCH 120/600] px_uploader: added ModemManager warning --- Tools/px_uploader.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 985e6ffd9d..a113f56284 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -63,6 +63,7 @@ import zlib import base64 import time import array +import os from sys import platform as _platform @@ -449,6 +450,12 @@ parser.add_argument('--baud', action="store", type=int, default=115200, help="Ba parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") args = parser.parse_args() +# warn people about ModemManager which interferes badly with Pixhawk +if os.path.exists("/usr/sbin/ModemManager"): + print("=======================================================================") + print("WARNING: You should uninstall ModemManager as it conflicts with Pixhawk") + print("=======================================================================") + # Load the firmware file fw = firmware(args.firmware) print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) From d6999384ceb905f82896b99dda6341c7b615ba7c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 11:42:46 +0200 Subject: [PATCH 121/600] Improve modem manager warning to avoid having smart people tell us we have Linux compatibility issues while we actually do not. --- Tools/px_uploader.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index a113f56284..cd7884f6d2 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -452,9 +452,9 @@ args = parser.parse_args() # warn people about ModemManager which interferes badly with Pixhawk if os.path.exists("/usr/sbin/ModemManager"): - print("=======================================================================") - print("WARNING: You should uninstall ModemManager as it conflicts with Pixhawk") - print("=======================================================================") + print("==========================================================================================================") + print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)") + print("==========================================================================================================") # Load the firmware file fw = firmware(args.firmware) From db304480d996c134fbc2525e5bc96a08178275a9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 May 2014 11:14:38 +1000 Subject: [PATCH 122/600] lsm303d: disable check_extremes code this could trigger with a bungee launch, and could cause higher latency due to SD card writes --- src/drivers/lsm303d/lsm303d.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8bf76dcc33..56baf32746 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -75,6 +75,10 @@ #endif static const int ERROR = -1; +// enable this to debug the buggy lsm303d sensor in very early +// prototype pixhawk boards +#define CHECK_EXTREMES 0 + /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -830,7 +834,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { +#if CHECK_EXTREMES check_extremes(arb); +#endif ret += sizeof(*arb); arb++; } From 644d4bb3dc6186d7908ed0aa75d973cfc0826253 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 10:06:52 +1000 Subject: [PATCH 123/600] FMUv2: added defines for FMUv3 sensors this enables EXT0 to EXT3 on external SPI bus, and gives correct names for FMUv3 board --- src/drivers/boards/px4fmu-v2/board_config.h | 10 ++++++++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 26 ++++++++++++++++++++- 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 36eb7bec4e..2ed51d7b11 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -108,6 +108,8 @@ __BEGIN_DECLS #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) #define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) #define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) +#define GPIO_SPI_CS_EXT2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define PX4_SPI_BUS_SENSORS 1 #define PX4_SPI_BUS_EXT 4 @@ -121,6 +123,14 @@ __BEGIN_DECLS /* External bus */ #define PX4_SPIDEV_EXT0 1 #define PX4_SPIDEV_EXT1 2 +#define PX4_SPIDEV_EXT2 3 +#define PX4_SPIDEV_EXT3 4 + +/* FMUv3 SPI on external bus */ +#define PX4_SPIDEV_EXT_MPU PX4_SPIDEV_EXT0 +#define PX4_SPIDEV_EXT_BARO PX4_SPIDEV_EXT1 +#define PX4_SPIDEV_EXT_ACCEL_MAG PX4_SPIDEV_EXT2 +#define PX4_SPIDEV_EXT_GYRO PX4_SPIDEV_EXT3 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 01dbd6e774..8c37d31a73 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -98,8 +98,12 @@ __EXPORT void weak_function stm32_spiinitialize(void) #ifdef CONFIG_STM32_SPI4 stm32_configgpio(GPIO_SPI_CS_EXT0); stm32_configgpio(GPIO_SPI_CS_EXT1); + stm32_configgpio(GPIO_SPI_CS_EXT2); + stm32_configgpio(GPIO_SPI_CS_EXT3); stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); #endif } @@ -174,12 +178,32 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, /* Making sure the other peripherals are not selected */ stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected); stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); break; case PX4_SPIDEV_EXT1: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected); stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + break; + + case PX4_SPIDEV_EXT2: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1); + break; + + case PX4_SPIDEV_EXT3: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT3, !selected); break; default: From fea4845ed97ca5219ceb8af0b0fb6d68603eea17 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:13:05 +1000 Subject: [PATCH 124/600] SPI: make _bus protected this allows runtime use of internal/external bus to determine if DRDY should be used on the L3GD20 --- src/drivers/device/spi.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 299575dc5c..03986da1e7 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -124,11 +124,13 @@ protected: LockMode locking_mode; /**< selected locking mode */ private: - int _bus; enum spi_dev_e _device; enum spi_mode_e _mode; uint32_t _frequency; struct spi_dev_s *_dev; + +protected: + int _bus; }; } // namespace device From e7360f40169a0d467448bca8f85d32a84025ca4e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:13:58 +1000 Subject: [PATCH 125/600] l3gd20: added -X switch for external bus --- src/drivers/l3gd20/l3gd20.cpp | 50 +++++++++++++++++++++++++++++------ 1 file changed, 42 insertions(+), 8 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 37e72388be..06b36d6882 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -821,7 +822,7 @@ L3GD20::measure() // if the gyro doesn't have any data ready then re-schedule // for 100 microseconds later. This ensures we don't double // read a value and then miss the next value - if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { + if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { perf_count(_reschedules); hrt_call_delay(&_call, 100); return; @@ -983,7 +984,7 @@ void info(); * Start the driver. */ void -start() +start(bool external_bus) { int fd; @@ -991,7 +992,15 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + if (external_bus) { +#ifdef PX4_SPI_BUS_EXT + g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO); +#else + errx(0, "External SPI not available"); +#endif + } else { + g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + } if (g_dev == nullptr) goto fail; @@ -1106,32 +1115,57 @@ info() } // namespace +void +l3gd20_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset'"); + warnx("options:"); + warnx(" -X (external bus)"); +} + int l3gd20_main(int argc, char *argv[]) { + bool external_bus = false; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "X")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + default: + l3gd20_usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - l3gd20::start(); + if (!strcmp(verb, "start")) + l3gd20::start(external_bus); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) l3gd20::test(); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) l3gd20::reset(); /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(verb, "info")) l3gd20::info(); errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); From e0dbc82d84c2ad0e36b88f6b6d3cbfab866b4c44 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:14:17 +1000 Subject: [PATCH 126/600] lsm303d: added -X option for external bus --- src/drivers/lsm303d/lsm303d.cpp | 51 ++++++++++++++++++++++++++------- 1 file changed, 40 insertions(+), 11 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 56baf32746..0428901a3b 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -85,6 +86,7 @@ static const int ERROR = -1; #define ADDR_INCREMENT (1<<6) #define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" +#define LSM303D_DEVICE_PATH_ACCEL_EXT "/dev/lsm303d_accel_ext" #define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" /* register addresses: A: accel, M: mag, T: temp */ @@ -1791,16 +1793,18 @@ void logging(); * Start the driver. */ void -start() +start(bool external_bus) { int fd, fd_mag; - if (g_dev != nullptr) errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); - + if (external_bus) { + g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG); + } else { + g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + } if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); goto fail; @@ -1998,44 +2002,69 @@ logging() } // namespace +void +lsm303d_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'"); + warnx("options:"); + warnx(" -X (external bus)"); +} + int lsm303d_main(int argc, char *argv[]) { + bool external_bus = false; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "X")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + default: + lsm303d_usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - lsm303d::start(); + if (!strcmp(verb, "start")) + lsm303d::start(external_bus); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) lsm303d::test(); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) lsm303d::reset(); /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(verb, "info")) lsm303d::info(); /* * dump device registers */ - if (!strcmp(argv[1], "regdump")) + if (!strcmp(verb, "regdump")) lsm303d::regdump(); /* * dump device registers */ - if (!strcmp(argv[1], "logging")) + if (!strcmp(verb, "logging")) lsm303d::logging(); errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); From 541dc1825cfca3724a7fbe08abfdf88b881b0d3a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:14:33 +1000 Subject: [PATCH 127/600] mpu6000: added -X option for external bus --- src/drivers/mpu6000/mpu6000.cpp | 48 ++++++++++++++++++++++++++++----- 1 file changed, 41 insertions(+), 7 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index fb4acc3601..a1c169ffc1 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -1424,7 +1425,7 @@ void info(); * Start the driver. */ void -start() +start(bool external_bus) { int fd; @@ -1433,7 +1434,15 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU); + if (external_bus) { +#ifdef PX4_SPI_BUS_EXT + g_dev = new MPU6000(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_MPU); +#else + errx(0, "External SPI not available"); +#endif + } else { + g_dev = new MPU6000(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_MPU); + } if (g_dev == nullptr) goto fail; @@ -1578,32 +1587,57 @@ info() } // namespace +void +mpu6000_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset'"); + warnx("options:"); + warnx(" -X (external bus)"); +} + int mpu6000_main(int argc, char *argv[]) { + bool external_bus = false; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "X")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + default: + mpu6000_usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - mpu6000::start(); + if (!strcmp(verb, "start")) + mpu6000::start(external_bus); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) mpu6000::test(); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) mpu6000::reset(); /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(verb, "info")) mpu6000::info(); errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); From ab90fe783287a068ee3654e488ea9144077586ab Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 12:15:43 +1000 Subject: [PATCH 128/600] ms5611: added -X option for external SPI bus --- src/drivers/ms5611/ms5611.cpp | 46 ++++++++++++++++++++++++------- src/drivers/ms5611/ms5611.h | 2 +- src/drivers/ms5611/ms5611_spi.cpp | 10 +++++-- 3 files changed, 44 insertions(+), 14 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 1ce93aeeaf..7a8d2eecf3 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include @@ -775,7 +776,7 @@ namespace ms5611 MS5611 *g_dev; -void start(); +void start(bool external_bus); void test(); void reset(); void info(); @@ -832,7 +833,7 @@ crc4(uint16_t *n_prom) * Start the driver. */ void -start() +start(bool external_bus) { int fd; prom_u prom_buf; @@ -845,7 +846,7 @@ start() /* create the driver, try SPI first, fall back to I2C if unsuccessful */ if (MS5611_spi_interface != nullptr) - interface = MS5611_spi_interface(prom_buf); + interface = MS5611_spi_interface(prom_buf, external_bus); if (interface == nullptr && (MS5611_i2c_interface != nullptr)) interface = MS5611_i2c_interface(prom_buf); @@ -1058,41 +1059,66 @@ calibrate(unsigned altitude) } // namespace +void +ms5611_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); + warnx("options:"); + warnx(" -X (external bus)"); +} + int ms5611_main(int argc, char *argv[]) { + bool external_bus = false; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "X")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + default: + ms5611_usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - ms5611::start(); + if (!strcmp(verb, "start")) + ms5611::start(external_bus); /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) ms5611::test(); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) ms5611::reset(); /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(verb, "info")) ms5611::info(); /* * Perform MSL pressure calibration given an altitude in metres */ - if (!strcmp(argv[1], "calibrate")) { + if (!strcmp(verb, "calibrate")) { if (argc < 2) errx(1, "missing altitude"); - long altitude = strtol(argv[2], nullptr, 10); + long altitude = strtol(argv[optind+1], nullptr, 10); ms5611::calibrate(altitude); } diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index 76fb84de8e..f0b3fd61d7 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom); } /* namespace */ /* interface factories */ -extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf) weak_function; +extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) weak_function; extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function; diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 8759d16a16..00d016aed3 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -62,7 +62,7 @@ #ifdef PX4_SPIDEV_BARO -device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf); +device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus); class MS5611_SPI : public device::SPI { @@ -115,9 +115,13 @@ private: }; device::Device * -MS5611_spi_interface(ms5611::prom_u &prom_buf) +MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) { - return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); + if (external_bus) { + return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); + } else { + return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); + } } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : From 19dbbf17e8b2fd37e43aa1a5cb6ae8c39012ab0f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 13:51:04 +1000 Subject: [PATCH 129/600] mpu6000: allow for two mpu6000 instances, one internal, one external split g_dev into g_dev_int and g_dev_ext --- src/drivers/mpu6000/mpu6000.cpp | 82 ++++++++++++++++++--------------- 1 file changed, 46 insertions(+), 36 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index a1c169ffc1..7f9e9fde43 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -78,6 +78,8 @@ #define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel" #define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro" +#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu6000_accel_ext" +#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu6000_gyro_ext" // MPU 6000 registers #define MPUREG_WHOAMI 0x75 @@ -178,7 +180,7 @@ class MPU6000_gyro; class MPU6000 : public device::SPI { public: - MPU6000(int bus, spi_dev_e device); + MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device); virtual ~MPU6000(); virtual int init(); @@ -346,7 +348,7 @@ private: class MPU6000_gyro : public device::CDev { public: - MPU6000_gyro(MPU6000 *parent); + MPU6000_gyro(MPU6000 *parent, const char *path); ~MPU6000_gyro(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); @@ -369,9 +371,9 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } -MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), - _gyro(new MPU6000_gyro(this)), +MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device) : + SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), _call_interval(0), _accel_reports(nullptr), @@ -1357,8 +1359,8 @@ MPU6000::print_info() _gyro_reports->print_info("gyro queue"); } -MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : - CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), +MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : + CDev("MPU6000_gyro", path), _parent(parent), _gyro_topic(-1), _gyro_class_instance(-1) @@ -1414,12 +1416,13 @@ MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) namespace mpu6000 { -MPU6000 *g_dev; +MPU6000 *g_dev_int; // on internal bus +MPU6000 *g_dev_ext; // on external bus -void start(); -void test(); -void reset(); -void info(); +void start(bool); +void test(bool); +void reset(bool); +void info(bool); /** * Start the driver. @@ -1428,30 +1431,33 @@ void start(bool external_bus) { int fd; + MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; + const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; - if (g_dev != nullptr) + if (*g_dev_ptr != nullptr) /* if already started, the still command succeeded */ errx(0, "already started"); /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - g_dev = new MPU6000(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_MPU); + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU); #else errx(0, "External SPI not available"); #endif } else { - g_dev = new MPU6000(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_MPU); + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU); } - if (g_dev == nullptr) + if (*g_dev_ptr == nullptr) goto fail; - if (OK != g_dev->init()) + if (OK != (*g_dev_ptr)->init()) goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); + fd = open(path_accel, O_RDONLY); if (fd < 0) goto fail; @@ -1464,9 +1470,9 @@ start(bool external_bus) exit(0); fail: - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + if (*g_dev_ptr != nullptr) { + delete (*g_dev_ptr); + *g_dev_ptr = nullptr; } errx(1, "driver start failed"); @@ -1478,24 +1484,26 @@ fail: * and automatic modes. */ void -test() +test(bool external_bus) { + const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; accel_report a_report; gyro_report g_report; ssize_t sz; /* get the driver */ - int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); + int fd = open(path_accel, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", - MPU_DEVICE_PATH_ACCEL); + path_accel); /* get the driver */ - int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY); + int fd_gyro = open(path_gyro, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", MPU_DEVICE_PATH_GYRO); + err(1, "%s open failed", path_gyro); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -1543,7 +1551,7 @@ test() /* XXX add poll-rate tests here too */ - reset(); + reset(external_bus); errx(0, "PASS"); } @@ -1551,9 +1559,10 @@ test() * Reset the driver. */ void -reset() +reset(bool external_bus) { - int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); + const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + int fd = open(path_accel, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1573,13 +1582,14 @@ reset() * Print a little info about the driver. */ void -info() +info(bool external_bus) { - if (g_dev == nullptr) + MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; + if (*g_dev_ptr == nullptr) errx(1, "driver not running"); - printf("state @ %p\n", g_dev); - g_dev->print_info(); + printf("state @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_info(); exit(0); } @@ -1626,19 +1636,19 @@ mpu6000_main(int argc, char *argv[]) * Test the driver/device. */ if (!strcmp(verb, "test")) - mpu6000::test(); + mpu6000::test(external_bus); /* * Reset the driver. */ if (!strcmp(verb, "reset")) - mpu6000::reset(); + mpu6000::reset(external_bus); /* * Print driver information. */ if (!strcmp(verb, "info")) - mpu6000::info(); + mpu6000::info(external_bus); errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } From e4e152a85b5a29cada6559197580a9dce93e45e3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 11:30:05 +1000 Subject: [PATCH 130/600] FMUv2: added define for PX4_I2C_BUS_ONBOARD needed for hmc5883 on main bus (for FMUv3) --- src/drivers/boards/px4fmu-v2/board_config.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 2ed51d7b11..0190a5b5b0 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -134,7 +134,8 @@ __BEGIN_DECLS /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 -#define PX4_I2C_BUS_LED 2 +#define PX4_I2C_BUS_ONBOARD 2 +#define PX4_I2C_BUS_LED PX4_I2C_BUS_ONBOARD /* Devices on the onboard bus. * From f56724f7dff1f6d1167f4fd61015ffe24a64c8c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 14:02:05 +1000 Subject: [PATCH 131/600] lib/conversion: added rotate_3f() will be used for user specified rotations in sensor drivers --- src/lib/conversion/rotation.cpp | 142 ++++++++++++++++++++++++++++++++ src/lib/conversion/rotation.h | 8 ++ 2 files changed, 150 insertions(+) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 614877b183..e177157333 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -49,3 +49,145 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix) rot_matrix->from_euler(roll, pitch, yaw); } + +#define HALF_SQRT_2 0.70710678118654757f + +__EXPORT void +rotate_3f(enum Rotation rot, float &x, float &y, float &z) +{ + float tmp; + switch (rot) { + case ROTATION_NONE: + case ROTATION_MAX: + return; + case ROTATION_YAW_45: { + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_YAW_90: { + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_YAW_135: { + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_YAW_180: + x = -x; y = -y; + return; + case ROTATION_YAW_225: { + tmp = HALF_SQRT_2*(y - x); + y = -HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_YAW_270: { + tmp = x; x = y; y = -tmp; + return; + } + case ROTATION_YAW_315: { + tmp = HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(y - x); + x = tmp; + return; + } + case ROTATION_ROLL_180: { + y = -y; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_45: { + tmp = HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_90: { + tmp = x; x = y; y = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_135: { + tmp = HALF_SQRT_2*(y - x); + y = HALF_SQRT_2*(y + x); + x = tmp; z = -z; + return; + } + case ROTATION_PITCH_180: { + x = -x; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_225: { + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(y - x); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_270: { + tmp = x; x = -y; y = -tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_315: { + tmp = HALF_SQRT_2*(x - y); + y = -HALF_SQRT_2*(x + y); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_90: { + tmp = z; z = y; y = -tmp; + return; + } + case ROTATION_ROLL_90_YAW_45: { + tmp = z; z = y; y = -tmp; + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_ROLL_90_YAW_90: { + tmp = z; z = y; y = -tmp; + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_ROLL_90_YAW_135: { + tmp = z; z = y; y = -tmp; + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_ROLL_270: { + tmp = z; z = -y; y = tmp; + return; + } + case ROTATION_ROLL_270_YAW_45: { + tmp = z; z = -y; y = tmp; + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_ROLL_270_YAW_90: { + tmp = z; z = -y; y = tmp; + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_ROLL_270_YAW_135: { + tmp = z; z = -y; y = tmp; + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_PITCH_90: { + tmp = z; z = -x; x = tmp; + return; + } + case ROTATION_PITCH_270: { + tmp = z; z = x; x = -tmp; + return; + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 0c56494c5e..5187b448f2 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -118,4 +118,12 @@ const rot_lookup_t rot_lookup[] = { __EXPORT void get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); + +/** + * rotate a 3 element float vector in-place + */ +__EXPORT void +rotate_3f(enum Rotation rot, float &x, float &y, float &z); + + #endif /* ROTATION_H_ */ From c681d6621d8e4b29c3be8d8b94bf765b42f10e49 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 14:02:28 +1000 Subject: [PATCH 132/600] mpu6000: added -R rotation option --- src/drivers/mpu6000/mpu6000.cpp | 32 +++++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 7f9e9fde43..f6e00feeef 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -180,7 +181,7 @@ class MPU6000_gyro; class MPU6000 : public device::SPI { public: - MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device); + MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation); virtual ~MPU6000(); virtual int init(); @@ -235,6 +236,8 @@ private: math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_z; + enum Rotation _rotation; + /** * Start automatic measurement. */ @@ -371,7 +374,7 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } -MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device) : +MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) : SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), @@ -394,7 +397,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ) + _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _rotation(rotation) { // disable debug() calls _debug_enabled = false; @@ -1304,6 +1308,9 @@ MPU6000::measure() arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); + // apply user specified rotation + rotate_3f(_rotation, arb.x, arb.y, arb.z); + arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1322,6 +1329,9 @@ MPU6000::measure() grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); + // apply user specified rotation + rotate_3f(_rotation, grb.x, grb.y, grb.z); + grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; @@ -1419,7 +1429,7 @@ namespace mpu6000 MPU6000 *g_dev_int; // on internal bus MPU6000 *g_dev_ext; // on external bus -void start(bool); +void start(bool, enum Rotation); void test(bool); void reset(bool); void info(bool); @@ -1428,7 +1438,7 @@ void info(bool); * Start the driver. */ void -start(bool external_bus) +start(bool external_bus, enum Rotation rotation) { int fd; MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; @@ -1442,12 +1452,12 @@ start(bool external_bus) /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU); + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); #else errx(0, "External SPI not available"); #endif } else { - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU); + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); } if (*g_dev_ptr == nullptr) @@ -1610,13 +1620,17 @@ mpu6000_main(int argc, char *argv[]) { bool external_bus = false; int ch; + enum Rotation rotation = ROTATION_NONE; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "X")) != EOF) { + while ((ch = getopt(argc, argv, "XR:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; default: mpu6000_usage(); exit(0); @@ -1630,7 +1644,7 @@ mpu6000_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) - mpu6000::start(external_bus); + mpu6000::start(external_bus, rotation); /* * Test the driver/device. From 1dfc7bad7b04af74d154b9c4bd3886f346f38d72 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 14:06:52 +1000 Subject: [PATCH 133/600] Merged lsm303d update, keeping default frequency --- src/drivers/lsm303d/lsm303d.cpp | 33 +++++++++++++++++++++++---------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 0428901a3b..f25ac8f874 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -69,6 +69,7 @@ #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -222,7 +223,7 @@ class LSM303D_mag; class LSM303D : public device::SPI { public: - LSM303D(int bus, const char* path, spi_dev_e device); + LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation); virtual ~LSM303D(); virtual int init(); @@ -311,7 +312,8 @@ private: uint64_t _last_log_us; uint64_t _last_log_sync_us; uint64_t _last_log_reg_us; - uint64_t _last_log_alarm_us; + uint64_t _last_log_alarm_us; + enum Rotation _rotation; /** * Start automatic measurement. @@ -491,7 +493,7 @@ private: }; -LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : +LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), _mag(new LSM303D_mag(this)), _call_accel_interval(0), @@ -525,7 +527,8 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _last_log_us(0), _last_log_sync_us(0), _last_log_reg_us(0), - _last_log_alarm_us(0) + _last_log_alarm_us(0), + _rotation(rotation) { // enable debug() calls _debug_enabled = true; @@ -1541,6 +1544,9 @@ LSM303D::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); + // apply user specified rotation + rotate_3f(_rotation, accel_report.x, accel_report.y, accel_report.z); + accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; @@ -1617,6 +1623,9 @@ LSM303D::mag_measure() mag_report.scaling = _mag_range_scale; mag_report.range_ga = (float)_mag_range_ga; + // apply user specified rotation + rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z); + _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ @@ -1782,7 +1791,7 @@ namespace lsm303d LSM303D *g_dev; -void start(); +void start(bool external_bus, enum Rotation rotation); void test(); void reset(); void info(); @@ -1793,7 +1802,7 @@ void logging(); * Start the driver. */ void -start(bool external_bus) +start(bool external_bus, enum Rotation rotation) { int fd, fd_mag; if (g_dev != nullptr) @@ -1801,9 +1810,9 @@ start(bool external_bus) /* create the driver */ if (external_bus) { - g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG); + g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation); } else { - g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation); } if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -2015,13 +2024,17 @@ lsm303d_main(int argc, char *argv[]) { bool external_bus = false; int ch; + enum Rotation rotation = ROTATION_NONE; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "X")) != EOF) { + while ((ch = getopt(argc, argv, "XR:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; default: lsm303d_usage(); exit(0); @@ -2035,7 +2048,7 @@ lsm303d_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) - lsm303d::start(external_bus); + lsm303d::start(external_bus, rotation); /* * Test the driver/device. From a049f0841d0a68edf2f1e5d10ba2ab24d15aa472 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jul 2014 14:10:38 +1000 Subject: [PATCH 134/600] Merged L3GD20 orientation flag while keeping original bus speed --- src/drivers/l3gd20/l3gd20.cpp | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 06b36d6882..f8cd509a2e 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -69,6 +69,7 @@ #include #include +#include #define L3GD20_DEVICE_PATH "/dev/l3gd20" @@ -185,7 +186,7 @@ extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI { public: - L3GD20(int bus, const char* path, spi_dev_e device); + L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation); virtual ~L3GD20(); virtual int init(); @@ -230,6 +231,8 @@ private: /* true if an L3G4200D is detected */ bool _is_l3g4200d; + enum Rotation _rotation; + /** * Start automatic measurement. */ @@ -329,7 +332,7 @@ private: int self_test(); }; -L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : +L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call_interval(0), _reports(nullptr), @@ -346,7 +349,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), - _is_l3g4200d(false) + _is_l3g4200d(false), + _rotation(rotation) { // enable debug() calls _debug_enabled = true; @@ -915,6 +919,9 @@ L3GD20::measure() report.y = _gyro_filter_y.apply(report.y); report.z = _gyro_filter_z.apply(report.z); + // apply user specified rotation + rotate_3f(_rotation, report.x, report.y, report.z); + report.scaling = _gyro_range_scale; report.range_rad_s = _gyro_range_rad_s; @@ -975,7 +982,7 @@ namespace l3gd20 L3GD20 *g_dev; -void start(); +void start(bool external_bus, enum Rotation rotation); void test(); void reset(); void info(); @@ -984,7 +991,7 @@ void info(); * Start the driver. */ void -start(bool external_bus) +start(bool external_bus, enum Rotation rotation) { int fd; @@ -994,12 +1001,12 @@ start(bool external_bus) /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO); + g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO, rotation); #else errx(0, "External SPI not available"); #endif } else { - g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO, rotation); } if (g_dev == nullptr) @@ -1128,13 +1135,17 @@ l3gd20_main(int argc, char *argv[]) { bool external_bus = false; int ch; + enum Rotation rotation = ROTATION_NONE; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "X")) != EOF) { + while ((ch = getopt(argc, argv, "XR:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; default: l3gd20_usage(); exit(0); @@ -1148,7 +1159,7 @@ l3gd20_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) - l3gd20::start(external_bus); + l3gd20::start(external_bus, rotation); /* * Test the driver/device. From d2487e771884d62f893b14629caae8437bd6998e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jul 2014 11:20:14 +1000 Subject: [PATCH 135/600] hmc5883: added support for -R rotation option --- src/drivers/hmc5883/hmc5883.cpp | 45 +++++++++++++++++++++++++++------ 1 file changed, 37 insertions(+), 8 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index b7b368a5e8..75acf32e31 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -71,6 +71,8 @@ #include #include +#include +#include /* * HMC5883 internal constants and data structures. @@ -130,7 +132,7 @@ static const int ERROR = -1; class HMC5883 : public device::I2C { public: - HMC5883(int bus); + HMC5883(int bus, enum Rotation rotation); virtual ~HMC5883(); virtual int init(); @@ -169,6 +171,7 @@ private: bool _calibrated; /**< the calibration is valid */ int _bus; /**< the bus the device is connected to */ + enum Rotation _rotation; struct mag_report _last_report; /**< used for info() */ @@ -319,7 +322,7 @@ private: extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); -HMC5883::HMC5883(int bus) : +HMC5883::HMC5883(int bus, enum Rotation rotation) : I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), _reports(nullptr), @@ -334,7 +337,8 @@ HMC5883::HMC5883(int bus) : _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), _sensor_ok(false), _calibrated(false), - _bus(bus) + _bus(bus), + _rotation(rotation) { // enable debug() calls _debug_enabled = false; @@ -862,6 +866,9 @@ HMC5883::collect() /* z remains z */ new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + // apply user specified rotation + rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { if (_mag_topic != -1) { @@ -1246,7 +1253,7 @@ const int ERROR = -1; HMC5883 *g_dev; -void start(); +void start(enum Rotation rotation); void test(); void reset(); void info(); @@ -1256,7 +1263,7 @@ int calibrate(); * Start the driver. */ void -start() +start(enum Rotation rotation) { int fd; @@ -1265,7 +1272,7 @@ start() errx(0, "already started"); /* create the driver, attempt expansion bus first */ - g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION); + g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION, rotation); if (g_dev != nullptr && OK != g_dev->init()) { delete g_dev; g_dev = nullptr; @@ -1275,7 +1282,7 @@ start() #ifdef PX4_I2C_BUS_ONBOARD /* if this failed, attempt onboard sensor */ if (g_dev == nullptr) { - g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD); + g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD, rotation); if (g_dev != nullptr && OK != g_dev->init()) { goto fail; } @@ -1474,14 +1481,36 @@ info() } // namespace +void +hmc5883_usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'"); + warnx("options:"); + warnx(" -R rotation"); +} + int hmc5883_main(int argc, char *argv[]) { + int ch; + enum Rotation rotation = ROTATION_NONE; + + while ((ch = getopt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; + default: + hmc5883_usage(); + exit(0); + } + } + /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) - hmc5883::start(); + hmc5883::start(rotation); /* * Test the driver/device. From d952e81ab7c5ef050784cf3766c5b7bf18909777 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jul 2014 11:48:44 +1000 Subject: [PATCH 136/600] added support for two instances of hmc5883 driver --- src/drivers/hmc5883/hmc5883.cpp | 115 ++++++++++++++++++++------------ 1 file changed, 72 insertions(+), 43 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 75acf32e31..b9270d2a0b 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -79,7 +79,8 @@ */ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 -#define HMC5883L_DEVICE_PATH "/dev/hmc5883" +#define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int" +#define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext" /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ @@ -132,7 +133,7 @@ static const int ERROR = -1; class HMC5883 : public device::I2C { public: - HMC5883(int bus, enum Rotation rotation); + HMC5883(int bus, const char *path, enum Rotation rotation); virtual ~HMC5883(); virtual int init(); @@ -322,8 +323,8 @@ private: extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); -HMC5883::HMC5883(int bus, enum Rotation rotation) : - I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), +HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : + I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ @@ -1251,13 +1252,14 @@ namespace hmc5883 #endif const int ERROR = -1; -HMC5883 *g_dev; +HMC5883 *g_dev_int; +HMC5883 *g_dev_ext; -void start(enum Rotation rotation); -void test(); -void reset(); +void start(int bus, enum Rotation rotation); +void test(int bus); +void reset(int bus); void info(); -int calibrate(); +int calibrate(int bus); /** * Start the driver. @@ -1267,47 +1269,69 @@ start(enum Rotation rotation) { int fd; - if (g_dev != nullptr) - /* if already started, the still command succeeded */ - errx(0, "already started"); - /* create the driver, attempt expansion bus first */ - g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION, rotation); - if (g_dev != nullptr && OK != g_dev->init()) { - delete g_dev; - g_dev = nullptr; + if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { + if (g_dev_ext != nullptr) + errx(0, "already started external"); + g_dev_ext = new HMC5883(PX4_I2C_BUS_EXPANSION, HMC5883L_DEVICE_PATH_EXT, rotation); + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { + delete g_dev_ext; + g_dev_ext = nullptr; + } } #ifdef PX4_I2C_BUS_ONBOARD /* if this failed, attempt onboard sensor */ - if (g_dev == nullptr) { - g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD, rotation); - if (g_dev != nullptr && OK != g_dev->init()) { + if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { + if (g_dev_int != nullptr) + errx(0, "already started internal"); + g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation); + if (g_dev_int != nullptr && OK != g_dev_int->init()) { + if (bus == PX4_I2C_BUS_ONBOARD) { + goto fail; + } + } + if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { goto fail; } } #endif - if (g_dev == nullptr) + if (g_dev_int == nullptr && g_dev_ext == nullptr) goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); + if (g_dev_int != nullptr) { + fd = open(HMC5883L_DEVICE_PATH_INT, O_RDONLY); + if (fd < 0) + goto fail; - if (fd < 0) - goto fail; + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + close(fd); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; + if (g_dev_ext != nullptr) { + fd = open(HMC5883L_DEVICE_PATH_EXT, O_RDONLY); + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + close(fd); + } exit(0); fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + if (g_dev_int != nullptr) { + delete g_dev_int; + g_dev_int = nullptr; + } + if (g_dev_ext != nullptr) { + delete g_dev_ext; + g_dev_ext = nullptr; } errx(1, "driver start failed"); @@ -1319,16 +1343,17 @@ fail: * and automatic modes. */ void -test() +test(int bus) { struct mag_report report; ssize_t sz; int ret; + const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); - int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); + int fd = open(path, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -1421,14 +1446,15 @@ test() * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10. * Using the self test method described above, the user can scale sensor */ -int calibrate() +int calibrate(int bus) { int ret; + const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); - int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); + int fd = open(path, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); @@ -1448,9 +1474,11 @@ int calibrate() * Reset the driver. */ void -reset() +reset(int bus) { - int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); + const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + + int fd = open(path, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1468,8 +1496,9 @@ reset() * Print a little info about the driver. */ void -info() +info(int bus) { + HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); if (g_dev == nullptr) errx(1, "driver not running"); @@ -1516,25 +1545,25 @@ hmc5883_main(int argc, char *argv[]) * Test the driver/device. */ if (!strcmp(argv[1], "test")) - hmc5883::test(); + hmc5883::test(bus); /* * Reset the driver. */ if (!strcmp(argv[1], "reset")) - hmc5883::reset(); + hmc5883::reset(bus); /* * Print driver information. */ if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) - hmc5883::info(); + hmc5883::info(bus); /* * Autocalibrate the scaling */ if (!strcmp(argv[1], "calibrate")) { - if (hmc5883::calibrate() == 0) { + if (hmc5883::calibrate(bus) == 0) { errx(0, "calibration successful"); } else { From 5e62ae7a9e2a7d3ea05d293900f7171884fbb448 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jul 2014 12:07:23 +1000 Subject: [PATCH 137/600] hmc5883: added -C option to calibrate on startup Conflicts: src/drivers/hmc5883/hmc5883.cpp --- src/drivers/hmc5883/hmc5883.cpp | 42 +++++++++++++++++++++++++++------ 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index b9270d2a0b..65c8c64d05 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1516,6 +1516,11 @@ hmc5883_usage() warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'"); warnx("options:"); warnx(" -R rotation"); + warnx(" -C calibrate on start"); + warnx(" -X only external bus"); +#ifdef PX4_I2C_BUS_ONBOARD + warnx(" -I only internal bus"); +#endif } int @@ -1523,46 +1528,69 @@ hmc5883_main(int argc, char *argv[]) { int ch; enum Rotation rotation = ROTATION_NONE; + bool calibrate = false; - while ((ch = getopt(argc, argv, "R:")) != EOF) { + while ((ch = getopt(argc, argv, "XIR:C")) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(optarg); break; +#ifdef PX4_I2C_BUS_ONBOARD + case 'I': + bus = PX4_I2C_BUS_ONBOARD; + break; +#endif + case 'X': + bus = PX4_I2C_BUS_EXPANSION; + break; + case 'C': + calibrate = true; + break; default: hmc5883_usage(); exit(0); } } + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - hmc5883::start(rotation); + if (!strcmp(verb, "start")) { + hmc5883::start(bus, rotation); + if (calibrate) { + if (hmc5883::calibrate(bus) == 0) { + errx(0, "calibration successful"); + + } else { + errx(1, "calibration failed"); + } + } + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(verb, "test")) hmc5883::test(bus); /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(verb, "reset")) hmc5883::reset(bus); /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(verb, "info") || !strcmp(verb, "status")) hmc5883::info(bus); /* * Autocalibrate the scaling */ - if (!strcmp(argv[1], "calibrate")) { + if (!strcmp(verb, "calibrate")) { if (hmc5883::calibrate(bus) == 0) { errx(0, "calibration successful"); From dfee93f3b128a7f23d74363b7700c80ababbe690 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 6 Jul 2014 22:37:40 +1000 Subject: [PATCH 138/600] hmc5883: fixed driver startup when trying both buses --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 65c8c64d05..25cbc06799 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1325,11 +1325,11 @@ start(enum Rotation rotation) exit(0); fail: - if (g_dev_int != nullptr) { + if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { delete g_dev_int; g_dev_int = nullptr; } - if (g_dev_ext != nullptr) { + if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { delete g_dev_ext; g_dev_ext = nullptr; } From 1c6ea067902708a2b1b3faf55938e6e8768abe18 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Jul 2014 08:52:16 +1000 Subject: [PATCH 139/600] hmc5883: fixed build warnings --- src/drivers/hmc5883/hmc5883.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 25cbc06799..72d6bdd959 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1255,10 +1255,11 @@ const int ERROR = -1; HMC5883 *g_dev_int; HMC5883 *g_dev_ext; +void hmc5883_usage(); void start(int bus, enum Rotation rotation); void test(int bus); void reset(int bus); -void info(); +void info(int bus); int calibrate(int bus); /** From 42716345859ded979189ea7f2548d512975c4dec Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Jul 2014 08:52:27 +1000 Subject: [PATCH 140/600] l3gd20: fixed a build warning --- src/drivers/l3gd20/l3gd20.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index f8cd509a2e..cad3ff9378 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -982,6 +982,7 @@ namespace l3gd20 L3GD20 *g_dev; +void l3gd20_usage(); void start(bool external_bus, enum Rotation rotation); void test(); void reset(); From 1b555f2d2e0684485fa47db7acfcf5555a7c6b16 Mon Sep 17 00:00:00 2001 From: akdslr Date: Tue, 15 Apr 2014 15:28:30 -0400 Subject: [PATCH 141/600] LL40LS driver: Added new driver to the config make files --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + 2 files changed, 2 insertions(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 4d727aa4de..cc0958c29a 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -25,6 +25,7 @@ MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx +MODULES += drivers/ll40ls MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 8e83df391a..adfbc2b7db 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -28,6 +28,7 @@ MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/sf0x +MODULES += drivers/ll40ls MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry From 36a9123822dca314f4efc4b4cb140159dbe9e634 Mon Sep 17 00:00:00 2001 From: akdslr Date: Tue, 15 Apr 2014 20:50:31 -0400 Subject: [PATCH 142/600] LL40LS driver: adding new range finder driver --- src/drivers/ll40ls/ll40ls.cpp | 880 ++++++++++++++++++++++++++++++++++ src/drivers/ll40ls/module.mk | 40 ++ 2 files changed, 920 insertions(+) create mode 100644 src/drivers/ll40ls/ll40ls.cpp create mode 100644 src/drivers/ll40ls/module.mk diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp new file mode 100644 index 0000000000..cb0eac91b2 --- /dev/null +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -0,0 +1,880 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ll40ls.cpp + * @author Allyson Kreft + * + * Driver for the PulsedLight Lidar-Lite range finders connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +/* Configuration Constants */ +#define LL40LS_BUS PX4_I2C_BUS_EXPANSION +#define LL40LS_BASEADDR 0x42 /* 7-bit address */ +#define LL40LS_DEVICE_PATH "/dev/ll40ls" + +/* LL40LS Registers addresses */ + +#define LL40LS_MEASURE_REG 0x00 /* Measure range register */ +#define LL40LS_MSRREG_ACQUIRE 0x61 /* Value to initiate a measurement */ +#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ + +/* Device limits */ +#define LL40LS_MIN_DISTANCE (0.00f) +#define LL40LS_MAX_DISTANCE (14.00f) + +#define LL40LS_CONVERSION_INTERVAL 60000 /* 60ms */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class LL40LS : public device::I2C +{ +public: + LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR); + virtual ~LL40LS(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE + * and LL40LS_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]); + +LL40LS::LL40LS(int bus, int address) : + I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000), + _min_distance(LL40LS_MIN_DISTANCE), + _max_distance(LL40LS_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _range_finder_topic(-1), + _class_instance(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), + _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")) +{ + // up the retries since the device misses the first measure attempts + I2C::_retries = 3; + + // enable debug() calls + _debug_enabled = false; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +LL40LS::~LL40LS() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); +} + +int +LL40LS::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + goto out; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + + if (_reports == nullptr) { + goto out; + } + + _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +LL40LS::probe() +{ + return measure(); +} + +void +LL40LS::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +LL40LS::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +LL40LS::get_minimum_distance() +{ + return _min_distance; +} + +float +LL40LS::get_maximum_distance() +{ + return _max_distance; +} + +int +LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +LL40LS::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(LL40LS_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +LL40LS::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; + ret = transfer(cmd, sizeof(cmd), nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +LL40LS::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + // read the high and low byte distance registers + uint8_t distance_reg = LL40LS_DISTHIGH_REG; + ret = transfer(&distance_reg, 1, &val[0], sizeof(val)); + + if (ret < 0) { + log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint16_t distance = val[0] << 8 | val[1]; + float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ + struct range_finder_report report; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { + report.valid = 1; + } + else { + report.valid = 0; + } + + /* publish it */ + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +LL40LS::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&LL40LS::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +LL40LS::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +LL40LS::cycle_trampoline(void *arg) +{ + LL40LS *dev = (LL40LS *)arg; + + dev->cycle(); +} + +void +LL40LS::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&LL40LS::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + log("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&LL40LS::cycle_trampoline, + this, + USEC2TICK(LL40LS_CONVERSION_INTERVAL)); +} + +void +LL40LS::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace ll40ls +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +LL40LS *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new LL40LS(LL40LS_BUS); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +ll40ls_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + ll40ls::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + ll40ls::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + ll40ls::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + ll40ls::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + ll40ls::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/ll40ls/module.mk b/src/drivers/ll40ls/module.mk new file mode 100644 index 0000000000..ab7d432690 --- /dev/null +++ b/src/drivers/ll40ls/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2013 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the PulsedLight Lidar-Lite driver. +# + +MODULE_COMMAND = ll40ls + +SRCS = ll40ls.cpp From 7f91f0cc3ef806a7959e06ab16d4094bcfe871bd Mon Sep 17 00:00:00 2001 From: akdslr Date: Wed, 30 Apr 2014 14:09:30 -0400 Subject: [PATCH 143/600] LL40LS driver: Updated the value to write to the register to trigger a measurement --- src/drivers/ll40ls/ll40ls.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index cb0eac91b2..2872078375 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -79,7 +79,7 @@ /* LL40LS Registers addresses */ #define LL40LS_MEASURE_REG 0x00 /* Measure range register */ -#define LL40LS_MSRREG_ACQUIRE 0x61 /* Value to initiate a measurement */ +#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ #define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ /* Device limits */ From 67b7a858881229b95483e986a6e4d90ab34ac257 Mon Sep 17 00:00:00 2001 From: akdslr Date: Thu, 5 Jun 2014 09:53:56 -0400 Subject: [PATCH 144/600] LL40LS driver: Changes from the May 4th plane test flight --- src/drivers/ll40ls/ll40ls.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 2872078375..bc012e2e92 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -86,7 +86,7 @@ #define LL40LS_MIN_DISTANCE (0.00f) #define LL40LS_MAX_DISTANCE (14.00f) -#define LL40LS_CONVERSION_INTERVAL 60000 /* 60ms */ +#define LL40LS_CONVERSION_INTERVAL 100000 /* 100ms */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -517,8 +517,8 @@ LL40LS::collect() return ret; } - uint16_t distance = val[0] << 8 | val[1]; - float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ + uint16_t distance = (val[0] << 8) | val[1]; + float si_units = distance * 0.01f; /* cm to m */ struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ @@ -532,8 +532,10 @@ LL40LS::collect() report.valid = 0; } - /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } if (_reports->force(&report)) { perf_count(_buffer_overflows); From ba54983cdf33b55b47f3b46c9b2461ab7217461a Mon Sep 17 00:00:00 2001 From: akdslr Date: Fri, 6 Jun 2014 08:52:06 -0400 Subject: [PATCH 145/600] MB12XX Driver: Added a class instance and device specific path --- src/drivers/mb12xx/mb12xx.cpp | 45 +++++++++++++++++++++++++---------- 1 file changed, 32 insertions(+), 13 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 5adb8cf698..b63e54dacb 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -74,6 +74,7 @@ /* Configuration Constants */ #define MB12XX_BUS PX4_I2C_BUS_EXPANSION #define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ +#define MB12XX_DEVICE_PATH "/dev/mb12xx" /* MB12xx Registers addresses */ @@ -124,6 +125,7 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; + int _class_instance; orb_advert_t _range_finder_topic; @@ -187,7 +189,7 @@ private: extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]); MB12XX::MB12XX(int bus, int address) : - I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), + I2C("MB12xx", MB12XX_DEVICE_PATH, bus, address, 100000), _min_distance(MB12XX_MIN_DISTANCE), _max_distance(MB12XX_MAX_DISTANCE), _reports(nullptr), @@ -195,6 +197,7 @@ MB12XX::MB12XX(int bus, int address) : _measure_ticks(0), _collect_phase(false), _range_finder_topic(-1), + _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) @@ -215,6 +218,15 @@ MB12XX::~MB12XX() if (_reports != nullptr) { delete _reports; } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int @@ -234,13 +246,18 @@ MB12XX::init() goto out; } - /* get a publish handle on the range finder topic */ - struct range_finder_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); - if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } } ret = OK; @@ -505,8 +522,10 @@ MB12XX::collect() report.distance = si_units; report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; - /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } if (_reports->force(&report)) { perf_count(_buffer_overflows); @@ -665,7 +684,7 @@ start() } /* set the poll rate to default, starts automatic data collection */ - fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + fd = open(MB12XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { goto fail; @@ -715,10 +734,10 @@ test() ssize_t sz; int ret; - int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + int fd = open(MB12XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + err(1, "%s open failed (try 'mb12xx start' if the driver is not running", MB12XX_DEVICE_PATH); } /* do a simple demand read */ @@ -776,7 +795,7 @@ test() void reset() { - int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + int fd = open(MB12XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { err(1, "failed "); From a6b52f1c9f720a8dd31099e7adeedee694f60729 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:49:24 +0200 Subject: [PATCH 146/600] HMC5883 post merge cleanup --- src/drivers/device/spi.cpp | 4 ++-- src/drivers/hmc5883/hmc5883.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index fed6c312c1..0b232e158a 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -69,11 +69,11 @@ SPI::SPI(const char *name, // protected locking_mode(LOCK_PREEMPTION), // private - _bus(bus), _device(device), _mode(mode), _frequency(frequency), - _dev(nullptr) + _dev(nullptr), + _bus(bus) { } diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 72d6bdd959..c2e4700d83 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1266,7 +1266,7 @@ int calibrate(int bus); * Start the driver. */ void -start(enum Rotation rotation) +start(int bus, enum Rotation rotation) { int fd; @@ -1528,6 +1528,7 @@ int hmc5883_main(int argc, char *argv[]) { int ch; + int bus = -1; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; From 369c7d277f2fea351ca4243debccc0a115f3f7e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:50:00 +0200 Subject: [PATCH 147/600] Board config cleanup for external bus support --- makefiles/config_px4fmu-v2_test.mk | 1 + src/drivers/boards/px4fmu-v1/board_config.h | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 66d9efbf8c..395a8f2ace 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -42,6 +42,7 @@ MODULES += modules/uORB LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter +MODULES += lib/conversion # # Libraries diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index c944007a56..a70a6240cb 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -86,7 +86,6 @@ __BEGIN_DECLS #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) #define PX4_SPI_BUS_SENSORS 1 -#define PX4_SPI_BUS_EXT 2 /* * Use these in place of the spi_dev_e enumeration to From a42ec7df1b417a34e072a68c6e34240a97d5ba80 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:50:24 +0200 Subject: [PATCH 148/600] MS5611: Deal with missing external bus --- src/drivers/ms5611/ms5611_spi.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 00d016aed3..5234ce8d6b 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -118,7 +118,11 @@ device::Device * MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) { if (external_bus) { + #ifdef PX4_SPI_BUS_EXT return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); + #else + return nullptr; + #endif } else { return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); } From 92426c5cfc8e9746fe42e4c9c087a5e25a4be658 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:51:33 +0200 Subject: [PATCH 149/600] LSM303D: deal with missing external bus --- src/drivers/lsm303d/lsm303d.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index f25ac8f874..296899ccdd 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1810,10 +1810,15 @@ start(bool external_bus, enum Rotation rotation) /* create the driver */ if (external_bus) { + #ifdef PX4_SPI_BUS_EXT g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation); + #else + errx(0, "External SPI not available"); + #endif } else { g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation); } + if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); goto fail; From 7d15e999f12fcfa7650c673e8be1daffbbfc46d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:52:10 +0200 Subject: [PATCH 150/600] LSM303: Fix usage function call to fit existing structure. --- src/drivers/lsm303d/lsm303d.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 296899ccdd..7ba0fcc88b 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1797,6 +1797,7 @@ void reset(); void info(); void regdump(); void logging(); +void usage(); /** * Start the driver. @@ -2013,17 +2014,17 @@ logging() exit(0); } - -} // namespace - void -lsm303d_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -R rotation"); } +} // namespace + int lsm303d_main(int argc, char *argv[]) { @@ -2041,7 +2042,7 @@ lsm303d_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(optarg); break; default: - lsm303d_usage(); + lsm303d::usage(); exit(0); } } From 875be65242dc58503c44ff522372b3cc2df83619 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:52:28 +0200 Subject: [PATCH 151/600] MPU6000: Fix usage function call to fit existing structure. --- src/drivers/mpu6000/mpu6000.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index f6e00feeef..5668b0865f 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1433,6 +1433,7 @@ void start(bool, enum Rotation); void test(bool); void reset(bool); void info(bool); +void usage(); /** * Start the driver. @@ -1604,17 +1605,17 @@ info(bool external_bus) exit(0); } - -} // namespace - void -mpu6000_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -R rotation"); } +} // namespace + int mpu6000_main(int argc, char *argv[]) { @@ -1632,7 +1633,7 @@ mpu6000_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(optarg); break; default: - mpu6000_usage(); + mpu6000::usage(); exit(0); } } From 812d326912feb41bb37848d61b1a6e6e1d18ab90 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:52:42 +0200 Subject: [PATCH 152/600] L3GD20: Fix usage function call to fit existing structure. --- src/drivers/l3gd20/l3gd20.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index cad3ff9378..64d1a7e552 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -982,7 +982,7 @@ namespace l3gd20 L3GD20 *g_dev; -void l3gd20_usage(); +void usage(); void start(bool external_bus, enum Rotation rotation); void test(); void reset(); @@ -1120,17 +1120,17 @@ info() exit(0); } - -} // namespace - void -l3gd20_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -R rotation"); } +} // namespace + int l3gd20_main(int argc, char *argv[]) { @@ -1148,7 +1148,7 @@ l3gd20_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(optarg); break; default: - l3gd20_usage(); + l3gd20::usage(); exit(0); } } From f2cbc7fe2778b4148bdecd59148ec96b84a248df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:53:10 +0200 Subject: [PATCH 153/600] MB12xx: Fix initialiser order in class --- src/drivers/mb12xx/mb12xx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index b63e54dacb..beb6c8e35e 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -196,8 +196,8 @@ MB12XX::MB12XX(int bus, int address) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), - _range_finder_topic(-1), _class_instance(-1), + _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) From 83f343f196c07d0a633ee8fc7d2324e8cb3d129c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:53:25 +0200 Subject: [PATCH 154/600] LL40LS: Fix initialiser order in class --- src/drivers/ll40ls/ll40ls.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index bc012e2e92..a69e6ee556 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -196,8 +196,8 @@ LL40LS::LL40LS(int bus, int address) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), - _range_finder_topic(-1), _class_instance(-1), + _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")) From ea4de4adc388e9353114e6e2c89eb27cec5b249e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:54:01 +0200 Subject: [PATCH 155/600] param command: fix warnings --- src/systemcmds/param/param.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 28e1b108b0..e110335e7c 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,7 +63,7 @@ static void do_import(const char* param_file_name); static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val, bool fail_on_not_found); -static void do_compare(const char* name, const char* vals[], unsigned comparisons); +static void do_compare(const char* name, char* vals[], unsigned comparisons); static void do_reset(void); static void do_reset_nostart(void); @@ -351,7 +351,7 @@ do_set(const char* name, const char* val, bool fail_on_not_found) } static void -do_compare(const char* name, const char* vals[], unsigned comparisons) +do_compare(const char* name, char* vals[], unsigned comparisons) { int32_t i; float f; From e380aa3f62c9db89f048f3683e3611e455de4ad2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 13:54:33 +0200 Subject: [PATCH 156/600] AT24C: Fix warning due to missing function prototype. --- src/systemcmds/mtd/24xxxx_mtd.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index 72200f4188..9913637970 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -161,6 +161,7 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); void at24c_test(void); +int at24c_nuke(void); /************************************************************************************ * Private Data From ac8f179f2dad99d664a6f9de4df954bea7fe5858 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 20:24:18 +1000 Subject: [PATCH 157/600] Tools: skip check_submodules.sh when NUTTX_SRC is set this avoids using submodules when a specific NuttX tree is specified --- Tools/check_submodules.sh | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index fb180ef47e..4b8789b288 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -1,5 +1,12 @@ #!/bin/sh +[ -n "$NUTTX_SRC" ] && { + # NUTTX_SRC is set, meaning user is overriding the NuttX tree to use. Don't + # use submodules to pull an alternatiie tree + echo "Skipping submodules as NUTTX_SRC is set to $NUTTX_SRC" + exit 0 +} + if [ -d NuttX/nuttx ]; then STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") From 52713bc0ba891854934f62ad856f1600e8ba3065 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 15:00:44 +0200 Subject: [PATCH 158/600] Revert "Tools: skip check_submodules.sh when NUTTX_SRC is set" This reverts commit ac8f179f2dad99d664a6f9de4df954bea7fe5858. --- Tools/check_submodules.sh | 7 ------- 1 file changed, 7 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 4b8789b288..fb180ef47e 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -1,12 +1,5 @@ #!/bin/sh -[ -n "$NUTTX_SRC" ] && { - # NUTTX_SRC is set, meaning user is overriding the NuttX tree to use. Don't - # use submodules to pull an alternatiie tree - echo "Skipping submodules as NUTTX_SRC is set to $NUTTX_SRC" - exit 0 -} - if [ -d NuttX/nuttx ]; then STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") From bc06d839eab504e039c27cd472bbd1162cb7bbf4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 20:24:18 +1000 Subject: [PATCH 159/600] Tools: skip check_submodules.sh when GIT_SUBMODULES_ARE_EVIL is set this avoids using submodules when a specific NuttX tree is specified --- Tools/check_submodules.sh | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index fb180ef47e..55bcc05ee0 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -1,5 +1,11 @@ #!/bin/sh +[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && { + # GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules + echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC" + exit 0 +} + if [ -d NuttX/nuttx ]; then STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") From 76f82bf237de367a3a3b214f2ba06faddb077c57 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 15:04:01 +0200 Subject: [PATCH 160/600] Updated submodule update instructions --- Tools/check_submodules.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 55bcc05ee0..7c3a3bfc2e 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -12,7 +12,7 @@ if [ -d NuttX/nuttx ]; if [ "$STATUSRETVAL" == "" ]; then echo "Checked NuttX submodule, correct version found" else - echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'" + echo "NuttX sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi @@ -28,7 +28,7 @@ if [ -d mavlink/include/mavlink/v1.0 ]; if [ "$STATUSRETVAL" == "" ]; then echo "Checked mavlink submodule, correct version found" else - echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'" + echo "mavlink sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" exit 1 fi From ee9233451244604f1522dda5e58d1deb7ec6473d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 20:23:07 +1000 Subject: [PATCH 161/600] build: fixed running build from external directory --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 8bf96ca230..7b7c00704f 100644 --- a/Makefile +++ b/Makefile @@ -210,11 +210,11 @@ menuconfig: endif $(NUTTX_SRC): - $(Q) (./Tools/check_submodules.sh) + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) .PHONY: checksubmodules checksubmodules: - $(Q) (./Tools/check_submodules.sh) + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) .PHONY: updatesubmodules updatesubmodules: From 6814ddccffd6f9f5a208ad87ffb5aa4045ba8543 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 8 Jul 2014 20:19:17 +0400 Subject: [PATCH 162/600] UAVCAN as a submodule --- .gitignore | 1 - .gitmodules | 3 +++ Makefile | 3 +++ Tools/check_submodules.sh | 16 ++++++++++++++++ uavcan | 1 + 5 files changed, 23 insertions(+), 1 deletion(-) create mode 160000 uavcan diff --git a/.gitignore b/.gitignore index d0c624543f..8b09e4783f 100644 --- a/.gitignore +++ b/.gitignore @@ -36,6 +36,5 @@ mavlink/include/mavlink/v0.9/ tags .tags_sorted_by_file .pydevproject -/uavcan .ropeproject *.orig diff --git a/.gitmodules b/.gitmodules index 8436b398e0..4b84afac2a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "NuttX"] path = NuttX url = git://github.com/PX4/NuttX.git +[submodule "uavcan"] + path = uavcan + url = git://github.com/pavel-kirienko/uavcan.git diff --git a/Makefile b/Makefile index 8bf96ca230..e2d50e8fc8 100644 --- a/Makefile +++ b/Makefile @@ -212,6 +212,9 @@ endif $(NUTTX_SRC): $(Q) (./Tools/check_submodules.sh) +$(UAVCAN_DIR): + $(Q) (./Tools/check_submodules.sh) + .PHONY: checksubmodules checksubmodules: $(Q) (./Tools/check_submodules.sh) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index fb180ef47e..a178b4a387 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -31,4 +31,20 @@ else git submodule update; fi + +if [ -d uavcan/libuavcan_drivers ]; +then + STATUSRETVAL=$(git status --porcelain | grep -i uavcan) + if [ "$STATUSRETVAL" == "" ]; then + echo "Checked uavcan submodule, correct version found" + else + echo "uavcan sub repo not at correct version. Try 'make updatesubmodules'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi +else + git submodule init + git submodule update +fi + exit 0 diff --git a/uavcan b/uavcan new file mode 160000 index 0000000000..f66c1a7de3 --- /dev/null +++ b/uavcan @@ -0,0 +1 @@ +Subproject commit f66c1a7de3076ff956bdf159dc3a166cbffe6089 From c39e572a063b8e17c2c8f896fdae4d0ec159960f Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 8 Jul 2014 23:58:16 +0200 Subject: [PATCH 163/600] Updated the Wing Wing params based on latest flight tests. --- ROMFS/px4fmu_common/init.d/3033_wingwing | 27 +++++++++++++++++------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index fff4b58df4..765685ccc0 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -11,25 +11,36 @@ if [ $DO_AUTOCONFIG == yes ] then param set BAT_N_CELLS 2 param set FW_AIRSPD_MAX 15 - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 11 + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 13 param set FW_ATT_TC 0.3 param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 12 + param set FW_L1_PERIOD 16 param set FW_LND_ANG 15 param set FW_LND_FLALT 5 param set FW_LND_HHDIST 15 param set FW_LND_HVIRT 13 param set FW_LND_TLALT 5 param set FW_THR_LND_MAX 0 - param set FW_P_ROLLFF 2 - param set FW_PR_FF 0.6 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.06 + param set FW_PR_FF 0.35 + param set FW_PR_I 0.005 + param set FW_PR_IMAX 0.4 + param set FW_PR_P 0.08 param set FW_RR_FF 0.6 + param set FW_RR_I 0.005 param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.09 + param set FW_RR_P 0.04 param set FW_THR_CRUISE 0.65 + param set MT_TKF_PIT_MAX 30.0 + param set MT_ACC_D 0.2 + param set MT_ACC_P 0.6 + param set MT_A_LP 0.5 + param set MT_PIT_OFF 0.1 + param set MT_PIT_I 0.1 + param set MT_THR_OFF 0.12 + param set MT_THR_I 0.35 + param set MT_THR_P 0.2 + param set MT_THR_FF 1.5 fi set MIXER FMU_Q From 9b2d444cc56eaaedcf271f200b93dcca94623209 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Jul 2014 14:08:09 +0200 Subject: [PATCH 164/600] dataman: use DM_KEY_WAYPOINTS_OFFBOARD() macro everywhere --- src/modules/mavlink/mavlink_mission.cpp | 4 ++-- src/modules/navigator/mission.cpp | 23 +++++------------------ 2 files changed, 7 insertions(+), 20 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index b2288469c9..0dda2337e6 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -216,7 +216,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ void MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) { - dm_item_t dm_item = _dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id); struct mission_item_s mission_item; if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { @@ -650,7 +650,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) return; } - dm_item_t dm_item = _transfer_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 53f0724cdc..06e915f270 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -201,19 +201,11 @@ Mission::update_offboard_mission() /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ - dm_item_t dm_current; + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); - if (_offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, - (size_t)_offboard_mission.count, - _navigator->get_geofence(), - _navigator->get_home_position()->alt); + missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), + _navigator->get_home_position()->alt); } else { warnx("offboard mission update failed"); @@ -474,12 +466,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s mission = &_offboard_mission; - if (_offboard_mission.dataman_id == 0) { - dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_item = DM_KEY_WAYPOINTS_OFFBOARD_1; - } + dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { From bc602ff1e2c2ab6c08a7d5edf0b1f03308e011f8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Jul 2014 14:38:38 +0200 Subject: [PATCH 165/600] mavlink submodule fixed --- mavlink/include/mavlink/v1.0 | 1 + 1 file changed, 1 insertion(+) create mode 160000 mavlink/include/mavlink/v1.0 diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 new file mode 160000 index 0000000000..d1ebe85eb6 --- /dev/null +++ b/mavlink/include/mavlink/v1.0 @@ -0,0 +1 @@ +Subproject commit d1ebe85eb6bb06d0078f3e0b8144adb131263628 From 3157b06fee99b57fe336f7772b293f8689ff8cdf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 15:52:19 +0200 Subject: [PATCH 166/600] Generalized the airspeed check --- .../commander/state_machine_helper.cpp | 29 +++++++------------ 1 file changed, 11 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6b96e3a3f6..d894c9db09 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -666,29 +667,21 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { - fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) { + fd = orb_subscribe(ORB_ID(airspeed)); + + struct airspeed_s airspeed; + + if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) || + hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); - ret = fd; goto system_eval; } - struct differential_pressure_s diff_pres; - - ret = read(fd, &diff_pres, sizeof(diff_pres)); - - if (ret == sizeof(diff_pres)) { - if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { - mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); - // XXX do not make this fatal yet - ret = OK; - } - } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); - /* this is frickin' fatal */ - ret = ERROR; - goto system_eval; + if (fabsf(airspeed.indicated_airspeed_m_s > 5.0f)) { + mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + // XXX do not make this fatal yet + ret = OK; } } From 5bb8c501122de7daece58c5770b6aca13c0066cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 16:14:21 +0200 Subject: [PATCH 167/600] Fixed the submodule check logic --- Tools/check_submodules.sh | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index db81ed04a7..a56de681f0 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -2,12 +2,19 @@ if [ -d NuttX/nuttx ]; then - STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") + STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<") if [ -z "$STATUSRETVAL" ]; then echo "Checked NuttX submodule, correct version found" else - echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'" + echo "" + echo "" + echo "NuttX sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" exit 1 fi else @@ -18,12 +25,19 @@ fi if [ -d mavlink/include/mavlink/v1.0 ]; then - STATUSRETVAL=$(git status --porcelain | grep -i "mavlink/include/mavlink/v1.0") + STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<") if [ -z "$STATUSRETVAL" ]; then echo "Checked mavlink submodule, correct version found" else - echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'" + echo "" + echo "" + echo "mavlink sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" exit 1 fi else From e57579fa21ce9be189475e41fbcdb961d7cd32d2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Jul 2014 17:08:23 +0200 Subject: [PATCH 168/600] mavlink, navigator: compile errors/warnings fixed --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- src/modules/mavlink/mavlink_mission.cpp | 2 +- src/modules/navigator/mission.cpp | 12 ++++-------- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5c7eefa98f..0d685c3d86 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -226,8 +226,9 @@ Mavlink::Mavlink() : _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), - _mission_result_sub(-1), + _mission_manager(nullptr), _mission_pub(-1), + _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), _total_counter(0), _verbose(false), @@ -245,7 +246,6 @@ Mavlink::Mavlink() : _param_component_id(0), _param_system_type(0), _param_use_hil_gps(0), - _mission_manager(nullptr), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 0dda2337e6..ca01344d4a 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -69,9 +69,9 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), + _dataman_id(0), _count(0), _current_seq(0), - _dataman_id(0), _transfer_dataman_id(0), _transfer_count(0), _transfer_seq(0), diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index eb8dcc7176..1268776514 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -72,10 +72,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), - _dist_1wp_ok(false), - _need_takeoff(true), - _takeoff(false) - + _dist_1wp_ok(false) { /* load initial params */ updateParams(); @@ -203,7 +200,7 @@ Mission::update_offboard_mission() /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ - dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), @@ -252,13 +249,12 @@ Mission::check_dist_1wp() } /* check if first waypoint is not too far from home */ - bool mission_valid = false; if (_param_dist_1wp.get() > 0.0f) { if (_navigator->get_vstatus()->condition_home_position_valid) { struct mission_item_s mission_item; /* find first waypoint (with lat/lon) item in datamanager */ - for (int i = 0; i < _offboard_mission.count; i++) { + for (unsigned i = 0; i < _offboard_mission.count; i++) { if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { @@ -281,7 +277,7 @@ Mission::check_dist_1wp() } else { /* item is too far from home */ - mavlink_log_info(_navigator->get_mavlink_fd(), "first wp is too far from home: %.1fm > %.1fm", dist_to_1wp, _param_dist_1wp.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "first wp is too far from home: %.1fm > %.1fm", (double)dist_to_1wp, (double)_param_dist_1wp.get()); return false; } } From 687612dd654bcdfb390bfdddf8a004b10119c2ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 18:34:33 +0200 Subject: [PATCH 169/600] Do not abort if the sensor reset failed, only abort on no data --- src/modules/commander/airspeed_calibration.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 5d21d89d04..923c1a3fff 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -95,10 +95,8 @@ int do_airspeed_calibration(int mavlink_fd) } if (!paramreset_successful) { - warn("FAILED to set scale / offsets for airspeed"); - mavlink_log_critical(mavlink_fd, "dpress reset failed"); - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); - return ERROR; + warn("FAILED to reset - assuming analog"); + mavlink_log_critical(mavlink_fd, "assuming analog sensor"); } while (calibration_counter < calibration_count) { From dca8daeec54624001f02acb7ad6f4c2d7304a5fb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Jul 2014 21:37:00 +0200 Subject: [PATCH 170/600] mavlink: use all outputs in HIL mode --- src/modules/mavlink/mavlink_messages.cpp | 55 ++++++++++-------------- 1 file changed, 23 insertions(+), 32 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 667be87b70..c7ad605c5e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1051,10 +1051,16 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + /* scale outputs depending on system type */ if (mavlink_system.type == MAV_TYPE_QUADROTOR || mavlink_system.type == MAV_TYPE_HEXAROTOR || mavlink_system.type == MAV_TYPE_OCTOROTOR) { - /* set number of valid outputs depending on vehicle type */ + /* multirotors: set number of rotor outputs depending on type */ + unsigned n; switch (mavlink_system.type) { @@ -1071,59 +1077,44 @@ protected: break; } - /* scale / assign outputs depending on system type */ - float out[8]; - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (i < n) { + /* scale PWM out 900..2100 us to 0..1 for rotors */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); } else { - /* send 0 when disarmed */ - out[i] = 0.0f; + /* scale PWM out 900..2100 us to -1..1 for other channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); } } else { - out[i] = -1.0f; + /* send 0 when disarmed */ + out[i] = 0.0f; } } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); } else { - - /* fixed wing: scale all channels except throttle -1 .. 1 - * because we know that we set the mixers up this way - */ - - float out[8]; - - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) { if (i != 3) { - /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ + /* scale PWM out 900..2100 us to -1..1 for normal channels */ out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); } else { - - /* scale fake PWM out 900..2100 us to 0..1 for throttle */ + /* scale PWM out 900..2100 us to 0..1 for throttle */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); } } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } } }; From c97e08bcf05455e9e9b582fe3216250a988890fb Mon Sep 17 00:00:00 2001 From: hxxnrx Date: Thu, 10 Jul 2014 21:50:23 +0200 Subject: [PATCH 171/600] Set IO PX4_I2C_BUS_ONBOARD I2C speed to 400KHz --- src/drivers/px4io/px4io_i2c.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) mode change 100644 => 100755 src/drivers/px4io/px4io_i2c.cpp diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp old mode 100644 new mode 100755 index 19776c40a2..c57ddf65b0 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -79,7 +79,7 @@ device::Device } PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : - I2C("PX4IO_i2c", nullptr, bus, address, 320000) + I2C("PX4IO_i2c", nullptr, bus, address, 400000) { _retries = 3; } From e8c5b8230d6dbdfdfb5ac9749a1b713d10c4521e Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Thu, 10 Jul 2014 23:01:41 -0400 Subject: [PATCH 172/600] px4io_uploader cleanup and minor optimization Remove redundant code Cleanup error handling in program function --- src/drivers/px4io/px4io_uploader.cpp | 51 +++++++++++----------------- src/drivers/px4io/uploader.h | 1 - 2 files changed, 20 insertions(+), 32 deletions(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index d134c0246e..bf6893a7e5 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -204,12 +204,8 @@ PX4IO_Uploader::upload(const char *filenames[]) if (bl_rev <= 2) { ret = verify_rev2(fw_size); - } else if(bl_rev == 3) { - ret = verify_rev3(fw_size); } else { - /* verify rev 4 and higher still uses the same approach and - * every version *needs* to be verified. - */ + /* verify rev 3 and higher. Every version *needs* to be verified. */ ret = verify_rev3(fw_size); } @@ -276,14 +272,14 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) int PX4IO_Uploader::recv(uint8_t *p, unsigned count) { + int ret; while (count--) { - int ret = recv(*p++, 5000); + ret = recv(*p++, 5000); if (ret != OK) - return ret; + break; } - - return OK; + return ret; } void @@ -314,21 +310,19 @@ PX4IO_Uploader::send(uint8_t c) #endif if (write(_io_fd, &c, 1) != 1) return -errno; - return OK; } int PX4IO_Uploader::send(uint8_t *p, unsigned count) { + int ret; while (count--) { - int ret = send(*p++); - + ret = send(*p++); if (ret != OK) - return ret; + break; } - - return OK; + return ret; } int @@ -419,12 +413,15 @@ PX4IO_Uploader::program(size_t fw_size) int ret; size_t sent = 0; - file_buf = (uint8_t *)malloc(PROG_MULTI_MAX); + file_buf = new uint8_t[PROG_MULTI_MAX]; if (!file_buf) { log("Can't allocate program buffer"); return -ENOMEM; } + ASSERT((fw_size & 3) == 0); + ASSERT((PROG_MULTI_MAX & 3) == 0); + log("programming %u bytes...", (unsigned)fw_size); ret = lseek(_fw_fd, 0, SEEK_SET); @@ -443,34 +440,26 @@ PX4IO_Uploader::program(size_t fw_size) (unsigned)sent, (int)count, (int)errno); - } - - if (count == 0) { - free(file_buf); - return OK; + ret = -errno; + break; } sent += count; - if (count < 0) - return -errno; - - ASSERT((count % 4) == 0); - send(PROTO_PROG_MULTI); send(count); - send(&file_buf[0], count); + send(file_buf, count); send(PROTO_EOC); ret = get_sync(1000); if (ret != OK) { - free(file_buf); - return ret; + break; } } - free(file_buf); - return OK; + + delete [] file_buf; + return ret; } int diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 55f63eef96..3e2142cf28 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -75,7 +75,6 @@ private: INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */ - READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */ }; From 50414257a8802fe7c51a370f598899c8b554914c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:09:57 +0200 Subject: [PATCH 173/600] Remove voltage field for digital sensors --- src/drivers/ets_airspeed/ets_airspeed.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 1a7e068fe5..d1a9fa57c8 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -186,7 +186,6 @@ ETSAirspeed::collect() report.differential_pressure_filtered_pa = (float)diff_pres_pa; report.differential_pressure_raw_pa = (float)diff_pres_pa_raw; report.temperature = -1000.0f; - report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; if (_airspeed_pub > 0 && !(_pub_blocked)) { From 56e1fe382b77200b3826b1ba438fc49ebab8f8ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:10:15 +0200 Subject: [PATCH 174/600] Remove voltage field for MEAS sensor --- src/drivers/meas_airspeed/meas_airspeed.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index c0f3c28e0f..7763f10578 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -265,7 +265,6 @@ MEASAirspeed::collect() } report.differential_pressure_raw_pa = diff_press_pa_raw; - report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; if (_airspeed_pub > 0 && !(_pub_blocked)) { From 3461d3d215843681b1537256d26b053e3f2b78e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:10:35 +0200 Subject: [PATCH 175/600] Introduce analog scaling parameter for analog airspeed sensor --- src/modules/sensors/sensor_params.c | 10 ++++++++-- src/modules/sensors/sensors.cpp | 18 +++++++++--------- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 5deb471d69..6b90a12391 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -194,16 +194,22 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); /** * Differential pressure sensor offset * + * The offset (zero-reading) in Pascal + * * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); /** - * Differential pressure sensor analog enabled + * Differential pressure sensor analog scaling + * + * Pick the appropriate scaling from the datasheet. + * this number defines the (linear) conversion from voltage + * to Pascal (pa). * * @group Sensor Calibration */ -PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); +PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); /** diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6e2d906e64..0cad0c0e58 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -248,7 +248,7 @@ private: float accel_offset[3]; float accel_scale[3]; float diff_pres_offset_pa; - float diff_pres_analog_enabled; + float diff_pres_analog_scale; int board_rotation; int external_mag_rotation; @@ -311,7 +311,7 @@ private: param_t mag_offset[3]; param_t mag_scale[3]; param_t diff_pres_offset_pa; - param_t diff_pres_analog_enabled; + param_t diff_pres_analog_scale; param_t rc_map_roll; param_t rc_map_pitch; @@ -590,7 +590,7 @@ Sensors::Sensors() : /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); - _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); + _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); @@ -798,7 +798,7 @@ Sensors::parameters_update() /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); - param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled)); + param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { @@ -1323,22 +1323,22 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ - float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) + float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor) /** * The voltage divider pulls the signal down, only act on * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) { + if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0)) { - float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor + float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale; _diff_pres.timestamp = t; - _diff_pres.differential_pressure_pa = diff_pres_pa; + _diff_pres.differential_pressure_pa = diff_pres_pa - _parameters.diff_pres_offset_pa; + _diff_pres.differential_pressure_raw_pa = diff_pres_pa; _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; _diff_pres.temperature = -1000.0f; - _diff_pres.voltage = voltage; /* announce the airspeed if needed, just publish else */ if (_diff_pres_pub > 0) { From 8d081a8b0dd002cda075ee9d8e087fdf54722ccf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:11:03 +0200 Subject: [PATCH 176/600] uORB: Remove voltage field from airspeed --- src/modules/uORB/topics/differential_pressure.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 01e14cda9e..cd48d3cb2f 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -58,7 +58,6 @@ struct differential_pressure_s { float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; From 25f3f6e7f2dd87a831d25a9348a67ed918407d96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:12:31 +0200 Subject: [PATCH 177/600] airspeed calibration improvements for analog sensors --- src/modules/commander/airspeed_calibration.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 923c1a3fff..12e527a681 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -76,7 +76,7 @@ int do_airspeed_calibration(int mavlink_fd) /* Reset sensor parameters */ struct airspeed_scale airscale = { - 0.0f, + diff_pres_offset, 1.0f, }; @@ -97,6 +97,12 @@ int do_airspeed_calibration(int mavlink_fd) if (!paramreset_successful) { warn("FAILED to reset - assuming analog"); mavlink_log_critical(mavlink_fd, "assuming analog sensor"); + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + close(diff_pres_sub); + return ERROR; + } } while (calibration_counter < calibration_count) { From 4c13c67504fb2b66c97ac6b902e67a864f1173e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:02:22 +0200 Subject: [PATCH 178/600] Hotfix: Close fd before reusing it again --- src/modules/commander/state_machine_helper.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6b96e3a3f6..ca95f139e4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -666,6 +666,8 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { + /* accel done, close it */ + close(fd); fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd < 0) { From 7ea76336ca03f724ba76661f986cc12cd0466ff8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:24:07 +0200 Subject: [PATCH 179/600] Better Doxygen for topics, no code changes --- src/drivers/drv_px4flow.h | 16 ++++++++++++++-- src/drivers/drv_range_finder.h | 9 +++++++++ src/drivers/drv_rc_input.h | 9 +++++++++ 3 files changed, 32 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index bef02d54ef..76ec55c3e8 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -46,10 +46,18 @@ #define PX4FLOW_DEVICE_PATH "/dev/px4flow" +/** + * @addtogroup topics + * @{ + */ + /** * Optical flow in NED body frame in SI units. * * @see http://en.wikipedia.org/wiki/International_System_of_Units + * + * @warning If possible the usage of the raw flow and performing rotation-compensation + * using the autopilot angular rate estimate is recommended. */ struct px4flow_report { @@ -57,14 +65,18 @@ struct px4flow_report { 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 */ - float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */ + float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */ + float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */ float ground_distance_m; /**< Altitude / distance to ground in meters */ uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ uint8_t sensor_id; /**< id of the sensor emitting the flow value */ }; +/** + * @} + */ + /* * ObjDev tag for px4flow data. */ diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index e45939b371..0f8362f588 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -50,6 +50,11 @@ enum RANGE_FINDER_TYPE { RANGE_FINDER_TYPE_LASER = 0, }; +/** + * @addtogroup topics + * @{ + */ + /** * range finder report structure. Reads from the device must be in multiples of this * structure. @@ -64,6 +69,10 @@ struct range_finder_report { uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ }; +/** + * @} + */ + /* * ObjDev tag for raw range finder data. */ diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 20763e2657..47fa8fa59a 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -67,6 +67,11 @@ */ #define RC_INPUT_RSSI_MAX 255 +/** + * @addtogroup topics + * @{ + */ + /** * Input signal type, value is a control position from zero to 100 * percent. @@ -141,6 +146,10 @@ struct rc_input_values { rc_input_t values[RC_INPUT_MAX_CHANNELS]; }; +/** + * @} + */ + /* * ObjDev tag for R/C inputs. */ From ee573fea9d27e7925b375abd95d540c0fda30d4c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:25:16 +0200 Subject: [PATCH 180/600] GPS driver: Print velocity as part of status command --- src/drivers/gps/gps.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 401b65dd4f..34dd630866 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -127,7 +127,7 @@ private: /** * Try to configure the GPS, handle outgoing communication to the GPS */ - void config(); + void config(); /** * Trampoline to the worker task @@ -486,6 +486,8 @@ GPS::print_info() warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); + warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); From b97c867420f477f2c3a7dbc073975f5d872194cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:51:13 +0200 Subject: [PATCH 181/600] Add a check command and fix error reporting --- src/modules/commander/commander.cpp | 19 +++++++++------ .../commander/state_machine_helper.cpp | 23 ++++++++----------- src/modules/commander/state_machine_helper.h | 2 ++ 3 files changed, 24 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2058cd92e2..106f05f572 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -286,15 +286,20 @@ int commander_main(int argc, char *argv[]) exit(0); } + /* commands needing the app to run below */ + if (!thread_running) { + warnx("\tcommander not started"); + exit(1); + } + if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running"); - print_status(); - - } else { - warnx("\tcommander not started"); - } + print_status(); + exit(0); + } + if (!strcmp(argv[1], "check")) { + int checkres = prearm_check(&status, mavlink_fd); + warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); exit(0); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ca95f139e4..d6b4d43adc 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -70,8 +70,6 @@ #endif static const int ERROR = -1; -static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); - // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even @@ -622,12 +620,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { int ret; + bool failed = false; int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); - ret = fd; + failed = true; goto system_eval; } @@ -635,6 +634,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret != OK) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + failed = true; goto system_eval; } @@ -646,22 +646,20 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) /* evaluate values */ float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - if (accel_scale < 9.78f || accel_scale > 9.83f) { + if (accel_scale < 9.75f || accel_scale > 9.85f) { mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); } if (accel_scale > 30.0f /* m/s^2 */) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; - } else { - ret = OK; } } else { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; } @@ -672,7 +670,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); - ret = fd; + failed = true; goto system_eval; } @@ -681,20 +679,19 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) ret = read(fd, &diff_pres, sizeof(diff_pres)); if (ret == sizeof(diff_pres)) { - if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { + if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) { mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); // XXX do not make this fatal yet - ret = OK; } } else { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; } } system_eval: close(fd); - return ret; + return (!failed); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 11072403ea..ddc5bf154c 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,4 +67,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); + #endif /* STATE_MACHINE_HELPER_H_ */ From a82d4fbb115e949a6a5c12d46308df1c20abfd99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:55:46 +0200 Subject: [PATCH 182/600] Hotfix: fix typo --- 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 106f05f572..6138a4c37d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -308,7 +308,7 @@ int commander_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "2")) { + if (!strcmp(argv[1], "disarm")) { arm_disarm(false, mavlink_fd, "command line"); exit(0); } From 79c5d434bd5911d11e6968e0a339b40a6f82e033 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 15:10:11 +0200 Subject: [PATCH 183/600] Commander: More hotfixes to prearm check routine --- src/modules/commander/commander.cpp | 5 ++++- src/modules/commander/state_machine_helper.cpp | 5 +++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6138a4c37d..18761665a2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -298,7 +298,9 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "check")) { - int checkres = prearm_check(&status, mavlink_fd); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + int checkres = prearm_check(&status, mavlink_fd_local); + close(mavlink_fd_local); warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); exit(0); } @@ -329,6 +331,7 @@ void usage(const char *reason) void print_status() { + warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); /* read all relevant states */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d6b4d43adc..5cbc95920a 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -664,11 +664,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { + /* accel done, close it */ close(fd); fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) { + if (fd <= 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; @@ -693,5 +694,5 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) system_eval: close(fd); - return (!failed); + return (failed); } From a118e8dbdd72e23a47841577dfcd7a45007eaedc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 15:17:21 +0200 Subject: [PATCH 184/600] Make commander error message more verbose --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5cbc95920a..ce1bd9ada5 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -647,7 +647,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_scale < 9.75f || accel_scale > 9.85f) { - mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); + mavlink_log_info(mavlink_fd, "#audio: Accel calib. recommended (%8.4f).", (double)accel_scale); } if (accel_scale > 30.0f /* m/s^2 */) { From 11d9724563f005ed722d326e8bef125bfec29865 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 16:03:27 +0200 Subject: [PATCH 185/600] Stop warning users just because they shake a bit, be more strict to catch sensor failures or calibration errors in time --- src/modules/commander/state_machine_helper.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ce1bd9ada5..363f5e8eb4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -644,14 +644,11 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret == sizeof(acc)) { /* evaluate values */ - float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - if (accel_scale < 9.75f || accel_scale > 9.85f) { - mavlink_log_info(mavlink_fd, "#audio: Accel calib. recommended (%8.4f).", (double)accel_scale); - } - - if (accel_scale > 30.0f /* m/s^2 */) { + if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); + mavlink_log_info(mavlink_fd, "#audio: hold still while arming"); /* this is frickin' fatal */ failed = true; goto system_eval; From aa055825984c121c0d2b74d81463282f6400688d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 21:04:09 +0200 Subject: [PATCH 186/600] FMUv1: Reduce excessive stack sizes --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index f2c7d22bfe..6717ae19a8 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -417,8 +417,8 @@ CONFIG_PREALLOC_TIMERS=50 # # Stack and heap information # -CONFIG_IDLETHREAD_STACKSIZE=4096 -CONFIG_USERMAIN_STACKSIZE=3500 +CONFIG_IDLETHREAD_STACKSIZE=3500 +CONFIG_USERMAIN_STACKSIZE=3000 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 From c474d2cbf1a9c78be5804b224c504b57f9248760 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 21:04:34 +0200 Subject: [PATCH 187/600] FMUv2: Reduce excessive stack sizes --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 7d5e6e9df5..a55c95a291 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,8 +451,8 @@ CONFIG_PREALLOC_TIMERS=50 # # Stack and heap information # -CONFIG_IDLETHREAD_STACKSIZE=6000 -CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_IDLETHREAD_STACKSIZE=3500 +CONFIG_USERMAIN_STACKSIZE=3000 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 From a3210f31fe431b31b971f9d25d41a5c18734120c Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 11 Jul 2014 21:30:59 +0200 Subject: [PATCH 188/600] Create a mixer for the Wing Wing. --- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/mixers/FMU_wingwing | 70 ++++++++++++++++++++++++ 2 files changed, 71 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/mixers/FMU_wingwing diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index fff4b58df4..75163168e3 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -32,7 +32,7 @@ then param set FW_THR_CRUISE 0.65 fi -set MIXER FMU_Q +set MIXER FMU_wingwing # Provide ESC a constant 1000 us pulse set PWM_OUTPUTS 4 set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_wingwing b/ROMFS/px4fmu_common/mixers/FMU_wingwing new file mode 100644 index 0000000000..806bfec734 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_wingwing @@ -0,0 +1,70 @@ +Delta-wing mixer for PX4FMU +=========================== + +Designed for Wing Wing Z-84 + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 6500 6500 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 -6500 -6500 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 From e4264aad825540c159888b68fc2a0dec599b2e9b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 11 Jul 2014 21:34:51 +0200 Subject: [PATCH 189/600] The wing wing is too small to have flaps, gimbals, etc. --- ROMFS/px4fmu_common/mixers/FMU_wingwing | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FMU_wingwing b/ROMFS/px4fmu_common/mixers/FMU_wingwing index 806bfec734..08333ba5cf 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_wingwing +++ b/ROMFS/px4fmu_common/mixers/FMU_wingwing @@ -49,22 +49,3 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 - -Gimbal / flaps / payload mixer for last four channels ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 From 0a3a13ac7061566aac97321f09329b6228f0ed7e Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 11 Jul 2014 21:38:12 +0200 Subject: [PATCH 190/600] Updated following flight tests today. --- ROMFS/px4fmu_common/init.d/3033_wingwing | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 765685ccc0..92f592aaf6 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -30,14 +30,13 @@ then param set FW_RR_I 0.005 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 - param set FW_THR_CRUISE 0.65 param set MT_TKF_PIT_MAX 30.0 param set MT_ACC_D 0.2 param set MT_ACC_P 0.6 param set MT_A_LP 0.5 param set MT_PIT_OFF 0.1 param set MT_PIT_I 0.1 - param set MT_THR_OFF 0.12 + param set MT_THR_OFF 0.65 param set MT_THR_I 0.35 param set MT_THR_P 0.2 param set MT_THR_FF 1.5 From f8115e4e2e2cc0ad55949016c7872125a6727fac Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 11 Jul 2014 21:51:35 +0200 Subject: [PATCH 191/600] Renamed mixer file. --- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/mixers/{FMU_wingwing => wingwing.mix} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename ROMFS/px4fmu_common/mixers/{FMU_wingwing => wingwing.mix} (100%) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 75163168e3..3cbbd555e3 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -32,7 +32,7 @@ then param set FW_THR_CRUISE 0.65 fi -set MIXER FMU_wingwing +set MIXER wingwing # Provide ESC a constant 1000 us pulse set PWM_OUTPUTS 4 set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_wingwing b/ROMFS/px4fmu_common/mixers/wingwing.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_wingwing rename to ROMFS/px4fmu_common/mixers/wingwing.mix From 899657613e44f0d4bbdb66644b35420d7381ff99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:09:25 +0200 Subject: [PATCH 192/600] airspeed cal: Improve user feedback --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 12e527a681..7599fc777c 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -96,7 +96,7 @@ int do_airspeed_calibration(int mavlink_fd) if (!paramreset_successful) { warn("FAILED to reset - assuming analog"); - mavlink_log_critical(mavlink_fd, "assuming analog sensor"); + mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); From e3afb669cae54b042f453605601fed861c52ee45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:09:47 +0200 Subject: [PATCH 193/600] sensors: Fix usage of offset for measurements --- src/modules/sensors/sensor_params.c | 2 +- src/modules/sensors/sensors.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 6b90a12391..8f6e7abe60 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -205,7 +205,7 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); * * Pick the appropriate scaling from the datasheet. * this number defines the (linear) conversion from voltage - * to Pascal (pa). + * to Pascal (pa). For the MPXV7002DP this is 1000. * * @group Sensor Calibration */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 0cad0c0e58..cda79693d2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1330,12 +1330,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw) * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0)) { + if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { - float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale; + float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; _diff_pres.timestamp = t; - _diff_pres.differential_pressure_pa = diff_pres_pa - _parameters.diff_pres_offset_pa; + _diff_pres.differential_pressure_pa = diff_pres_pa; _diff_pres.differential_pressure_raw_pa = diff_pres_pa; _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; _diff_pres.temperature = -1000.0f; From 9c63aba9a72083afe1e5f818c6559760c5b6b1cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:44:54 +0200 Subject: [PATCH 194/600] Ensure NaN check is executed before any parts of the filter are run. Fix GPS velocity reset for case of initialized GPS --- .../ekf_att_pos_estimator_main.cpp | 26 +++++++++++-------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5d768b73dd..bc5f709e58 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1140,6 +1140,20 @@ FixedwingEstimator::task_main() float initVelNED[3]; + // Guard before running any parts of the filter + // of NaN / invalid values + if (_ekf->statesInitialised) { + + // We're apparently initialized in this case now + + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } + } + /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1204,16 +1218,6 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } else if (_ekf->statesInitialised) { - // We're apparently initialized in this case now - - int check = check_filter_state(); - - if (check) { - // Let the system re-initialize itself - continue; - } - - // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 @@ -1281,7 +1285,7 @@ FixedwingEstimator::task_main() // run the fusion step _ekf->FuseVelposNED(); - } else if (_ekf->statesInitialised) { + } else if (!_gps_initialized && _ekf->statesInitialised) { // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; From 2fa4434a03abe057e65a5c8ba29f8cd9cd75ca93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 00:17:50 +0200 Subject: [PATCH 195/600] Limit output of driver to positive range --- src/modules/sensors/sensors.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cda79693d2..1ffd7f02f1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -501,6 +501,7 @@ Sensors::Sensors() : _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); + memset(&_diff_pres, 0, sizeof(_diff_pres)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1332,12 +1333,13 @@ Sensors::adc_poll(struct sensor_combined_s &raw) */ if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { - float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f; _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; - _diff_pres.differential_pressure_raw_pa = diff_pres_pa; - _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; + _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f); _diff_pres.temperature = -1000.0f; /* announce the airspeed if needed, just publish else */ From 065bf013a407da5e2a70db82013126c20e2ad429 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:29:30 +0200 Subject: [PATCH 196/600] More obvious error message for mission reject --- src/modules/navigator/mission.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 1268776514..ba766cd10f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -277,7 +277,7 @@ Mission::check_dist_1wp() } else { /* item is too far from home */ - mavlink_log_info(_navigator->get_mavlink_fd(), "first wp is too far from home: %.1fm > %.1fm", (double)dist_to_1wp, (double)_param_dist_1wp.get()); + mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get()); return false; } } From 5616a07bf306b3fa2bc70078e9c8c26a086065dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:29:56 +0200 Subject: [PATCH 197/600] Final default values and more comments for params --- src/modules/navigator/mission_params.c | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 5cb3782c93..881caa24e0 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -59,22 +59,26 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); /** - * Enable onboard mission + * Enable persistent onboard mission storage + * + * When enabled, missions that have been uploaded by the GCS are stored + * and reloaded after reboot persistently. * * @min 0 * @max 1 * @group Mission */ -PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0); +PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); /** * Maximal horizontal distance from home to first waypoint * - * Failsafe check to prevent running mission stored from previous flight on new place. - * Value < 0 means that check disabled. + * Failsafe check to prevent running mission stored from previous flight at a new takeoff location. + * Set a value of zero or less to disable. The mission will not be started if the current + * waypoint is more distant than MIS_DIS_1WP from the current position. * - * @min -1 - * @max 200 + * @min 0 + * @max 250 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 50); +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175); From 629ab5540fcb62b4c0ecc49e964f2d3fcc0b8a4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:30:49 +0200 Subject: [PATCH 198/600] Fix severity handling of text messages --- src/modules/mavlink/mavlink_main.cpp | 39 ++++++++++++++++++----- src/modules/mavlink/mavlink_main.h | 46 ++++++++++++++++++++++------ 2 files changed, 68 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0d685c3d86..cd0581af43 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -492,6 +492,7 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) // printf("logmsg: %s\n", txt); struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); + msg.severity = (unsigned char)cmd; Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { @@ -840,7 +841,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); - send_statustext("[mavlink pm] sending list"); + send_statustext_info("[pm] sending list"); } } break; @@ -864,7 +865,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown: %s", name); - send_statustext(buf); + send_statustext_info(buf); } else { /* set and send parameter */ @@ -901,17 +902,28 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav } int -Mavlink::send_statustext(const char *string) +Mavlink::send_statustext_info(const char *string) { - return send_statustext(MAV_SEVERITY_INFO, string); + return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); } int -Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) +Mavlink::send_statustext_critical(const char *string) +{ + return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); +} + +int +Mavlink::send_statustext_emergency(const char *string) +{ + return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); +} + +int +Mavlink::send_statustext(unsigned severity, const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; - statustext.severity = severity; int i = 0; @@ -929,6 +941,19 @@ Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) /* Enforce null termination */ statustext.text[i] = '\0'; + /* Map severity */ + switch (severity) { + case MAVLINK_IOC_SEND_TEXT_INFO: + statustext.severity = MAV_SEVERITY_INFO; + break; + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + statustext.severity = MAV_SEVERITY_CRITICAL; + break; + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + statustext.severity = MAV_SEVERITY_EMERGENCY; + break; + } + mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); return OK; @@ -1449,7 +1474,7 @@ Mavlink::task_main(int argc, char *argv[]) int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { - send_statustext(msg.text); + send_statustext(msg.severity, msg.text); } } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1f4fb759fe..acfc8b90eb 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -157,9 +157,9 @@ public: */ int set_hil_enabled(bool hil_enabled); - void send_message(const mavlink_message_t *msg); + void send_message(const mavlink_message_t *msg); - void handle_message(const mavlink_message_t *msg); + void handle_message(const mavlink_message_t *msg); MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); @@ -174,17 +174,43 @@ public: mavlink_channel_t get_channel(); - void configure_stream_threadsafe(const char *stream_name, float rate); + void configure_stream_threadsafe(const char *stream_name, float rate); bool _task_should_exit; /**< if true, mavlink task should exit */ int get_mavlink_fd() { return _mavlink_fd; } - int send_statustext(const char *string); - int send_statustext(enum MAV_SEVERITY severity, const char *string); - MavlinkStream * get_streams() const { return _streams; } + /** + * Send a status text with loglevel INFO + * + * @param string the message to send (will be capped by mavlink max string length) + */ + int send_statustext_info(const char *string); - float get_rate_mult(); + /** + * Send a status text with loglevel CRITICAL + * + * @param string the message to send (will be capped by mavlink max string length) + */ + int send_statustext_critical(const char *string); + + /** + * Send a status text with loglevel EMERGENCY + * + * @param string the message to send (will be capped by mavlink max string length) + */ + int send_statustext_emergency(const char *string); + + /** + * Send a status text with loglevel + * + * @param string the message to send (will be capped by mavlink max string length) + * @param severity the log level, one of + */ + int send_statustext(unsigned severity, const char *string); + MavlinkStream * get_streams() const { return _streams; } + + float get_rate_mult(); /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } @@ -195,13 +221,13 @@ public: bool message_buffer_write(const void *ptr, int size); - void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } - void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } /** * Count a transmision error */ - void count_txerr(); + void count_txerr(); protected: Mavlink *next; From 7b768d11c3c8d57807495bf2abb324bb5f14aa14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 14:31:09 +0200 Subject: [PATCH 199/600] Improve mission feedback, fix compile errors --- src/modules/mavlink/mavlink_mission.cpp | 58 +++++++++++++++---------- src/modules/mavlink/mavlink_mission.h | 36 +++++++-------- 2 files changed, 52 insertions(+), 42 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index ca01344d4a..a62d34019a 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -189,7 +189,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq) } else { if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); } - _mavlink->send_statustext("ERROR: wp index out of bounds"); + _mavlink->send_statustext_critical("ERROR: wp index out of bounds"); } } @@ -238,7 +238,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext("#audio: Unable to read from micro SD"); + _mavlink->send_statustext_critical("Unable to read from micro SD"); if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } } @@ -262,7 +262,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } } else { - _mavlink->send_statustext("ERROR: Waypoint index exceeds list capacity"); + _mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity"); if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); } } @@ -314,7 +314,7 @@ MavlinkMissionManager::eventloop() /* check for timed-out operations */ if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) { - _mavlink->send_statustext("Operation timeout"); + _mavlink->send_statustext_critical("Operation timeout"); if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); } @@ -390,6 +390,8 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); } } else { + _mavlink->send_statustext_critical("WPM: ERR: not all items sent -> IDLE"); + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); } } @@ -397,7 +399,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) } } else { - _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + _mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch"); if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } } @@ -421,18 +423,20 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } + + _mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID"); } } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); } - _mavlink->send_statustext("IGN WP CURR CMD: Not in list"); + _mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list"); } } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } - _mavlink->send_statustext("IGN WP CURR CMD: Busy"); + _mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy"); } } } @@ -459,6 +463,8 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } + + _mavlink->send_statustext_info("WPM: mission is empty"); } send_mission_count(msg->sysid, msg->compid, _count); @@ -466,7 +472,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } - _mavlink->send_statustext("IGN REQUEST LIST: Busy"); + _mavlink->send_statustext_critical("IGN REQUEST LIST: Busy"); } } } @@ -489,7 +495,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) _transfer_seq++; - } else if (wpr.seq == _transfer_seq - 1 && wpr.seq >= 0) { + } else if (wpr.seq == _transfer_seq - 1) { if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); } } else { @@ -506,36 +512,36 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) _state = MAVLINK_WPM_STATE_IDLE; send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected"); return; } /* double check bounds in case of items count changed */ - if (wpr.seq >= 0 && wpr.seq < _count) { + if (wpr.seq < _count) { send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [0, %u]", wpr.seq, msg->sysid, wpr.seq, _count - 1); } + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)_count - 1); } _state = MAVLINK_WPM_STATE_IDLE; send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected"); } } else if (_state == MAVLINK_WPM_STATE_IDLE) { if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); } - _mavlink->send_statustext("IGN MISSION_ITEM_REQUEST: No transfer"); + _mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST: No active transfer"); } else { if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); } - _mavlink->send_statustext("REJ. WP CMD: Busy"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy"); } } else { - _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch"); if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } } @@ -565,7 +571,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) /* alternate dataman ID anyway to let navigator know about changes */ update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); - _mavlink->send_statustext("WP COUNT 0: CLEAR MISSION"); + _mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION"); // TODO send ACK? return; @@ -588,19 +594,19 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) /* looks like our MISSION_REQUEST was lost, try again */ if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); } - _mavlink->send_statustext("WP CMD OK AGAIN"); + _mavlink->send_statustext_info("WP CMD OK TRY AGAIN"); } else { if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); } - _mavlink->send_statustext("REJ. WP CMD: Busy"); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy"); return; } } else { if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); } - _mavlink->send_statustext("IGN MISSION_COUNT: Busy"); + _mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy"); return; } @@ -629,13 +635,13 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) } else if (_state == MAVLINK_WPM_STATE_IDLE) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); } - _mavlink->send_statustext("IGN MISSION_ITEM: No transfer"); + _mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer"); return; } else { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } - _mavlink->send_statustext("IGN MISSION_ITEM: Busy"); + _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); return; } @@ -645,6 +651,8 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) if (ret != OK) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); _state = MAVLINK_WPM_STATE_IDLE; return; @@ -656,7 +664,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext("#audio: Unable to write on micro SD"); + _mavlink->send_statustext_critical("Unable to write on micro SD"); _state = MAVLINK_WPM_STATE_IDLE; return; } @@ -674,6 +682,8 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) /* got all new mission items successfully */ if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } + _mavlink->send_statustext_info("WPM: Transfer complete."); + _state = MAVLINK_WPM_STATE_IDLE; if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { @@ -712,7 +722,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) } } else { - _mavlink->send_statustext("IGN WP CLEAR CMD: Busy"); + _mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy"); if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index d8695cf83d..1964a45e8e 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -142,35 +142,35 @@ public: void set_verbose(bool v) { _verbose = v; } private: - Mavlink* _mavlink; + Mavlink* _mavlink; mavlink_channel_t _channel; - uint8_t _comp_id; + uint8_t _comp_id; - enum MAVLINK_WPM_STATES _state; ///< Current state + enum MAVLINK_WPM_STATES _state; ///< Current state - uint64_t _time_last_recv; - uint64_t _time_last_sent; + uint64_t _time_last_recv; + uint64_t _time_last_sent; - uint32_t _action_timeout; - uint32_t _retry_timeout; - unsigned _max_count; ///< Maximal count of mission items + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximal count of mission items - int _dataman_id; ///< Dataman storage ID for active mission - unsigned _count; ///< Count of items in active mission - int _current_seq; ///< Current item sequence in active mission + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission int _transfer_dataman_id; ///< Dataman storage ID for current transmission - unsigned _transfer_count; ///< Items count in current transmission - int _transfer_seq; ///< Item sequence in current transmission - int _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) - uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission - uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + unsigned _transfer_seq; ///< Item sequence in current transmission + unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission + uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission int _offboard_mission_sub; int _mission_result_sub; - orb_advert_t _offboard_mission_pub; + orb_advert_t _offboard_mission_pub; - MavlinkRateLimiter _slow_rate_limiter; + MavlinkRateLimiter _slow_rate_limiter; bool _verbose; }; From e64a28e736224da5d1db8e3477eeeffc0b3b1f6c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 12 Jul 2014 17:34:36 +0400 Subject: [PATCH 200/600] Building UAVCAN without run-time checks. This saves 9.5KB of flash and reduces CPU usage. --- src/modules/uavcan/module.mk | 10 +++++----- uavcan | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 2c75944d4e..1ef6f0cfa8 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,9 +40,9 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os -SRCS += uavcan_main.cpp \ - uavcan_clock.cpp \ - esc_controller.cpp \ +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp \ + esc_controller.cpp \ gnss_receiver.cpp # @@ -53,7 +53,7 @@ SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. -override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS # # libuavcan drivers for STM32 @@ -68,7 +68,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM # TODO: Add make target for this, or invoke dsdlc manually. # The second option assumes that the generated headers shall be saved # under the version control, which may be undesirable. -# The first option requires python3 and python3-mako for the sources to be built. +# The first option requires any Python and the Python Mako library for the sources to be built. # $(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) INCLUDE_DIRS += dsdlc_generated diff --git a/uavcan b/uavcan index f66c1a7de3..af065e9ca9 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit f66c1a7de3076ff956bdf159dc3a166cbffe6089 +Subproject commit af065e9ca9e3ba9100c125d3ab739313d0500ca8 From a0d150332ab67dbc501b10fa8adb97d91a79f23c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 15:39:04 +0200 Subject: [PATCH 201/600] Add note to param about tubes --- src/modules/sensors/sensor_params.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 8f6e7abe60..38b1907612 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -207,6 +207,9 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); * this number defines the (linear) conversion from voltage * to Pascal (pa). For the MPXV7002DP this is 1000. * + * NOTE: If the sensor always registers zero, try switching + * the static and dynamic tubes. + * * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); From 024c8213a10d83743caea21206d21f3de497b18a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 12 Jul 2014 17:45:05 +0400 Subject: [PATCH 202/600] Fixed check_submodules.sh for UAVCAN --- Tools/check_submodules.sh | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index abe3088c7f..8fd9a8f003 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -46,14 +46,22 @@ else fi -if [ -d uavcan/libuavcan_drivers ]; +if [ -d uavcan ] then - STATUSRETVAL=$(git status --porcelain | grep -i uavcan) - if [ "$STATUSRETVAL" == "" ]; then + STATUSRETVAL=$(git submodule summary | grep -A20 -i uavcan | grep "<") + if [ -z "$STATUSRETVAL" ] + then echo "Checked uavcan submodule, correct version found" else - echo "uavcan sub repo not at correct version. Try 'make updatesubmodules'" + echo "" + echo "" + echo "uavcan sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" exit 1 fi else From 45617e9f4385e80846c571b237e220216192d6ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 16:09:14 +0200 Subject: [PATCH 203/600] Airspeed calibration improvement, enforce correct pitot order --- .../commander/airspeed_calibration.cpp | 137 ++++++++++++++---- src/modules/ekf_att_pos_estimator/InertialNav | 1 + 2 files changed, 112 insertions(+), 26 deletions(-) create mode 160000 src/modules/ekf_att_pos_estimator/InertialNav diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 7599fc777c..a1a20abc98 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -64,14 +64,12 @@ int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); - const int calibration_count = 2000; + const unsigned calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; - int calibration_counter = 0; float diff_pres_offset = 0.0f; /* Reset sensor parameters */ @@ -105,29 +103,58 @@ int do_airspeed_calibration(int mavlink_fd) } } - while (calibration_counter < calibration_count) { + for (unsigned i = 0; i < (sizeof(diff_pres_offset) / sizeof(diff_pres_offset[0])); i++) { - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; + unsigned calibration_counter = 0; - int poll_ret = poll(fds, 1, 1000); + if (i == 0) { + mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + usleep(500 * 1000); + } - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; + float diff_pres_offset = 0.0f; - if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + + if (i > 0) { + + if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } + + /* do not log negative values in the second go */ + if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + close(diff_pres_sub); + return ERROR; + } + } + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; } - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); - close(diff_pres_sub); - return ERROR; } } @@ -151,14 +178,72 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - tune_neutral(true); - close(diff_pres_sub); - return OK; - } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } + + /* wait 500 ms to ensure parameter propagated through the system */ + usleep(500 * 1000); + + calibration_counter = 0; + diff_pres_offset = 0.0f; + + /* just take a few samples and make sure pitot tubes are not reversed */ + while (calibration_counter < 50) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + float curr_avg = (diff_pres_offset / calibration_counter); + + if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } + + /* do not log negative values in the second go */ + if (curr_avg < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + close(diff_pres_sub); + + /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ + + diff_pres_offset = 0.0f; + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + close(diff_pres_sub); + return ERROR; + } + + /* save */ + (void)param_save_default(); + + return ERROR; + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } + } + + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + tune_neutral(true); + close(diff_pres_sub); + return OK; } diff --git a/src/modules/ekf_att_pos_estimator/InertialNav b/src/modules/ekf_att_pos_estimator/InertialNav new file mode 160000 index 0000000000..8b65d755b8 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/InertialNav @@ -0,0 +1 @@ +Subproject commit 8b65d755b8c4834f90a323172c25d91c45068e21 From 4dc65d6f09bcf891ee228ef9fc6b576251fc8b65 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 16:09:49 +0200 Subject: [PATCH 204/600] NuttX I2C fixes --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 2a94cc8e5b..7e1b97bcf1 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 2a94cc8e5babb7d5661aedc09201239d39644332 +Subproject commit 7e1b97bcf10d8495169eec355988ca5890bfd5df From 93d444d1aade59b5e88f41b71c842a00ab950c64 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:09:12 +1000 Subject: [PATCH 205/600] device: added a _device_id to all drivers this device ID identifies a specific device via the tuple of (bus, bus type, address, devtype). This allows device specific configuration data to be stored along with a device ID, so the code can know when the user has changed device configuration (such as removing an external compass), and either invalidate the device configuration or force the user to re-calibrate --- src/drivers/device/device.cpp | 12 ++++++++++++ src/drivers/device/device.h | 25 +++++++++++++++++++++++++ src/drivers/device/i2c.cpp | 8 +++++++- src/drivers/device/spi.cpp | 6 ++++++ src/drivers/drv_device.h | 7 +++++++ 5 files changed, 57 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index 6834551493..f1f777dce0 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -42,6 +42,7 @@ #include #include #include +#include namespace device { @@ -93,6 +94,13 @@ Device::Device(const char *name, _irq_attached(false) { sem_init(&_lock, 0, 1); + + /* setup a default device ID. When bus_type is UNKNOWN the + other fields are invalid */ + _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; + _device_id.devid_s.bus = 0; + _device_id.devid_s.address = 0; + _device_id.devid_s.devtype = 0; } Device::~Device() @@ -238,6 +246,10 @@ Device::write(unsigned offset, void *data, unsigned count) int Device::ioctl(unsigned operation, unsigned &arg) { + switch (operation) { + case DEVIOCGDEVICEID: + return (int)_device_id.devid; + } return -ENODEV; } diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index d99f229220..c98386eb07 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -124,9 +124,34 @@ public: */ virtual int ioctl(unsigned operation, unsigned &arg); + /* + device bus types for DEVID + */ + enum DeviceBusType { + DeviceBusType_UNKNOWN = 0, + DeviceBusType_I2C = 1, + DeviceBusType_SPI = 2 + }; + + /* + broken out + */ + struct DeviceStructure { + enum DeviceBusType bus_type; + uint8_t bus; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; + + union DeviceId { + struct DeviceStructure devid_s; + uint32_t devid; + }; + protected: const char *_name; /**< driver name */ bool _debug_enabled; /**< if true, debug messages are printed */ + union DeviceId _device_id; /**< device identifier information */ /** * Constructor diff --git a/src/drivers/device/i2c.cpp b/src/drivers/device/i2c.cpp index a416801eb7..286778c014 100644 --- a/src/drivers/device/i2c.cpp +++ b/src/drivers/device/i2c.cpp @@ -62,6 +62,12 @@ I2C::I2C(const char *name, _frequency(frequency), _dev(nullptr) { + // fill in _device_id fields for a I2C device + _device_id.devid_s.bus_type = DeviceBusType_I2C; + _device_id.devid_s.bus = bus; + _device_id.devid_s.address = address; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; } I2C::~I2C() @@ -201,4 +207,4 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs) return ret; } -} // namespace device \ No newline at end of file +} // namespace device diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index 1ab1dc6997..200acac05a 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -75,6 +75,12 @@ SPI::SPI(const char *name, _dev(nullptr), _bus(bus) { + // fill in _device_id fields for a SPI device + _device_id.devid_s.bus_type = DeviceBusType_SPI; + _device_id.devid_s.bus = bus; + _device_id.devid_s.address = (uint8_t)device; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; } SPI::~SPI() diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index b310beb746..19d55cef38 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -59,4 +59,11 @@ /** check publication block status */ #define DEVIOCGPUBBLOCK _DEVICEIOC(1) +/** + * Return device ID, to enable matching of configuration parameters + * (such as compass offsets) to specific sensors + */ +#define DEVIOCGDEVICEID _DEVICEIOC(2) + + #endif /* _DRV_DEVICE_H */ From a2739707bb18a1aed0dcc0f809badce606aaed51 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:09:26 +1000 Subject: [PATCH 206/600] drv_mag: added devtypes for magnetometers --- src/drivers/drv_mag.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 06107bd3d1..a259ac9c06 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -81,6 +81,13 @@ struct mag_scale { */ ORB_DECLARE(sensor_mag); + +/* + * mag device types, for _device_id + */ +#define DRV_MAG_DEVTYPE_HMC5883 1 +#define DRV_MAG_DEVTYPE_LSM303D 2 + /* * ioctl() definitions */ From 30a6a3d0b6cc53fef45264d140ce3026d986af83 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:09:38 +1000 Subject: [PATCH 207/600] hmc5883: setup device type --- src/drivers/hmc5883/hmc5883.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index c2e4700d83..cd2b5c48e5 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -341,6 +341,8 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : _bus(bus), _rotation(rotation) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + // enable debug() calls _debug_enabled = false; From c6b0dc1ee85348d8dbe398457e9631e095ec9c61 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:09:47 +1000 Subject: [PATCH 208/600] lsm303d: setup device type --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 7ba0fcc88b..45e7750552 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -530,6 +530,8 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _last_log_alarm_us(0), _rotation(rotation) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; + // enable debug() calls _debug_enabled = true; From 6cffa948fe6edc1e58b7d55acf119e8793461510 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Jul 2014 21:37:45 +1000 Subject: [PATCH 209/600] device: pass CDev::ioctl() to superclass this allows DEVIOCGDEVICEID to work. --- src/drivers/device/cdev.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 6e2ebb1f7b..39fb89501d 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -268,6 +268,13 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg) break; } + /* try the superclass. The different ioctl() function form + * means we need to copy arg */ + unsigned arg2 = arg; + int ret = Device::ioctl(cmd, arg2); + if (ret != -ENODEV) + return ret; + return -ENOTTY; } From 20cd5026d817a4f17b96906d1c93fc3cbaa498dd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 Jul 2014 09:49:43 +1000 Subject: [PATCH 210/600] device: use bitfields to keep DeviceStructure small this keeps it small enough to fit in a float, which makes it possible to see the full value in a MAVLink tlog Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/device/device.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index c98386eb07..7df234cab0 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -134,11 +134,14 @@ public: }; /* - broken out + broken out device elements. The bitfields are used to keep + the overall value small enough to fit in a float accurately, + which makes it possible to transport over the MAVLink + parameter protocol without loss of information. */ struct DeviceStructure { - enum DeviceBusType bus_type; - uint8_t bus; // which instance of the bus type + enum DeviceBusType bus_type:3; + uint8_t bus:5; // which instance of the bus type uint8_t address; // address on the bus (eg. I2C address) uint8_t devtype; // device class specific device type }; From 8a3a87331da2077cd1da4c3da8fe2743f188a4a4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 Jul 2014 22:21:53 +1000 Subject: [PATCH 211/600] mpu6000: fixed internal/external mixup in pointers Thanks to Emile for spotting this! Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/mpu6000/mpu6000.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 5668b0865f..1b3a96a0d4 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1442,7 +1442,7 @@ void start(bool external_bus, enum Rotation rotation) { int fd; - MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; @@ -1595,7 +1595,7 @@ reset(bool external_bus) void info(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_int:&g_dev_ext; + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; if (*g_dev_ptr == nullptr) errx(1, "driver not running"); From b288b010f12213a4388b627bce7fd6cb4cdedea5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 18:42:00 +0200 Subject: [PATCH 212/600] tests, drive by: Fix double comparison, use reasonable margin based on context of test --- src/systemcmds/tests/test_bson.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c index 6130fe7635..12d598df48 100644 --- a/src/systemcmds/tests/test_bson.c +++ b/src/systemcmds/tests/test_bson.c @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -123,7 +124,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE); return 1; } - if (node->d != sample_double) { + if (fabs(node->d - sample_double) > 1e-12) { warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double); return 1; } From ffebe45c4ce6cb248314141d91abcb74fbf9174e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 18:42:25 +0200 Subject: [PATCH 213/600] mavlink: Handle unhandled enum cases --- src/modules/mavlink/mavlink_messages.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c7ad605c5e..7c864f1277 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -170,6 +170,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_LAND: + /* fallthrough */ + case NAVIGATION_STATE_DESCEND: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -190,6 +192,17 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; + case NAVIGATION_STATE_OFFBOARD: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; + break; + + case NAVIGATION_STATE_MAX: + /* this is an unused case, ignore */ + break; + } *mavlink_custom_mode = custom_mode.data; From 0144a7dacfdb398c6133a9bf2df35facdbdfe6e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 18:43:09 +0200 Subject: [PATCH 214/600] mavlink: Optimize to native types where possible, move things to optimize alignment --- src/modules/mavlink/mavlink_mission.cpp | 6 +++--- src/modules/mavlink/mavlink_mission.h | 11 ++++++----- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index a62d34019a..068774c471 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -61,8 +61,6 @@ static const int ERROR = -1; MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _mavlink(mavlink), - _channel(mavlink->get_channel()), - _comp_id(MAV_COMP_ID_MISSIONPLANNER), _state(MAVLINK_WPM_STATE_IDLE), _time_last_recv(0), _time_last_sent(0), @@ -82,7 +80,9 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _mission_result_sub(-1), _offboard_mission_pub(-1), _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), - _verbose(false) + _verbose(false), + _channel(mavlink->get_channel()), + _comp_id(MAV_COMP_ID_MISSIONPLANNER) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 1964a45e8e..f63c32f243 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -143,8 +143,6 @@ public: private: Mavlink* _mavlink; - mavlink_channel_t _channel; - uint8_t _comp_id; enum MAVLINK_WPM_STATES _state; ///< Current state @@ -153,7 +151,7 @@ private: uint32_t _action_timeout; uint32_t _retry_timeout; - unsigned _max_count; ///< Maximal count of mission items + unsigned _max_count; ///< Maximum number of mission items int _dataman_id; ///< Dataman storage ID for active mission unsigned _count; ///< Count of items in active mission @@ -163,8 +161,8 @@ private: unsigned _transfer_count; ///< Items count in current transmission unsigned _transfer_seq; ///< Item sequence in current transmission unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) - uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission - uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission + unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission + unsigned _transfer_partner_compid; ///< Partner component ID for current transmission int _offboard_mission_sub; int _mission_result_sub; @@ -173,4 +171,7 @@ private: MavlinkRateLimiter _slow_rate_limiter; bool _verbose; + + mavlink_channel_t _channel; + uint8_t _comp_id; }; From 005dd206d17920761f8e27b21d54770da59faa13 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 12 Jul 2014 09:51:22 +1000 Subject: [PATCH 215/600] hmc5883: periodically check the config and range registers this copes with I2C comms errors causing the range or config registers to become corrupted, leading to bad reading. This is easily reproducible with a 1.3m I2C cable in the same run of cable as a GPS UART cable. The error happens every half hour or so. Conflicts: mavlink/include/mavlink/v1.0 src/drivers/hmc5883/hmc5883.cpp --- src/drivers/hmc5883/hmc5883.cpp | 141 ++++++++++++++++++++++++++------ 1 file changed, 114 insertions(+), 27 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index cd2b5c48e5..4aef43102e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -166,6 +166,8 @@ private: perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + perf_counter_t _range_errors; + perf_counter_t _conf_errors; /* status reporting */ bool _sensor_ok; /**< sensor was found and reports ok */ @@ -176,6 +178,9 @@ private: struct mag_report _last_report; /**< used for info() */ + uint8_t _range_bits; + uint8_t _conf_reg; + /** * Test whether the device supported by the driver is present at a * specific address. @@ -233,6 +238,23 @@ private: */ int set_range(unsigned range); + /** + * check the sensor range. + * + * checks that the range of the sensor is correctly set, to + * cope with communication errors causing the range to change + */ + void check_range(void); + + /** + * check the sensor configuration. + * + * checks that the config of the sensor is correctly set, to + * cope with communication errors causing the configuration to + * change + */ + void check_conf(void); + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -336,10 +358,15 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), + _range_errors(perf_alloc(PC_COUNT, "hmc5883_range_errors")), + _conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")), _sensor_ok(false), _calibrated(false), _bus(bus), - _rotation(rotation) + _rotation(rotation), + _last_report{0}, + _range_bits(0), + _conf_reg(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; @@ -373,6 +400,8 @@ HMC5883::~HMC5883() perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_buffer_overflows); + perf_free(_range_errors); + perf_free(_conf_errors); } int @@ -403,45 +432,43 @@ out: int HMC5883::set_range(unsigned range) { - uint8_t range_bits; - if (range < 1) { - range_bits = 0x00; + _range_bits = 0x00; _range_scale = 1.0f / 1370.0f; _range_ga = 0.88f; } else if (range <= 1) { - range_bits = 0x01; + _range_bits = 0x01; _range_scale = 1.0f / 1090.0f; _range_ga = 1.3f; } else if (range <= 2) { - range_bits = 0x02; + _range_bits = 0x02; _range_scale = 1.0f / 820.0f; _range_ga = 1.9f; } else if (range <= 3) { - range_bits = 0x03; + _range_bits = 0x03; _range_scale = 1.0f / 660.0f; _range_ga = 2.5f; } else if (range <= 4) { - range_bits = 0x04; + _range_bits = 0x04; _range_scale = 1.0f / 440.0f; _range_ga = 4.0f; } else if (range <= 4.7f) { - range_bits = 0x05; + _range_bits = 0x05; _range_scale = 1.0f / 390.0f; _range_ga = 4.7f; } else if (range <= 5.6f) { - range_bits = 0x06; + _range_bits = 0x06; _range_scale = 1.0f / 330.0f; _range_ga = 5.6f; } else { - range_bits = 0x07; + _range_bits = 0x07; _range_scale = 1.0f / 230.0f; _range_ga = 8.1f; } @@ -451,7 +478,7 @@ int HMC5883::set_range(unsigned range) /* * Send the command to set the range */ - ret = write_reg(ADDR_CONF_B, (range_bits << 5)); + ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); if (OK != ret) perf_count(_comms_errors); @@ -462,7 +489,53 @@ int HMC5883::set_range(unsigned range) if (OK != ret) perf_count(_comms_errors); - return !(range_bits_in == (range_bits << 5)); + return !(range_bits_in == (_range_bits << 5)); +} + +/** + check that the range register has the right value. This is done + periodically to cope with I2C bus noise causing the range of the + compass changing. + */ +void HMC5883::check_range(void) +{ + int ret; + + uint8_t range_bits_in; + ret = read_reg(ADDR_CONF_B, range_bits_in); + if (OK != ret) { + perf_count(_comms_errors); + return; + } + if (range_bits_in != (_range_bits<<5)) { + perf_count(_range_errors); + ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); + if (OK != ret) + perf_count(_comms_errors); + } +} + +/** + check that the configuration register has the right value. This is + done periodically to cope with I2C bus noise causing the + configuration of the compass to change. + */ +void HMC5883::check_conf(void) +{ + int ret; + + uint8_t conf_reg_in; + ret = read_reg(ADDR_CONF_A, conf_reg_in); + if (OK != ret) { + perf_count(_comms_errors); + return; + } + if (conf_reg_in != _conf_reg) { + perf_count(_conf_errors); + ret = write_reg(ADDR_CONF_A, _conf_reg); + if (OK != ret) + perf_count(_comms_errors); + } } int @@ -796,7 +869,7 @@ HMC5883::collect() } report; int ret = -EIO; uint8_t cmd; - + uint8_t check_counter; perf_begin(_sample_perf); struct mag_report new_report; @@ -895,6 +968,21 @@ HMC5883::collect() /* notify anyone waiting for data */ poll_notify(POLLIN); + /* + periodically check the range register and configuration + registers. With a bad I2C cable it is possible for the + registers to become corrupt, leading to bad readings. It + doesn't happen often, but given the poor cables some + vehicles have it is worth checking for. + */ + check_counter = perf_event_count(_sample_perf) % 256; + if (check_counter == 0) { + check_range(); + } + if (check_counter == 128) { + check_conf(); + } + ret = OK; out: @@ -1168,25 +1256,24 @@ int HMC5883::set_excitement(unsigned enable) { int ret; /* arm the excitement strap */ - uint8_t conf_reg; - ret = read_reg(ADDR_CONF_A, conf_reg); + ret = read_reg(ADDR_CONF_A, _conf_reg); if (OK != ret) perf_count(_comms_errors); if (((int)enable) < 0) { - conf_reg |= 0x01; + _conf_reg |= 0x01; } else if (enable > 0) { - conf_reg |= 0x02; + _conf_reg |= 0x02; } else { - conf_reg &= ~0x03; + _conf_reg &= ~0x03; } - // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg); + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); - ret = write_reg(ADDR_CONF_A, conf_reg); + ret = write_reg(ADDR_CONF_A, _conf_reg); if (OK != ret) perf_count(_comms_errors); @@ -1196,7 +1283,7 @@ int HMC5883::set_excitement(unsigned enable) //print_info(); - return !(conf_reg == conf_reg_ret); + return !(_conf_reg == conf_reg_ret); } int @@ -1257,12 +1344,12 @@ const int ERROR = -1; HMC5883 *g_dev_int; HMC5883 *g_dev_ext; -void hmc5883_usage(); void start(int bus, enum Rotation rotation); void test(int bus); void reset(int bus); void info(int bus); int calibrate(int bus); +void usage(); /** * Start the driver. @@ -1511,10 +1598,8 @@ info(int bus) exit(0); } -} // namespace - void -hmc5883_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'"); warnx("options:"); @@ -1526,6 +1611,8 @@ hmc5883_usage() #endif } +} // namespace + int hmc5883_main(int argc, char *argv[]) { @@ -1551,7 +1638,7 @@ hmc5883_main(int argc, char *argv[]) calibrate = true; break; default: - hmc5883_usage(); + hmc5883::usage(); exit(0); } } From 67e3a904b6526f9268530d9c8e9529585c0be235 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:05:13 +0200 Subject: [PATCH 216/600] fix ms5611 code style for usage call --- src/drivers/ms5611/ms5611.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 7a8d2eecf3..fe669b5f52 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -781,6 +781,7 @@ void test(); void reset(); void info(); void calibrate(unsigned altitude); +void usage(); /** * MS5611 crc4 cribbed from the datasheet @@ -1057,16 +1058,16 @@ calibrate(unsigned altitude) exit(0); } -} // namespace - void -ms5611_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); warnx("options:"); warnx(" -X (external bus)"); } +} // namespace + int ms5611_main(int argc, char *argv[]) { @@ -1080,7 +1081,7 @@ ms5611_main(int argc, char *argv[]) external_bus = true; break; default: - ms5611_usage(); + ms5611::usage(); exit(0); } } From 0d1ac4235411e8f05f96bcbe51558d92f0d86cf6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:10:08 +0200 Subject: [PATCH 217/600] airspeed calibration: Update and resolve compile errors --- .../commander/airspeed_calibration.cpp | 75 ++++++++----------- 1 file changed, 33 insertions(+), 42 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a1a20abc98..421e53ae02 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -51,6 +51,7 @@ #include #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -103,58 +104,48 @@ int do_airspeed_calibration(int mavlink_fd) } } - for (unsigned i = 0; i < (sizeof(diff_pres_offset) / sizeof(diff_pres_offset[0])); i++) { + unsigned calibration_counter = 0; - unsigned calibration_counter = 0; + mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + usleep(500 * 1000); - if (i == 0) { - mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); - usleep(500 * 1000); - } + while (calibration_counter < calibration_count) { - float diff_pres_offset = 0.0f; + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; - while (calibration_counter < calibration_count) { + int poll_ret = poll(fds, 1, 1000); - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - int poll_ret = poll(fds, 1, 1000); + if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - - if (i > 0) { - - if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); - usleep(5000 * 1000); - continue; - } - - /* do not log negative values in the second go */ - if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); - close(diff_pres_sub); - return ERROR; - } - } - - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; - - if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); - } - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + /* do not log negative values in the second go */ + if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); return ERROR; } + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; } } From e6b5e3ae613e89189ac69cf0b174b10002d51068 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:22:36 +0200 Subject: [PATCH 218/600] Add note about need to scan external buses first --- ROMFS/px4fmu_common/init.d/rc.sensors | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 1e14930fe3..be54ea98b0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -24,6 +24,7 @@ fi if ver hwcmp PX4FMU_V2 then + # IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST if lsm303d start then echo "[init] Using LSM303D" From 8960c9e0a8b165ce15cf1864d61f8f764935a081 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:28:10 +0200 Subject: [PATCH 219/600] better submodule instructions --- Tools/check_submodules.sh | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index cc6e7d1c05..8adc6b6c7a 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -12,8 +12,12 @@ if [ -d NuttX/nuttx ]; if [ -z "$STATUSRETVAL" ]; then echo "Checked NuttX submodule, correct version found" else - echo "NuttX sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + echo "" + echo "" + echo " NuttX sub repo not at correct version. Try 'git submodule update'" + echo " or follow instructions on http://pixhawk.org/dev/git/submodules" + echo "" + echo " DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!" echo "" echo "" echo "New commits required:" @@ -33,6 +37,8 @@ if [ -d mavlink/include/mavlink/v1.0 ]; if [ -z "$STATUSRETVAL" ]; then echo "Checked mavlink submodule, correct version found" else + echo "" + echo "" echo "mavlink sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" echo "" From d6632ee2dda39de78be1bbfa6754af8b59c58655 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:34:06 +0200 Subject: [PATCH 220/600] ardrone: Optimize for size, since performance is good at any rate --- src/drivers/ardrone_interface/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/ardrone_interface/module.mk b/src/drivers/ardrone_interface/module.mk index d8e6c76c62..285db1351c 100644 --- a/src/drivers/ardrone_interface/module.mk +++ b/src/drivers/ardrone_interface/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = ardrone_interface SRCS = ardrone_interface.c \ ardrone_motor_control.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os From 85301e1172a0ab7cf726ba8ffc5386ab7ede363d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:34:28 +0200 Subject: [PATCH 221/600] frsky: Optimize for size --- src/drivers/frsky_telemetry/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk index 9a49589ee5..78ad6f67ec 100644 --- a/src/drivers/frsky_telemetry/module.mk +++ b/src/drivers/frsky_telemetry/module.mk @@ -41,3 +41,5 @@ SRCS = frsky_data.c \ frsky_telemetry.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os From 144bb89e027701f0b4c1661685d770a013c100f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:34:40 +0200 Subject: [PATCH 222/600] HoTT: optimize for size --- src/drivers/hott/hott_sensors/module.mk | 2 ++ src/drivers/hott/hott_telemetry/module.mk | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk index b5f5762ba7..47aea6cafb 100644 --- a/src/drivers/hott/hott_sensors/module.mk +++ b/src/drivers/hott/hott_sensors/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = hott_sensors SRCS = hott_sensors.cpp \ ../messages.cpp \ ../comms.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk index b19cbd14cf..cd7bdbc85b 100644 --- a/src/drivers/hott/hott_telemetry/module.mk +++ b/src/drivers/hott/hott_telemetry/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = hott_telemetry SRCS = hott_telemetry.cpp \ ../messages.cpp \ ../comms.cpp + +MAXOPTIMIZATION = -Os From aaf2652e26106b0850226f25cd12ce1304775522 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:34:58 +0200 Subject: [PATCH 223/600] MKBLCTRL: optimize for size --- src/drivers/mkblctrl/module.mk | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/mkblctrl/module.mk b/src/drivers/mkblctrl/module.mk index 3ac263b002..6daa14aa53 100644 --- a/src/drivers/mkblctrl/module.mk +++ b/src/drivers/mkblctrl/module.mk @@ -37,6 +37,8 @@ MODULE_COMMAND = mkblctrl -SRCS = mkblctrl.cpp +SRCS = mkblctrl.cpp INCLUDE_DIRS += $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common + +MAXOPTIMIZATION = -Os From 70d0ff492220371c57f9ee3d1f4fedb2fcf1199f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:35:11 +0200 Subject: [PATCH 224/600] SF0X: optimize for size --- src/drivers/sf0x/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/sf0x/module.mk b/src/drivers/sf0x/module.mk index dc93a90e78..dc2c66d567 100644 --- a/src/drivers/sf0x/module.mk +++ b/src/drivers/sf0x/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = sf0x SRCS = sf0x.cpp + +MAXOPTIMIZATION = -Os From 959bf6a2c8ac656b1536762a2a7aea0c5349f5d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:35:49 +0200 Subject: [PATCH 225/600] ll40ls: Optimize for size --- src/drivers/ll40ls/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/ll40ls/module.mk b/src/drivers/ll40ls/module.mk index ab7d432690..fb627afeea 100644 --- a/src/drivers/ll40ls/module.mk +++ b/src/drivers/ll40ls/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = ll40ls SRCS = ll40ls.cpp + +MAXOPTIMIZATION = -Os From 8a6e69ed671d3de6b267518f1cd425b24e48c71e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 20:08:36 +0200 Subject: [PATCH 226/600] Fix up RAM usage of fixed wing apps --- src/modules/fw_att_control/module.mk | 2 ++ src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/fw_pos_control_l1/module.mk | 4 +++- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk index 1e600757ea..89c6494c53 100644 --- a/src/modules/fw_att_control/module.mk +++ b/src/modules/fw_att_control/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = fw_att_control SRCS = fw_att_control_main.cpp \ fw_att_control_params.c + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3d6c62434c..98ccd09a5e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1445,7 +1445,7 @@ FixedwingPositionControl::start() _control_task = task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 3500, + 2000, (main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index af155fe089..15b575b50f 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# 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 @@ -43,3 +43,5 @@ SRCS = fw_pos_control_l1_main.cpp \ mtecs/mTecs.cpp \ mtecs/limitoverride.cpp \ mtecs/mTecs_params.c + +MODULE_STACKSIZE = 1200 From f32a51f51597bb66c7e404a7dc8e723c32f44743 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 20:14:01 +0200 Subject: [PATCH 227/600] Fixed battery ignore voltage to a higher value --- 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 6e2d906e64..c40e52a0d6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -133,7 +133,7 @@ #endif #define BATT_V_LOWPASS 0.001f -#define BATT_V_IGNORE_THRESHOLD 3.5f +#define BATT_V_IGNORE_THRESHOLD 4.8f /** * HACK - true temperature is much less than indicated temperature in baro, From c059fb03ea5ba85446d93df4db73e867f01e288d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 21:33:09 +0200 Subject: [PATCH 228/600] blinkm: Make driver flash efficient --- src/drivers/blinkm/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/blinkm/module.mk b/src/drivers/blinkm/module.mk index b48b90f3f4..c906733175 100644 --- a/src/drivers/blinkm/module.mk +++ b/src/drivers/blinkm/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = blinkm SRCS = blinkm.cpp + +MAXOPTIMIZATION = -Os From 69937702b8ff4ee052e960f6427e4653b4743e2a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 21:33:21 +0200 Subject: [PATCH 229/600] gps: Flash efficiency --- src/drivers/gps/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index eb382c4b26..b008184244 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -43,3 +43,5 @@ SRCS = gps.cpp \ ubx.cpp MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os From 5f8baed876a805c2cfae7c0ed0250ae20b501336 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 21:33:34 +0200 Subject: [PATCH 230/600] mb12xx: flash efficiency --- src/drivers/mb12xx/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/mb12xx/module.mk b/src/drivers/mb12xx/module.mk index 4e00ada025..d751e93e4d 100644 --- a/src/drivers/mb12xx/module.mk +++ b/src/drivers/mb12xx/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = mb12xx SRCS = mb12xx.cpp + +MAXOPTIMIZATION = -Os From 7bf0e6f3e26ee9e10073293677563f6862758557 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 21:47:26 +0200 Subject: [PATCH 231/600] Better airspeed feedback --- src/modules/commander/airspeed_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 421e53ae02..a1b8816ff0 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -200,13 +200,13 @@ int do_airspeed_calibration(int mavlink_fd) float curr_avg = (diff_pres_offset / calibration_counter); if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", curr_avg); usleep(5000 * 1000); continue; } /* do not log negative values in the second go */ - if (curr_avg < calc_indicated_airspeed(-5.0f)) { + if (curr_avg < -calc_indicated_airspeed(5.0f)) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); From 67d23253c3b3ec04bfd9d5f7e09d3d405ceba7c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:00:50 +0200 Subject: [PATCH 232/600] airspeed cal build fix --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a1b8816ff0..529a5b32f5 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -200,7 +200,7 @@ int do_airspeed_calibration(int mavlink_fd) float curr_avg = (diff_pres_offset / calibration_counter); if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", curr_avg); + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); usleep(5000 * 1000); continue; } From fe5d35bc546232b2d897e7509c28441178b6e7ea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:04:04 +0200 Subject: [PATCH 233/600] Reduce IO buf space reasonably --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 6717ae19a8..a651faffab 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -679,7 +679,7 @@ CONFIG_BUILTIN=y # # Standard C Library Options # -CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STDIO_BUFFER_SIZE=180 CONFIG_STDIO_LINEBUFFER=y CONFIG_NUNGET_CHARS=2 CONFIG_LIB_HOMEDIR="/" From 4824a4e8ac93d992e1401f768291cca65ba46acc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:18:59 +0200 Subject: [PATCH 234/600] Fix for dynamic / static part of calibration --- .../commander/airspeed_calibration.cpp | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 529a5b32f5..01db34be25 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -121,19 +121,6 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); - usleep(5000 * 1000); - continue; - } - - /* do not log negative values in the second go */ - if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); - close(diff_pres_sub); - return ERROR; - } - diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; @@ -197,16 +184,16 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; - float curr_avg = (diff_pres_offset / calibration_counter); + float curr_avg_airspeed = calc_indicated_airspeed(diff_pres_offset / calibration_counter); - if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { + if (fabsf(curr_avg) < 10.0f) { mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); usleep(5000 * 1000); continue; } /* do not log negative values in the second go */ - if (curr_avg < -calc_indicated_airspeed(5.0f)) { + if (curr_avg_airspeed < -calc_indicated_airspeed(5.0f)) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); From 34abf5c40cda538f204d313cc49715b5a938d168 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:50:56 +0200 Subject: [PATCH 235/600] airspeed cal: Fix up logic --- .../commander/airspeed_calibration.cpp | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 01db34be25..47c9c7af55 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -165,11 +165,8 @@ int do_airspeed_calibration(int mavlink_fd) /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - calibration_counter = 0; - diff_pres_offset = 0.0f; - /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 50) { + while (calibration_counter < 10) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -181,19 +178,15 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; - - float curr_avg_airspeed = calc_indicated_airspeed(diff_pres_offset / calibration_counter); - - if (fabsf(curr_avg) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); - usleep(5000 * 1000); + if (fabsf(diff_pres.differential_pressure_raw_pa) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f Pa)", + (double)diff_pres.differential_pressure_raw_pa); + usleep(3000 * 1000); continue; } - /* do not log negative values in the second go */ - if (curr_avg_airspeed < -calc_indicated_airspeed(5.0f)) { + /* do not allow negative values */ + if (diff_pres.differential_pressure_raw_pa < 0.0f) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); @@ -209,7 +202,11 @@ int do_airspeed_calibration(int mavlink_fd) /* save */ (void)param_save_default(); + close(diff_pres_sub); return ERROR; + } else { + close(diff_pres_sub); + return OK; } } else if (poll_ret == 0) { From 65409ad2c8ba10d95cbcfa088951abc40eb46774 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 23:30:00 +0200 Subject: [PATCH 236/600] Airspeed calibration improvements --- .../commander/airspeed_calibration.cpp | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 47c9c7af55..d52cafb8f3 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -125,7 +125,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { @@ -165,6 +165,8 @@ int do_airspeed_calibration(int mavlink_fd) /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); + calibration_counter = 0; + /* just take a few samples and make sure pitot tubes are not reversed */ while (calibration_counter < 10) { @@ -178,16 +180,18 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (fabsf(diff_pres.differential_pressure_raw_pa) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f Pa)", - (double)diff_pres.differential_pressure_raw_pa); + float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; + + if (fabsf(calibrated_pa) < 9.0f) { + mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%.1f Pa, #h101)", + (double)calibrated_pa); usleep(3000 * 1000); continue; } /* do not allow negative values */ - if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + if (calibrated_pa < 0.0f) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static vs dynamic ports,restart"); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -205,8 +209,9 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); return ERROR; } else { - close(diff_pres_sub); - return OK; + mavlink_log_info(mavlink_fd, "positive pressure: (%.1f Pa)", + (double)diff_pres.differential_pressure_raw_pa); + break; } } else if (poll_ret == 0) { @@ -217,6 +222,8 @@ int do_airspeed_calibration(int mavlink_fd) } } + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); tune_neutral(true); close(diff_pres_sub); From cdfbe9bcc41a6e8e8b2a6f95bd69283ee5176966 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 23:30:34 +0200 Subject: [PATCH 237/600] px4io: Do not forward excessively low battery voltages --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 24da4c68b5..7d78b0d275 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1383,7 +1383,7 @@ void PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) { /* only publish if battery has a valid minimum voltage */ - if (vbatt <= 3300) { + if (vbatt <= 4900) { return; } From 1b6ad3e1990bec84489f949b195adba4412e1c7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:37:10 +0200 Subject: [PATCH 238/600] ekf: Logic cleanup and print code cleanup --- .../ekf_att_pos_estimator_main.cpp | 137 +++++++----------- 1 file changed, 49 insertions(+), 88 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index bc5f709e58..5ce6bdb0ac 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -559,61 +559,26 @@ FixedwingEstimator::check_filter_state() int check = _ekf->CheckAndBound(&ekf_report); - const char* ekfname = "att pos estimator: "; + const char* const feedback[] = { 0, + "NaN in states, resetting", + "stale IMU data, resetting", + "got initial position lock", + "excessive gyro offsets", + "GPS velocity divergence", + "excessive covariances", + "unknown condition"}; - switch (check) { - case 0: - /* all ok */ - break; - case 1: - { - const char* str = "NaN in states, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 2: - { - const char* str = "stale IMU data, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 3: - { - const char* str = "switching to dynamic state"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 4: - { - const char* str = "excessive gyro offsets"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 5: - { - const char* str = "GPS velocity divergence"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 6: - { - const char* str = "excessive covariances"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; + // Print out error condition + if (check) { + unsigned warn_index = static_cast(check); + unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0])); + + if (max_warn_index < warn_index) { + warn_index = max_warn_index; } - default: - { - const char* str = "unknown reset condition"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - } + warnx("reset: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]); } struct estimator_status_report rep; @@ -1140,20 +1105,6 @@ FixedwingEstimator::task_main() float initVelNED[3]; - // Guard before running any parts of the filter - // of NaN / invalid values - if (_ekf->statesInitialised) { - - // We're apparently initialized in this case now - - int check = check_filter_state(); - - if (check) { - // Let the system re-initialize itself - continue; - } - } - /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1216,8 +1167,17 @@ FixedwingEstimator::task_main() _baro_gps_offset = 0.0f; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + } else if (_ekf->statesInitialised) { + // We're apparently initialized in this case now + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } + // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 @@ -1285,7 +1245,7 @@ FixedwingEstimator::task_main() // run the fusion step _ekf->FuseVelposNED(); - } else if (!_gps_initialized && _ekf->statesInitialised) { + } else if (!_gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; @@ -1307,7 +1267,7 @@ FixedwingEstimator::task_main() _ekf->fusePosData = false; } - if (newHgtData && _ekf->statesInitialised) { + if (newHgtData) { // Could use a blend of GPS and baro alt data if desired _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; @@ -1321,7 +1281,7 @@ FixedwingEstimator::task_main() } // Fuse Magnetometer Measurements - if (newDataMag && _ekf->statesInitialised) { + if (newDataMag) { _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data @@ -1335,7 +1295,7 @@ FixedwingEstimator::task_main() } // Fuse Airspeed Measurements - if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { + if (newAdsData && _ekf->VtasMeas > 7.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); @@ -1403,7 +1363,7 @@ FixedwingEstimator::task_main() _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); - _airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s; + _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; /* crude land detector for fixedwing only, @@ -1494,27 +1454,28 @@ FixedwingEstimator::task_main() } + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; + + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } + } + } } - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = _ekf->P[14][14]; - _wind.covariance_east = _ekf->P[15][15]; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - } } } From be5ac5e4127d09187ef0e19a7955cca9b0f1b378 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:38:52 +0200 Subject: [PATCH 239/600] Fix mavlink log header docs. --- src/include/mavlink/mavlink_log.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 0ea655cac3..6d56c546a7 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: 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 From 716e20fab6d63f3dc6f80c70f382d6b0f45f9340 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:39:13 +0200 Subject: [PATCH 240/600] Fix accel cal docs. --- src/modules/commander/accelerometer_calibration.cpp | 3 +-- src/modules/commander/accelerometer_calibration.h | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index be465ab766..7a4e7a766d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Copyright (c) 2013 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 diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 1cf9c09775..6b7e16d449 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Copyright (c) 2013 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 From 259014b0d50426b0fbed3b9eac5a1d34aaa5b211 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:40:09 +0200 Subject: [PATCH 241/600] Documentation only and unused defines only cleanup. --- src/modules/commander/airspeed_calibration.h | 2 +- src/modules/commander/baro_calibration.cpp | 2 +- src/modules/commander/baro_calibration.h | 2 +- src/modules/commander/calibration_messages.h | 3 +-- src/modules/commander/calibration_routines.cpp | 3 +-- src/modules/commander/calibration_routines.h | 3 +-- src/modules/commander/mag_calibration.h | 2 +- src/modules/commander/state_machine_helper.h | 10 ++++------ 8 files changed, 11 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h index 71c701fc2e..8c6b65ff1e 100644 --- a/src/modules/commander/airspeed_calibration.h +++ b/src/modules/commander/airspeed_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 3123c40876..da98644b40 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 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 diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h index bc42bc6cfc..ce78ae7005 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/commander/baro_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 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 diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index fd8b8564df..b1e209efc0 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Copyright (c) 2013 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 diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 43f341ae7a..5796204bfd 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012 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 diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index e3e7fbafdd..3c8e49ee9e 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012 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 diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h index a101cd7b1f..c14f948cc1 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index ddc5bf154c..bb1b87e712 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * 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 @@ -36,14 +34,14 @@ /** * @file state_machine_helper.h * State machine helper functions definitions + * + * @author Thomas Gubler + * @author Julian Oes */ #ifndef STATE_MACHINE_HELPER_H_ #define STATE_MACHINE_HELPER_H_ -#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor) -#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock - #include #include #include From ea11bc6c4bf7180ae4e321eebcaf817eebb18e6c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:41:39 +0200 Subject: [PATCH 242/600] Command handling: Fix up local variable usage and status prints. --- src/modules/commander/commander.cpp | 67 ++++++++++++++++------------- 1 file changed, 38 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e14dff197a..009e0ac010 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -403,10 +403,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char return arming_res; } -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) +bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, + struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, + struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } @@ -425,7 +427,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); + transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); @@ -434,43 +436,43 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_ret = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_ret = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_ret = main_state_transition(status, MAIN_STATE_ACRO); + main_ret = main_state_transition(status_local, MAIN_STATE_ACRO); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ - main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD); + main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD); } } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_ret = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_ret = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); } } } @@ -485,22 +487,25 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. - // We use an float epsilon delta to test float equality. - if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { - mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1); + // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints + // for logic state parameters + + if (static_cast(cmd->param1 + 0.5f) != 0 && static_cast(cmd->param1 + 0.5f) != 1) { + mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1); } else { + bool cmd_arms = (static_cast(cmd->param1 + 0.5f) == 1); + // Flick to inair restore first if this comes from an onboard system - if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { - status->arming_state = ARMING_STATE_IN_AIR_RESTORE; + if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { + status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE; } - transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); + transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); if (arming_res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd"); cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { @@ -512,20 +517,24 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_OVERRIDE_GOTO: { // TODO listen vehicle_command topic directly from navigator (?) - unsigned int mav_goto = cmd->param1; + + // Increase by 0.5f and rely on the integer cast + // implicit floor(). This is the *safest* way to + // convert from floats representing small ints to actual ints. + unsigned int mav_goto = (cmd->param1 + 0.5f); if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; - mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); + status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER; + mavlink_log_critical(mavlink_fd, "Pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); + status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION; + mavlink_log_critical(mavlink_fd, "Continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", + mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", (double)cmd->param1, (double)cmd->param2, (double)cmd->param3, @@ -543,7 +552,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe //XXX: to enable the parachute, a param needs to be set //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { + if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -561,7 +570,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (use_current) { /* use current position */ - if (status->condition_global_position_valid) { + if (status_local->condition_global_position_valid) { home->lat = global_pos->lat; home->lon = global_pos->lon; home->alt = global_pos->alt; @@ -598,7 +607,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } /* mark home position as set */ - status->condition_home_position_valid = true; + status_local->condition_home_position_valid = true; } } break; From 3ec9ffa66166b10101887cc4077d1b02d4a0897b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:42:02 +0200 Subject: [PATCH 243/600] Buzzer and led: Fix -Wshadow warnings. --- 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 009e0ac010..85c9ccbaa7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -697,11 +697,11 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds"); + warnx("ERROR: LED INIT FAIL"); } if (buzzer_init() != OK) { - warnx("ERROR: Failed to initialize buzzer"); + warnx("ERROR: BUZZER INIT FAIL"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); From 76f7960d77046c33a771be49b57e32c957e7a2ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:43:56 +0200 Subject: [PATCH 244/600] Mavlink text feedback: Remove now unneeded audio tag for critical messages, make overall printing more efficient. Remove buffers where no buffers are needed. --- src/modules/commander/commander.cpp | 55 +++++++++++++---------------- 1 file changed, 25 insertions(+), 30 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 85c9ccbaa7..76cfd5feb9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -775,8 +775,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); } else { - warnx("reading mission state failed"); - mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + const char *missionfail = "reading mission state failed"; + warnx("%s", missionfail); + mavlink_log_critical(mavlink_fd, missionfail); /* initialize mission state in dataman */ mission.dataman_id = 0; @@ -789,8 +790,6 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(offboard_mission), mission_pub, &mission); } - mavlink_log_info(mavlink_fd, "[cmd] started"); - int ret; pthread_attr_t commander_low_prio_attr; @@ -1083,7 +1082,7 @@ int commander_thread_main(int argc, char *argv[]) arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } } @@ -1187,10 +1186,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "#audio: LANDED"); + mavlink_log_critical(mavlink_fd, "LANDED MODE"); } else { - mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); + mavlink_log_critical(mavlink_fd, "IN AIR MODE"); } } } @@ -1270,14 +1269,14 @@ int commander_thread_main(int argc, char *argv[]) /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); + mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); + mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { @@ -1339,12 +1338,12 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + mavlink_log_critical(mavlink_fd, "detected RC signal first time"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); + mavlink_log_critical(mavlink_fd, "RC signal regained"); status_changed = true; } } @@ -1385,7 +1384,7 @@ int commander_thread_main(int argc, char *argv[]) * the system can be armed in auto if armed via the GCS. */ if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1405,16 +1404,16 @@ int commander_thread_main(int argc, char *argv[]) if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + mavlink_log_info(mavlink_fd, "ARMED by RC"); } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + mavlink_log_info(mavlink_fd, "DISARMED by RC"); } arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + mavlink_log_critical(mavlink_fd, "arming state transition denied"); tune_negative(true); } @@ -1428,12 +1427,12 @@ int commander_thread_main(int argc, char *argv[]) } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + mavlink_log_critical(mavlink_fd, "main state transition denied"); } } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } @@ -1445,14 +1444,14 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { /* handle the case where data link was regained */ if (telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i); + mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; } have_link = true; } else { if (!telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i); + mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; } } @@ -1467,7 +1466,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST"); + mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; status_changed = true; } @@ -2010,15 +2009,13 @@ set_control_mode() } void -print_reject_mode(struct vehicle_status_s *status, const char *msg) +print_reject_mode(struct vehicle_status_s *status_local, const char *msg) { hrt_abstime t = hrt_absolute_time(); if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; - char s[80]; - sprintf(s, "#audio: REJECT %s", msg); - mavlink_log_critical(mavlink_fd, s); + mavlink_log_critical(mavlink_fd, "REJECT %s", msg); /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ @@ -2033,9 +2030,7 @@ print_reject_arm(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; - char s[80]; - sprintf(s, "#audio: %s", msg); - mavlink_log_critical(mavlink_fd, s); + mavlink_log_critical(mavlink_fd, msg); tune_negative(true); } } @@ -2048,12 +2043,12 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command); tune_negative(true); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command); tune_negative(true); break; @@ -2064,7 +2059,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command); tune_negative(true); break; From f2b30be92ae946e8e40faade6cb10edfb037fdaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:44:27 +0200 Subject: [PATCH 245/600] commander status leds: Fix -Wshadow warning. --- src/modules/commander/commander.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 76cfd5feb9..e5ea237fc0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1650,22 +1650,22 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed) +control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed) { /* driving rgbled */ if (changed) { bool set_normal_color = false; /* set mode */ - if (status->arming_state == ARMING_STATE_ARMED) { + if (status_local->arming_state == ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); - } else if (status->arming_state == ARMING_STATE_STANDBY) { + } else if (status_local->arming_state == ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; @@ -1676,12 +1676,12 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { + if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { - if (status->condition_local_position_valid) { + if (status_local->condition_local_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); } else { @@ -1714,7 +1714,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a #endif /* give system warnings on error LED, XXX maybe add memory usage warning too */ - if (status->load > 0.95f) { + if (status_local->load > 0.95f) { if (leds_counter % 2 == 0) { led_toggle(LED_AMBER); } From 66d5bc20c095a4dbd937b21c4f5fc1a67205f2d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:45:02 +0200 Subject: [PATCH 246/600] commander RC handling: Fix -Wshadow warnings. --- src/modules/commander/commander.cpp | 42 ++++++++++++++--------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e5ea237fc0..0c4d48dead 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1727,16 +1727,16 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } transition_result_t -set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) +set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_OFFBOARD); + res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { - print_reject_mode(status, "OFFBOARD"); + print_reject_mode(status_local, "OFFBOARD"); } else { return res; @@ -1751,78 +1751,78 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin case SWITCH_POS_OFF: // MANUAL if (sp_man->acro_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_ACRO); + res = main_state_transition(status_local, MAIN_STATE_ACRO); } else { - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); } // TRANSITION_DENIED is not possible here break; case SWITCH_POS_MIDDLE: // ASSIST if (sp_man->posctl_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSCTL); + res = main_state_transition(status_local, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "POSCTL"); + print_reject_mode(status_local, "POSCTL"); } // fallback to ALTCTL - res = main_state_transition(status, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } if (sp_man->posctl_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTCTL"); + print_reject_mode(status_local, "ALTCTL"); } // fallback to MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case SWITCH_POS_ON: // AUTO if (sp_man->return_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_AUTO_RTL); + res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_RTL"); + print_reject_mode(status_local, "AUTO_RTL"); // fallback to LOITER if home position not set - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } } else if (sp_man->loiter_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_LOITER"); + print_reject_mode(status_local, "AUTO_LOITER"); } else { - res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_MISSION"); + print_reject_mode(status_local, "AUTO_MISSION"); // fallback to LOITER if home position not set - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1830,21 +1830,21 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } // fallback to POSCTL - res = main_state_transition(status, MAIN_STATE_POSCTL); + res = main_state_transition(status_local, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to ALTCTL - res = main_state_transition(status, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; From afc8908d38ff2eaabc2ca35363ac04697152cb6b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:45:32 +0200 Subject: [PATCH 247/600] commander: More docs-only changes in headers. --- src/modules/commander/commander_helper.cpp | 10 ++++++---- src/modules/commander/commander_helper.h | 9 ++++++--- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 80e6861f61..d5fe122cb6 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * 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 @@ -37,6 +34,11 @@ /** * @file commander_helper.cpp * Commander helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * */ #include diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index e75f2592f5..a49c9e263d 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * 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 @@ -36,6 +34,9 @@ /** * @file commander_helper.h * Commander helper functions definitions + * + * @author Thomas Gubler + * @author Julian Oes */ #ifndef COMMANDER_HELPER_H_ @@ -77,6 +78,8 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern); * Use integral of current if battery capacity known (BAT_CAPACITY parameter set), * else use simple estimate based on voltage. * + * @param voltage the current battery voltage + * @param discharged the discharged capacity * @return the estimated remaining capacity in 0..1 */ float battery_remaining_estimate_voltage(float voltage, float discharged); From 7525c290f2cdfd058d54e5e6eb2d2e522d070889 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:46:36 +0200 Subject: [PATCH 248/600] commander: more text feedback improvements. --- .../commander/state_machine_helper.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 363f5e8eb4..87a60e7fda 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -87,7 +87,7 @@ static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char *state_names[ARMING_STATE_MAX] = { +static const char * const state_names[ARMING_STATE_MAX] = { "ARMING_STATE_INIT", "ARMING_STATE_STANDBY", "ARMING_STATE_ARMED", @@ -160,7 +160,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!"); valid_transition = false; } @@ -625,7 +625,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING"); failed = true; goto system_eval; } @@ -633,7 +633,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION"); failed = true; goto system_eval; } @@ -647,14 +647,14 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); - mavlink_log_info(mavlink_fd, "#audio: hold still while arming"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE"); + mavlink_log_critical(mavlink_fd, "hold still while arming"); /* this is frickin' fatal */ failed = true; goto system_eval; } } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ"); /* this is frickin' fatal */ failed = true; goto system_eval; @@ -667,7 +667,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd <= 0) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; } @@ -678,11 +678,11 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret == sizeof(diff_pres)) { if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) { - mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + mavlink_log_critical(mavlink_fd, "ARM WARNING: AIRSPEED CALIBRATION MISSING"); // XXX do not make this fatal yet } } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED READ"); /* this is frickin' fatal */ failed = true; goto system_eval; From 0d51230d4e3b87dd67819e594895b24a0a8a0306 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:04:43 +0200 Subject: [PATCH 249/600] commander: Final MAVLink text feedback cleanup --- src/modules/commander/state_machine_helper.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87a60e7fda..56c59ad3df 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -171,16 +171,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if power is not good if (!status->condition_power_input_valid) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); valid_transition = false; } // Fail transition if power levels on the avionics rail // are measured but are insufficient - if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.9f)) { + if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.9f))) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); valid_transition = false; } } From a32577377b8d735c77ffaaee15e2bbfa74be703f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:38:43 +0200 Subject: [PATCH 250/600] EKF init improvements --- .../ekf_att_pos_estimator_main.cpp | 8 ++++++++ .../ekf_att_pos_estimator/estimator_23states.cpp | 16 +++++++++++----- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5ce6bdb0ac..ee15612801 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -610,6 +610,10 @@ FixedwingEstimator::check_filter_state() rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); + // rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); + // rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); + // rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); + // rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); @@ -1246,6 +1250,10 @@ FixedwingEstimator::task_main() _ekf->FuseVelposNED(); } else if (!_gps_initialized) { + + // force static mode + _ekf->staticMode = true; + // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index f33a1d7805..e83c37bbdc 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2206,7 +2206,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d void AttPosEKF::OnGroundCheck() { - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } @@ -2806,12 +2806,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) current_ekf_state.statesNaN = false; current_ekf_state.velHealth = true; - //current_ekf_state.posHealth = ?; - //current_ekf_state.hgtHealth = ?; + current_ekf_state.posHealth = true; + current_ekf_state.hgtHealth = true; current_ekf_state.velTimeout = false; - //current_ekf_state.posTimeout = ?; - //current_ekf_state.hgtTimeout = ?; + current_ekf_state.posTimeout = false; + current_ekf_state.hgtTimeout = false; + + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; // Fill variables with valid data velNED[0] = initvelNED[0]; From 744eed91dca8a5f2ad8ead7ffa812d25755bcc93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:43:10 +0200 Subject: [PATCH 251/600] Fix calibration counter usage --- .../commander/airspeed_calibration.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index d52cafb8f3..a2f11b14a4 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -168,7 +168,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter = 0; /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 10) { + while (calibration_counter < 300) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -182,16 +182,19 @@ int do_airspeed_calibration(int mavlink_fd) float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; - if (fabsf(calibrated_pa) < 9.0f) { - mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%.1f Pa, #h101)", - (double)calibrated_pa); - usleep(3000 * 1000); + if (fabsf(calibrated_pa) < 50.0f) { + if (calibration_counter % 20 == 0) { + mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", + (int)calibrated_pa); + } + usleep(100 * 1000); + calibration_counter++; continue; } /* do not allow negative values */ if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static vs dynamic ports,restart"); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static vs dynamic ports,restart", (int)calibrated_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -209,8 +212,8 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); return ERROR; } else { - mavlink_log_info(mavlink_fd, "positive pressure: (%.1f Pa)", - (double)diff_pres.differential_pressure_raw_pa); + mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + (int)calibrated_pa); break; } From 8590d555b432f206e2b88893ea5198c1398aa99c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:44:57 +0200 Subject: [PATCH 252/600] Fix calibration counter usage, take every sample --- src/modules/commander/airspeed_calibration.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a2f11b14a4..10d04aae93 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -167,8 +167,8 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter = 0; - /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 300) { + /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ + while (calibration_counter < 1500) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -181,14 +181,13 @@ int do_airspeed_calibration(int mavlink_fd) orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; + calibration_counter++; if (fabsf(calibrated_pa) < 50.0f) { - if (calibration_counter % 20 == 0) { + if (calibration_counter % 100 == 0) { mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", (int)calibrated_pa); } - usleep(100 * 1000); - calibration_counter++; continue; } From c610050dc305884a704b51c00cfff5f2f9984bcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 14:55:23 +0200 Subject: [PATCH 253/600] EKF hotfix: Add missing vector zero calls --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index f33a1d7805..3da92b264e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2280,7 +2280,7 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max) } if (!isfinite(val)) { - ekf_debug("constrain: non-finite!"); + //ekf_debug("constrain: non-finite!"); } return ret; @@ -2920,6 +2920,8 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + dAngIMU.zero(); + dVelIMU.zero(); lastGyroOffset.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { From b4b3a2a2c68a523af5141a4452e533befc873384 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 14:58:57 +0200 Subject: [PATCH 254/600] EKF hotfix: Force zero initialization of vectors --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 ++-- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 2 +- src/modules/ekf_att_pos_estimator/estimator_utilities.h | 8 ++++++++ 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5d768b73dd..a835599e79 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -743,8 +743,8 @@ FixedwingEstimator::task_main() /* sets also parameters in the EKF object */ parameters_update(); - Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; - Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; + Vector3f lastAngRate; + Vector3f lastAccel; /* wakeup source(s) */ struct pollfd fds[2]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 3da92b264e..a41b58250f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -73,7 +73,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float qUpdated[4]; float quatMag; float deltaQuat[4]; - const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; + const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS); // Remove sensor bias errors correctedDelAng.x = dAngIMU.x - states[10]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 5648cb05cd..0cafdc808c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -19,6 +19,14 @@ public: float y; float z; + Vector3f() { zero(); } + + Vector3f(float a, float b, float c) : + x(a), + y(b), + z(c) + {} + float length(void) const; void zero(void); }; From 3d505c6f42779eddc983e0f1a25c59d998cdd041 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 15:38:42 +0200 Subject: [PATCH 255/600] EKF hotfix: Force all fields to initialized --- .../estimator_23states.cpp | 151 +++++++++++++----- 1 file changed, 112 insertions(+), 39 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index a41b58250f..7f9486eb5c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -5,47 +5,120 @@ #define EKF_COVARIANCE_DIVERGED 1.0e8f -AttPosEKF::AttPosEKF() +AttPosEKF::AttPosEKF() : + covTimeStepMax(0.0f), + covDelAngMax(0.0f), + rngFinderPitch(0.0f), + a1(0.0f), + a2(0.0f), + a3(0.0f), + yawVarScale(0.0f), + windVelSigma(0.0f), + dAngBiasSigma(0.0f), + dVelBiasSigma(0.0f), + magEarthSigma(0.0f), + magBodySigma(0.0f), + gndHgtSigma(0.0f), + vneSigma(0.0f), + vdSigma(0.0f), + posNeSigma(0.0f), + posDSigma(0.0f), + magMeasurementSigma(0.0f), + airspeedMeasurementSigma(0.0f), + gyroProcessNoise(0.0f), + accelProcessNoise(0.0f), + EAS2TAS(1.0f), + magstate{}, + resetMagState{}, + KH{}, + KHP{}, + P{}, + Kfusion{}, + states{}, + resetStates{}, + storedStates{}, + statetimeStamp{}, + statesAtVelTime{}, + statesAtPosTime{}, + statesAtHgtTime{}, + statesAtMagMeasTime{}, + statesAtVtasMeasTime{}, + statesAtRngTime{}, + statesAtOptFlowTime{}, + correctedDelAng(), + correctedDelVel(), + summedDelAng(), + summedDelVel(), + accNavMag(), + earthRateNED(), + angRate(), + lastGyroOffset(), + delAngTotal(), + Tbn(), + Tnb(), + accel(), + dVelIMU(), + dAngIMU(), + dtIMU(0), + fusionModeGPS(0), + innovVelPos{}, + varInnovVelPos{}, + velNED{}, + accelGPSNED{}, + posNE{}, + hgtMea(0.0f), + baroHgtOffset(0.0f), + rngMea(0.0f), + innovMag{}, + varInnovMag{}, + magData{}, + losData{}, + innovVtas(0.0f), + innovRng(0.0f), + innovOptFlow{}, + varInnovOptFlow{}, + varInnovVtas(0.0f), + varInnovRng(0.0f), + VtasMeas(0.0f), + magDeclination(0.0f), + latRef(0.0f), + lonRef(-M_PI_F), + hgtRef(0.0f), + refSet(false), + magBias(), + covSkipCount(0), + gpsLat(0.0), + gpsLon(-M_PI), + gpsHgt(0.0f), + GPSstatus(0), + baroHgt(0.0f), + statesInitialised(false), + fuseVelData(false), + fusePosData(false), + fuseHgtData(false), + fuseMagData(false), + fuseVtasData(false), + fuseRngData(false), + fuseOptFlowData(false), - /* NOTE: DO NOT initialize class members here. Use ZeroVariables() - * instead to allow clean in-air re-initialization. - */ + inhibitWindStates(true), + inhibitMagStates(true), + inhibitGndHgtState(true), + + onGround(true), + staticMode(true), + useAirspeed(true), + useCompass(true), + useRangeFinder(false), + useOpticalFlow(false), + + ekfDiverged(false), + lastReset(0), + current_ekf_state{}, + last_ekf_error{}, + numericalProtection(true), + storeIndex(0) { - summedDelAng.zero(); - summedDelVel.zero(); - - fusionModeGPS = 0; - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - onGround = true; - staticMode = true; - useAirspeed = true; - useCompass = true; - useRangeFinder = true; - useOpticalFlow = true; - numericalProtection = true; - refSet = false; - storeIndex = 0; - gpsHgt = 0.0f; - baroHgt = 0.0f; - GPSstatus = 0; - VtasMeas = 0.0f; - magDeclination = 0.0f; - dAngIMU.zero(); - dVelIMU.zero(); - velNED[0] = 0.0f; - velNED[1] = 0.0f; - velNED[2] = 0.0f; - accelGPSNED[0] = 0.0f; - accelGPSNED[1] = 0.0f; - accelGPSNED[2] = 0.0f; - delAngTotal.zero(); - ekfDiverged = false; - lastReset = 0; - memset(&last_ekf_error, 0, sizeof(last_ekf_error)); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); ZeroVariables(); From e8855423bea0938c607ed2ab8bde5ebec094b5a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 15:39:35 +0200 Subject: [PATCH 256/600] EKF hotfix: Remove unused variables, use default initializer list for vectors --- src/modules/ekf_att_pos_estimator/estimator_23states.h | 2 -- src/modules/ekf_att_pos_estimator/estimator_utilities.h | 4 +--- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index ff311649a1..faa6735caa 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -172,8 +172,6 @@ public: unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction // GPS input data variables - float gpsCourse; - float gpsVelD; double gpsLat; double gpsLon; float gpsHgt; diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 0cafdc808c..6d1f47b685 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -19,9 +19,7 @@ public: float y; float z; - Vector3f() { zero(); } - - Vector3f(float a, float b, float c) : + Vector3f(float a=0.0f, float b=0.0f, float c=0.0f) : x(a), y(b), z(c) From d4a867071a2183069d8902292357133856cd2ffd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 15:44:15 +0200 Subject: [PATCH 257/600] airspeed: Better calibration messages --- src/modules/commander/airspeed_calibration.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 10d04aae93..469f156327 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -193,7 +193,7 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "%d Pa: swap static vs dynamic ports,restart", (int)calibrated_pa); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)calibrated_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -206,9 +206,12 @@ int do_airspeed_calibration(int mavlink_fd) } /* save */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0); (void)param_save_default(); close(diff_pres_sub); + + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", From a1c5f87deaf3e8626f838b191be5b57f33d3dc46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 16:26:47 +0200 Subject: [PATCH 258/600] Fix C++ warnings --- .../ekf_att_pos_estimator_main.cpp | 13 ++++++++++++- .../ekf_att_pos_estimator/estimator_utilities.cpp | 7 +++++-- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index a835599e79..bc40ce3c47 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -112,6 +112,10 @@ public: */ FixedwingEstimator(); + /* we do not want people ever copying this class */ + FixedwingEstimator(const FixedwingEstimator& that) = delete; + FixedwingEstimator operator=(const FixedwingEstimator&) = delete; + /** * Destructor, also kills the sensors task. */ @@ -362,9 +366,10 @@ FixedwingEstimator::FixedwingEstimator() : _mag_offsets({}), #ifdef SENSOR_COMBINED_SUB - _sensor_combined({}), + _sensor_combined{}, #endif + _pos_ref{}, _baro_ref(0.0f), _baro_ref_offset(0.0f), _baro_gps_offset(0.0f), @@ -381,12 +386,18 @@ FixedwingEstimator::FixedwingEstimator() : /* states */ _baro_init(false), _gps_initialized(false), + _gps_start_time(0), + _filter_start_time(0), + _last_sensor_timestamp(0), + _last_run(0), _gyro_valid(false), _accel_valid(false), _mag_valid(false), _ekf_logging(true), _debug(0), _mavlink_fd(-1), + _parameters{}, + _parameter_handles{}, _ekf(nullptr), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 29a8c8d1e3..77cc1eeeb8 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -45,8 +45,11 @@ void Vector3f::zero(void) z = 0.0f; } -Mat3f::Mat3f() { - identity(); +Mat3f::Mat3f() : + x{1.0f, 0.0f, 0.0f}, + y{0.0f, 1.0f, 0.0f}, + z{0.0f, 0.0f, 1.0f} +{ } void Mat3f::identity() { From 17c5e925fb928d9c0926d8495a2db7a7b464c15f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 16:27:08 +0200 Subject: [PATCH 259/600] Enable C++ warnings for EKF --- src/modules/ekf_att_pos_estimator/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index dc5220bf0c..36d854dddb 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -41,3 +41,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ estimator_23states.cpp \ estimator_utilities.cpp + +EXTRACXXFLAGS = -Weffc++ From f219c05f0f53ee8b8f5dbe24318678be6c255dd9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 16:27:30 +0200 Subject: [PATCH 260/600] Fix a set of C++ warnings in mathlib --- src/lib/mathlib/math/Matrix.hpp | 23 ++++++++++++++--------- src/lib/mathlib/math/Vector.hpp | 9 ++++++--- 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index ea0cf4ca1e..c2109b479d 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -69,27 +69,32 @@ public: /** * trivial ctor - * note that this ctor will not initialize elements + * Initializes the elements to zero. */ - MatrixBase() { - arm_mat = {M, N, &data[0][0]}; + MatrixBase() : + data{}, + arm_mat{M, N, &data[0][0]} + { } /** * copyt ctor */ - MatrixBase(const MatrixBase &m) { - arm_mat = {M, N, &data[0][0]}; + MatrixBase(const MatrixBase &m) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, m.data, sizeof(data)); } - MatrixBase(const float *d) { - arm_mat = {M, N, &data[0][0]}; + MatrixBase(const float *d) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, d, sizeof(data)); } - MatrixBase(const float d[M][N]) { - arm_mat = {M, N, &data[0][0]}; + MatrixBase(const float d[M][N]) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, d, sizeof(data)); } diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index c7323c2153..5017b7c793 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -69,10 +69,13 @@ public: /** * trivial ctor - * note that this ctor will not initialize elements + * initializes elements to zero */ - VectorBase() { - arm_col = {N, 1, &data[0]}; + VectorBase() : + data{}, + arm_col{N, 1, &data[0]} + { + } /** From 36c20681621c2b99c1486aa0e3700cdbd86bd42d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:42:34 +0200 Subject: [PATCH 261/600] Removed spurious submodule entry --- src/modules/ekf_att_pos_estimator/InertialNav | 1 - 1 file changed, 1 deletion(-) delete mode 160000 src/modules/ekf_att_pos_estimator/InertialNav diff --git a/src/modules/ekf_att_pos_estimator/InertialNav b/src/modules/ekf_att_pos_estimator/InertialNav deleted file mode 160000 index 8b65d755b8..0000000000 --- a/src/modules/ekf_att_pos_estimator/InertialNav +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8b65d755b8c4834f90a323172c25d91c45068e21 From 1dffa750d8b91af8f600ba4cf1fbcdcc61edf80f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:44:32 +0200 Subject: [PATCH 262/600] added detailed print --- src/modules/commander/airspeed_calibration.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 469f156327..bc12037c3b 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -162,6 +162,8 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } + mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)calibrated_pa); + /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); From 56ad0c708d2695515489802676cc295f040b3606 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:53:50 +0200 Subject: [PATCH 263/600] Fixed compile error --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index bc12037c3b..2e3f89e652 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -162,7 +162,7 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)calibrated_pa); + mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); From 7968c6864e1255b4a65427187119aec2c3fc7ae0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 00:04:02 +0200 Subject: [PATCH 264/600] Force update of offset, do not add offset in final value --- .../commander/airspeed_calibration.cpp | 21 +++++++++++++------ .../commander/state_machine_helper.cpp | 4 ++-- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 2e3f89e652..ea81ac6a64 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -140,6 +140,16 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { + int fd_scale = open(AIRSPEED_DEVICE_PATH, 0); + airscale.offset_pa = diff_pres_offset; + if (fd_scale > 0) { + if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + mavlink_log_critical(mavlink_fd, "airspeed offset update failed"); + } + + close(fd_scale); + } + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); @@ -182,20 +192,19 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; calibration_counter++; - if (fabsf(calibrated_pa) < 50.0f) { + if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 100 == 0) { mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", - (int)calibrated_pa); + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ - if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)calibrated_pa); + if (diff_pres.differential_pressure_raw_pa < 0.0f) { + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -217,7 +226,7 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } else { mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", - (int)calibrated_pa); + (int)diff_pres.differential_pressure_raw_pa); break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 09ea12c38a..372ba9d7dc 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -668,8 +668,8 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) struct airspeed_s airspeed; - if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) || - hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) { + if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || + (hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) { mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; From 180b83cc6dc6872c5248993b5409ac835a201114 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 00:41:12 +0200 Subject: [PATCH 265/600] Better analog error handling --- src/modules/commander/airspeed_calibration.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index ea81ac6a64..92d50a0251 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -94,9 +94,17 @@ int do_airspeed_calibration(int mavlink_fd) } if (!paramreset_successful) { - warn("FAILED to reset - assuming analog"); - mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + /* only warn if analog scaling is zero */ + float analog_scaling = 0.0f; + param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)) + if (fabsf(analog_scaling) < 0.1f) { + mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + close(diff_pres_sub); + return ERROR; + } + + /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); From ec8d395a2d270bd77e873f616a53ee0771c94165 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:07:30 +0200 Subject: [PATCH 266/600] build fix --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 92d50a0251..423d886828 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -97,7 +97,7 @@ int do_airspeed_calibration(int mavlink_fd) /* only warn if analog scaling is zero */ float analog_scaling = 0.0f; - param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)) + param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); close(diff_pres_sub); From 5f176d14fed9ace9f5265a40b7ace1e6d00a7690 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:11:33 +0200 Subject: [PATCH 267/600] ekf: Only return from start handler with all allocations done --- .../ekf_att_pos_estimator_main.cpp | 25 +++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index a835599e79..247814a7e3 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -124,6 +124,13 @@ public: */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + /** * Print the current status. */ @@ -151,7 +158,8 @@ public: private: bool _task_should_exit; /**< if true, sensor task should exit */ - int _estimator_task; /**< task handle for sensor task */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _estimator_task; /**< task handle for sensor task */ #ifndef SENSOR_COMBINED_SUB int _gyro_sub; /**< gyro sensor subscription */ int _accel_sub; /**< accel sensor subscription */ @@ -313,12 +321,13 @@ namespace estimator #endif static const int ERROR = -1; -FixedwingEstimator *g_estimator; +FixedwingEstimator *g_estimator = nullptr; } FixedwingEstimator::FixedwingEstimator() : _task_should_exit(false), + _task_running(false), _estimator_task(-1), /* subscriptions */ @@ -772,6 +781,8 @@ FixedwingEstimator::task_main() _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; + _task_running = true; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -1518,6 +1529,8 @@ FixedwingEstimator::task_main() perf_end(_loop_perf); } + _task_running = false; + warnx("exiting.\n"); _estimator_task = -1; @@ -1670,6 +1683,14 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (estimator::g_estimator == nullptr || !estimator::g_estimator->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } From 8a0e83c9cd9a54f09c6bc5c2c89917d87a451192 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:11:55 +0200 Subject: [PATCH 268/600] fw att ctrl: Only return from start handler with all allocations done --- .../fw_att_control/fw_att_control_main.cpp | 28 ++++++++++++++++--- 1 file changed, 24 insertions(+), 4 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 3cd4ce9285..4cdba735a0 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * 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 @@ -99,13 +98,21 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle for sensor task */ int _att_sub; /**< vehicle attitude subscription */ @@ -276,6 +283,7 @@ private: * Main sensor collection task. */ void task_main(); + }; namespace att_control @@ -287,12 +295,13 @@ namespace att_control #endif static const int ERROR = -1; -FixedwingAttitudeControl *g_control; +FixedwingAttitudeControl *g_control = nullptr; } FixedwingAttitudeControl::FixedwingAttitudeControl() : _task_should_exit(false), + _task_running(false), _control_task(-1), /* subscriptions */ @@ -598,6 +607,8 @@ FixedwingAttitudeControl::task_main() fds[1].fd = _att_sub; fds[1].events = POLLIN; + _task_running = true; + while (!_task_should_exit) { static int loop_counter = 0; @@ -921,6 +932,7 @@ FixedwingAttitudeControl::task_main() warnx("exiting.\n"); _control_task = -1; + _task_running = false; _exit(0); } @@ -966,6 +978,14 @@ int fw_att_control_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } From efa5d8f57f303a51f618b7c108f516d5e857c3b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:12:17 +0200 Subject: [PATCH 269/600] fw pos ctrl: Only return from start handler with all allocations done --- .../fw_pos_control_l1_main.cpp | 26 ++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 98ccd09a5e..08c996ebc0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * 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 @@ -120,10 +119,18 @@ public: */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + private: int _mavlink_fd; bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle for sensor task */ int _global_pos_sub; @@ -391,13 +398,14 @@ namespace l1_control #endif static const int ERROR = -1; -FixedwingPositionControl *g_control; +FixedwingPositionControl *g_control = nullptr; } FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), _task_should_exit(false), + _task_running(false), _control_task(-1), /* subscriptions */ @@ -1290,6 +1298,8 @@ FixedwingPositionControl::task_main() fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; + _task_running = true; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -1390,6 +1400,8 @@ FixedwingPositionControl::task_main() perf_end(_loop_perf); } + _task_running = false; + warnx("exiting.\n"); _control_task = -1; @@ -1478,6 +1490,14 @@ int fw_pos_control_l1_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (l1_control::g_control == nullptr || !l1_control::g_control->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } From 9ce7820e419d2ffa379fb7a3cc168f500623fa3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:32:51 +0200 Subject: [PATCH 270/600] Make instructions in commander more obvious for airspeed calibration --- src/modules/commander/airspeed_calibration.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 423d886828..598cfe9e2d 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,15 +180,16 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); calibration_counter = 0; + const int maxcount = 3000; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ - while (calibration_counter < 1500) { + while (calibration_counter < maxcount) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -204,7 +205,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 100 == 0) { - mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", + mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -212,6 +213,8 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { + mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); @@ -246,6 +249,12 @@ int do_airspeed_calibration(int mavlink_fd) } } + if (calibration_counter == maxcount) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); From f3549d775cb049bcde93c3e860c3adbad3763364 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:33:35 +0200 Subject: [PATCH 271/600] Airspeed driver: Use the known sensor offset for raw value as well --- src/drivers/ets_airspeed/ets_airspeed.cpp | 3 +++ src/drivers/meas_airspeed/meas_airspeed.cpp | 5 ++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index d1a9fa57c8..c15a0cee43 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -172,6 +172,9 @@ ETSAirspeed::collect() diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset; } + // The raw value still should be compensated for the known offset + diff_pres_pa_raw -= _diff_pres_offset; + // Track maximum differential pressure measured (so we can work out top speed). if (diff_pres_pa > _max_differential_pressure_pa) { _max_differential_pressure_pa = diff_pres_pa; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 7763f10578..07611f9039 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -225,7 +225,10 @@ MEASAirspeed::collect() // correct for 5V rail voltage if possible voltage_correction(diff_press_pa_raw, temperature); - float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset); + // the raw value still should be compensated for the known offset + diff_press_pa_raw -= _diff_pres_offset; + + float diff_press_pa = fabsf(diff_press_pa_raw); /* note that we return both the absolute value with offset From 83652794c7709ee8058587a99e81826d4959ed49 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 15 Jul 2014 00:06:24 +0200 Subject: [PATCH 272/600] Add a mixer for the Phantom so it can be uniquely adjusted for this airframe. --- ROMFS/px4fmu_common/init.d/3031_phantom | 2 +- ROMFS/px4fmu_common/mixers/phantom.mix | 69 +++++++++++++++++++++++++ 2 files changed, 70 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/mixers/phantom.mix diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index d05c3174f9..654b0bdab1 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -36,7 +36,7 @@ then param set FW_T_TIME_CONST 5 fi -set MIXER FMU_Q +set MIXER phantom # Provide ESC a constant 1000 us pulse set PWM_OUTPUTS 4 diff --git a/ROMFS/px4fmu_common/mixers/phantom.mix b/ROMFS/px4fmu_common/mixers/phantom.mix new file mode 100644 index 0000000000..7fe6dc0551 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/phantom.mix @@ -0,0 +1,69 @@ +Delta-wing mixer for PX4FMU +=========================== + +Designed for the Phantom FX-61 + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4/Pixhawk. The configuration assumes the elevon servos are connected to +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor are set so that pitch will have more travel than roll. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 6500 6500 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 -6500 -6500 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 From 9f837267de861cfd8313984f2ec64b8482a72161 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 15 Jul 2014 00:10:08 +0200 Subject: [PATCH 273/600] Updated the comments. --- ROMFS/px4fmu_common/mixers/phantom.mix | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/phantom.mix b/ROMFS/px4fmu_common/mixers/phantom.mix index 7fe6dc0551..00c37a16c1 100644 --- a/ROMFS/px4fmu_common/mixers/phantom.mix +++ b/ROMFS/px4fmu_common/mixers/phantom.mix @@ -1,7 +1,5 @@ -Delta-wing mixer for PX4FMU -=========================== - -Designed for the Phantom FX-61 +Phantom FX-61 mixer +=================== This file defines mixers suitable for controlling a delta wing aircraft using PX4/Pixhawk. The configuration assumes the elevon servos are connected to From 61286451361c0e4547caf2385f56ee6cc8afffb1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 07:37:17 +0200 Subject: [PATCH 274/600] mathlib: More C++ fixes --- src/lib/mathlib/math/Matrix.hpp | 2 ++ src/lib/mathlib/math/Vector.hpp | 12 ++++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index c2109b479d..ca931e2daf 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -77,6 +77,8 @@ public: { } + virtual ~MatrixBase() {}; + /** * copyt ctor */ diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 5017b7c793..0ddf776155 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -78,19 +78,23 @@ public: } + virtual ~VectorBase() {}; + /** * copy ctor */ - VectorBase(const VectorBase &v) { - arm_col = {N, 1, &data[0]}; + VectorBase(const VectorBase &v) : + arm_col{N, 1, &data[0]} + { memcpy(data, v.data, sizeof(data)); } /** * setting ctor */ - VectorBase(const float d[N]) { - arm_col = {N, 1, &data[0]}; + VectorBase(const float d[N]) : + arm_col{N, 1, &data[0]} + { memcpy(data, d, sizeof(data)); } From e91a4217ad23a0f5190b0e34b7621a5170632a43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 07:54:12 +0200 Subject: [PATCH 275/600] Fix warning --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 598cfe9e2d..0e58c68b60 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -186,7 +186,7 @@ int do_airspeed_calibration(int mavlink_fd) usleep(500 * 1000); calibration_counter = 0; - const int maxcount = 3000; + const unsigned maxcount = 3000; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { From b0b6ee06448c266dc97d8efbd48be548f4a57ee6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 18:22:38 +0200 Subject: [PATCH 276/600] Forbid copy constructor in CDev --- src/drivers/device/device.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 7df234cab0..2d105bb791 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -240,6 +240,7 @@ private: * @param context Pointer to the interrupted context. */ static void dev_interrupt(int irq, void *context); + }; /** @@ -469,6 +470,10 @@ private: * @return OK, or -errno on error. */ int remove_poll_waiter(struct pollfd *fds); + + /* do not allow copying this class */ + CDev(const CDev&); + CDev operator=(const CDev&); }; /** From 1ac2b307e4424d3b6555ab3ca21c43d8de19b81e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 18:23:17 +0200 Subject: [PATCH 277/600] Enable stricter compile mode and ensure the most relevant bits are initialized. Needs more work to avoid the remaining warnings --- src/modules/mavlink/mavlink_commands.cpp | 7 +++++-- src/modules/mavlink/mavlink_ftp.cpp | 16 ++++++++++------ src/modules/mavlink/mavlink_main.cpp | 10 +++++++++- src/modules/mavlink/mavlink_main.h | 4 ++++ src/modules/mavlink/mavlink_mission.h | 4 ++++ src/modules/mavlink/mavlink_orb_subscription.h | 4 ++++ src/modules/mavlink/mavlink_receiver.cpp | 12 ++++++------ src/modules/mavlink/mavlink_receiver.h | 4 ++++ src/modules/mavlink/mavlink_stream.h | 4 ++++ src/modules/mavlink/module.mk | 2 ++ 10 files changed, 52 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp index fccd4d9a59..b502c3c863 100644 --- a/src/modules/mavlink/mavlink_commands.cpp +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -40,9 +40,12 @@ #include "mavlink_commands.h" -MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0) +MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : + _cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))), + _cmd{}, + _channel(channel), + _cmd_time(0) { - _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); } void diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 675a6870e1..6a2c900afe 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -50,16 +50,20 @@ MavlinkFTP::getServer() return _server; } -MavlinkFTP::MavlinkFTP() +MavlinkFTP::MavlinkFTP() : + _session_fds{}, + _workBufs{}, + _workFree{}, + _lock{} { // initialise the request freelist dq_init(&_workFree); sem_init(&_lock, 0, 1); - - // initialize session list - for (size_t i=0; i Date: Tue, 15 Jul 2014 22:08:40 +0200 Subject: [PATCH 278/600] Do not initialize variable if value is never read --- src/drivers/hmc5883/hmc5883.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index ad17703001..573b4fbba5 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -867,7 +867,8 @@ HMC5883::collect() struct { int16_t x, y, z; } report; - int ret = -EIO; + + int ret; uint8_t cmd; uint8_t check_counter; From 5ef4e08c580a4f15628ed16194a680508ea044bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:14:30 +0200 Subject: [PATCH 279/600] hmc5883: Support for three sensors --- src/drivers/hmc5883/hmc5883.cpp | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 573b4fbba5..26014c6d8b 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -162,6 +162,7 @@ private: orb_advert_t _mag_topic; orb_advert_t _subsystem_pub; + orb_id_t _mag_orb_id; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -355,6 +356,7 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : _class_instance(-1), _mag_topic(-1), _subsystem_pub(-1), + _mag_orb_id(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -423,6 +425,20 @@ HMC5883::init() _class_instance = register_class_devname(MAG_DEVICE_PATH); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: + _mag_orb_id = ORB_ID(sensor_mag0); + break; + + case CLASS_DEVICE_SECONDARY: + _mag_orb_id = ORB_ID(sensor_mag1); + break; + + case CLASS_DEVICE_TERTIARY: + _mag_orb_id = ORB_ID(sensor_mag2); + break; + } + ret = OK; /* sensor is ok, but not calibrated */ _sensor_ok = true; @@ -946,16 +962,16 @@ HMC5883::collect() // apply user specified rotation rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); - if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + if (!(_pub_blocked)) { if (_mag_topic != -1) { /* publish it */ - orb_publish(ORB_ID(sensor_mag0), _mag_topic, &new_report); + orb_publish(_mag_orb_id, _mag_topic, &new_report); } else { - _mag_topic = orb_advertise(ORB_ID(sensor_mag0), &new_report); + _mag_topic = orb_advertise(_mag_orb_id, &new_report); if (_mag_topic < 0) - debug("failed to create sensor_mag publication"); + debug("ADVERT FAIL"); } } From 32ed1eae80f4004daa63b4331a66b774becf651f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:14:50 +0200 Subject: [PATCH 280/600] mpu6000: Support for up to three accels / gyros --- src/drivers/mpu6000/mpu6000.cpp | 48 ++++++++++++++++----------------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 25fd7d8a8e..09eec0caf3 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -215,6 +215,7 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; + orb_id_t _accel_orb_id; int _accel_class_instance; RingBuffer *_gyro_reports; @@ -367,6 +368,7 @@ protected: private: MPU6000 *_parent; orb_advert_t _gyro_topic; + orb_id_t _gyro_orb_id; int _gyro_class_instance; }; @@ -383,6 +385,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _accel_orb_id(nullptr), _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_range_scale(0.0f), @@ -505,17 +508,23 @@ MPU6000::init() /* measurement will have generated a report, publish */ switch (_accel_class_instance) { case CLASS_DEVICE_PRIMARY: - _accel_topic = orb_advertise(ORB_ID(sensor_accel0), &arp); + _accel_orb_id = ORB_ID(sensor_accel0); break; case CLASS_DEVICE_SECONDARY: - _accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp); + _accel_orb_id = ORB_ID(sensor_accel1); + break; + + case CLASS_DEVICE_TERTIARY: + _accel_orb_id = ORB_ID(sensor_accel2); break; } + _accel_topic = orb_advertise(_accel_orb_id, &arp); + if (_accel_topic < 0) { - warnx("failed to create sensor_accel publication"); + warnx("ADVERT FAIL"); } @@ -525,17 +534,23 @@ MPU6000::init() switch (_gyro->_gyro_class_instance) { case CLASS_DEVICE_PRIMARY: - _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp); + _gyro->_gyro_orb_id = ORB_ID(sensor_gyro0); break; case CLASS_DEVICE_SECONDARY: - _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp); + _gyro->_gyro_orb_id = ORB_ID(sensor_gyro1); + break; + + case CLASS_DEVICE_TERTIARY: + _gyro->_gyro_orb_id = ORB_ID(sensor_gyro2); break; } + _gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp); + if (_gyro->_gyro_topic < 0) { - warnx("failed to create sensor_gyro publication"); + warnx("ADVERT FAIL"); } out: @@ -1360,28 +1375,12 @@ MPU6000::measure() if (!(_pub_blocked)) { /* publish it */ - switch (_accel_class_instance) { - case CLASS_DEVICE_PRIMARY: - orb_publish(ORB_ID(sensor_accel0), _accel_topic, &arb); - break; - - case CLASS_DEVICE_SECONDARY: - orb_publish(ORB_ID(sensor_accel1), _accel_topic, &arb); - break; - } + orb_publish(_accel_orb_id, _accel_topic, &arb); } if (!(_pub_blocked)) { /* publish it */ - switch (_gyro->_gyro_class_instance) { - case CLASS_DEVICE_PRIMARY: - orb_publish(ORB_ID(sensor_gyro0), _gyro->_gyro_topic, &grb); - break; - - case CLASS_DEVICE_SECONDARY: - orb_publish(ORB_ID(sensor_gyro1), _gyro->_gyro_topic, &grb); - break; - } + orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1402,6 +1401,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : CDev("MPU6000_gyro", path), _parent(parent), _gyro_topic(-1), + _gyro_orb_id(nullptr), _gyro_class_instance(-1) { } From 65367f7a99907fbd6a204ba44ee443816c635482 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:16:04 +0200 Subject: [PATCH 281/600] L3GD20: Support for up to three gyros --- src/drivers/l3gd20/l3gd20.cpp | 33 ++++++++++++++++++--------------- 1 file changed, 18 insertions(+), 15 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 9e5eb00dba..5e90733ff3 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -213,6 +213,7 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; + orb_id_t _orb_id; int _class_instance; unsigned _current_rate; @@ -339,6 +340,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), + _orb_id(nullptr), _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_DEFAULT), @@ -399,6 +401,20 @@ L3GD20::init() _class_instance = register_class_devname(GYRO_DEVICE_PATH); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: + _orb_id = ORB_ID(sensor_gyro0); + break; + + case CLASS_DEVICE_SECONDARY: + _orb_id = ORB_ID(sensor_gyro1); + break; + + case CLASS_DEVICE_TERTIARY: + _orb_id = ORB_ID(sensor_gyro2); + break; + } + reset(); measure(); @@ -407,12 +423,7 @@ L3GD20::init() struct gyro_report grp; _reports->get(&grp); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp); - - } else if (_class_instance == CLASS_DEVICE_SECONDARY) { - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp); - } + _gyro_topic = orb_advertise(_orb_id, &grp); if (_gyro_topic < 0) { debug("failed to create sensor_gyro publication"); @@ -935,15 +946,7 @@ L3GD20::measure() /* publish for subscribers */ if (!(_pub_blocked)) { /* publish it */ - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - orb_publish(ORB_ID(sensor_gyro0), _gyro_topic, &report); - break; - - case CLASS_DEVICE_SECONDARY: - orb_publish(ORB_ID(sensor_gyro1), _gyro_topic, &report); - break; - } + orb_publish(_orb_id, _gyro_topic, &report); } _read++; From 64e33b8896d717f73a1b1ad3189b97c98c9bddfd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:18:04 +0200 Subject: [PATCH 282/600] uORB: Support up to three topics per sensor --- src/modules/uORB/objects_common.cpp | 3 +++ src/modules/uORB/topics/sensor_combined.h | 24 +++++++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 679d6ea4f5..08c3ce1ac1 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -48,14 +48,17 @@ #include ORB_DEFINE(sensor_mag0, struct mag_report); ORB_DEFINE(sensor_mag1, struct mag_report); +ORB_DEFINE(sensor_mag2, struct mag_report); #include ORB_DEFINE(sensor_accel0, struct accel_report); ORB_DEFINE(sensor_accel1, struct accel_report); +ORB_DEFINE(sensor_accel2, struct accel_report); #include ORB_DEFINE(sensor_gyro0, struct gyro_report); ORB_DEFINE(sensor_gyro1, struct gyro_report); +ORB_DEFINE(sensor_gyro2, struct gyro_report); #include ORB_DEFINE(sensor_baro0, struct baro_report); diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index fa3367de9b..06dfcdab30 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -95,6 +95,30 @@ struct sensor_combined_s { float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ + int16_t gyro1_raw[3]; /**< Raw sensor values of angular velocity */ + float gyro1_rad_s[3]; /**< Angular velocity in radian per seconds */ + uint64_t gyro1_timestamp; /**< Gyro timestamp */ + + int16_t accelerometer1_raw[3]; /**< Raw acceleration in NED body frame */ + float accelerometer1_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ + uint64_t accelerometer1_timestamp; /**< Accelerometer timestamp */ + + int16_t magnetometer1_raw[3]; /**< Raw magnetic field in NED body frame */ + float magnetometer1_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ + uint64_t magnetometer1_timestamp; /**< Magnetometer timestamp */ + + int16_t gyro2_raw[3]; /**< Raw sensor values of angular velocity */ + float gyro2_rad_s[3]; /**< Angular velocity in radian per seconds */ + uint64_t gyro2_timestamp; /**< Gyro timestamp */ + + int16_t accelerometer2_raw[3]; /**< Raw acceleration in NED body frame */ + float accelerometer2_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ + uint64_t accelerometer2_timestamp; /**< Accelerometer timestamp */ + + int16_t magnetometer2_raw[3]; /**< Raw magnetic field in NED body frame */ + float magnetometer2_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ + uint64_t magnetometer2_timestamp; /**< Magnetometer timestamp */ + float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ From b8b6974e22b7292d5251f1d2c49b2c81e09cc06b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:19:45 +0200 Subject: [PATCH 283/600] sdlog2: Add support for up to three IMU sensors --- src/modules/sdlog2/sdlog2.c | 66 ++++++++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 10 +++-- 2 files changed, 73 insertions(+), 3 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0d36fa2c6e..f534c0f4ce 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1073,6 +1073,12 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime magnetometer_timestamp = 0; hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; + hrt_abstime gyro1_timestamp = 0; + hrt_abstime accelerometer1_timestamp = 0; + hrt_abstime magnetometer1_timestamp = 0; + hrt_abstime gyro2_timestamp = 0; + hrt_abstime accelerometer2_timestamp = 0; + hrt_abstime magnetometer2_timestamp = 0; /* initialize calculated mean SNR */ float snr_mean = 0.0f; @@ -1209,6 +1215,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- SENSOR COMBINED --- */ if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) { bool write_IMU = false; + bool write_IMU1 = false; + bool write_IMU2 = false; bool write_SENS = false; if (buf.sensor.timestamp != gyro_timestamp) { @@ -1260,6 +1268,64 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(SENS); } + if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) { + accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp; + write_IMU1 = true; + } + + if (buf.sensor.gyro1_timestamp != gyro1_timestamp) { + gyro1_timestamp = buf.sensor.gyro1_timestamp; + write_IMU1 = true; + } + + if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) { + magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp; + write_IMU1 = true; + } + + if (write_IMU1) { + log_msg.msg_type = LOG_IMU1_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + + if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) { + accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp; + write_IMU2 = true; + } + + if (buf.sensor.gyro2_timestamp != gyro2_timestamp) { + gyro2_timestamp = buf.sensor.gyro2_timestamp; + write_IMU2 = true; + } + + if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) { + magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp; + write_IMU2 = true; + } + + if (write_IMU2) { + log_msg.msg_type = LOG_IMU2_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + } /* --- ATTITUDE --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 87f7f718f5..b14ef04ccf 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -73,6 +73,8 @@ struct log_ATSP_s { /* --- IMU - IMU SENSORS --- */ #define LOG_IMU_MSG 4 +#define LOG_IMU1_MSG 22 +#define LOG_IMU2_MSG 23 struct log_IMU_s { float acc_x; float acc_y; @@ -276,8 +278,8 @@ struct log_DIST_s { uint8_t flags; }; -// ID 22 available -// ID 23 available +/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ + /* --- PWR - ONBOARD POWER SYSTEM --- */ #define LOG_PWR_MSG 24 @@ -420,7 +422,9 @@ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), - LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), From 9e97994ef92d3ef3496384c4b11c734138c5f3a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:20:02 +0200 Subject: [PATCH 284/600] drivers: Up to three units support --- src/drivers/device/device.h | 3 ++- src/drivers/drv_accel.h | 1 + src/drivers/drv_gyro.h | 1 + src/drivers/drv_mag.h | 2 +- 4 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index b65649d9da..813e1542bb 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -540,7 +540,8 @@ private: // class instance for primary driver of each class enum CLASS_DEVICE { CLASS_DEVICE_PRIMARY=0, - CLASS_DEVICE_SECONDARY=1 + CLASS_DEVICE_SECONDARY=1, + CLASS_DEVICE_TERTIARY=2 }; #endif /* _DEVICE_DEVICE_H */ diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index a6abaec70f..1f98d966bd 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -83,6 +83,7 @@ struct accel_scale { */ ORB_DECLARE(sensor_accel0); ORB_DECLARE(sensor_accel1); +ORB_DECLARE(sensor_accel2); /* * ioctl() definitions diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 3635cbce14..41b013a443 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -83,6 +83,7 @@ struct gyro_scale { */ ORB_DECLARE(sensor_gyro0); ORB_DECLARE(sensor_gyro1); +ORB_DECLARE(sensor_gyro2); /* * ioctl() definitions diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index a6c509d319..5ddf5d08e8 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -81,7 +81,7 @@ struct mag_scale { */ ORB_DECLARE(sensor_mag0); ORB_DECLARE(sensor_mag1); - +ORB_DECLARE(sensor_mag2); /* * mag device types, for _device_id From c467d1635efda46bc7200b95741381fc98e161ae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:21:00 +0200 Subject: [PATCH 285/600] Build fixes for example --- src/examples/matlab_csv_serial/matlab_csv_serial.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 1327d1a232..c66bebeecd 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 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 @@ -60,6 +60,7 @@ #include #include #include +#include #include __EXPORT int matlab_csv_serial_main(int argc, char *argv[]); @@ -153,7 +154,6 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) /* Try to set baud rate */ struct termios uart_config; int termios_state; - bool is_usb = false; /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(serial_fd, &uart_config)) < 0) { @@ -225,7 +225,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0); - orb_copy(ORB_ID(sensor_accel1), accel0_sub, &accel1); + orb_copy(ORB_ID(sensor_accel1), accel1_sub, &accel1); orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0); orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1); From b6bac2c88d44fcb7ee0b1fd579f0f0fcc19c2410 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:22:04 +0200 Subject: [PATCH 286/600] sensors: Support for up to three sensors of the IMU types --- src/modules/sensors/sensors.cpp | 110 +++++++++++++++++++++++++++++++- 1 file changed, 107 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 388cd2dcc5..4e8a8c01d3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -199,9 +199,15 @@ private: bool _hil_enabled; /**< if true, HIL is active */ bool _publishing; /**< if true, we are publishing sensor data */ - int _gyro_sub; /**< raw gyro data subscription */ - int _accel_sub; /**< raw accel data subscription */ - int _mag_sub; /**< raw mag data subscription */ + int _gyro_sub; /**< raw gyro0 data subscription */ + int _accel_sub; /**< raw accel0 data subscription */ + int _mag_sub; /**< raw mag0 data subscription */ + int _gyro1_sub; /**< raw gyro1 data subscription */ + int _accel1_sub; /**< raw accel1 data subscription */ + int _mag1_sub; /**< raw mag1 data subscription */ + int _gyro2_sub; /**< raw gyro2 data subscription */ + int _accel2_sub; /**< raw accel2 data subscription */ + int _mag2_sub; /**< raw mag2 data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ int _airspeed_sub; /**< airspeed subscription */ @@ -478,6 +484,12 @@ Sensors::Sensors() : _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), + _gyro1_sub(-1), + _accel1_sub(-1), + _mag1_sub(-1), + _gyro2_sub(-1), + _accel2_sub(-1), + _mag2_sub(-1), _rc_sub(-1), _baro_sub(-1), _vcontrol_mode_sub(-1), @@ -1019,6 +1031,48 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_timestamp = accel_report.timestamp; } + + orb_check(_accel1_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report); + + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; + + raw.accelerometer1_m_s2[0] = vect(0); + raw.accelerometer1_m_s2[1] = vect(1); + raw.accelerometer1_m_s2[2] = vect(2); + + raw.accelerometer1_raw[0] = accel_report.x_raw; + raw.accelerometer1_raw[1] = accel_report.y_raw; + raw.accelerometer1_raw[2] = accel_report.z_raw; + + raw.accelerometer1_timestamp = accel_report.timestamp; + } + + orb_check(_accel2_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report); + + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; + + raw.accelerometer2_m_s2[0] = vect(0); + raw.accelerometer2_m_s2[1] = vect(1); + raw.accelerometer2_m_s2[2] = vect(2); + + raw.accelerometer2_raw[0] = accel_report.x_raw; + raw.accelerometer2_raw[1] = accel_report.y_raw; + raw.accelerometer2_raw[2] = accel_report.z_raw; + + raw.accelerometer2_timestamp = accel_report.timestamp; + } } void @@ -1045,6 +1099,48 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.timestamp = gyro_report.timestamp; } + + orb_check(_gyro1_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report); + + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; + + raw.gyro1_rad_s[0] = vect(0); + raw.gyro1_rad_s[1] = vect(1); + raw.gyro1_rad_s[2] = vect(2); + + raw.gyro1_raw[0] = gyro_report.x_raw; + raw.gyro1_raw[1] = gyro_report.y_raw; + raw.gyro1_raw[2] = gyro_report.z_raw; + + raw.gyro1_timestamp = gyro_report.timestamp; + } + + orb_check(_gyro2_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report); + + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; + + raw.gyro2_rad_s[0] = vect(0); + raw.gyro2_rad_s[1] = vect(1); + raw.gyro2_rad_s[2] = vect(2); + + raw.gyro2_raw[0] = gyro_report.x_raw; + raw.gyro2_raw[1] = gyro_report.y_raw; + raw.gyro2_raw[2] = gyro_report.z_raw; + + raw.gyro2_timestamp = gyro_report.timestamp; + } } void @@ -1060,6 +1156,8 @@ Sensors::mag_poll(struct sensor_combined_s &raw) math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + // XXX we need device-id based handling here + if (_mag_is_external) { vect = _external_mag_rotation * vect; @@ -1621,6 +1719,12 @@ Sensors::task_main() _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag0)); + _gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); + _accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); + _mag1_sub = orb_subscribe(ORB_ID(sensor_mag1)); + _gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2)); + _accel2_sub = orb_subscribe(ORB_ID(sensor_accel2)); + _mag2_sub = orb_subscribe(ORB_ID(sensor_mag2)); _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); From 43bc2c3ef2a867a015e7198c797d089d6252fdde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:32:03 +0200 Subject: [PATCH 287/600] LSM303D: Support for tertiary sensors --- src/drivers/lsm303d/lsm303d.cpp | 47 ++++++++++++++++----------------- 1 file changed, 23 insertions(+), 24 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index ff0eda60bb..2750f8755e 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -284,6 +284,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; + orb_id_t _accel_orb_id; int _accel_class_instance; unsigned _accel_read; @@ -485,6 +486,7 @@ private: LSM303D *_parent; orb_advert_t _mag_topic; + orb_id_t _mag_orb_id; int _mag_class_instance; void measure(); @@ -508,6 +510,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), + _accel_orb_id(nullptr), _accel_class_instance(-1), _accel_read(0), _mag_read(0), @@ -618,16 +621,22 @@ LSM303D::init() /* measurement will have generated a report, publish */ switch (_mag->_mag_class_instance) { case CLASS_DEVICE_PRIMARY: - _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag0), &mrp); + _mag->_mag_orb_id = ORB_ID(sensor_mag0); break; case CLASS_DEVICE_SECONDARY: - _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag1), &mrp); + _mag->_mag_orb_id = ORB_ID(sensor_mag1); + break; + + case CLASS_DEVICE_TERTIARY: + _mag->_mag_orb_id = ORB_ID(sensor_mag2); break; } + _mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp); + if (_mag->_mag_topic < 0) { - warnx("failed to create sensor_mag publication"); + warnx("ADVERT ERR"); } _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); @@ -639,17 +648,22 @@ LSM303D::init() /* measurement will have generated a report, publish */ switch (_accel_class_instance) { case CLASS_DEVICE_PRIMARY: - _accel_topic = orb_advertise(ORB_ID(sensor_accel0), &arp); + _accel_orb_id = ORB_ID(sensor_accel0); break; case CLASS_DEVICE_SECONDARY: - _accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp); + _accel_orb_id = ORB_ID(sensor_accel1); break; + case CLASS_DEVICE_TERTIARY: + _accel_orb_id = ORB_ID(sensor_accel2); + break; } + _accel_topic = orb_advertise(_accel_orb_id, &arp); + if (_accel_topic < 0) { - warnx("failed to create sensor_accel publication"); + warnx("ADVERT ERR"); } out: @@ -1570,15 +1584,7 @@ LSM303D::measure() if (!(_pub_blocked)) { /* publish it */ - switch (_accel_class_instance) { - case CLASS_DEVICE_PRIMARY: - orb_publish(ORB_ID(sensor_accel0), _accel_topic, &accel_report); - break; - - case CLASS_DEVICE_SECONDARY: - orb_publish(ORB_ID(sensor_accel1), _accel_topic, &accel_report); - break; - } + orb_publish(_accel_orb_id, _accel_topic, &accel_report); } _accel_read++; @@ -1655,15 +1661,7 @@ LSM303D::mag_measure() if (!(_pub_blocked)) { /* publish it */ - switch (_mag->_mag_class_instance) { - case CLASS_DEVICE_PRIMARY: - orb_publish(ORB_ID(sensor_mag0), _mag->_mag_topic, &mag_report); - break; - - case CLASS_DEVICE_SECONDARY: - orb_publish(ORB_ID(sensor_mag1), _mag->_mag_topic, &mag_report); - break; - } + orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report); } _mag_read++; @@ -1757,6 +1755,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), _parent(parent), _mag_topic(-1), + _mag_orb_id(nullptr), _mag_class_instance(-1) { } From f89573a6ed6e43c9e5935b92929b6082089226c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 07:51:24 +0200 Subject: [PATCH 288/600] HMC5883 driver: stack size adjustments of start handler and start return review / comments --- src/drivers/hmc5883/hmc5883.cpp | 9 ++++++--- src/drivers/hmc5883/module.mk | 5 ++--- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 4aef43102e..0cde1045c2 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -867,7 +867,7 @@ HMC5883::collect() struct { int16_t x, y, z; } report; - int ret = -EIO; + int ret; uint8_t cmd; uint8_t check_counter; @@ -1157,7 +1157,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) out: if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to set new scale / offsets for mag"); + warn("failed to set new scale / offsets for mag"); } /* set back to normal mode */ @@ -1353,6 +1353,9 @@ void usage(); /** * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. */ void start(int bus, enum Rotation rotation) @@ -1443,7 +1446,7 @@ test(int bus) int fd = open(path, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); + err(1, "%s open failed (try 'hmc5883 start')", path); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index 07377556d3..f51a2b580d 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -37,7 +37,6 @@ MODULE_COMMAND = hmc5883 -# XXX seems excessive, check if 2048 is sufficient -MODULE_STACKSIZE = 4096 - SRCS = hmc5883.cpp + +MODULE_STACKSIZE = 1200 From c2f97e628cf060d35db22526b450b919f0c6cf59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 07:51:47 +0200 Subject: [PATCH 289/600] L3GD20 driver: stack size adjustments of start handler and start return review / comments --- src/drivers/l3gd20/l3gd20.cpp | 3 +++ src/drivers/l3gd20/module.mk | 2 ++ 2 files changed, 5 insertions(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 64d1a7e552..2273f8aecc 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -990,6 +990,9 @@ void info(); /** * Start the driver. + * + * This function call only returns once the driver + * started or failed to detect the sensor. */ void start(bool external_bus, enum Rotation rotation) diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk index 23e77e8712..83752e08be 100644 --- a/src/drivers/l3gd20/module.mk +++ b/src/drivers/l3gd20/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = l3gd20 SRCS = l3gd20.cpp + +MODULE_STACKSIZE = 1200 From fa6f69581ed7dca03ca6ee2cfb8565343400f419 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 07:52:18 +0200 Subject: [PATCH 290/600] LSM303D: start handler fix and start routine review --- src/drivers/lsm303d/lsm303d.cpp | 3 +++ src/drivers/lsm303d/module.mk | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 45e7750552..d70bff69ad 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1803,6 +1803,9 @@ void usage(); /** * Start the driver. + * + * This function call only returns once the driver is + * up and running or failed to detect the sensor. */ void start(bool external_bus, enum Rotation rotation) diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index e40f718c54..6e9c22799f 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -5,4 +5,4 @@ MODULE_COMMAND = lsm303d SRCS = lsm303d.cpp - +MODULE_STACKSIZE = 1200 From 91bedc5c1c84fa79945f32462dbe3fe30c0ff5e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 07:54:09 +0200 Subject: [PATCH 291/600] airspeed drivers: Start handlers stack fixes and start handler review --- src/drivers/ets_airspeed/ets_airspeed.cpp | 3 +++ src/drivers/ets_airspeed/module.mk | 3 ++- src/drivers/meas_airspeed/meas_airspeed.cpp | 3 +++ src/drivers/meas_airspeed/module.mk | 3 ++- 4 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index c15a0cee43..f98d615a2c 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -286,6 +286,9 @@ void info(); /** * Start the driver. + * + * This function only returns if the sensor is up and running + * or could not be detected successfully. */ void start(int i2c_bus) diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk index 15346c5c55..966a5b8190 100644 --- a/src/drivers/ets_airspeed/module.mk +++ b/src/drivers/ets_airspeed/module.mk @@ -36,6 +36,7 @@ # MODULE_COMMAND = ets_airspeed -MODULE_STACKSIZE = 2048 SRCS = ets_airspeed.cpp + +MODULE_STACKSIZE = 1200 diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 07611f9039..d643fcad61 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -420,6 +420,9 @@ void info(); /** * Start the driver. + * + * This function call only returns once the driver is up and running + * or failed to detect the sensor. */ void start(int i2c_bus) diff --git a/src/drivers/meas_airspeed/module.mk b/src/drivers/meas_airspeed/module.mk index fed4078b63..84192d1b56 100644 --- a/src/drivers/meas_airspeed/module.mk +++ b/src/drivers/meas_airspeed/module.mk @@ -36,6 +36,7 @@ # MODULE_COMMAND = meas_airspeed -MODULE_STACKSIZE = 2048 SRCS = meas_airspeed.cpp + +MODULE_STACKSIZE = 1200 From 5b38b5e37170ffb079ef228f117f7e8b594353c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 07:55:53 +0200 Subject: [PATCH 292/600] MPU6K: Start handler startup and stack review and adjustments --- src/drivers/mpu6000/module.mk | 5 ++--- src/drivers/mpu6000/mpu6000.cpp | 5 ++++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk index c7d9cd3ef1..6087e1509e 100644 --- a/src/drivers/mpu6000/module.mk +++ b/src/drivers/mpu6000/module.mk @@ -37,7 +37,6 @@ MODULE_COMMAND = mpu6000 -# XXX seems excessive, check if 2048 is not sufficient -MODULE_STACKSIZE = 4096 - SRCS = mpu6000.cpp + +MODULE_STACKSIZE = 1200 diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1b3a96a0d4..442f0a37cc 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1437,6 +1437,9 @@ void usage(); /** * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. */ void start(bool external_bus, enum Rotation rotation) @@ -1507,7 +1510,7 @@ test(bool external_bus) int fd = open(path_accel, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", + err(1, "%s open failed (try 'mpu6000 start')", path_accel); /* get the driver */ From b08e3d21cdca5c21396b47280b0958d6592a80d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:47:12 +0200 Subject: [PATCH 293/600] Making lowpass filter init bullet proof --- src/lib/mathlib/math/filter/LowPassFilter2p.hpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 74cd5d78c0..4360651751 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -46,10 +46,18 @@ class __EXPORT LowPassFilter2p { public: // constructor - LowPassFilter2p(float sample_freq, float cutoff_freq) { + LowPassFilter2p(float sample_freq, float cutoff_freq) : + _cutoff_freq(cutoff_freq), + _a1(0.0f), + _a2(0.0f), + _b0(0.0f), + _b1(0.0f), + _b2(0.0f), + _delay_element_1(0.0f), + _delay_element_2(0.0f) + { // set initial parameters set_cutoff_frequency(sample_freq, cutoff_freq); - _delay_element_1 = _delay_element_2 = 0; } /** From 8107205b9ede8d9d36feffb50e116bb8e4dda78a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:47:31 +0200 Subject: [PATCH 294/600] device driver: Fix compile warnings --- src/drivers/device/ringbuffer.h | 4 ++++ src/drivers/device/spi.h | 5 +++++ 2 files changed, 9 insertions(+) diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index a9e22eaa6b..b26e2e7c83 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -162,6 +162,10 @@ private: volatile unsigned _tail; /**< removal point in _item_size units */ unsigned _next(unsigned index); + + /* we don't want this class to be copied */ + RingBuffer(const RingBuffer&); + RingBuffer operator=(const RingBuffer&); }; RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 54849c8c37..1d98376898 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -129,10 +129,15 @@ private: uint32_t _frequency; struct spi_dev_s *_dev; + /* this class does not allow copying */ + SPI(const SPI&); + SPI operator=(const SPI&); + protected: int _bus; int _transfer(uint8_t *send, uint8_t *recv, unsigned len); + }; } // namespace device From f162a3e8d406e52d42ad84ea80ca6678577b9263 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:47:49 +0200 Subject: [PATCH 295/600] LSM303D: Ensure init and constructors --- src/drivers/lsm303d/lsm303d.cpp | 13 +++++++++++++ src/drivers/lsm303d/module.mk | 2 ++ 2 files changed, 15 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index d70bff69ad..84ec18f286 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -461,6 +461,10 @@ private: * @return OK if the value can be supported. */ int mag_set_samplerate(unsigned frequency); + + /* this class cannot be copied */ + LSM303D(const LSM303D&); + LSM303D operator=(const LSM303D&); }; /** @@ -490,20 +494,28 @@ private: void measure(); void measure_trampoline(void *arg); + + /* this class does not allow copying due to ptr data members */ + LSM303D_mag(const LSM303D_mag&); + LSM303D_mag operator=(const LSM303D_mag&); }; LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), _mag(new LSM303D_mag(this)), + _accel_call{}, + _mag_call{}, _call_accel_interval(0), _call_mag_interval(0), _accel_reports(nullptr), _mag_reports(nullptr), + _accel_scale{}, _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), _accel_samplerate(0), _accel_onchip_filter_bandwith(0), + _mag_scale{}, _mag_range_ga(0.0f), _mag_range_scale(0.0f), _mag_samplerate(0), @@ -524,6 +536,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _reg7_expected(0), _accel_log_fd(-1), _accel_logging_enabled(false), + _last_extreme_us(0), _last_log_us(0), _last_log_sync_us(0), _last_log_reg_us(0), diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index 6e9c22799f..b4f3974f4d 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -6,3 +6,5 @@ MODULE_COMMAND = lsm303d SRCS = lsm303d.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ From 5baa3690e02067380f35a551e634b67551ed214f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:48:10 +0200 Subject: [PATCH 296/600] L3GD20: Ensure init and constructors --- src/drivers/l3gd20/l3gd20.cpp | 6 ++++++ src/drivers/l3gd20/module.mk | 2 ++ 2 files changed, 8 insertions(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 2273f8aecc..f72db82c09 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -330,12 +330,18 @@ private: * @return 0 on success, 1 on failure */ int self_test(); + + /* this class does not allow copying */ + L3GD20(const L3GD20&); + L3GD20 operator=(const L3GD20&); }; L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), + _call{}, _call_interval(0), _reports(nullptr), + _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk index 83752e08be..5630e7aeca 100644 --- a/src/drivers/l3gd20/module.mk +++ b/src/drivers/l3gd20/module.mk @@ -6,3 +6,5 @@ MODULE_COMMAND = l3gd20 SRCS = l3gd20.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ From e68200b4ba53e6594306e7925aaac3e47f0ef217 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:53:47 +0200 Subject: [PATCH 297/600] HMC driver: Full initialization --- src/drivers/hmc5883/hmc5883.cpp | 5 +++++ src/drivers/hmc5883/module.mk | 2 ++ 2 files changed, 7 insertions(+) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 0cde1045c2..3a2069680e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -337,6 +337,9 @@ private: */ int check_offset(); + /* this class has pointer data members, do not allow copying it */ + HMC5883(const HMC5883&); + HMC5883 operator=(const HMC5883&); }; /* @@ -347,8 +350,10 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000), + _work{}, _measure_ticks(0), _reports(nullptr), + _scale{}, _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _collect_phase(false), diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index f51a2b580d..5daa01dc57 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = hmc5883 SRCS = hmc5883.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ From 23dd7e752d4760a13e90ec7ed6cf003e56c4baff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:54:00 +0200 Subject: [PATCH 298/600] airspeed driver: better init --- src/drivers/airspeed/airspeed.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 0b8e949c9d..d6a88714b6 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -107,6 +107,10 @@ private: RingBuffer *_reports; perf_counter_t _buffer_overflows; + /* this class has pointer data members and should not be copied */ + Airspeed(const Airspeed&); + Airspeed& operator=(const Airspeed&); + protected: virtual int probe(); From da4967e8e4256a31550a10090cbf6bfc12166b61 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:54:17 +0200 Subject: [PATCH 299/600] I2C driver: forbid copy constructor --- src/drivers/device/i2c.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 5498793523..705b398b05 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -138,6 +138,9 @@ private: uint16_t _address; uint32_t _frequency; struct i2c_dev_s *_dev; + + I2C(const device::I2C&); + I2C operator=(const device::I2C&); }; } // namespace device From 7ca184de45a49ca2bde5733a54843975e193a237 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:54:40 +0200 Subject: [PATCH 300/600] MEAS airspeed: Stricter initialization and constructors --- src/drivers/meas_airspeed/meas_airspeed.cpp | 4 ++-- src/drivers/meas_airspeed/module.mk | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index d643fcad61..6ab4377167 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -140,9 +140,9 @@ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, CONVERSION_INTERVAL, path), _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), - _t_system_power(-1) + _t_system_power(-1), + system_power{} { - memset(&system_power, 0, sizeof(system_power)); } int diff --git a/src/drivers/meas_airspeed/module.mk b/src/drivers/meas_airspeed/module.mk index 84192d1b56..2a15b669f4 100644 --- a/src/drivers/meas_airspeed/module.mk +++ b/src/drivers/meas_airspeed/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = meas_airspeed SRCS = meas_airspeed.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ From 164b4ef4ce7e4452e92a3854a51b025f45319e59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:58:18 +0200 Subject: [PATCH 301/600] MPU6K: Cleaner init --- src/drivers/mpu6000/module.mk | 2 ++ src/drivers/mpu6000/mpu6000.cpp | 9 +++++++++ 2 files changed, 11 insertions(+) diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk index 6087e1509e..5b4893b127 100644 --- a/src/drivers/mpu6000/module.mk +++ b/src/drivers/mpu6000/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = mpu6000 SRCS = mpu6000.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 442f0a37cc..db9d4d7e11 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -343,6 +343,9 @@ private: */ void _set_sample_rate(uint16_t desired_sample_rate_hz); + /* do not allow to copy this class due to pointer data members */ + MPU6000(const MPU6000&); + MPU6000 operator=(const MPU6000&); }; /** @@ -369,6 +372,9 @@ private: orb_advert_t _gyro_topic; int _gyro_class_instance; + /* do not allow to copy this class due to pointer data members */ + MPU6000_gyro(const MPU6000_gyro&); + MPU6000_gyro operator=(const MPU6000_gyro&); }; /** driver 'main' command */ @@ -378,13 +384,16 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), + _call{}, _call_interval(0), _accel_reports(nullptr), + _accel_scale{}, _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), _accel_class_instance(-1), _gyro_reports(nullptr), + _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _sample_rate(1000), From 85ccecea9c67b9f0a61fa46ec8dee324114077c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:05:15 +0200 Subject: [PATCH 302/600] FMU driver: stricter init --- src/drivers/px4fmu/fmu.cpp | 5 +++++ src/drivers/px4fmu/module.mk | 2 ++ 2 files changed, 7 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8cc1141aa5..82977a032f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -179,6 +179,9 @@ private: uint32_t gpio_read(void); int gpio_ioctl(file *filp, int cmd, unsigned long arg); + /* do not allow to copy due to ptr data members */ + PX4FMU(const PX4FMU&); + PX4FMU operator=(const PX4FMU&); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -242,6 +245,7 @@ PX4FMU::PX4FMU() : _task(-1), _armed_sub(-1), _outputs_pub(-1), + _armed{}, _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), @@ -252,6 +256,7 @@ PX4FMU::PX4FMU() : _groups_subscribed(0), _control_subs{-1}, _poll_fds_num(0), + _pwm_limit{}, _failsafe_pwm{0}, _disarmed_pwm{0}, _num_failsafe_set(0), diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index eeb59e1a1f..a60f1a434a 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -6,3 +6,5 @@ MODULE_COMMAND = fmu SRCS = fmu.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ From c5b13b7dbcb36f75d88fa24b879c3f202d01da38 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:05:29 +0200 Subject: [PATCH 303/600] IO driver: stricter init --- src/drivers/px4io/module.mk | 2 ++ src/drivers/px4io/px4io.cpp | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index c14f1f7836..5b838fb75e 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -46,3 +46,5 @@ SRCS = px4io.cpp \ INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7d78b0d275..711674886c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -453,6 +453,9 @@ private: */ void io_handle_vservo(uint16_t vservo, uint16_t vrssi); + /* do not allow to copy this class due to ptr data members */ + PX4IO(const PX4IO&); + PX4IO operator=(const PX4IO&); }; namespace @@ -496,6 +499,8 @@ PX4IO::PX4IO(device::Device *interface) : _to_battery(0), _to_servorail(0), _to_safety(0), + _outputs{}, + _servorail_status{}, _primary_pwm_device(false), _lockdown_override(false), _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor From 654aaa0ca852b95e4e2bec5cf9b77ca3242d1d63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:05:50 +0200 Subject: [PATCH 304/600] Mixer: forbid copy constructors due to ptr data members --- src/modules/systemlib/mixer/mixer.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 225570fa40..cdf60febc7 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -231,6 +231,10 @@ protected: static const char * skipline(const char *buf, unsigned &buflen); private: + + /* do not allow to copy due to prt data members */ + Mixer(const Mixer&); + Mixer& operator=(const Mixer&); }; /** @@ -307,6 +311,10 @@ public: private: Mixer *_first; /**< linked list of mixers */ + + /* do not allow to copy due to pointer data members */ + MixerGroup(const MixerGroup&); + MixerGroup operator=(const MixerGroup&); }; /** @@ -424,6 +432,10 @@ private: mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index); + + /* do not allow to copy due to ptr data members */ + SimpleMixer(const SimpleMixer&); + SimpleMixer operator=(const SimpleMixer&); }; /** @@ -522,6 +534,9 @@ private: unsigned _rotor_count; const Rotor *_rotors; + /* do not allow to copy due to ptr data members */ + MultirotorMixer(const MultirotorMixer&); + MultirotorMixer operator=(const MultirotorMixer&); }; #endif From 02f56aae8cf34334580ade6e73bf2d58b12689f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:24:09 +0200 Subject: [PATCH 305/600] Navigator: Enable more strict compile warnings --- src/modules/navigator/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 637eaae593..b501989961 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -54,3 +54,5 @@ SRCS = navigator_main.cpp \ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ From 3ca15ab157b395b00ff225e419ae662551bc6b81 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:24:31 +0200 Subject: [PATCH 306/600] Controllib block: Make copy constructor private --- src/modules/controllib/block/Block.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 736698e213..9bd80b15bd 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -93,6 +93,11 @@ protected: List _subscriptions; List _publications; List _params; + +private: + /* this class has pointer data members and should not be copied (private constructor) */ + Block(const control::Block&); + Block operator=(const control::Block&); }; class __EXPORT SuperBlock : From 63c6e31ba70170029549d60b6ce3b235692ccc80 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:24:50 +0200 Subject: [PATCH 307/600] Fix the most obvious compile warnings --- src/modules/navigator/navigator.h | 6 ++++++ src/modules/navigator/navigator_main.cpp | 16 ++++++++-------- src/modules/navigator/navigator_mode.h | 6 ++++++ 3 files changed, 20 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index bf6e2ea0e6..8edbb63b35 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -222,5 +222,11 @@ private: * Publish a new position setpoint triplet for position controllers */ void publish_position_setpoint_triplet(); + + /* this class has ptr data members, so it should not be copied, + * consequently the copy constructors are private. + */ + Navigator(const Navigator&); + Navigator operator=(const Navigator&); }; #endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1a5ba4c1a4..824ed912d6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -106,16 +106,16 @@ Navigator::Navigator() : _onboard_mission_sub(-1), _offboard_mission_sub(-1), _pos_sp_triplet_pub(-1), - _vstatus({}), - _control_mode({}), - _global_pos({}), - _home_pos({}), - _mission_item({}), - _nav_caps({}), - _pos_sp_triplet({}), + _vstatus{}, + _control_mode{}, + _global_pos{}, + _home_pos{}, + _mission_item{}, + _nav_caps{}, + _pos_sp_triplet{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), - _geofence({}), + _geofence{}, _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index a7ba79bba7..de5545dcb1 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -88,6 +88,12 @@ protected: private: bool _first_run; + + /* this class has ptr data members, so it should not be copied, + * consequently the copy constructors are private. + */ + NavigatorMode(const NavigatorMode&); + NavigatorMode operator=(const NavigatorMode&); }; #endif From d39d5cc9da99ec17251428d57d232c30564a663b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 16 Jul 2014 11:26:32 +0200 Subject: [PATCH 308/600] SYS_EXT_MAG parameter added for magnetometer selection --- ROMFS/px4fmu_common/init.d/rc.sensors | 27 +++++++++++++++++---------- src/modules/systemlib/system_params.c | 11 +++++++++++ 2 files changed, 28 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index be54ea98b0..f50e9aff7f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -6,12 +6,6 @@ ms5611 start adc start -# Mag might be external -if hmc5883 start -then - echo "[init] Using HMC5883" -fi - if mpu6000 start then echo "[init] Using MPU6000" @@ -22,12 +16,25 @@ then echo "[init] Using L3GD20(H)" fi -if ver hwcmp PX4FMU_V2 +# Use selected (internal/external) magnetometer +if param compare SYS_EXT_MAG 0 then - # IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST - if lsm303d start + if hmc5883 -I start then - echo "[init] Using LSM303D" + echo "[init] Using internal HMC5883" + fi + + if ver hwcmp PX4FMU_V2 + then + if lsm303d start + then + echo "[init] Using internal LSM303D" + fi + fi +else + if hmc5883 -X start + then + echo "[init] Using external HMC5883" fi fi diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 702e435ac9..8cb9236200 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -82,3 +82,14 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1); * @group System */ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); + +/** +* Set usage of external magnetometer +* +* Set to 1 to use external magnetometer instead of internal one. +* +* @min 0 +* @max 1 +* @group System +*/ +PARAM_DEFINE_INT32(SYS_EXT_MAG, 0); From 0c5ce3d3a23a32556b38bb0936b315d12290e4ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 13:42:47 +0200 Subject: [PATCH 309/600] Hotfix: Allow the HMC5883 driver to continue to operate in auto mode --- src/drivers/hmc5883/hmc5883.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 4aef43102e..03a7e6f37e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1341,8 +1341,8 @@ namespace hmc5883 #endif const int ERROR = -1; -HMC5883 *g_dev_int; -HMC5883 *g_dev_ext; +HMC5883 *g_dev_int = nullptr; +HMC5883 *g_dev_ext = nullptr; void start(int bus, enum Rotation rotation); void test(int bus); @@ -1378,6 +1378,11 @@ start(int bus, enum Rotation rotation) errx(0, "already started internal"); g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation); if (g_dev_int != nullptr && OK != g_dev_int->init()) { + + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; + if (bus == PX4_I2C_BUS_ONBOARD) { goto fail; } From 698fcb6b8687b5901145ec90462297c398fbf5c3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 16 Jul 2014 14:22:48 +0200 Subject: [PATCH 310/600] commander: only warn about lost link if link was working before --- 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 0c4d48dead..953feec2a7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1441,7 +1441,7 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { + if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { /* handle the case where data link was regained */ if (telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i regained", i); From c6c9c49823a4c19e156f4ce70bde781890ab04f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 14:58:43 +0200 Subject: [PATCH 311/600] Implement the external mag param in a fashion that retains backward compatibility --- ROMFS/px4fmu_common/init.d/rc.sensors | 40 +++++++++++++++++---------- src/modules/sensors/sensor_params.c | 13 +++++++++ src/modules/systemlib/system_params.c | 11 -------- 3 files changed, 38 insertions(+), 26 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index f50e9aff7f..121dc89d3b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -8,33 +8,43 @@ adc start if mpu6000 start then - echo "[init] Using MPU6000" + echo "Internal MPU6000" fi if l3gd20 start then - echo "[init] Using L3GD20(H)" + echo "Internal L3GD20(H)" fi -# Use selected (internal/external) magnetometer -if param compare SYS_EXT_MAG 0 +# MAG selection +if param compare SENS_EXT_MAG 2 then if hmc5883 -I start then - echo "[init] Using internal HMC5883" - fi - - if ver hwcmp PX4FMU_V2 - then - if lsm303d start - then - echo "[init] Using internal LSM303D" - fi + echo "Internal HMC5883" fi else - if hmc5883 -X start + # Use only external as primary + if param compare SENS_EXT_MAG 1 then - echo "[init] Using external HMC5883" + if hmc5883 -X start + then + echo "External HMC5883" + fi + else + # auto-detect the primary, prefer external + if hmc5883 start + then + echo "Default HMC5883" + fi + fi +fi + +if ver hwcmp PX4FMU_V2 +then + if lsm303d start + then + echo "Internal LSM303D" fi fi diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 38b1907612..7ce6ef5ef6 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -292,6 +292,19 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); */ PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); +/** +* Set usage of external magnetometer +* +* * Set to 0 (default) to auto-detect (will try to get the external as primary) +* * Set to 1 to force the external magnetometer as primary +* * Set to 2 to force the internal magnetometer as primary +* +* @min 0 +* @max 2 +* @group Sensor Calibration +*/ +PARAM_DEFINE_INT32(SENS_EXT_MAG, 0); + /** * RC Channel 1 Minimum diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 8cb9236200..702e435ac9 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -82,14 +82,3 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1); * @group System */ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); - -/** -* Set usage of external magnetometer -* -* Set to 1 to use external magnetometer instead of internal one. -* -* @min 0 -* @max 1 -* @group System -*/ -PARAM_DEFINE_INT32(SYS_EXT_MAG, 0); From ede1deaed6303c9ba22663fec950b9a2a66c53e5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 15:02:25 +0200 Subject: [PATCH 312/600] Add support for external sensors in startup --- ROMFS/px4fmu_common/init.d/rc.sensors | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 121dc89d3b..5a755b89e4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -6,11 +6,21 @@ ms5611 start adc start +if mpu6000 -X start +then + echo "External MPU6000" +fi + if mpu6000 start then echo "Internal MPU6000" fi +if l3gd20 -X start +then + echo "External L3GD20(H)" +fi + if l3gd20 start then echo "Internal L3GD20(H)" @@ -42,6 +52,11 @@ fi if ver hwcmp PX4FMU_V2 then + if lsm303d -X start + then + echo "External LSM303D" + fi + if lsm303d start then echo "Internal LSM303D" From 0b81d9883a67a6277d85a603fde4ebbbc001d20f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 15:17:33 +0200 Subject: [PATCH 313/600] Strip excessive binary text to save some flash, start all three sensor sets --- ROMFS/px4fmu_common/init.d/rc.sensors | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 5a755b89e4..ecb408a543 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -8,22 +8,18 @@ adc start if mpu6000 -X start then - echo "External MPU6000" fi if mpu6000 start then - echo "Internal MPU6000" fi if l3gd20 -X start then - echo "External L3GD20(H)" fi if l3gd20 start then - echo "Internal L3GD20(H)" fi # MAG selection @@ -31,7 +27,6 @@ if param compare SENS_EXT_MAG 2 then if hmc5883 -I start then - echo "Internal HMC5883" fi else # Use only external as primary @@ -39,13 +34,11 @@ else then if hmc5883 -X start then - echo "External HMC5883" fi else # auto-detect the primary, prefer external if hmc5883 start then - echo "Default HMC5883" fi fi fi @@ -54,12 +47,10 @@ if ver hwcmp PX4FMU_V2 then if lsm303d -X start then - echo "External LSM303D" fi if lsm303d start then - echo "Internal LSM303D" fi fi @@ -70,11 +61,9 @@ then else if ets_airspeed start then - echo "[init] Using ETS airspeed sensor (bus 3)" else if ets_airspeed start -b 1 then - echo "[init] Using ETS airspeed sensor (bus 1)" fi fi fi From c2565d998359c29a66c9ebbc641ea748186c7455 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 15:17:54 +0200 Subject: [PATCH 314/600] PCA8574: flash efficiency --- src/drivers/pca8574/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk index 825ee9bb7a..c53ed9ab2b 100644 --- a/src/drivers/pca8574/module.mk +++ b/src/drivers/pca8574/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = pca8574 SRCS = pca8574.cpp + +MAXOPTIMIZATION = -Os From ca98070f8b479f5d643d810a077ad09e84d32721 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 15:18:07 +0200 Subject: [PATCH 315/600] RGBLED: flash efficiency --- src/drivers/rgbled/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk index 39b53ec9ed..c287e35f31 100644 --- a/src/drivers/rgbled/module.mk +++ b/src/drivers/rgbled/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = rgbled SRCS = rgbled.cpp + +MAXOPTIMIZATION = -Os From d9e3c9423e7d78a71acb31b0a2b3f9cc8b31a55d Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 16 Jul 2014 23:41:13 -0400 Subject: [PATCH 316/600] Clean up mavlink module Deal with all mavlink module compiler warnings. No critical errors found --- src/modules/mavlink/mavlink_messages.cpp | 144 +++++++++++++++++++++++ 1 file changed, 144 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7c864f1277..6885bebde5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -258,7 +258,16 @@ private: MavlinkOrbSubscription *status_sub; MavlinkOrbSubscription *pos_sp_triplet_sub; + /* do not allow top copying this class */ + MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); + MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); + protected: + explicit MavlinkStreamHeartbeat() : MavlinkStream(), + status_sub(nullptr), + pos_sp_triplet_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -322,7 +331,15 @@ public: private: MavlinkOrbSubscription *status_sub; + /* do not allow top copying this class */ + MavlinkStreamSysStatus(MavlinkStreamSysStatus &); + MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); + protected: + explicit MavlinkStreamSysStatus() : MavlinkStream(), + status_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -384,8 +401,13 @@ private: uint64_t mag_timestamp; uint64_t baro_timestamp; + /* do not allow top copying this class */ + MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); + MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); + protected: explicit MavlinkStreamHighresIMU() : MavlinkStream(), + sensor_sub(nullptr), sensor_time(0), accel_timestamp(0), gyro_timestamp(0), @@ -469,8 +491,14 @@ private: MavlinkOrbSubscription *att_sub; uint64_t att_time; + /* do not allow top copying this class */ + MavlinkStreamAttitude(MavlinkStreamAttitude &); + MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); + + protected: explicit MavlinkStreamAttitude() : MavlinkStream(), + att_sub(nullptr), att_time(0) {} @@ -520,8 +548,13 @@ private: MavlinkOrbSubscription *att_sub; uint64_t att_time; + /* do not allow top copying this class */ + MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); + MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); + protected: explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), + att_sub(nullptr), att_time(0) {} @@ -589,12 +622,21 @@ private: MavlinkOrbSubscription *airspeed_sub; uint64_t airspeed_time; + /* do not allow top copying this class */ + MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); + MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); + protected: explicit MavlinkStreamVFRHUD() : MavlinkStream(), + att_sub(nullptr), att_time(0), + pos_sub(nullptr), pos_time(0), + armed_sub(nullptr), armed_time(0), + act_sub(nullptr), act_time(0), + airspeed_sub(nullptr), airspeed_time(0) {} @@ -665,8 +707,13 @@ private: MavlinkOrbSubscription *gps_sub; uint64_t gps_time; + /* do not allow top copying this class */ + MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); + MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); + protected: explicit MavlinkStreamGPSRawInt() : MavlinkStream(), + gps_sub(nullptr), gps_time(0) {} @@ -726,9 +773,15 @@ private: MavlinkOrbSubscription *home_sub; uint64_t home_time; + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); + MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); + protected: explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), + pos_sub(nullptr), pos_time(0), + home_sub(nullptr), home_time(0) {} @@ -789,8 +842,13 @@ private: MavlinkOrbSubscription *pos_sub; uint64_t pos_time; + /* do not allow top copying this class */ + MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); + MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); + protected: explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), + pos_sub(nullptr), pos_time(0) {} @@ -845,8 +903,13 @@ private: MavlinkOrbSubscription *pos_sub; uint64_t pos_time; + /* do not allow top copying this class */ + MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); + MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); + protected: explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), + pos_sub(nullptr), pos_time(0) {} @@ -899,7 +962,15 @@ public: private: MavlinkOrbSubscription *home_sub; + /* do not allow top copying this class */ + MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); + protected: + explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), + home_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); @@ -962,8 +1033,13 @@ private: MavlinkOrbSubscription *act_sub; uint64_t act_time; + /* do not allow top copying this class */ + MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); + MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); + protected: explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), + act_sub(nullptr), act_time(0) {} @@ -1033,10 +1109,17 @@ private: MavlinkOrbSubscription *act_sub; uint64_t act_time; + /* do not allow top copying this class */ + MavlinkStreamHILControls(MavlinkStreamHILControls &); + MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); + protected: explicit MavlinkStreamHILControls() : MavlinkStream(), + status_sub(nullptr), status_time(0), + pos_sp_triplet_sub(nullptr), pos_sp_triplet_time(0), + act_sub(nullptr), act_time(0) {} @@ -1159,7 +1242,15 @@ public: private: MavlinkOrbSubscription *pos_sp_triplet_sub; + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); + MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); + protected: + explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), + pos_sp_triplet_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); @@ -1208,8 +1299,13 @@ private: MavlinkOrbSubscription *pos_sp_sub; uint64_t pos_sp_time; + /* do not allow top copying this class */ + MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); + MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); + protected: explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), + pos_sp_sub(nullptr), pos_sp_time(0) {} @@ -1261,8 +1357,13 @@ private: MavlinkOrbSubscription *att_sp_sub; uint64_t att_sp_time; + /* do not allow top copying this class */ + MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); + MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); + protected: explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), + att_sp_sub(nullptr), att_sp_time(0) {} @@ -1314,8 +1415,13 @@ private: MavlinkOrbSubscription *att_rates_sp_sub; uint64_t att_rates_sp_time; + /* do not allow top copying this class */ + MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); + MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); + protected: explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), + att_rates_sp_sub(nullptr), att_rates_sp_time(0) {} @@ -1367,8 +1473,13 @@ private: MavlinkOrbSubscription *rc_sub; uint64_t rc_time; + /* do not allow top copying this class */ + MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); + MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); + protected: explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), + rc_sub(nullptr), rc_time(0) {} @@ -1456,8 +1567,13 @@ private: MavlinkOrbSubscription *manual_sub; uint64_t manual_time; + /* do not allow top copying this class */ + MavlinkStreamManualControl(MavlinkStreamManualControl &); + MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); + protected: explicit MavlinkStreamManualControl() : MavlinkStream(), + manual_sub(nullptr), manual_time(0) {} @@ -1510,8 +1626,13 @@ private: MavlinkOrbSubscription *flow_sub; uint64_t flow_time; + /* do not allow top copying this class */ + MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); + MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); + protected: explicit MavlinkStreamOpticalFlow() : MavlinkStream(), + flow_sub(nullptr), flow_time(0) {} @@ -1563,8 +1684,13 @@ private: MavlinkOrbSubscription *att_ctrl_sub; uint64_t att_ctrl_time; + /* do not allow top copying this class */ + MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); + MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); + protected: explicit MavlinkStreamAttitudeControls() : MavlinkStream(), + att_ctrl_sub(nullptr), att_ctrl_time(0) {} @@ -1626,8 +1752,13 @@ private: MavlinkOrbSubscription *debug_sub; uint64_t debug_time; + /* do not allow top copying this class */ + MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); + MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); + protected: explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), + debug_sub(nullptr), debug_time(0) {} @@ -1678,7 +1809,15 @@ public: private: MavlinkOrbSubscription *status_sub; + /* do not allow top copying this class */ + MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); + MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); + protected: + explicit MavlinkStreamCameraCapture() : MavlinkStream(), + status_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -1729,8 +1868,13 @@ private: MavlinkOrbSubscription *range_sub; uint64_t range_time; + /* do not allow top copying this class */ + MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); + MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); + protected: explicit MavlinkStreamDistanceSensor() : MavlinkStream(), + range_sub(nullptr), range_time(0) {} From 878fd1d7134fb53c358c0289ff25c9a6633c73ae Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 17 Jul 2014 09:50:11 +0200 Subject: [PATCH 317/600] navigator: members initialization 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 824ed912d6..93cbfd7b1e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -105,6 +105,7 @@ Navigator::Navigator() : _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), + _param_update_sub(-1), _pos_sp_triplet_pub(-1), _vstatus{}, _control_mode{}, @@ -124,6 +125,8 @@ Navigator::Navigator() : _loiter(this, "LOI"), _rtl(this, "RTL"), _offboard(this, "OFF"), + _can_loiter_at_sp(false), + _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD") { From facffe2b9eeee24f712119be91ff8aea99cfde0f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 17 Jul 2014 17:38:04 +0200 Subject: [PATCH 318/600] mtecs: add altitude prefiltering --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 10 ++++++++-- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 1 + src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 7 +++++++ src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- src/modules/uORB/topics/tecs_status.h | 1 + 6 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 2e37d166e2..749f57a2ba 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -59,6 +59,7 @@ mTecs::mTecs() : _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), _flightPathAngleLowpass(this, "FPA_LP"), + _altitudeLowpass(this, "ALT_LP"), _airspeedLowpass(this, "A_LP"), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), @@ -93,18 +94,23 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* time measurement */ updateTimeMeasurement(); + /* Filter altitude */ + float altitudeFiltered = _altitudeLowpass.update(altitude); + + /* calculate flight path angle setpoint from altitude setpoint */ - float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered); /* Debug output */ if (_counter % 10 == 0) { debug("***"); - debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp); } /* Write part of the status message */ _status.altitudeSp = altitudeSp; _status.altitude = altitude; + _status.altitudeFiltered = altitudeFiltered; /* use flightpath angle setpoint for total energy control */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index efa89a5d3d..ae6867d382 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -115,6 +115,7 @@ protected: /* Other calculation Blocks */ control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ + control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */ control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 5b92387807..4ca31fe201 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); */ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); +/** + * Lowpass (cutoff freq.) for altitude + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f); + /** * Lowpass (cutoff freq.) for the flight path angle * diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f534c0f4ce..e0a3e7603b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1595,6 +1595,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_TECS_MSG; log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; + log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index b14ef04ccf..853a3811f6 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -334,6 +334,7 @@ struct log_GS1B_s { struct log_TECS_s { float altitudeSp; float altitude; + float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; @@ -454,7 +455,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index c4d0c18749..33055018ce 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -66,6 +66,7 @@ struct tecs_status_s { float altitudeSp; float altitude; + float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; From c01567c047f295e3ffea7d01a7e239ac4f79a498 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Jul 2014 21:02:17 +0200 Subject: [PATCH 319/600] Check if waypoint altitude is relative. Fixes #1197 --- src/modules/navigator/mission_feasibility_checker.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index dd7f4c8013..d45488d5a3 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } - if (home_alt > missionitem.altitude) { + /* calculate the global waypoint altitude */ + float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt ? missionitem.altitude; + + if (home_alt > wp_alt) { if (throw_error) { - mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); return false; } else { - mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return true; } } From 101e7b1383b83c9b06ef147dbef1ff375c11b84f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Jul 2014 21:06:42 +0200 Subject: [PATCH 320/600] navigator: Feedback strings / text and logic was not consistent in previous state, fixed. --- src/modules/navigator/mission_feasibility_checker.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index d45488d5a3..606521f200 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -136,14 +136,14 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } /* calculate the global waypoint altitude */ - float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt ? missionitem.altitude; + float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; if (home_alt > wp_alt) { if (throw_error) { - mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return false; } else { - mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); return true; } } From 9559668f0fb7477e2f777beff899589a387d26c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Jul 2014 23:25:12 +0200 Subject: [PATCH 321/600] uORB: Add navigation state --- src/modules/uORB/topics/position_setpoint_triplet.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 673c0e4919..4a19321809 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -95,6 +95,8 @@ struct position_setpoint_triplet_s struct position_setpoint_s previous; struct position_setpoint_s current; struct position_setpoint_s next; + + unsigned nav_state; /**< report the navigation state */ }; /** From b8da27561613a0890fd21e43064b457a58daaab3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Jul 2014 23:25:33 +0200 Subject: [PATCH 322/600] Log nav mode --- src/modules/sdlog2/sdlog2.c | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f534c0f4ce..e078b47d63 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1432,17 +1432,20 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { - log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */ - log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); - log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); - log_msg.body.log_GPSP.alt = buf.triplet.current.alt; - log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.type = buf.triplet.current.type; - log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; - log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; - LOGBUFFER_WRITE_AND_COUNT(GPSP); + + if (buf.triplet.current.valid) { + log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); + log_msg.body.log_GPSP.alt = buf.triplet.current.alt; + log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; + log_msg.body.log_GPSP.type = buf.triplet.current.type; + log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; + log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; + LOGBUFFER_WRITE_AND_COUNT(GPSP); + } } /* --- VICON POSITION --- */ From ba7df4c170e069812387e08445cfa0e7789bf34d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Jul 2014 23:26:04 +0200 Subject: [PATCH 323/600] Enable nav mode logging in navigator --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 93cbfd7b1e..331a9a728f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -458,7 +458,7 @@ void Navigator::publish_position_setpoint_triplet() { /* update navigation state */ - /* TODO: set nav_state */ + _pos_sp_triplet.nav_state = _vstatus.nav_state; /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { From dc612d75c788bc8b2a90c200a13a5a5a0e3a68d9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 18 Jul 2014 00:15:16 +0200 Subject: [PATCH 324/600] BlockDerivative: initialize in first run --- src/modules/controllib/blocks.cpp | 7 ++++++- src/modules/controllib/blocks.hpp | 2 ++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index c6c374300e..04cb023a8d 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -293,7 +293,12 @@ int blockIntegralTrapTest() float BlockDerivative::update(float input) { - float output = _lowPass.update((input - getU()) / getDt()); + float output = 0.0f; + if (_initialized) { + output = _lowPass.update((input - getU()) / getDt()); + } else { + _initialized = true; + } setU(input); return output; } diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 66e9290381..b0545b60a5 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -238,6 +238,7 @@ public: BlockDerivative(SuperBlock *parent, const char *name) : SuperBlock(parent, name), _u(0), + _initialized(false), _lowPass(this, "LP") {}; virtual ~BlockDerivative() {}; @@ -249,6 +250,7 @@ public: protected: // attributes float _u; /**< previous input */ + bool _initialized; BlockLowPass _lowPass; /**< low pass filter */ }; From 6c50e510a5fcadab19a6d5ff709c0da473e4ace0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 07:28:49 +0200 Subject: [PATCH 325/600] Derivative fix: Comments and code style --- src/modules/controllib/blocks.cpp | 8 +++++++- src/modules/controllib/blocks.hpp | 15 +++++++++++++++ 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 04cb023a8d..0175acda99 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -293,10 +293,16 @@ int blockIntegralTrapTest() float BlockDerivative::update(float input) { - float output = 0.0f; + float output; if (_initialized) { output = _lowPass.update((input - getU()) / getDt()); } else { + // if this is the first call to update + // we have no valid derivative + // and so we use the assumption the + // input value is not changing much, + // which is the best we can do here. + output = 0.0f; _initialized = true; } setU(input); diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index b0545b60a5..37d7832b33 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -242,6 +242,21 @@ public: _lowPass(this, "LP") {}; virtual ~BlockDerivative() {}; + + /** + * Update the state and get current derivative + * + * This call updates the state and gets the current + * derivative. As the derivative is only valid + * on the second call to update, it will return + * no change (0) on the first. To get a closer + * estimate of the derivative on the first call, + * call setU() one time step before using the + * return value of update(). + * + * @param input the variable to calculate the derivative of + * @return the current derivative + */ float update(float input); // accessors void setU(float u) {_u = u;} From ce78b399691d43bd150c0a5928f08c98076a3892 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 07:49:29 +0200 Subject: [PATCH 326/600] Remove all unused TECS parameters --- .../fw_pos_control_l1_main.cpp | 54 ------ .../fw_pos_control_l1_params.c | 169 ------------------ 2 files changed, 223 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 6c251c2375..1fcab20697 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -199,19 +199,6 @@ private: float l1_period; float l1_damping; - float time_const; - float min_sink_rate; - float max_sink_rate; - float max_climb_rate; - float throttle_damp; - float integrator_gain; - float vertical_accel_limit; - float height_comp_filter_omega; - float speed_comp_filter_omega; - float roll_throttle_compensation; - float speed_weight; - float pitch_damping; - float airspeed_min; float airspeed_trim; float airspeed_max; @@ -242,19 +229,6 @@ private: param_t l1_period; param_t l1_damping; - param_t time_const; - param_t min_sink_rate; - param_t max_sink_rate; - param_t max_climb_rate; - param_t throttle_damp; - param_t integrator_gain; - param_t vertical_accel_limit; - param_t height_comp_filter_omega; - param_t speed_comp_filter_omega; - param_t roll_throttle_compensation; - param_t speed_weight; - param_t pitch_damping; - param_t airspeed_min; param_t airspeed_trim; param_t airspeed_max; @@ -470,21 +444,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); - _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); - _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); - _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); - _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); - _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); - _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); - _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); - _parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA"); - _parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA"); - _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); - _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); - _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); - _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); - _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); - /* fetch initial parameter values */ parameters_update(); } @@ -535,19 +494,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); - param_get(_parameter_handles.time_const, &(_parameters.time_const)); - param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); - param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); - param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); - param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain)); - param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit)); - param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega)); - param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega)); - param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation)); - param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); - param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); - param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); - param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 52128e1b7a..35b1f2ca96 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -154,175 +154,6 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); -/** - * Maximum climb rate - * - * This is the best climb rate that the aircraft can achieve with - * the throttle set to THR_MAX and the airspeed set to the - * default value. For electric aircraft make sure this number can be - * achieved towards the end of flight when the battery voltage has reduced. - * The setting of this parameter can be checked by commanding a positive - * altitude change of 100m in loiter, RTL or guided mode. If the throttle - * required to climb is close to THR_MAX and the aircraft is maintaining - * airspeed, then this parameter is set correctly. If the airspeed starts - * to reduce, then the parameter is set to high, and if the throttle - * demand required to climb and maintain speed is noticeably less than - * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or - * FW_THR_MAX reduced. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); - -/** - * Minimum descent rate - * - * This is the sink rate of the aircraft with the throttle - * set to THR_MIN and flown at the same airspeed as used - * to measure FW_T_CLMB_MAX. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); - -/** - * Maximum descent rate - * - * This sets the maximum descent rate that the controller will use. - * If this value is too large, the aircraft can over-speed on descent. - * This should be set to a value that can be achieved without - * exceeding the lower pitch angle limit and without over-speeding - * the aircraft. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); - -/** - * TECS time constant - * - * This is the time constant of the TECS control algorithm (in seconds). - * Smaller values make it faster to respond, larger values make it slower - * to respond. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); - -/** - * Throttle damping factor - * - * This is the damping gain for the throttle demand loop. - * Increase to add damping to correct for oscillations in speed and height. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); - -/** - * Integrator gain - * - * This is the integrator gain on the control loop. - * Increasing this gain increases the speed at which speed - * and height offsets are trimmed out, but reduces damping and - * increases overshoot. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); - -/** - * Maximum vertical acceleration - * - * This is the maximum vertical acceleration (in metres/second square) - * either up or down that the controller will use to correct speed - * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) - * allows for reasonably aggressive pitch changes if required to recover - * from under-speed conditions. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); - -/** - * Complementary filter "omega" parameter for height - * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse vertical acceleration and barometric height to obtain - * an estimate of height rate and height. Increasing this frequency weights - * the solution more towards use of the barometer, whilst reducing it weights - * the solution more towards use of the accelerometer data. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); - -/** - * Complementary filter "omega" parameter for speed - * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse longitudinal acceleration and airspeed to obtain an - * improved airspeed estimate. Increasing this frequency weights the solution - * more towards use of the arispeed sensor, whilst reducing it weights the - * solution more towards use of the accelerometer data. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); - -/** - * Roll -> Throttle feedforward - * - * Increasing this gain turn increases the amount of throttle that will - * be used to compensate for the additional drag created by turning. - * Ideally this should be set to approximately 10 x the extra sink rate - * in m/s created by a 45 degree bank turn. Increase this gain if - * the aircraft initially loses energy in turns and reduce if the - * aircraft initially gains energy in turns. Efficient high aspect-ratio - * aircraft (eg powered sailplanes) can use a lower value, whereas - * inefficient low aspect-ratio models (eg delta wings) can use a higher value. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); - -/** - * Speed <--> Altitude priority - * - * This parameter adjusts the amount of weighting that the pitch control - * applies to speed vs height errors. Setting it to 0.0 will cause the - * pitch control to control height and ignore speed errors. This will - * normally improve height accuracy but give larger airspeed errors. - * Setting it to 2.0 will cause the pitch control loop to control speed - * and ignore height errors. This will normally reduce airspeed errors, - * but give larger height errors. The default value of 1.0 allows the pitch - * control to simultaneously control height and speed. - * Note to Glider Pilots - set this parameter to 2.0 (The glider will - * adjust its pitch angle to maintain airspeed, ignoring changes in height). - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); - -/** - * Pitch damping factor - * - * This is the damping gain for the pitch demand loop. Increase to add - * damping to correct for oscillations in height. The default value of 0.0 - * will work well provided the pitch to servo controller has been tuned - * properly. - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); - -/** - * Height rate P factor - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); - /** * Speed rate P factor * From 4a8e4d37cd926bed7b85ba92a5214f604a4ea192 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 07:55:02 +0200 Subject: [PATCH 327/600] Remove one left-over TECS param --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 35b1f2ca96..27039ff51f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -154,13 +154,6 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); -/** - * Speed rate P factor - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); - /** * Landing slope angle * From f7c330fa6073eaff7d1d7073b16a51e40f8b5f9d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 07:58:59 +0200 Subject: [PATCH 328/600] Remove all handles to TECS params in startup scripts --- ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil | 9 --------- ROMFS/px4fmu_common/init.d/3031_phantom | 4 ---- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 4 ---- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 4 ---- 4 files changed, 21 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index c753ded233..941f5664ad 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -26,15 +26,6 @@ then param set FW_RR_P 0.1 param set FW_R_LIM 45 param set FW_R_RMAX 0 - param set FW_T_CLMB_MAX 5 - param set FW_T_HRATE_P 0.02 - param set FW_T_PTCH_DAMP 0 - param set FW_T_RLL2THR 15 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 3 - param set FW_T_VERT_ACC 7 param set FW_YR_FF 0.0 param set FW_YR_I 0 param set FW_YR_IMAX 0.2 diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 654b0bdab1..31dfe71007 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -30,10 +30,6 @@ then param set FW_RR_P 0.08 param set FW_R_LIM 50 param set FW_R_RMAX 0 - param set FW_T_HRATE_P 0.01 - param set FW_T_RLL2THR 15 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 5 fi set MIXER phantom diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 465166f253..7d0dc5bff3 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -30,10 +30,6 @@ then param set FW_RR_P 0.03 param set FW_R_LIM 60 param set FW_R_RMAX 0 - param set FW_T_HRATE_P 0.01 - param set FW_T_RLL2THR 15 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 5 fi set MIXER FMU_X5 diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 7dbda54d3a..9a21504035 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -33,10 +33,6 @@ then param set FW_RR_P 0.03 param set FW_R_LIM 60 param set FW_R_RMAX 0 - param set FW_T_HRATE_P 0.01 - param set FW_T_RLL2THR 15 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 5 fi set MIXER FMU_Q From 90a5ae1afd25e5e31d269f0d0f5e5052f068d0b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 08:04:13 +0200 Subject: [PATCH 329/600] Remove MT_ENABLED param and handles --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 - src/modules/fw_pos_control_l1/mtecs/mTecs.h | 2 -- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 11 ----------- 3 files changed, 14 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 749f57a2ba..8f0e57e208 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -49,7 +49,6 @@ namespace fwPosctrl { mTecs::mTecs() : SuperBlock(NULL, "MT"), /* Parameters */ - _mTecsEnabled(this, "ENABLED"), _airspeedMin(this, "FW_AIRSPD_MIN", false), /* Publications */ _status(&getPublications(), ORB_ID(tecs_status)), diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index ae6867d382..99830fba6a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -90,14 +90,12 @@ public: void resetDerivatives(float airspeed); /* Accessors */ - bool getEnabled() { return _mTecsEnabled.get() > 0; } float getThrottleSetpoint() { return _throttleSp; } float getPitchSetpoint() { return _pitchSp; } float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); } protected: /* parameters */ - control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ control::BlockParamFloat _airspeedMin; /**< minimal airspeed */ /* Publications */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 4ca31fe201..6e9e1d88e0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -46,17 +46,6 @@ * Controller parameters, accessible via MAVLink */ -/** - * mTECS enabled - * - * Set to 1 to enable mTECS - * - * @min 0 - * @max 1 - * @group mTECS - */ -PARAM_DEFINE_INT32(MT_ENABLED, 1); - /** * Total Energy Rate Control Feedforward * Maps the total energy rate setpoint to the throttle setpoint From 81f60329e47f8b31d9261c0ef46c09780f9d8194 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 08:07:35 +0200 Subject: [PATCH 330/600] Remove last reference to mtecs enabled param --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 1fcab20697..5894aeb0b7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -756,10 +756,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_pos_control_mode) { /* reset integrators */ - if (_mTecs.getEnabled()) { - _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); - } + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); } _was_pos_control_mode = true; From 9a53fd96482bd31da98af97de5cde88127d6c7f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 11:18:32 +0200 Subject: [PATCH 331/600] Add force failsafe flag --- src/modules/px4iofirmware/mixer.cpp | 9 ++++++++- src/modules/px4iofirmware/protocol.h | 5 +++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 2f721bf1e1..606c639f94 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -111,7 +111,7 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; } - /* default to failsafe mixing */ + /* default to failsafe mixing - it will be forced below if flag is set */ source = MIX_FAILSAFE; /* @@ -154,6 +154,13 @@ mixer_tick(void) } } + /* + * Check if we should force failsafe - and do it if we have to + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) { + source = MIX_FAILSAFE; + } + /* * Set failsafe status flag depending on mixing source */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index d5a6b1ec4a..0507836871 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -179,6 +179,7 @@ #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ +#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ @@ -253,6 +254,10 @@ enum { /* DSM bind states */ /* PWM failsafe values - zero disables the output */ #define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */ +#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */ + /* Debug and test page - not used in normal operation */ #define PX4IO_PAGE_TEST 127 #define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ From 903b482378516f26ef0faa4d658597e0af2fb35d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 11:19:05 +0200 Subject: [PATCH 332/600] PX4IO driver: Add support to set force failsafe --- src/drivers/drv_pwm_output.h | 3 +++ src/drivers/px4io/px4io.cpp | 11 +++++++++++ 2 files changed, 14 insertions(+) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 972573f9ff..84815fdfbb 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -202,6 +202,9 @@ ORB_DECLARE(output_pwm); /** force safety switch off (to disable use of safety switch) */ #define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) +/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */ +#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) + /* * * diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 711674886c..f5ff6e55e5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2222,6 +2222,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); break; + case PWM_SERVO_SET_FORCE_FAILSAFE: + /* force failsafe mode instantly */ + if (arg == 0) { + /* clear force failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, 0); + } else { + /* set force failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); + } + break; + case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ From 5c8c58a1e6a2977b4a2f6d81928d4bfc64c87649 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 11:19:37 +0200 Subject: [PATCH 333/600] pwm system command: Allow to force failsave (forcefail command) --- src/systemcmds/pwm/pwm.c | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index e0e6ca5376..77d8ef8dbd 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -635,7 +635,23 @@ pwm_main(int argc, char *argv[]) } exit(0); + } else if (!strcmp(argv[1], "forcefail")) { + + if (argc < 3) { + errx(1, "arg missing [on|off]"); + } else { + + if (!strcmp(argv[2], "on")) { + /* force failsafe */ + ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 1); + } else { + /* force failsafe */ + ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 0); + } + } + } + usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info"); return 0; } From a2da34be16531404b4c4507ab71b0518a51d60c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 20:53:01 +0200 Subject: [PATCH 334/600] Fixes #1207 --- src/modules/mavlink/mavlink_main.cpp | 31 +++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 76b5459a3e..e235ff0f9c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -525,22 +525,39 @@ void Mavlink::mavlink_update_system(void) _param_component_id = param_find("MAV_COMP_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); - _param_initialized = true; } /* update system and component id */ int32_t system_id; param_get(_param_system_id, &system_id); - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - int32_t component_id; param_get(_param_component_id, &component_id); - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; + + /* only allow system ID and component ID updates + * after reboot - not during operation */ + if (!_param_initialized) { + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + _param_initialized = true; + } + + /* warn users that they need to reboot to take this + * into effect + */ + if (system_id != mavlink_system.sysid) { + send_statustext_critical("Save params and reboot to change SYSID"); + } + + if (component_id != mavlink_system.compid) { + send_statustext_critical("Save params and reboot to change COMPID"); } int32_t system_type; From 528253e33a49702a9c394a5b25d823e4db0a4e7d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 18 Jul 2014 23:24:07 +0400 Subject: [PATCH 335/600] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index af065e9ca9..767462c992 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit af065e9ca9e3ba9100c125d3ab739313d0500ca8 +Subproject commit 767462c9924d7afbb789dc3e4c76d31261c557d3 From a4e1a665833f566e2faaa80d221971c679b57d17 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 10:52:50 +0200 Subject: [PATCH 336/600] Revert "Remove last reference to mtecs enabled param" This reverts commit 81f60329e47f8b31d9261c0ef46c09780f9d8194. --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5894aeb0b7..1fcab20697 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -756,8 +756,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_pos_control_mode) { /* reset integrators */ - _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } } _was_pos_control_mode = true; From 57794925fe0fcaf5736163f8ad43a4551a1a9b71 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 10:53:04 +0200 Subject: [PATCH 337/600] Revert "Remove MT_ENABLED param and handles" This reverts commit 90a5ae1afd25e5e31d269f0d0f5e5052f068d0b1. --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 + src/modules/fw_pos_control_l1/mtecs/mTecs.h | 2 ++ src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 11 +++++++++++ 3 files changed, 14 insertions(+) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 8f0e57e208..749f57a2ba 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -49,6 +49,7 @@ namespace fwPosctrl { mTecs::mTecs() : SuperBlock(NULL, "MT"), /* Parameters */ + _mTecsEnabled(this, "ENABLED"), _airspeedMin(this, "FW_AIRSPD_MIN", false), /* Publications */ _status(&getPublications(), ORB_ID(tecs_status)), diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 99830fba6a..ae6867d382 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -90,12 +90,14 @@ public: void resetDerivatives(float airspeed); /* Accessors */ + bool getEnabled() { return _mTecsEnabled.get() > 0; } float getThrottleSetpoint() { return _throttleSp; } float getPitchSetpoint() { return _pitchSp; } float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); } protected: /* parameters */ + control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ control::BlockParamFloat _airspeedMin; /**< minimal airspeed */ /* Publications */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 6e9e1d88e0..4ca31fe201 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -46,6 +46,17 @@ * Controller parameters, accessible via MAVLink */ +/** + * mTECS enabled + * + * Set to 1 to enable mTECS + * + * @min 0 + * @max 1 + * @group mTECS + */ +PARAM_DEFINE_INT32(MT_ENABLED, 1); + /** * Total Energy Rate Control Feedforward * Maps the total energy rate setpoint to the throttle setpoint From 322089a390f416a9936fb4ad1ad0ed4359aeaa57 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 11:29:49 +0200 Subject: [PATCH 338/600] Fix unused params --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 1fcab20697..0b111f7bd3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -212,9 +212,6 @@ private: float throttle_land_max; - float heightrate_p; - float speedrate_p; - float land_slope_angle; float land_H1_virt; float land_flare_alt_relative; @@ -242,9 +239,6 @@ private: param_t throttle_land_max; - param_t heightrate_p; - param_t speedrate_p; - param_t land_slope_angle; param_t land_H1_virt; param_t land_flare_alt_relative; @@ -494,9 +488,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); - param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); - param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); - param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); From 689536893caea0cde8cafb3a51cd7e471338cfb0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 12:46:52 +0200 Subject: [PATCH 339/600] px4io driver: force failsafe depending on actuator armed --- src/drivers/px4io/px4io.cpp | 20 +++++++++++++------- src/modules/uORB/topics/actuator_armed.h | 3 ++- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f5ff6e55e5..5ada635f32 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1149,6 +1149,12 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; } + if (armed.force_failsafe) { + set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } else { + clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -2435,7 +2441,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; case PX4IO_CHECK_CRC: { - /* check IO firmware CRC against passed value */ + /* check IO firmware CRC against passed value */ uint32_t io_crc = 0; ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); if (ret != OK) @@ -2695,7 +2701,7 @@ checkcrc(int argc, char *argv[]) int fd = open(argv[1], O_RDONLY); if (fd == -1) { printf("open of %s failed - %d\n", argv[1], errno); - exit(1); + exit(1); } const uint32_t app_size_max = 0xf000; uint32_t fw_crc = 0; @@ -2710,7 +2716,7 @@ checkcrc(int argc, char *argv[]) close(fd); while (nbytes < app_size_max) { uint8_t b = 0xff; - fw_crc = crc32part(&b, 1, fw_crc); + fw_crc = crc32part(&b, 1, fw_crc); nbytes++; } @@ -2723,7 +2729,7 @@ checkcrc(int argc, char *argv[]) if (ret != OK) { printf("check CRC failed - %d\n", ret); - exit(1); + exit(1); } printf("CRCs match\n"); exit(0); @@ -2753,12 +2759,12 @@ bind(int argc, char *argv[]) pulses = DSMX_BIND_PULSES; else if (!strcmp(argv[2], "dsmx8")) pulses = DSMX8_BIND_PULSES; - else + else errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]); // Test for custom pulse parameter if (argc > 3) pulses = atoi(argv[3]); - if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) errx(1, "system must not be armed"); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 @@ -2960,7 +2966,7 @@ lockdown(int argc, char *argv[]) (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); warnx("ACTUATORS ARE NOW SAFE IN HIL."); } - + } else { errx(1, "driver not loaded, exiting"); } diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index a98d3fc3a4..0f6c9aca17 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -56,6 +56,7 @@ struct actuator_armed_s { bool armed; /**< Set to true if system is armed */ bool ready_to_arm; /**< Set to true if system is ready to be armed */ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + bool force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */ }; /** @@ -65,4 +66,4 @@ struct actuator_armed_s { /* register this as object request broker structure */ ORB_DECLARE(actuator_armed); -#endif \ No newline at end of file +#endif From 7b3654a5764bdc2ff0e2a894d193dc59976ff828 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 13:08:54 +0200 Subject: [PATCH 340/600] checkout latest mavlink master --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index d1ebe85eb6..27c9f70f21 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit d1ebe85eb6bb06d0078f3e0b8144adb131263628 +Subproject commit 27c9f70f21121e352215cb441f9fb97f56333e23 From 78b8e4d59f606eb56a475b69d61da8a83613d73d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 13:23:35 +0200 Subject: [PATCH 341/600] really checkout latest mavlink master --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 27c9f70f21..04b1ad5b28 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 27c9f70f21121e352215cb441f9fb97f56333e23 +Subproject commit 04b1ad5b284d5e916858ca9f928e93d97bbf6ad9 From 3a0fc36c67ef89faa0b20e847eb16de6362ef3e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:12:33 +0200 Subject: [PATCH 342/600] Consider the throttle load for battery voltage calculation --- src/modules/commander/commander.cpp | 11 ++++++++++- src/modules/commander/commander_helper.cpp | 10 +++++++--- src/modules/commander/commander_helper.h | 3 ++- src/modules/commander/commander_params.c | 4 ++-- 4 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 953feec2a7..7332c6eaa7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -918,6 +918,11 @@ int commander_thread_main(int argc, char *argv[]) struct system_power_s system_power; memset(&system_power, 0, sizeof(system_power)); + /* Subscribe to actuator controls (outputs) */ + int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -1199,13 +1204,17 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; - status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah); + + /* get throttle (if armed), as we only care about energy negative throttle also counts */ + float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f; + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle); } } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index d5fe122cb6..a4aafa1f63 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -281,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) } } -float battery_remaining_estimate_voltage(float voltage, float discharged) +float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) { float ret = 0; static param_t bat_v_empty_h; static param_t bat_v_full_h; static param_t bat_n_cells_h; static param_t bat_capacity_h; + static param_t bat_v_load_drop_h; static float bat_v_empty = 3.2f; static float bat_v_full = 4.0f; + static float bat_v_load_drop = 0.1f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static bool initialized = false; @@ -300,20 +302,22 @@ float battery_remaining_estimate_voltage(float voltage, float discharged) bat_v_full_h = param_find("BAT_V_FULL"); bat_n_cells_h = param_find("BAT_N_CELLS"); bat_capacity_h = param_find("BAT_CAPACITY"); + bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); initialized = true; } if (counter % 100 == 0) { param_get(bat_v_empty_h, &bat_v_empty); param_get(bat_v_full_h, &bat_v_full); + param_get(bat_v_load_drop_h, &bat_v_load_drop); param_get(bat_n_cells_h, &bat_n_cells); param_get(bat_capacity_h, &bat_capacity); } counter++; - /* remaining charge estimate based on voltage */ - float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty)); + /* remaining charge estimate based on voltage and internal resistance (drop under load) */ + float remaining_voltage = (voltage - bat_n_cells * (bat_v_empty - (bat_v_load_drop * throttle_normalized)) / (bat_n_cells * (bat_v_full - bat_v_empty)); if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index a49c9e263d..4a77fe4871 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -80,8 +80,9 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern); * * @param voltage the current battery voltage * @param discharged the discharged capacity + * @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy. * @return the estimated remaining capacity in 0..1 */ -float battery_remaining_estimate_voltage(float voltage, float discharged); +float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized); #endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 4750f9d5cb..448bc53cd4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.3f); /** * Full cell voltage. @@ -65,7 +65,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.15f); /** * Number of cells. From 3db78a4ab31b79f092940f4186549132a40ed855 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:34:19 +0200 Subject: [PATCH 343/600] commander: Warn at 18 and 9% battery remaining instead of 25 and 10% to not trigger the warning too early (at 50%) as it was before --- 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 7332c6eaa7..cb447f8ce0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1276,13 +1276,13 @@ int commander_thread_main(int argc, char *argv[]) } /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.9f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); From 5fc3bc787a495d15099e177b99c2c4d6a1a4f56c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:38:39 +0200 Subject: [PATCH 344/600] code style only: Fix indendation in mavlink.h --- src/modules/mavlink/mavlink_main.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index da989d6c51..adca0e88f2 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -250,13 +250,13 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; - MavlinkMissionManager *_mission_manager; + MavlinkMissionManager *_mission_manager; - orb_advert_t _mission_pub; + orb_advert_t _mission_pub; int _mission_result_sub; - MAVLINK_MODE _mode; + MAVLINK_MODE _mode; - mavlink_channel_t _channel; + mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; From de0dd71061b1b388e87c25a3d4fd0080f0c9cf8b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 14:39:19 +0200 Subject: [PATCH 345/600] pwm: add missing exit --- src/systemcmds/pwm/pwm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 77d8ef8dbd..403567cb4c 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -70,7 +70,7 @@ usage(const char *reason) { if (reason != NULL) warnx("%s", reason); - errx(1, + errx(1, "usage:\n" "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" "\n" @@ -649,7 +649,7 @@ pwm_main(int argc, char *argv[]) ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 0); } } - + exit(0); } usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info"); From 0b743a9f5c29cee3b3d7c6d602091453e1091973 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 14:39:41 +0200 Subject: [PATCH 346/600] parse flighttermination command --- src/modules/commander/commander.cpp | 21 ++++++++------------- src/modules/uORB/topics/vehicle_command.h | 1 + 2 files changed, 9 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 953feec2a7..04450a44fb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -546,24 +546,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; -#if 0 /* Flight termination */ - case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command - - //XXX: to enable the parachute, a param needs to be set - //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) { - transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); - cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; - + case VEHICLE_CMD_DO_FLIGHTTERMINATION: { + if (cmd->param1 > 0.5f) { + //XXX update state machine? + armed_local->force_failsafe = true; + warnx("forcing failsafe"); } else { - /* reject parachute depoyment not armed */ - cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + armed_local->force_failsafe = false; + warnx("disabling failsafe"); } - + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } break; -#endif case VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index c21a29b130..7db33d98b3 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -76,6 +76,7 @@ enum VEHICLE_CMD { VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ From 730a520362caf9c9d3e506a31441d9921e008144 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:59:13 +0200 Subject: [PATCH 347/600] Print mavlink radio module rates --- src/modules/mavlink/mavlink_main.cpp | 51 ++++++++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 10 +++++ src/modules/mavlink/mavlink_receiver.cpp | 3 +- 3 files changed, 59 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 76b5459a3e..e25851c7d5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -247,6 +247,7 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _rstatus{}, _message_buffer{}, _message_buffer_mutex{}, _param_initialized(false), @@ -424,6 +425,29 @@ Mavlink::destroy_all_instances() return OK; } +int +Mavlink::get_status_all_instances() +{ + Mavlink *inst = ::_mavlink_instances; + + unsigned iterations = 0; + + warnx("waiting for instances to stop"); + + while (inst != nullptr) { + + printf("instance #%u:\n", iterations); + inst->display_status(); + + /* move on */ + inst = inst->next; + iterations++; + } + + /* return an error if there are no instances */ + return (iterations == 0); +} + bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { @@ -1395,7 +1419,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); configure_stream("ATTITUDE", 10.0f * rate_mult); - configure_stream("VFR_HUD", 10.0f * rate_mult); + configure_stream("VFR_HUD", 8.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); @@ -1665,6 +1689,27 @@ void Mavlink::display_status() { warnx("running"); + + if (_rstatus.timestamp > 0) { + printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + printf("\t3DR RADIO\n"); + break; + default: + printf("\tUNKNOWN RADIO\n"); + break; + } + + printf("\trssi:\t%d\t\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); + printf("\tnoise:\t%d\tus\n", _rstatus.noise); + printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); + printf("\tfixed:\t%u\tus\n", _rstatus.fixed); + } } int @@ -1752,8 +1797,8 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); - // } else if (!strcmp(argv[1], "status")) { - // mavlink::g_mavlink->status(); + } else if (!strcmp(argv[1], "status")) { + return Mavlink::get_status_all_instances(); } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream_command(argc, argv); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index adca0e88f2..d51120462a 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -51,6 +51,7 @@ #include #include #include +#include #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" @@ -97,6 +98,8 @@ public: static int destroy_all_instances(); + static int get_status_all_instances(); + static bool instance_exists(const char *device_name, Mavlink *self); static void forward_message(const mavlink_message_t *msg, Mavlink *self); @@ -229,6 +232,11 @@ public: */ void count_txerr(); + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + protected: Mavlink *next; @@ -285,6 +293,8 @@ private: bool _flow_control_enabled; + struct telemetry_status_s _rstatus; ///< receive status + struct mavlink_message_buffer { int write_ptr; int read_ptr; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ee7ec34da..528c8b72a3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -400,8 +400,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); tstatus.timestamp = hrt_absolute_time(); tstatus.heartbeat_time = _telemetry_heartbeat_time; From 87cc7a81ff8b3bdc9cc5a5b4fca91f8d08988774 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:08:56 +0200 Subject: [PATCH 348/600] Support force fail in valid filter --- src/modules/px4iofirmware/registers.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index b372599977..fcd53f1f1b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -189,7 +189,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ - PX4IO_P_SETUP_ARMING_LOCKDOWN) + PX4IO_P_SETUP_ARMING_LOCKDOWN | \ + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) From f3fe9c2fdf7a5d54cbd647551ab03211e3e64458 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:09:07 +0200 Subject: [PATCH 349/600] Print force fail status --- src/drivers/px4io/px4io.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f5ff6e55e5..c6ee08fbe2 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2002,7 +2002,7 @@ PX4IO::print_status(bool extended_status) ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), @@ -2010,7 +2010,9 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), - ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : "")); + ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "") + ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), From f3ec1cd580a7a06699caba5d99b785f56316d1c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:10:20 +0200 Subject: [PATCH 350/600] pwm command: Add missing exit 0 status --- src/systemcmds/pwm/pwm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 77d8ef8dbd..742f45484e 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -649,7 +649,7 @@ pwm_main(int argc, char *argv[]) ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 0); } } - + exit(0); } usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info"); From fd5065535435f9f3fa3c32b620db471f905072c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:15:10 +0200 Subject: [PATCH 351/600] add missing hint to pwm usage --- src/systemcmds/pwm/pwm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 403567cb4c..87c7fc6479 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -652,7 +652,7 @@ pwm_main(int argc, char *argv[]) exit(0); } - usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info"); + usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail"); return 0; } From 74f31618f2ca4119350372d120988c009d67064e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:21:29 +0200 Subject: [PATCH 352/600] report error in pwm command if any --- src/systemcmds/pwm/pwm.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 87c7fc6479..c8d698b86a 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -648,6 +648,10 @@ pwm_main(int argc, char *argv[]) /* force failsafe */ ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 0); } + + if (ret != OK) { + warnx("FAILED setting forcefail %s", argv[2]); + } } exit(0); } From ed31d6a59a21753e326ba03933ce0eb962b8412a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:56:37 +0200 Subject: [PATCH 353/600] Cleanup of heartbeat handling and status printing. Ready to go mainline --- src/modules/mavlink/mavlink_main.cpp | 32 ++++++++++++---------- src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++------ src/modules/mavlink/mavlink_receiver.h | 1 - src/modules/uORB/topics/telemetry_status.h | 3 +- 4 files changed, 32 insertions(+), 24 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e25851c7d5..0e8f17c77e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -432,11 +432,9 @@ Mavlink::get_status_all_instances() unsigned iterations = 0; - warnx("waiting for instances to stop"); - while (inst != nullptr) { - printf("instance #%u:\n", iterations); + printf("\ninstance #%u:\n", iterations); inst->display_status(); /* move on */ @@ -1688,27 +1686,33 @@ Mavlink::start(int argc, char *argv[]) void Mavlink::display_status() { - warnx("running"); + + if (_rstatus.heartbeat_time > 0) { + printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + } if (_rstatus.timestamp > 0) { - printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + printf("\ttype:\t\t"); switch (_rstatus.type) { case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: - printf("\t3DR RADIO\n"); + printf("3DR RADIO\n"); break; default: - printf("\tUNKNOWN RADIO\n"); + printf("UNKNOWN RADIO\n"); break; } - printf("\trssi:\t%d\t\n", _rstatus.rssi); - printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); - printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); - printf("\tnoise:\t%d\tus\n", _rstatus.noise); - printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); - printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); - printf("\tfixed:\t%u\tus\n", _rstatus.fixed); + printf("\trssi:\t\t%d\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf); + printf("\tnoise:\t\t%d\n", _rstatus.noise); + printf("\tremote noise:\t%u\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\n", _rstatus.rxerrors); + printf("\tfixed:\t\t%u\n", _rstatus.fixed); + } else { + printf("\tno telem status.\n"); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 528c8b72a3..454d730d83 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -111,7 +111,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _telemetry_heartbeat_time(0), _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), @@ -403,7 +402,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); tstatus.timestamp = hrt_absolute_time(); - tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.telem_time = tstatus.timestamp; + /* tstatus.heartbeat_time is set by system heartbeats */ tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -460,16 +460,20 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) /* ignore own heartbeats, accept only heartbeats from GCS */ if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - _telemetry_heartbeat_time = hrt_absolute_time(); + + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); + + hrt_abstime tnow = hrt_absolute_time(); + + /* always set heartbeat, publish only if telemetry link not up */ + tstatus.heartbeat_time = tnow; /* if no radio status messages arrive, lets at least publish that heartbeats were received */ if (!_radio_status_available) { - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = _telemetry_heartbeat_time; - tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.timestamp = tnow; + /* telem_time indicates the timestamp of a telemetry status packet and we got none */ + tstatus.telem_time = 0; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; if (_telemetry_status_pub < 0) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b64a060e3a..f4d0c52d88 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -148,7 +148,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - hrt_abstime _telemetry_heartbeat_time; bool _radio_status_available; int _control_mode_sub; int _hil_frames; diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 3347724a53..1297c1a9d3 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -57,7 +57,8 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; - uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ + uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ + uint64_t telem_time; /**< Time of last received telemetry status packet, 0 for none */ enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ uint8_t rssi; /**< local signal strength */ uint8_t remote_rssi; /**< remote signal strength */ From 5ace06f3b820d43bc313347f3d23b121a36fbef1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:57:42 +0200 Subject: [PATCH 354/600] Add hint that heartbeats are only considered if coming from a GCS --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0e8f17c77e..7bd2b9754b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1688,7 +1688,7 @@ Mavlink::display_status() { if (_rstatus.heartbeat_time > 0) { - printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } if (_rstatus.timestamp > 0) { From 6e3ebd3a36ce8bf3a9132b76986074ad8d9fcf35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 16:00:37 +0200 Subject: [PATCH 355/600] Fix code style, no actual code edits --- src/modules/mavlink/mavlink_main.cpp | 166 +++++++++++++++------------ 1 file changed, 92 insertions(+), 74 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7bd2b9754b..cb8c298c04 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -81,7 +81,7 @@ #include "mavlink_commands.h" #ifndef MAVLINK_CRC_EXTRA - #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems #endif /* oddly, ERROR is not defined for c++ */ @@ -154,7 +154,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length instance = Mavlink::get_instance(6); break; #endif - default: + + default: return; } @@ -169,17 +170,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { /* Disable hardware flow control: * if no successful write since a defined time * and if the last try was not the last successful write */ if (last_write_try_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && - last_write_success_times[(unsigned)channel] != - last_write_try_times[(unsigned)channel]) - { + hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && + last_write_success_times[(unsigned)channel] != + last_write_try_times[(unsigned)channel]) { warnx("DISABLING HARDWARE FLOW CONTROL"); instance->enable_flow_control(false); } @@ -202,8 +202,10 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } ssize_t ret = write(uart, ch, desired); + if (ret != desired) { instance->count_txerr(); + } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } @@ -232,33 +234,33 @@ Mavlink::Mavlink() : _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), - _logbuffer{}, - _total_counter(0), - _receive_thread{}, - _verbose(false), - _forwarding_on(false), - _passing_on(false), - _ftp_on(false), - _uart_fd(-1), - _baudrate(57600), - _datarate(10000), - _mavlink_param_queue_index(0), - mavlink_link_termination_allowed(false), - _subscribe_to_stream(nullptr), - _subscribe_to_stream_rate(0.0f), - _flow_control_enabled(true), - _rstatus{}, - _message_buffer{}, - _message_buffer_mutex{}, - _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_system_type(0), - _param_use_hil_gps(0), + _logbuffer {}, + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(10000), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), - _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; @@ -637,7 +639,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * case 921600: speed = B921600; break; default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", + baud); return -EINVAL; } @@ -867,8 +870,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { mavlink_param_request_list_t req; mavlink_msg_param_request_list_decode(msg, &req); + if (req.target_system == mavlink_system.sysid && - (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { + (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); send_statustext_info("[pm] sending list"); @@ -883,7 +887,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav mavlink_param_set_t mavlink_param_set; mavlink_msg_param_set_decode(msg, &mavlink_param_set); - if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { + if (mavlink_param_set.target_system == mavlink_system.sysid + && ((mavlink_param_set.target_component == mavlink_system.compid) + || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); @@ -910,7 +916,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav mavlink_param_request_read_t mavlink_param_request_read; mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { + if (mavlink_param_request_read.target_system == mavlink_system.sysid + && ((mavlink_param_request_read.target_component == mavlink_system.compid) + || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { /* when no index is given, loop through string ids and compare them */ if (mavlink_param_request_read.param_index == -1) { /* local name buffer to enforce null-terminated string */ @@ -973,15 +981,17 @@ Mavlink::send_statustext(unsigned severity, const char *string) /* Map severity */ switch (severity) { - case MAVLINK_IOC_SEND_TEXT_INFO: - statustext.severity = MAV_SEVERITY_INFO; - break; - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - statustext.severity = MAV_SEVERITY_CRITICAL; - break; - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - statustext.severity = MAV_SEVERITY_EMERGENCY; - break; + case MAVLINK_IOC_SEND_TEXT_INFO: + statustext.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + statustext.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + statustext.severity = MAV_SEVERITY_EMERGENCY; + break; } mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); @@ -1098,12 +1108,14 @@ Mavlink::message_buffer_init(int size) _message_buffer.size = size; _message_buffer.write_ptr = 0; _message_buffer.read_ptr = 0; - _message_buffer.data = (char*)malloc(_message_buffer.size); + _message_buffer.data = (char *)malloc(_message_buffer.size); int ret; + if (_message_buffer.data == 0) { ret = ERROR; _message_buffer.size = 0; + } else { ret = OK; } @@ -1475,7 +1487,8 @@ Mavlink::task_main(int argc, char *argv[]) if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate); + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, + (double)_subscribe_to_stream_rate); } else { warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); @@ -1514,45 +1527,46 @@ Mavlink::task_main(int argc, char *argv[]) bool is_part; uint8_t *read_ptr; - uint8_t *write_ptr; + uint8_t *write_ptr; pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); + int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { - // Reconstruct message from buffer + // Reconstruct message from buffer mavlink_message_t msg; - write_ptr = (uint8_t*)&msg; + write_ptr = (uint8_t *)&msg; + + // Pull a single message from the buffer + size_t read_count = available; + + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); - // Pull a single message from the buffer - size_t read_count = available; - if (read_count > sizeof(mavlink_message_t)) { - read_count = sizeof(mavlink_message_t); - } - - memcpy(write_ptr, read_ptr, read_count); - - // We hold the mutex until after we complete the second part of the buffer. If we don't - // we may end up breaking the empty slot overflow detection semantics when we mark the - // possibly partial read below. - pthread_mutex_lock(&_message_buffer_mutex); - message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ if (is_part && read_count < sizeof(mavlink_message_t)) { - write_ptr += read_count; - available = message_buffer_get_ptr((void**)&read_ptr, &is_part); - read_count = sizeof(mavlink_message_t) - read_count; - memcpy(write_ptr, read_ptr, read_count); + write_ptr += read_count; + available = message_buffer_get_ptr((void **)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } - - pthread_mutex_unlock(&_message_buffer_mutex); - _mavlink_resend_uart(_channel, &msg); + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, &msg); } } @@ -1606,6 +1620,7 @@ Mavlink::task_main(int argc, char *argv[]) message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } + /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -1627,6 +1642,7 @@ int Mavlink::start_helper(int argc, char *argv[]) /* out of memory */ res = -ENOMEM; warnx("OUT OF MEM"); + } else { /* this will actually only return once MAVLink exits */ res = instance->task_main(argc, argv); @@ -1696,10 +1712,11 @@ Mavlink::display_status() printf("\ttype:\t\t"); switch (_rstatus.type) { - case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: printf("3DR RADIO\n"); break; - default: + + default: printf("UNKNOWN RADIO\n"); break; } @@ -1711,6 +1728,7 @@ Mavlink::display_status() printf("\tremote noise:\t%u\n", _rstatus.remote_noise); printf("\trx errors:\t%u\n", _rstatus.rxerrors); printf("\tfixed:\t\t%u\n", _rstatus.fixed); + } else { printf("\tno telem status.\n"); } From 52a9513e64dacf1004fd0e11df3daf5eb607b0f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 16:22:02 +0200 Subject: [PATCH 356/600] Update rates --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cb8c298c04..2e3a9ca331 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1454,7 +1454,7 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(35000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; From 41003a243793e465a71e9b142e7c9aac132d3923 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 19 Jul 2014 17:37:13 +0200 Subject: [PATCH 357/600] mavlink: TX/RX rate counters added --- src/modules/mavlink/mavlink_main.cpp | 28 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 23 +++++++++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 3 +++ 3 files changed, 54 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2e3a9ca331..ea60bb2260 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -197,6 +197,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (buf_free < desired) { /* we don't want to send anything just in half, so return */ instance->count_txerr(); + instance->count_txerrbytes(desired); return; } } @@ -205,9 +206,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (ret != desired) { instance->count_txerr(); + instance->count_txerrbytes(desired); } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; + instance->count_txbytes(desired); } } } @@ -249,6 +252,13 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), _rstatus {}, _message_buffer {}, _message_buffer_mutex {}, @@ -1570,6 +1580,20 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* update TX/RX rates*/ + if (t > _bytes_timestamp + 1000000) { + if (_bytes_timestamp != 0) { + float dt = (t - _bytes_timestamp) / 1000.0f; + _rate_tx = _bytes_tx / dt; + _rate_txerr = _bytes_txerr / dt; + _rate_rx = _bytes_rx / dt; + _bytes_tx = 0; + _bytes_txerr = 0; + _bytes_rx = 0; + } + _bytes_timestamp = t; + } + perf_end(_loop_perf); } @@ -1732,6 +1756,10 @@ Mavlink::display_status() } else { printf("\tno telem status.\n"); } + printf("\trates:\n"); + printf("\ttx: %.3f kB/s\n", (double)_rate_tx); + printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); + printf("\trx: %.3f kB/s\n", (double)_rate_rx); } int diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index d51120462a..70d13acb09 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -232,6 +232,21 @@ public: */ void count_txerr(); + /** + * Count transmitted bytes + */ + void count_txbytes(unsigned n) { _bytes_tx += n; }; + + /** + * Count bytes not transmitted because of errors + */ + void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; + + /** + * Count received bytes + */ + void count_rxbytes(unsigned n) { _bytes_rx += n; }; + /** * Get the receive status of this MAVLink link */ @@ -293,6 +308,14 @@ private: bool _flow_control_enabled; + unsigned _bytes_tx; + unsigned _bytes_txerr; + unsigned _bytes_rx; + uint64_t _bytes_timestamp; + float _rate_tx; + float _rate_txerr; + float _rate_rx; + struct telemetry_status_s _rstatus; ///< receive status struct mavlink_message_buffer { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 454d730d83..54c412ce7c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -958,6 +958,9 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->handle_message(&msg); } } + + /* count received bytes */ + _mavlink->count_rxbytes(nread); } } From 5d2489880c2a42bc67554efa7715ba3a761c5d17 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 21:14:14 +0200 Subject: [PATCH 358/600] Fix low voltage warning threshold to 9%, not 90% --- src/modules/commander/commander.cpp | 2 +- src/modules/commander/commander_params.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cb447f8ce0..562e9a1b6d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1282,7 +1282,7 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.9f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 448bc53cd4..a09e2d6bf4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.3f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); /** * Full cell voltage. From e7d8d9c91fe72169d292a5e0ac44133a218f27a0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 19 Jul 2014 23:19:49 +0200 Subject: [PATCH 359/600] position_estimator_inav: parameters descriptions added --- .../position_estimator_inav_params.c | 174 ++++++++++++++++++ 1 file changed, 174 insertions(+) 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 0581f8236b..98b31d17b3 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,22 +40,196 @@ #include "position_estimator_inav_params.h" +/** + * Z axis weight for barometer + * + * Weight (cutoff frequency) for barometer altitude measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); + +/** + * Z axis weight for GPS + * + * Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); + +/** + * Z axis weight for sonar + * + * Weight (cutoff frequency) for sonar measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); + +/** + * XY axis weight for GPS position + * + * Weight (cutoff frequency) for GPS position measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); + +/** + * XY axis weight for GPS velocity + * + * Weight (cutoff frequency) for GPS velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); + +/** + * XY axis weight for optical flow + * + * Weight (cutoff frequency) for optical flow (velocity) measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); + +/** + * XY axis weight for resetting velocity + * + * When velocity sources lost slowly decrease estimated horizontal velocity with this weight. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); + +/** + * XY axis weight factor for GPS when optical flow available + * + * When optical flow data available, multiply GPS weights (for position and velocity) by this factor. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); + +/** + * Accelerometer bias estimation weight + * + * Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. + * + * @min 0.0 + * @max 0.1 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); + +/** + * Optical flow scale factor + * + * Factor to convert raw optical flow (in pixels) to radians [rad/px]. + * + * @min 0.0 + * @max 1.0 + * @unit rad/px + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); + +/** + * Minimal acceptable optical flow quality + * + * 0 - lowest quality, 1 - best quality. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); + +/** + * Weight for sonar filter + * + * Sonar filter detects spikes on sonar measurements and used to detect new surface level. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); + +/** + * Sonar maximal error for new surface + * + * If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable). + * + * @min 0.0 + * @max 1.0 + * @unit m + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); + +/** + * Land detector time + * + * Vehicle assumed landed if no altitude changes happened during this time on low throttle. + * + * @min 0.0 + * @max 10.0 + * @unit s + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); + +/** + * Land detector altitude dispersion threshold + * + * Dispersion threshold for triggering land detector. + * + * @min 0.0 + * @max 10.0 + * @unit m + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); + +/** + * Land detector throttle threshold + * + * Value should be lower than minimal hovering thrust. Half of it is good choice. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); + +/** + * GPS delay + * + * GPS delay compensation + * + * @min 0.0 + * @max 1.0 + * @unit s + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) From cd8a0cd21771fb3a6fb8cddfc3da322eb04f10db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 23:39:02 +0200 Subject: [PATCH 360/600] mavlink: Only send event-based messages if there is space in the buffer --- src/modules/mavlink/mavlink_main.cpp | 60 +++++++++++++++++++++++++--- src/modules/mavlink/mavlink_main.h | 17 +++++++- src/modules/mavlink/mavlink_stream.h | 13 ++++++ 3 files changed, 84 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ea60bb2260..e6b558ca69 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -246,7 +246,8 @@ Mavlink::Mavlink() : _ftp_on(false), _uart_fd(-1), _baudrate(57600), - _datarate(10000), + _datarate(1000), + _datarate_events(500), _mavlink_param_queue_index(0), mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), @@ -352,6 +353,18 @@ Mavlink::count_txerr() perf_count(_txerr_perf); } +unsigned +Mavlink::get_free_tx_buf() +{ + unsigned buf_free; + + if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { + return buf_free; + } else { + return 0; + } +} + void Mavlink::set_mode(enum MAVLINK_MODE mode) { @@ -1083,6 +1096,31 @@ Mavlink::configure_stream(const char *stream_name, const float rate) return ERROR; } +void +Mavlink::adjust_stream_rates(const float multiplier) +{ + /* do not allow to push us to zero */ + if (multiplier < 0.01f) { + return; + } + + /* search if stream exists */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + /* set new interval */ + unsigned interval = stream->get_interval(); + interval /= multiplier; + + /* allow max ~600 Hz */ + if (interval < 1600) { + interval = 1600; + } + + /* set new interval */ + stream->set_interval(interval * multiplier); + } +} + void Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) { @@ -1464,7 +1502,7 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter fast_rate_limiter(35000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; @@ -1519,10 +1557,22 @@ Mavlink::task_main(int argc, char *argv[]) } if (fast_rate_limiter.check(t)) { - mavlink_pm_queued_send(); - _mission_manager->eventloop(); - if (!mavlink_logbuffer_is_empty(&_logbuffer)) { + unsigned buf_free = get_free_tx_buf(); + + /* only send messages if they fit the buffer */ + if (buf_free >= (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + mavlink_pm_queued_send(); + } + + buf_free = get_free_tx_buf(); + if (buf_free >= (MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + _mission_manager->eventloop(); + } + + buf_free = get_free_tx_buf(); + if (buf_free >= (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) && + (!mavlink_logbuffer_is_empty(&_logbuffer))) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 70d13acb09..e9d6b9c86b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -247,6 +247,13 @@ public: */ void count_rxbytes(unsigned n) { _bytes_rx += n; }; + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + /** * Get the receive status of this MAVLink link */ @@ -292,7 +299,8 @@ private: bool _ftp_on; int _uart_fd; int _baudrate; - int _datarate; + int _datarate; ///< data rate for normal streams (attitude, position, etc.) + int _datarate_events; ///< data rate for params, waypoints, text messages /** * If the queue index is not at 0, the queue sending @@ -385,6 +393,13 @@ private: int configure_stream(const char *stream_name, const float rate); + /** + * Adjust the stream rates based on the current rate + * + * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease + */ + void adjust_stream_rates(const float multiplier); + int message_buffer_init(int size); void message_buffer_destroy(); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 20e1c7c449..f539d225b8 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -56,7 +56,20 @@ public: MavlinkStream(); virtual ~MavlinkStream(); + + /** + * Get the interval + * + * @param interval the inveral in microseconds (us) between messages + */ void set_interval(const unsigned int interval); + + /** + * Get the interval + * + * @return the inveral in microseconds (us) between messages + */ + unsigned get_interval() { return _interval; } void set_channel(mavlink_channel_t channel); /** From 1f3625371d787cdc452b45ad9d9a01423ae51f96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 10:18:00 +0200 Subject: [PATCH 361/600] Obey radio status in opportunistic transmissions as well. Last missing bit are adaptive rates --- src/modules/mavlink/mavlink_main.cpp | 29 +++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e6b558ca69..e7e96dc3a4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -359,7 +359,34 @@ Mavlink::get_free_tx_buf() unsigned buf_free; if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { - return buf_free; + if (_rstatus.timestamp > 0 && + (hrt_elapsed_time(&_rstatus.timestamp) < (2 * 1000 * 1000))) { + + unsigned low_buf; + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + low_buf = 50; + break; + default: + low_buf = 50; + break; + } + + if (_rstatus.txbuf < low_buf) { + /* + * If the TX buf measure is initialized and up to date and low + * return 0 to slow event based transmission + */ + return 0; + } else { + /* there is no SW flow control option, just use what our buffer tells us */ + return buf_free; + } + + } else { + return buf_free; + } } else { return 0; } From 5fb2a92e7704c2e298128addf890722b6b03289a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 14:28:05 +0200 Subject: [PATCH 362/600] commander: Do not confuse developers with wrong comments, do not confuse users with not at all helpful "general error" messages. --- src/modules/commander/commander.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 04450a44fb..48cbc254fd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1407,8 +1407,12 @@ int commander_thread_main(int argc, char *argv[]) arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "arming state transition denied"); + /* + * the arming transition can be denied to a number of reasons: + * - pre-flight check failed (sensors not ok or not calibrated) + * - safety not disabled + * - system not in manual mode + */ tune_negative(true); } From b3a80025b3bf514e987d19b7292ba0f9d16d7d1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 14:28:24 +0200 Subject: [PATCH 363/600] Do not confuse operators / users with technical error messages --- src/modules/commander/state_machine_helper.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 372ba9d7dc..4ca8471fc9 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -216,11 +216,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current } if (ret == TRANSITION_DENIED) { - static const char *errMsg = "INVAL: %s - %s"; - - mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); - - warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); + /* only print to console here as this is too technical to be useful during operation */ + warnx("INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]); } return ret; From f3b8890601e40e5131c55e6f96ad1452a53410eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 14:31:23 +0200 Subject: [PATCH 364/600] commander: Explain sensor arming fail case to users --- src/modules/commander/state_machine_helper.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4ca8471fc9..16ed8dd52b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -200,6 +200,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); valid_transition = false; } From a5f538dc5c9570b59c7135570a662651cb857698 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 16:06:31 +0200 Subject: [PATCH 365/600] Commander: Print technical feedback as last resort if no feedback was provided before --- .../commander/state_machine_helper.cpp | 24 +++++++++++++------ 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 16ed8dd52b..7b26e3e8cb 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; - arming_state_t current_arming_state = status->arming_state; + bool feedback_provided = false; /* only check transition if the new state is actually different from the current one */ if (new_arming_state == current_arming_state) { @@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if pre-arm check fails if (prearm_ret) { + /* the prearm check already prints the reject reason */ + feedback_provided = true; valid_transition = false; // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!"); - + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); + feedback_provided = true; valid_transition = false; } @@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (!status->condition_power_input_valid) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); + feedback_provided = true; valid_transition = false; } @@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current (status->avionics_power_rail_voltage < 4.9f))) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); + feedback_provided = true; valid_transition = false; } } @@ -201,6 +205,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); + feedback_provided = true; valid_transition = false; } @@ -217,8 +222,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current } if (ret == TRANSITION_DENIED) { - /* only print to console here as this is too technical to be useful during operation */ - warnx("INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]); + const char * str = "INVAL: %s - %s"; + /* only print to console here by default as this is too technical to be useful during operation */ + warnx(str, state_names[status->arming_state], state_names[new_arming_state]); + + /* print to MAVLink if we didn't provide any feedback yet */ + if (!feedback_provided) { + mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]); + } } return ret; @@ -646,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE"); - mavlink_log_critical(mavlink_fd, "hold still while arming"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still"); /* this is frickin' fatal */ failed = true; goto system_eval; From 3935540c7df0f67946b637af6dc2d3145453c326 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 16:23:43 +0200 Subject: [PATCH 366/600] Set full voltage correctly to ensure percentage range fits. Force all users to new value by param renaming. Since this will tend to show batteries as more drained than before, this is a change in a safe direction and will not trigger unnoticed discharges. --- src/modules/commander/commander_helper.cpp | 6 +++--- src/modules/commander/commander_params.c | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index a4aafa1f63..239b77df04 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -289,8 +289,8 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float static param_t bat_n_cells_h; static param_t bat_capacity_h; static param_t bat_v_load_drop_h; - static float bat_v_empty = 3.2f; - static float bat_v_full = 4.0f; + static float bat_v_empty = 3.4f; + static float bat_v_full = 4.2f; static float bat_v_load_drop = 0.1f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; @@ -299,7 +299,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float if (!initialized) { bat_v_empty_h = param_find("BAT_V_EMPTY"); - bat_v_full_h = param_find("BAT_V_FULL"); + bat_v_full_h = param_find("BAT_V_CHARGED"); bat_n_cells_h = param_find("BAT_N_CELLS"); bat_capacity_h = param_find("BAT_CAPACITY"); bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index a09e2d6bf4..dea12db409 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -65,7 +65,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.15f); +PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); /** * Number of cells. From 264fe884a2825ae430a19ba61e8c89d6a214f5cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 16:28:44 +0200 Subject: [PATCH 367/600] Fixed load voltage calculation --- src/modules/commander/commander_helper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 239b77df04..d87af2d7d4 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -317,7 +317,8 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float remaining_voltage = (voltage - bat_n_cells * (bat_v_empty - (bat_v_load_drop * throttle_normalized)) / (bat_n_cells * (bat_v_full - bat_v_empty)); + float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized); + float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty)); if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ From c95de36d3a01bda1d3664fbc46df3a5fc35fe4da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 19:42:31 +0200 Subject: [PATCH 368/600] commander: Add missing parameter definition --- src/modules/commander/commander_helper.cpp | 2 +- src/modules/commander/commander_params.c | 10 ++++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index d87af2d7d4..2022e99fb3 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -291,7 +291,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float static param_t bat_v_load_drop_h; static float bat_v_empty = 3.4f; static float bat_v_full = 4.2f; - static float bat_v_load_drop = 0.1f; + static float bat_v_load_drop = 0.06f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static bool initialized = false; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index dea12db409..0e4973b5fa 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -67,6 +67,16 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); */ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); +/** + * Voltage drop per cell on 100% load + * + * This implicitely defines the internal resistance + * to maximum current ratio and assumes linearity. + * + * @group Battery Calibration + */ +PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.06f); + /** * Number of cells. * From ee7cd04e4c480a1931223889081074c469be0dbb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Jul 2014 20:34:32 +0200 Subject: [PATCH 369/600] mavlink: use send_message() method of Mavlink class instead of using helpers --- src/modules/mavlink/mavlink_main.cpp | 237 +++++++++-------------- src/modules/mavlink/mavlink_main.h | 6 + src/modules/mavlink/mavlink_messages.cpp | 30 ++- src/modules/mavlink/mavlink_stream.cpp | 13 +- src/modules/mavlink/mavlink_stream.h | 8 +- 5 files changed, 122 insertions(+), 172 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e7e96dc3a4..e3c452fb2c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -94,11 +94,15 @@ static const int ERROR = -1; #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate +#define TX_BUFFER_GAP 256 + static Mavlink *_mavlink_instances = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; + /** * mavlink app start / stop handling function * @@ -108,113 +112,6 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); extern mavlink_system_t mavlink_system; -static uint64_t last_write_success_times[6] = {0}; -static uint64_t last_write_try_times[6] = {0}; - -/* - * Internal function to send the bytes through the right serial port - */ -void -mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) -{ - - Mavlink *instance; - - switch (channel) { - case MAVLINK_COMM_0: - instance = Mavlink::get_instance(0); - break; - - case MAVLINK_COMM_1: - instance = Mavlink::get_instance(1); - break; - - case MAVLINK_COMM_2: - instance = Mavlink::get_instance(2); - break; - - case MAVLINK_COMM_3: - instance = Mavlink::get_instance(3); - break; -#ifdef MAVLINK_COMM_4 - - case MAVLINK_COMM_4: - instance = Mavlink::get_instance(4); - break; -#endif -#ifdef MAVLINK_COMM_5 - - case MAVLINK_COMM_5: - instance = Mavlink::get_instance(5); - break; -#endif -#ifdef MAVLINK_COMM_6 - - case MAVLINK_COMM_6: - instance = Mavlink::get_instance(6); - break; -#endif - - default: - return; - } - - int uart = instance->get_uart_fd(); - - ssize_t desired = (sizeof(uint8_t) * length); - - /* - * Check if the OS buffer is full and disable HW - * flow control if it continues to be full - */ - int buf_free = 0; - - if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - - /* Disable hardware flow control: - * if no successful write since a defined time - * and if the last try was not the last successful write - */ - if (last_write_try_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && - last_write_success_times[(unsigned)channel] != - last_write_try_times[(unsigned)channel]) { - warnx("DISABLING HARDWARE FLOW CONTROL"); - instance->enable_flow_control(false); - } - - } - - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (instance->should_transmit()) { - last_write_try_times[(unsigned)channel] = hrt_absolute_time(); - - /* check if there is space in the buffer, let it overflow else */ - if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { - - if (buf_free < desired) { - /* we don't want to send anything just in half, so return */ - instance->count_txerr(); - instance->count_txerrbytes(desired); - return; - } - } - - ssize_t ret = write(uart, ch, desired); - - if (ret != desired) { - instance->count_txerr(); - instance->count_txerrbytes(desired); - - } else { - last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; - instance->count_txbytes(desired); - } - } -} - static void usage(void); Mavlink::Mavlink() : @@ -253,6 +150,8 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _last_write_success_time(0), + _last_write_try_time(0), _bytes_tx(0), _bytes_txerr(0), _bytes_rx(0), @@ -359,30 +258,10 @@ Mavlink::get_free_tx_buf() unsigned buf_free; if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { - if (_rstatus.timestamp > 0 && - (hrt_elapsed_time(&_rstatus.timestamp) < (2 * 1000 * 1000))) { + if (_rstatus.telem_time > 0 && + (hrt_elapsed_time(&_rstatus.telem_time) < (2 * 1000 * 1000))) { - unsigned low_buf; - - switch (_rstatus.type) { - case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: - low_buf = 50; - break; - default: - low_buf = 50; - break; - } - - if (_rstatus.txbuf < low_buf) { - /* - * If the TX buf measure is initialized and up to date and low - * return 0 to slow event based transmission - */ - return 0; - } else { - /* there is no SW flow control option, just use what our buffer tells us */ - return buf_free; - } + return (unsigned)(buf_free * 0.01f * _rstatus.txbuf); } else { return buf_free; @@ -808,13 +687,93 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } +bool +Mavlink::check_can_send(const int prio, const unsigned packet_len) +{ + /* + * Check if the OS buffer is full and disable HW + * flow control if it continues to be full + */ + int buf_free = 0; + + if (get_flow_control_enabled() + && ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free) == 0) { + + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (_last_write_try_time != 0 && + hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && + _last_write_success_time != _last_write_try_time) { + warnx("DISABLING HARDWARE FLOW CONTROL"); + enable_flow_control(false); + } + } + + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (should_transmit()) { + _last_write_try_time = hrt_absolute_time(); + + /* check if there is space in the buffer, let it overflow else */ + ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); + + if (buf_free >= (prio <= 0 ? packet_len : TX_BUFFER_GAP)) { + warnx("\t\t\tSKIP %i bytes prio %i", packet_len, (int)prio); + /* we don't want to send anything just in half, so return */ + count_txerr(); + count_txerrbytes(packet_len); + return true; + } + } + return false; +} + void -Mavlink::send_message(const mavlink_message_t *msg) +Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio) { uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - mavlink_send_uart_bytes(_channel, buf, len); + uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + if (!check_can_send(prio, packet_len)) { + return; + } + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; // TODO use internal seq counter? + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid; + + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); + + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + crc_accumulate_buffer(&checksum, &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); +#if MAVLINK_CRC_EXTRA + crc_accumulate(crc_extra, &checksum); +#endif + + buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + + /* send message to UART */ + ssize_t ret = write(_uart_fd, buf, packet_len); + + if (ret != packet_len) { + count_txerr(); + count_txerrbytes(packet_len); + + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); + } } void @@ -1585,21 +1544,17 @@ Mavlink::task_main(int argc, char *argv[]) if (fast_rate_limiter.check(t)) { - unsigned buf_free = get_free_tx_buf(); - /* only send messages if they fit the buffer */ - if (buf_free >= (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + if (check_can_send(0, MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { mavlink_pm_queued_send(); } - buf_free = get_free_tx_buf(); - if (buf_free >= (MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + if (check_can_send(0, MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { _mission_manager->eventloop(); } - buf_free = get_free_tx_buf(); - if (buf_free >= (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) && - (!mavlink_logbuffer_is_empty(&_logbuffer))) { + if (!mavlink_logbuffer_is_empty(&_logbuffer) && + check_can_send(0, MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e9d6b9c86b..1c62b03d21 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -160,6 +160,10 @@ public: */ int set_hil_enabled(bool hil_enabled); + bool check_can_send(const int prio, const unsigned packet_len); + + void send_message(const uint8_t msgid, const void *msg, const int prio); + void send_message(const mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg); @@ -315,6 +319,8 @@ private: float _subscribe_to_stream_rate; bool _flow_control_enabled; + uint64_t _last_write_success_time; + uint64_t _last_write_try_time; unsigned _bytes_tx; unsigned _bytes_txerr; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6885bebde5..fe33985d20 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -249,9 +249,9 @@ public: return MAVLINK_MSG_ID_HEARTBEAT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamHeartbeat(); + return new MavlinkStreamHeartbeat(mavlink); } private: @@ -263,15 +263,15 @@ private: MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); protected: - explicit MavlinkStreamHeartbeat() : MavlinkStream(), + explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), status_sub(nullptr), pos_sp_triplet_sub(nullptr) {} - void subscribe(Mavlink *mavlink) + void subscribe() { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); + status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); } void send(const hrt_abstime t) @@ -290,17 +290,15 @@ protected: memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); } - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + mavlink_heartbeat_t msg; + msg.base_mode = 0; + msg.custom_mode = 0; + get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); + msg.type = mavlink_system.type; + msg.autopilot = mavlink_system.type; + msg.mavlink_version = 3; - mavlink_msg_heartbeat_send(_channel, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); + _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 1); } }; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 7279206dbb..79dc23cc68 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -43,9 +43,9 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : +MavlinkStream::MavlinkStream(Mavlink *mavlink) : next(nullptr), - _channel(MAVLINK_COMM_0), + _mavlink(mavlink), _interval(1000000), _last_sent(0) { @@ -64,15 +64,6 @@ MavlinkStream::set_interval(const unsigned int interval) _interval = interval; } -/** - * Set mavlink channel - */ -void -MavlinkStream::set_channel(mavlink_channel_t channel) -{ - _channel = channel; -} - /** * Update subscriptions and send message if necessary */ diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index f539d225b8..7d3afbbb59 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -54,7 +54,7 @@ class MavlinkStream public: MavlinkStream *next; - MavlinkStream(); + MavlinkStream(Mavlink *mavlink); virtual ~MavlinkStream(); /** @@ -76,14 +76,14 @@ public: * @return 0 if updated / sent, -1 if unchanged */ int update(const hrt_abstime t); - static MavlinkStream *new_instance(); + static MavlinkStream *new_instance(const Mavlink *mavlink); static const char *get_name_static(); - virtual void subscribe(Mavlink *mavlink) = 0; + virtual void subscribe() = 0; virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; protected: - mavlink_channel_t _channel; + Mavlink * _mavlink; unsigned int _interval; virtual void send(const hrt_abstime t) = 0; From 25d1cc3995f3ecafe2408582296d6dedfe49ce53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 07:35:23 +0200 Subject: [PATCH 370/600] Final value for battery load param default --- src/modules/commander/commander_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0e4973b5fa..dba68700b4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -75,7 +75,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.06f); +PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); /** * Number of cells. From afff12374240240ec2bbf2ba043b37cca8b3298a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 13:30:56 +0200 Subject: [PATCH 371/600] Add PX4IO voltage dataset and linear fit --- Tools/tests-host/data/fit_linear_voltage.m | 14 +++++ Tools/tests-host/data/px4io_v1.3.csv | 70 ++++++++++++++++++++++ 2 files changed, 84 insertions(+) create mode 100644 Tools/tests-host/data/fit_linear_voltage.m create mode 100644 Tools/tests-host/data/px4io_v1.3.csv diff --git a/Tools/tests-host/data/fit_linear_voltage.m b/Tools/tests-host/data/fit_linear_voltage.m new file mode 100644 index 0000000000..a0384b4865 --- /dev/null +++ b/Tools/tests-host/data/fit_linear_voltage.m @@ -0,0 +1,14 @@ +close all; +clear all; +M = importdata('px4io_v1.3.csv'); +voltage = M.data(:, 1); +counts = M.data(:, 2); +plot(counts, voltage, 'b*-', 'LineWidth', 2, 'MarkerSize', 15); +coeffs = polyfit(counts, voltage, 1); +fittedC = linspace(min(counts), max(counts), 500); +fittedV = polyval(coeffs, fittedC); +hold on +plot(fittedC, fittedV, 'r-', 'LineWidth', 3); + +slope = coeffs(1) +y_intersection = coeffs(2) \ No newline at end of file diff --git a/Tools/tests-host/data/px4io_v1.3.csv b/Tools/tests-host/data/px4io_v1.3.csv new file mode 100644 index 0000000000..2100d840ac --- /dev/null +++ b/Tools/tests-host/data/px4io_v1.3.csv @@ -0,0 +1,70 @@ +voltage, counts +4.3, 950 +4.4, 964 +4.5, 986 +4.6, 1009 +4.7, 1032 +4.8, 1055 +4.9, 1078 +5.0, 1101 +5.2, 1124 +5.3, 1148 +5.4, 1171 +5.5, 1195 +6.0, 1304 +6.1, 1329 +6.2, 1352 +7.0, 1517 +7.1, 1540 +7.2, 1564 +7.3, 1585 +7.4, 1610 +7.5, 1636 +8.0, 1728 +8.1, 1752 +8.2, 1755 +8.3, 1798 +8.4, 1821 +9.0, 1963 +9.1, 1987 +9.3, 2010 +9.4, 2033 +10.0, 2174 +10.1, 2198 +10.2, 2221 +10.3, 2245 +10.4, 2268 +11.0, 2385 +11.1, 2409 +11.2, 2432 +11.3, 2456 +11.4, 2480 +11.5, 2502 +11.6, 2526 +11.7, 2550 +11.8, 2573 +11.9, 2597 +12.0, 2621 +12.1, 2644 +12.3, 2668 +12.4, 2692 +12.5, 2716 +12.6, 2737 +12.7, 2761 +13.0, 2832 +13.5, 2950 +13.6, 2973 +14.1, 3068 +14.2, 3091 +14.7, 3209 +15.0, 3280 +15.1, 3304 +15.5, 3374 +15.6, 3397 +15.7, 3420 +16.0, 3492 +16.1, 3514 +16.2, 3538 +16.9, 3680 +17.0, 3704 +17.1, 3728 \ No newline at end of file From 956c084f31af9ec2aa80d61eee17e7506b4c7172 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 13:31:35 +0200 Subject: [PATCH 372/600] Fit IO voltage estimation using a new dataset, cross-validated with a second unit. Pending testing --- src/modules/px4iofirmware/registers.c | 23 +++++------------------ 1 file changed, 5 insertions(+), 18 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fcd53f1f1b..7fbb0ecf19 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -739,30 +739,17 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val { /* * Coefficients here derived by measurement of the 5-16V - * range on one unit: + * range on one unit, validated on sample points of another unit * - * V counts - * 5 1001 - * 6 1219 - * 7 1436 - * 8 1653 - * 9 1870 - * 10 2086 - * 11 2303 - * 12 2522 - * 13 2738 - * 14 2956 - * 15 3172 - * 16 3389 + * Data in Tools/tests-host/data folder. * - * slope = 0.0046067 - * intercept = 0.3863 + * slope = 0.004585267878277 (int: 4585) + * intercept = 0.016646394188076 (int: 16646 * - * Intercept corrected for best results @ 12V. */ unsigned counts = adc_measure(ADC_VBATT); if (counts != 0xffff) { - unsigned mV = (4150 + (counts * 46)) / 10 - 200; + unsigned mV = (16646 + (counts * 4585)) / 1000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VBATT] = corrected; From 0fc73a1484ac9e9cbc2f52ec7e81b28a109777eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 13:46:38 +0200 Subject: [PATCH 373/600] Fix comment, missing brace in comment --- src/modules/px4iofirmware/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 7fbb0ecf19..59b3043aa3 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -744,7 +744,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * Data in Tools/tests-host/data folder. * * slope = 0.004585267878277 (int: 4585) - * intercept = 0.016646394188076 (int: 16646 + * intercept = 0.016646394188076 (int: 16646) * */ unsigned counts = adc_measure(ADC_VBATT); From 331623bbd40978adf14c2034e75f31c937c34fba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 13:48:59 +0200 Subject: [PATCH 374/600] Fix missing line endings --- Tools/tests-host/data/fit_linear_voltage.m | 2 +- Tools/tests-host/data/px4io_v1.3.csv | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/tests-host/data/fit_linear_voltage.m b/Tools/tests-host/data/fit_linear_voltage.m index a0384b4865..7d0c2c27fc 100644 --- a/Tools/tests-host/data/fit_linear_voltage.m +++ b/Tools/tests-host/data/fit_linear_voltage.m @@ -11,4 +11,4 @@ hold on plot(fittedC, fittedV, 'r-', 'LineWidth', 3); slope = coeffs(1) -y_intersection = coeffs(2) \ No newline at end of file +y_intersection = coeffs(2) diff --git a/Tools/tests-host/data/px4io_v1.3.csv b/Tools/tests-host/data/px4io_v1.3.csv index 2100d840ac..b41ee8f1f9 100644 --- a/Tools/tests-host/data/px4io_v1.3.csv +++ b/Tools/tests-host/data/px4io_v1.3.csv @@ -67,4 +67,4 @@ voltage, counts 16.2, 3538 16.9, 3680 17.0, 3704 -17.1, 3728 \ No newline at end of file +17.1, 3728 From 178b0f7399ab881e44f2d2ecff809aea53a4397d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 19:25:23 +0200 Subject: [PATCH 375/600] Cross-check with nominal values from divider --- src/modules/px4iofirmware/registers.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 59b3043aa3..8c15c66c1f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -743,13 +743,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * * Data in Tools/tests-host/data folder. * - * slope = 0.004585267878277 (int: 4585) + * measured slope = 0.004585267878277 (int: 4585) + * nominal theoretic slope: 0.00459340659 (int: 4593) * intercept = 0.016646394188076 (int: 16646) + * nominal theoretic intercept: 0.00 (int: 0) * */ unsigned counts = adc_measure(ADC_VBATT); if (counts != 0xffff) { - unsigned mV = (16646 + (counts * 4585)) / 1000; + unsigned mV = (0 + (counts * 4593)) / 1000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VBATT] = corrected; From 07d11583bb0926ac9fe1cf2f1c7e11c683606a4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 20:21:20 +0200 Subject: [PATCH 376/600] Rely on theoretical value, as the closed-loop test with multimeter suggests this is the most accurate measurement --- src/modules/px4iofirmware/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8c15c66c1f..43161aa70e 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -751,7 +751,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val */ unsigned counts = adc_measure(ADC_VBATT); if (counts != 0xffff) { - unsigned mV = (0 + (counts * 4593)) / 1000; + unsigned mV = (166460 + (counts * 45934)) / 10000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VBATT] = corrected; From 940becd0c1e39168e4836db5927e989f2d780ad3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 22 Jul 2014 02:55:14 +0400 Subject: [PATCH 377/600] UAVCAN update: Mako dependency removed --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 767462c992..dca2611c31 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 767462c9924d7afbb789dc3e4c76d31261c557d3 +Subproject commit dca2611c3186eaa1cac42557f07b013e2dc633d3 From a063f58e006689bee2e28d85930cba11245b9597 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Tue, 22 Jul 2014 12:36:28 +0800 Subject: [PATCH 378/600] Add vision weight parameters to structure --- .../position_estimator_inav_params.c | 6 ++++++ .../position_estimator_inav_params.h | 4 ++++ 2 files changed, 10 insertions(+) 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 0d9fbef278..36e5567566 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -43,9 +43,11 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); +PARAM_DEFINE_FLOAT(INAV_W_Z_VISION_P, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_VISION_P, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); @@ -64,9 +66,11 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_vision_p = param_find("INAV_W_Z_VISION_P"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); + h->w_xy_vision_p = param_find("INAV_W_XY_VISION_P"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); @@ -88,9 +92,11 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str { param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); + param_get(h->w_z_vision_p, &(p->w_z_vision_p)); param_get(h->w_z_sonar, &(p->w_z_sonar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); + param_get(h->w_xy_vision_p, &(p->w_xy_vision_p)); param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_xy_res_v, &(p->w_xy_res_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 08098d6c30..a64205c1f5 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -44,9 +44,11 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; + float w_z_vision_p; float w_z_sonar; float w_xy_gps_p; float w_xy_gps_v; + float w_xy_vision_p; float w_xy_flow; float w_xy_res_v; float w_gps_flow; @@ -65,9 +67,11 @@ struct position_estimator_inav_params { struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; + param_t w_z_vision_p; param_t w_z_sonar; param_t w_xy_gps_p; param_t w_xy_gps_v; + param_t w_xy_vision_p; param_t w_xy_flow; param_t w_xy_res_v; param_t w_gps_flow; From dd0e39972fb5a8e4b881e71d8bdb53e1ce82e380 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Tue, 22 Jul 2014 14:00:49 +0800 Subject: [PATCH 379/600] Add weight parameter for vision velocity --- .../position_estimator_inav/position_estimator_inav_params.c | 3 +++ .../position_estimator_inav/position_estimator_inav_params.h | 2 ++ 2 files changed, 5 insertions(+) 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 36e5567566..1867f65dad 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -48,6 +48,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_VISION_P, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_VISION_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); @@ -71,6 +72,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); h->w_xy_vision_p = param_find("INAV_W_XY_VISION_P"); + h->w_xy_vision_v = param_find("INAV_W_XY_VISION_V"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); @@ -97,6 +99,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); param_get(h->w_xy_vision_p, &(p->w_xy_vision_p)); + param_get(h->w_xy_vision_v, &(p->w_xy_vision_v)); param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_xy_res_v, &(p->w_xy_res_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index a64205c1f5..d0a65e42ea 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -49,6 +49,7 @@ struct position_estimator_inav_params { float w_xy_gps_p; float w_xy_gps_v; float w_xy_vision_p; + float w_xy_vision_v; float w_xy_flow; float w_xy_res_v; float w_gps_flow; @@ -72,6 +73,7 @@ struct position_estimator_inav_param_handles { param_t w_xy_gps_p; param_t w_xy_gps_v; param_t w_xy_vision_p; + param_t w_xy_vision_v; param_t w_xy_flow; param_t w_xy_res_v; param_t w_gps_flow; From ef74f4ad8920b59a80158ac8820ed497c9dbbe13 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Tue, 22 Jul 2014 20:48:18 +0800 Subject: [PATCH 380/600] Shorten vision parameter name --- .../position_estimator_inav_params.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 1867f65dad..90d8d2b502 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -43,12 +43,12 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); -PARAM_DEFINE_FLOAT(INAV_W_Z_VISION_P, 0.5f); +PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_VISION_P, 5.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_VISION_V, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); @@ -67,12 +67,12 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); - h->w_z_vision_p = param_find("INAV_W_Z_VISION_P"); + h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); - h->w_xy_vision_p = param_find("INAV_W_XY_VISION_P"); - h->w_xy_vision_v = param_find("INAV_W_XY_VISION_V"); + h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P"); + h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); From dfbbc6600e67618e030c366dca5b440dcd0ddcb7 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Tue, 22 Jul 2014 21:01:10 +0800 Subject: [PATCH 381/600] Seperate vision position estimate correction logic from gps. Add vision weight parameters. Fixes false positive of valid position indication --- .../position_estimator_inav_main.c | 103 +++++++++++++++--- 1 file changed, 87 insertions(+), 16 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 71abcc1704..398fddd692 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -235,6 +235,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float eph_flow = 1.0f; + float eph_vision = 0.5f; + float epv_vision = 0.5f; + float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); @@ -281,6 +284,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; + + float corr_vision[3][2] = { + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) + { 0.0f, 0.0f }, // D (pos, vel) + }; + float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; @@ -637,27 +647,33 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) x_est[1] = vision.vx; y_est[0] = vision.y; y_est[1] = vision.vy; - z_est[0] = vision.z; - z_est[1] = vision.vz; - + /* only reset the z estimate if the z weight parameter is not zero */ + if (params.w_z_vision_p > MIN_VALID_W) + { + z_est[0] = vision.z; + z_est[1] = vision.vz; + } + vision_valid = true; + warnx("VISION estimate valid"); + mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); } /* calculate correction for position */ - corr_gps[0][0] = vision.x - x_est[0]; - corr_gps[1][0] = vision.y - y_est[0]; - corr_gps[2][0] = vision.z - z_est[0]; + corr_vision[0][0] = vision.x - x_est[0]; + corr_vision[1][0] = vision.y - y_est[0]; + corr_vision[2][0] = vision.z - z_est[0]; /* calculate correction for velocity */ - corr_gps[0][1] = vision.vx - x_est[1]; - corr_gps[1][1] = vision.vy - y_est[1]; - corr_gps[2][1] = vision.vz - z_est[1]; + corr_vision[0][1] = vision.vx - x_est[1]; + corr_vision[1][1] = vision.vy - y_est[1]; + corr_vision[2][1] = vision.vz - z_est[1]; - eph = 0.05f; - epv = 0.05f; + // eph_flow = 0.05f; + // epv_vision = 0.05f; - w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, eph); - w_gps_z = min_eph_epv / fmaxf(min_eph_epv, epv); + // w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, eph); + // w_gps_z = min_eph_epv / fmaxf(min_eph_epv, epv); } } @@ -810,12 +826,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* use GPS if it's valid and reference position initialized */ - bool use_gps_xy = (ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W) || vision_valid; - bool use_gps_z = (ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W) || vision_valid; + bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; + bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; + /* use VISION if it's valid and has a valid weight parameter */ + bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W; + bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); - bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || vision_valid; + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); @@ -834,6 +853,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; + float w_xy_vision_p = params.w_xy_vision_p; + float w_xy_vision_v = params.w_xy_vision_v; + float w_z_vision_p = params.w_z_vision_p; + /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_xy_gps_p *= params.w_gps_flow; @@ -874,6 +897,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + /* accelerometer bias correction for VISION (use buffered rotation matrix) */ + accel_bias_corr[0] = 0.0f; + accel_bias_corr[1] = 0.0f; + accel_bias_corr[2] = 0.0f; + + if (use_vision_xy) { + accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p; + accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v; + accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p; + accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v; + } + + if (use_vision_z) { + accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; + } + + /* transform error vector from NED frame to body frame */ + for (int i = 0; i < 3; i++) { + float c = 0.0f; + + for (int j = 0; j < 3; j++) { + c += R_gps[j][i] * accel_bias_corr[j]; + } + + if (isfinite(c)) { + acc_bias[i] += c * params.w_acc_bias * dt; + } + } + /* accelerometer bias correction for flow and baro (assume that there is no delay) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; @@ -916,10 +968,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); } + if (use_vision_z) { + epv = fminf(epv, epv_vision); + inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); + } + if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); + memset(corr_vision, 0, sizeof(corr_vision)); corr_baro = 0; } else { @@ -957,11 +1015,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + if (use_vision_xy) { + eph = fminf(eph, eph_vision); + + inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p); + inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); + + if (w_xy_vision_v > MIN_VALID_W) { + inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_vision_v); + inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_vision_v); + } + } + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, 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_gps, 0, sizeof(corr_gps)); + memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_flow, 0, sizeof(corr_flow)); } else { From 1b5d4e5bd4fbee8f994c95d69be7660fd211d2f4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Jul 2014 17:42:34 +0200 Subject: [PATCH 382/600] Comment out uavcan due to build breakage, will go back in ASAP --- makefiles/config_px4fmu-v2_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a498a1b402..fea1bade33 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -74,7 +74,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -MODULES += modules/uavcan +#MODULES += modules/uavcan # # Estimation modules (EKF/ SO3 / other filters) From e6594e519295ec081c6f962fca6b4fbdd190846f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 22 Jul 2014 10:20:10 +0900 Subject: [PATCH 383/600] tone_alarm: add EKF_WARNING tune Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index b7981e9c4b..19f792d19c 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -149,6 +149,7 @@ enum { TONE_GPS_WARNING_TUNE, TONE_ARMING_FAILURE_TUNE, TONE_PARACHUTE_RELEASE_TUNE, + TONE_EKF_WARNING_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 810f4aacc4..03c7bd399f 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -336,6 +336,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<< Date: Sat, 19 Jul 2014 14:38:39 +0200 Subject: [PATCH 384/600] code style only: Fix indendation in mavlink.h --- src/modules/mavlink/mavlink_main.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index da989d6c51..adca0e88f2 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -250,13 +250,13 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; - MavlinkMissionManager *_mission_manager; + MavlinkMissionManager *_mission_manager; - orb_advert_t _mission_pub; + orb_advert_t _mission_pub; int _mission_result_sub; - MAVLINK_MODE _mode; + MAVLINK_MODE _mode; - mavlink_channel_t _channel; + mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; From 0c0b1a4c666671e964d295fe6aea64d7b6a98fc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:59:13 +0200 Subject: [PATCH 385/600] Print mavlink radio module rates --- src/modules/mavlink/mavlink_main.cpp | 51 ++++++++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 10 +++++ src/modules/mavlink/mavlink_receiver.cpp | 3 +- 3 files changed, 59 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 76b5459a3e..e25851c7d5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -247,6 +247,7 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _rstatus{}, _message_buffer{}, _message_buffer_mutex{}, _param_initialized(false), @@ -424,6 +425,29 @@ Mavlink::destroy_all_instances() return OK; } +int +Mavlink::get_status_all_instances() +{ + Mavlink *inst = ::_mavlink_instances; + + unsigned iterations = 0; + + warnx("waiting for instances to stop"); + + while (inst != nullptr) { + + printf("instance #%u:\n", iterations); + inst->display_status(); + + /* move on */ + inst = inst->next; + iterations++; + } + + /* return an error if there are no instances */ + return (iterations == 0); +} + bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { @@ -1395,7 +1419,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); configure_stream("ATTITUDE", 10.0f * rate_mult); - configure_stream("VFR_HUD", 10.0f * rate_mult); + configure_stream("VFR_HUD", 8.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); @@ -1665,6 +1689,27 @@ void Mavlink::display_status() { warnx("running"); + + if (_rstatus.timestamp > 0) { + printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + printf("\t3DR RADIO\n"); + break; + default: + printf("\tUNKNOWN RADIO\n"); + break; + } + + printf("\trssi:\t%d\t\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); + printf("\tnoise:\t%d\tus\n", _rstatus.noise); + printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); + printf("\tfixed:\t%u\tus\n", _rstatus.fixed); + } } int @@ -1752,8 +1797,8 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); - // } else if (!strcmp(argv[1], "status")) { - // mavlink::g_mavlink->status(); + } else if (!strcmp(argv[1], "status")) { + return Mavlink::get_status_all_instances(); } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream_command(argc, argv); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index adca0e88f2..d51120462a 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -51,6 +51,7 @@ #include #include #include +#include #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" @@ -97,6 +98,8 @@ public: static int destroy_all_instances(); + static int get_status_all_instances(); + static bool instance_exists(const char *device_name, Mavlink *self); static void forward_message(const mavlink_message_t *msg, Mavlink *self); @@ -229,6 +232,11 @@ public: */ void count_txerr(); + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + protected: Mavlink *next; @@ -285,6 +293,8 @@ private: bool _flow_control_enabled; + struct telemetry_status_s _rstatus; ///< receive status + struct mavlink_message_buffer { int write_ptr; int read_ptr; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ee7ec34da..528c8b72a3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -400,8 +400,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); tstatus.timestamp = hrt_absolute_time(); tstatus.heartbeat_time = _telemetry_heartbeat_time; From 91becef344833dbb19230ec3f3c69750bce216e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:56:37 +0200 Subject: [PATCH 386/600] Cleanup of heartbeat handling and status printing. Ready to go mainline --- src/modules/mavlink/mavlink_main.cpp | 32 ++++++++++++---------- src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++------ src/modules/mavlink/mavlink_receiver.h | 1 - src/modules/uORB/topics/telemetry_status.h | 3 +- 4 files changed, 32 insertions(+), 24 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e25851c7d5..0e8f17c77e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -432,11 +432,9 @@ Mavlink::get_status_all_instances() unsigned iterations = 0; - warnx("waiting for instances to stop"); - while (inst != nullptr) { - printf("instance #%u:\n", iterations); + printf("\ninstance #%u:\n", iterations); inst->display_status(); /* move on */ @@ -1688,27 +1686,33 @@ Mavlink::start(int argc, char *argv[]) void Mavlink::display_status() { - warnx("running"); + + if (_rstatus.heartbeat_time > 0) { + printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + } if (_rstatus.timestamp > 0) { - printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + printf("\ttype:\t\t"); switch (_rstatus.type) { case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: - printf("\t3DR RADIO\n"); + printf("3DR RADIO\n"); break; default: - printf("\tUNKNOWN RADIO\n"); + printf("UNKNOWN RADIO\n"); break; } - printf("\trssi:\t%d\t\n", _rstatus.rssi); - printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); - printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); - printf("\tnoise:\t%d\tus\n", _rstatus.noise); - printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); - printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); - printf("\tfixed:\t%u\tus\n", _rstatus.fixed); + printf("\trssi:\t\t%d\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf); + printf("\tnoise:\t\t%d\n", _rstatus.noise); + printf("\tremote noise:\t%u\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\n", _rstatus.rxerrors); + printf("\tfixed:\t\t%u\n", _rstatus.fixed); + } else { + printf("\tno telem status.\n"); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 528c8b72a3..454d730d83 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -111,7 +111,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _telemetry_heartbeat_time(0), _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), @@ -403,7 +402,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); tstatus.timestamp = hrt_absolute_time(); - tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.telem_time = tstatus.timestamp; + /* tstatus.heartbeat_time is set by system heartbeats */ tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -460,16 +460,20 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) /* ignore own heartbeats, accept only heartbeats from GCS */ if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - _telemetry_heartbeat_time = hrt_absolute_time(); + + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); + + hrt_abstime tnow = hrt_absolute_time(); + + /* always set heartbeat, publish only if telemetry link not up */ + tstatus.heartbeat_time = tnow; /* if no radio status messages arrive, lets at least publish that heartbeats were received */ if (!_radio_status_available) { - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = _telemetry_heartbeat_time; - tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.timestamp = tnow; + /* telem_time indicates the timestamp of a telemetry status packet and we got none */ + tstatus.telem_time = 0; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; if (_telemetry_status_pub < 0) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b64a060e3a..f4d0c52d88 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -148,7 +148,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - hrt_abstime _telemetry_heartbeat_time; bool _radio_status_available; int _control_mode_sub; int _hil_frames; diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 3347724a53..1297c1a9d3 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -57,7 +57,8 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; - uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ + uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ + uint64_t telem_time; /**< Time of last received telemetry status packet, 0 for none */ enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ uint8_t rssi; /**< local signal strength */ uint8_t remote_rssi; /**< remote signal strength */ From 63f1ee184a63ecabd93ce0cfa91afe97fe7e65d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:57:42 +0200 Subject: [PATCH 387/600] Add hint that heartbeats are only considered if coming from a GCS --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0e8f17c77e..7bd2b9754b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1688,7 +1688,7 @@ Mavlink::display_status() { if (_rstatus.heartbeat_time > 0) { - printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } if (_rstatus.timestamp > 0) { From b344b94d3039273a1659a4b5640e7dc197922767 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 16:00:37 +0200 Subject: [PATCH 388/600] Fix code style, no actual code edits --- src/modules/mavlink/mavlink_main.cpp | 166 +++++++++++++++------------ 1 file changed, 92 insertions(+), 74 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7bd2b9754b..cb8c298c04 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -81,7 +81,7 @@ #include "mavlink_commands.h" #ifndef MAVLINK_CRC_EXTRA - #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems #endif /* oddly, ERROR is not defined for c++ */ @@ -154,7 +154,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length instance = Mavlink::get_instance(6); break; #endif - default: + + default: return; } @@ -169,17 +170,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { /* Disable hardware flow control: * if no successful write since a defined time * and if the last try was not the last successful write */ if (last_write_try_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && - last_write_success_times[(unsigned)channel] != - last_write_try_times[(unsigned)channel]) - { + hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && + last_write_success_times[(unsigned)channel] != + last_write_try_times[(unsigned)channel]) { warnx("DISABLING HARDWARE FLOW CONTROL"); instance->enable_flow_control(false); } @@ -202,8 +202,10 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } ssize_t ret = write(uart, ch, desired); + if (ret != desired) { instance->count_txerr(); + } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } @@ -232,33 +234,33 @@ Mavlink::Mavlink() : _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), - _logbuffer{}, - _total_counter(0), - _receive_thread{}, - _verbose(false), - _forwarding_on(false), - _passing_on(false), - _ftp_on(false), - _uart_fd(-1), - _baudrate(57600), - _datarate(10000), - _mavlink_param_queue_index(0), - mavlink_link_termination_allowed(false), - _subscribe_to_stream(nullptr), - _subscribe_to_stream_rate(0.0f), - _flow_control_enabled(true), - _rstatus{}, - _message_buffer{}, - _message_buffer_mutex{}, - _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_system_type(0), - _param_use_hil_gps(0), + _logbuffer {}, + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(10000), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), - _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; @@ -637,7 +639,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * case 921600: speed = B921600; break; default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", + baud); return -EINVAL; } @@ -867,8 +870,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { mavlink_param_request_list_t req; mavlink_msg_param_request_list_decode(msg, &req); + if (req.target_system == mavlink_system.sysid && - (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { + (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); send_statustext_info("[pm] sending list"); @@ -883,7 +887,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav mavlink_param_set_t mavlink_param_set; mavlink_msg_param_set_decode(msg, &mavlink_param_set); - if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { + if (mavlink_param_set.target_system == mavlink_system.sysid + && ((mavlink_param_set.target_component == mavlink_system.compid) + || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); @@ -910,7 +916,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav mavlink_param_request_read_t mavlink_param_request_read; mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { + if (mavlink_param_request_read.target_system == mavlink_system.sysid + && ((mavlink_param_request_read.target_component == mavlink_system.compid) + || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { /* when no index is given, loop through string ids and compare them */ if (mavlink_param_request_read.param_index == -1) { /* local name buffer to enforce null-terminated string */ @@ -973,15 +981,17 @@ Mavlink::send_statustext(unsigned severity, const char *string) /* Map severity */ switch (severity) { - case MAVLINK_IOC_SEND_TEXT_INFO: - statustext.severity = MAV_SEVERITY_INFO; - break; - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - statustext.severity = MAV_SEVERITY_CRITICAL; - break; - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - statustext.severity = MAV_SEVERITY_EMERGENCY; - break; + case MAVLINK_IOC_SEND_TEXT_INFO: + statustext.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + statustext.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + statustext.severity = MAV_SEVERITY_EMERGENCY; + break; } mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); @@ -1098,12 +1108,14 @@ Mavlink::message_buffer_init(int size) _message_buffer.size = size; _message_buffer.write_ptr = 0; _message_buffer.read_ptr = 0; - _message_buffer.data = (char*)malloc(_message_buffer.size); + _message_buffer.data = (char *)malloc(_message_buffer.size); int ret; + if (_message_buffer.data == 0) { ret = ERROR; _message_buffer.size = 0; + } else { ret = OK; } @@ -1475,7 +1487,8 @@ Mavlink::task_main(int argc, char *argv[]) if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate); + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, + (double)_subscribe_to_stream_rate); } else { warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); @@ -1514,45 +1527,46 @@ Mavlink::task_main(int argc, char *argv[]) bool is_part; uint8_t *read_ptr; - uint8_t *write_ptr; + uint8_t *write_ptr; pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); + int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { - // Reconstruct message from buffer + // Reconstruct message from buffer mavlink_message_t msg; - write_ptr = (uint8_t*)&msg; + write_ptr = (uint8_t *)&msg; + + // Pull a single message from the buffer + size_t read_count = available; + + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); - // Pull a single message from the buffer - size_t read_count = available; - if (read_count > sizeof(mavlink_message_t)) { - read_count = sizeof(mavlink_message_t); - } - - memcpy(write_ptr, read_ptr, read_count); - - // We hold the mutex until after we complete the second part of the buffer. If we don't - // we may end up breaking the empty slot overflow detection semantics when we mark the - // possibly partial read below. - pthread_mutex_lock(&_message_buffer_mutex); - message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ if (is_part && read_count < sizeof(mavlink_message_t)) { - write_ptr += read_count; - available = message_buffer_get_ptr((void**)&read_ptr, &is_part); - read_count = sizeof(mavlink_message_t) - read_count; - memcpy(write_ptr, read_ptr, read_count); + write_ptr += read_count; + available = message_buffer_get_ptr((void **)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } - - pthread_mutex_unlock(&_message_buffer_mutex); - _mavlink_resend_uart(_channel, &msg); + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, &msg); } } @@ -1606,6 +1620,7 @@ Mavlink::task_main(int argc, char *argv[]) message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } + /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -1627,6 +1642,7 @@ int Mavlink::start_helper(int argc, char *argv[]) /* out of memory */ res = -ENOMEM; warnx("OUT OF MEM"); + } else { /* this will actually only return once MAVLink exits */ res = instance->task_main(argc, argv); @@ -1696,10 +1712,11 @@ Mavlink::display_status() printf("\ttype:\t\t"); switch (_rstatus.type) { - case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: printf("3DR RADIO\n"); break; - default: + + default: printf("UNKNOWN RADIO\n"); break; } @@ -1711,6 +1728,7 @@ Mavlink::display_status() printf("\tremote noise:\t%u\n", _rstatus.remote_noise); printf("\trx errors:\t%u\n", _rstatus.rxerrors); printf("\tfixed:\t\t%u\n", _rstatus.fixed); + } else { printf("\tno telem status.\n"); } From ef363edfcd810e3b153609f0a723f53db0cd3885 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 19 Jul 2014 17:37:13 +0200 Subject: [PATCH 389/600] mavlink: TX/RX rate counters added --- src/modules/mavlink/mavlink_main.cpp | 28 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 23 +++++++++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 3 +++ 3 files changed, 54 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cb8c298c04..0c6f8c42f8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -197,6 +197,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (buf_free < desired) { /* we don't want to send anything just in half, so return */ instance->count_txerr(); + instance->count_txerrbytes(desired); return; } } @@ -205,9 +206,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (ret != desired) { instance->count_txerr(); + instance->count_txerrbytes(desired); } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; + instance->count_txbytes(desired); } } } @@ -249,6 +252,13 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), _rstatus {}, _message_buffer {}, _message_buffer_mutex {}, @@ -1570,6 +1580,20 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* update TX/RX rates*/ + if (t > _bytes_timestamp + 1000000) { + if (_bytes_timestamp != 0) { + float dt = (t - _bytes_timestamp) / 1000.0f; + _rate_tx = _bytes_tx / dt; + _rate_txerr = _bytes_txerr / dt; + _rate_rx = _bytes_rx / dt; + _bytes_tx = 0; + _bytes_txerr = 0; + _bytes_rx = 0; + } + _bytes_timestamp = t; + } + perf_end(_loop_perf); } @@ -1732,6 +1756,10 @@ Mavlink::display_status() } else { printf("\tno telem status.\n"); } + printf("\trates:\n"); + printf("\ttx: %.3f kB/s\n", (double)_rate_tx); + printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); + printf("\trx: %.3f kB/s\n", (double)_rate_rx); } int diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index d51120462a..70d13acb09 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -232,6 +232,21 @@ public: */ void count_txerr(); + /** + * Count transmitted bytes + */ + void count_txbytes(unsigned n) { _bytes_tx += n; }; + + /** + * Count bytes not transmitted because of errors + */ + void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; + + /** + * Count received bytes + */ + void count_rxbytes(unsigned n) { _bytes_rx += n; }; + /** * Get the receive status of this MAVLink link */ @@ -293,6 +308,14 @@ private: bool _flow_control_enabled; + unsigned _bytes_tx; + unsigned _bytes_txerr; + unsigned _bytes_rx; + uint64_t _bytes_timestamp; + float _rate_tx; + float _rate_txerr; + float _rate_rx; + struct telemetry_status_s _rstatus; ///< receive status struct mavlink_message_buffer { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 454d730d83..54c412ce7c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -958,6 +958,9 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->handle_message(&msg); } } + + /* count received bytes */ + _mavlink->count_rxbytes(nread); } } From a48bede96f79c626e17584f75865ef7839744bf0 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Wed, 23 Jul 2014 14:24:51 +0800 Subject: [PATCH 390/600] Remove unused commented code --- .../position_estimator_inav/position_estimator_inav_main.c | 6 ------ 1 file changed, 6 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 398fddd692..77e94a27c4 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -669,12 +669,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_vision[1][1] = vision.vy - y_est[1]; corr_vision[2][1] = vision.vz - z_est[1]; - // eph_flow = 0.05f; - // epv_vision = 0.05f; - - // w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, eph); - // w_gps_z = min_eph_epv / fmaxf(min_eph_epv, epv); - } } From 7f0e67522878d1e850c22553fc53e93283ed92f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jul 2014 08:24:56 +0200 Subject: [PATCH 391/600] Pass over drivers to reduce build size --- src/drivers/airspeed/module.mk | 2 ++ src/drivers/bma180/module.mk | 2 ++ src/drivers/ets_airspeed/module.mk | 2 ++ src/drivers/hil/module.mk | 2 ++ src/drivers/led/module.mk | 2 ++ src/drivers/md25/module.mk | 2 ++ src/drivers/meas_airspeed/module.mk | 2 ++ src/drivers/ms5611/module.mk | 2 ++ src/drivers/px4flow/module.mk | 2 ++ src/drivers/roboclaw/module.mk | 2 ++ 10 files changed, 20 insertions(+) diff --git a/src/drivers/airspeed/module.mk b/src/drivers/airspeed/module.mk index 4eef061610..5fbc75309c 100644 --- a/src/drivers/airspeed/module.mk +++ b/src/drivers/airspeed/module.mk @@ -36,3 +36,5 @@ # SRCS = airspeed.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/bma180/module.mk b/src/drivers/bma180/module.mk index 4c60ee082c..33433307a6 100644 --- a/src/drivers/bma180/module.mk +++ b/src/drivers/bma180/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = bma180 SRCS = bma180.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk index 966a5b8190..8aaaf0ebb0 100644 --- a/src/drivers/ets_airspeed/module.mk +++ b/src/drivers/ets_airspeed/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = ets_airspeed SRCS = ets_airspeed.cpp MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk index f8895f5d5a..f1fc49fb3d 100644 --- a/src/drivers/hil/module.mk +++ b/src/drivers/hil/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = hil SRCS = hil.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/led/module.mk b/src/drivers/led/module.mk index 777f3e4425..5b7b4491b7 100644 --- a/src/drivers/led/module.mk +++ b/src/drivers/led/module.mk @@ -36,3 +36,5 @@ # SRCS = led.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/md25/module.mk b/src/drivers/md25/module.mk index 13821a6b58..3f9cf2d891 100644 --- a/src/drivers/md25/module.mk +++ b/src/drivers/md25/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = md25 SRCS = md25.cpp \ md25_main.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/meas_airspeed/module.mk b/src/drivers/meas_airspeed/module.mk index 2a15b669f4..6f5909978b 100644 --- a/src/drivers/meas_airspeed/module.mk +++ b/src/drivers/meas_airspeed/module.mk @@ -42,3 +42,5 @@ SRCS = meas_airspeed.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk index 20f8aa1737..ee74058fc1 100644 --- a/src/drivers/ms5611/module.mk +++ b/src/drivers/ms5611/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = ms5611 SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk index d3062e4579..460bec7b96 100644 --- a/src/drivers/px4flow/module.mk +++ b/src/drivers/px4flow/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = px4flow SRCS = px4flow.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk index 1abecf198b..c5e55bdc30 100644 --- a/src/drivers/roboclaw/module.mk +++ b/src/drivers/roboclaw/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = roboclaw SRCS = roboclaw_main.cpp \ RoboClaw.cpp + +MAXOPTIMIZATION = -Os From a7d2963e2bca060493f787cf637b0e1b0d9d829e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jul 2014 08:27:45 +0200 Subject: [PATCH 392/600] Enable UAVCAN --- makefiles/config_px4fmu-v2_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index fea1bade33..a498a1b402 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -74,7 +74,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -#MODULES += modules/uavcan +MODULES += modules/uavcan # # Estimation modules (EKF/ SO3 / other filters) From 38d3efa985be394c846e6afe294006ab074335f7 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Wed, 23 Jul 2014 15:12:26 +0800 Subject: [PATCH 393/600] Correct vision velocity estimate correction type --- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 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 77e94a27c4..d3192fec21 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1016,8 +1016,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); if (w_xy_vision_v > MIN_VALID_W) { - inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_vision_v); - inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_vision_v); + inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v); + inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v); } } From 4722b609ccfa35c438c1ef74caba2793c9e04225 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 11:03:04 +0200 Subject: [PATCH 394/600] mavlink: parameters handling moved to MavlinkParametersManager class --- src/modules/mavlink/mavlink_parameters.cpp | 197 +++++++++++++++++++++ src/modules/mavlink/mavlink_parameters.h | 114 ++++++++++++ 2 files changed, 311 insertions(+) create mode 100644 src/modules/mavlink/mavlink_parameters.cpp create mode 100644 src/modules/mavlink/mavlink_parameters.h diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp new file mode 100644 index 0000000000..b1dae918c4 --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -0,0 +1,197 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_parameters.cpp + * Mavlink parameters manager implementation. + * + * @author Anton Babushkin + */ + +//#include + +#include "mavlink_parameters.h" + +explicit +MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), + _send_all_index(-1) +{ +} + +unsigned +MavlinkParametersManager::get_size() +{ + if (_send_all_index() >= 0) { + return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + +void +MavlinkParametersManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + /* request all parameters */ + mavlink_param_request_list_t req_list; + mavlink_msg_param_request_list_decode(msg, &req_list); + + if (req_list.target_system == mavlink_system.sysid && + (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + + _send_all_index = 0; + _mavlink->send_statustext_info("[pm] sending list"); + } + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: { + /* set parameter */ + if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); + + if (set.target_system == mavlink_system.sysid && + (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter, set and send it */ + param_t param = param_find(name); + + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param: %s", name); + _mavlink->send_statustext_info(buf); + + } else { + /* set and send parameter */ + param_set(param, &(set.param_value)); + send_param(param); + } + } + } + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + /* request one parameter */ + mavlink_param_request_read_t req_read; + mavlink_msg_param_request_read_decode(msg, &req_read); + + if (req_read.target_system == mavlink_system.sysid && + (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + + /* when no index is given, loop through string ids and compare them */ + if (req_read.param_index < 0) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter and send it */ + send_param(param_find(name)); + + } else { + /* when index is >= 0, send this parameter again */ + send_param(param_for_index(req_read.param_index)); + } + } + break; + } + + default: + break; + } +} + +void +MavlinkParametersManager::send(const hrt_abstime t) +{ + /* send all parameters if requested */ + if (_send_all_index >= 0) { + send_param(param_for_index(_send_all_index)); + _send_all_index++; + if (_send_all_index >= param_count()) { + _send_all_index = -1; + } + } +} + +void +MavlinkParametersManager::send_param(param_t param) +{ + if (param == PARAM_INVALID) { + return; + } + + mavlink_param_value_t msg; + + /* + * get param value, since MAVLink encodes float and int params in the same + * space during transmission, copy param onto float val_buf + */ + if (param_get(param, &msg.param_value) != OK) { + return; + } + + msg.param_count = param_count(); + msg.param_index = param_get_index(param); + + /* copy parameter name */ + strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + + /* query parameter type */ + param_type_t type = param_type(param); + + /* + * Map onboard parameter type to MAVLink type, + * endianess matches (both little endian) + */ + if (type == PARAM_TYPE_INT32) { + msg.param_type = MAVLINK_TYPE_INT32_T; + + } else if (type == PARAM_TYPE_FLOAT) { + msg.param_type = MAVLINK_TYPE_FLOAT; + + } else { + msg.param_type = MAVLINK_TYPE_FLOAT; + } + + _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); +} diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h new file mode 100644 index 0000000000..9b7a04a733 --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.h @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_parameters.h + * Mavlink parameters manager definition. + * + * @author Anton Babushkin + */ + +#pragma once + +#include "mavlink_stream.h" + +class MavlinkParametersManager : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkParametersManager::get_name_static(); + } + + static const char *get_name_static() + { + return "PARAM_VALUE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_PARAM_VALUE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkParametersManager(mavlink); + } + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + + /** + * Send one parameter identified by index. + * + * @param index The index of the parameter to send. + * @return zero on success, nonzero else. + */ + void start_send_one(int index); + + + /** + * Send one parameter identified by name. + * + * @param name The index of the parameter to send. + * @return zero on success, nonzero else. + */ + int start_send_for_name(const char *name); + + /** + * Start sending the parameter queue. + * + * This function will not directly send parameters, but instead + * activate the sending of one parameter on each call of + * mavlink_pm_queued_send(). + * @see mavlink_pm_queued_send() + */ + void start_send_all(); + +private: + int _send_all_index; + + /* do not allow top copying this class */ + MavlinkParametersManager(MavlinkParametersManager &); + MavlinkParametersManager& operator = (const MavlinkParametersManager &); + +protected: + explicit MavlinkParametersManager(Mavlink *mavlink); + + void subscribe() {} + + void send(const hrt_abstime t); + + void send_param(param_t param); +}; From 344a34bb725a65769a227157f41363fd5d180a14 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 11:03:37 +0200 Subject: [PATCH 395/600] mavlink: MavlinkStream API updated --- src/modules/mavlink/mavlink_stream.cpp | 5 +++-- src/modules/mavlink/mavlink_stream.h | 13 ++++++++++--- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 79dc23cc68..da3a551725 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -71,11 +71,12 @@ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; + unsigned int interval = _interval * _mavlink->get_rate_mult(); - if (dt > 0 && dt >= _interval) { + if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); - _last_sent = (t / _interval) * _interval; + _last_sent = t; return 0; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 7d3afbbb59..646931c07b 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -46,8 +46,6 @@ class Mavlink; class MavlinkStream; -#include "mavlink_main.h" - class MavlinkStream { @@ -70,7 +68,6 @@ public: * @return the inveral in microseconds (us) between messages */ unsigned get_interval() { return _interval; } - void set_channel(mavlink_channel_t channel); /** * @return 0 if updated / sent, -1 if unchanged @@ -82,6 +79,16 @@ public: virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; + /** + * @return true if steam rate shouldn't be adjusted + */ + virtual bool const_rate() { return false; } + + /** + * Get maximal total messages size on update + */ + virtual unsigned get_size() = 0; + protected: Mavlink * _mavlink; unsigned int _interval; From a5f2d1b06645d4db33751aa8392fcbaf7f0856cc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 11:11:49 +0200 Subject: [PATCH 396/600] mavlink: new message sending API, includes fixed --- src/modules/mavlink/mavlink_ftp.h | 1 + src/modules/mavlink/mavlink_main.cpp | 308 +- src/modules/mavlink/mavlink_main.h | 58 +- src/modules/mavlink/mavlink_messages.cpp | 3186 ++++++++++---------- src/modules/mavlink/mavlink_messages.h | 4 +- src/modules/mavlink/mavlink_mission.cpp | 26 +- src/modules/mavlink/mavlink_parameters.cpp | 6 +- src/modules/mavlink/mavlink_parameters.h | 3 + src/modules/mavlink/module.mk | 1 + 9 files changed, 1716 insertions(+), 1877 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 59237a4c4e..1bd1158fba 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -55,6 +55,7 @@ #include #include "mavlink_messages.h" +#include "mavlink_main.h" class MavlinkFTP { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e3c452fb2c..dca76c950f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -102,6 +102,7 @@ static Mavlink *_mavlink_instances = nullptr; static struct file_operations fops; static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; /** * mavlink app start / stop handling function @@ -130,6 +131,7 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_manager(nullptr), + _parameters_manager(nullptr), _mission_pub(-1), _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), @@ -145,6 +147,7 @@ Mavlink::Mavlink() : _baudrate(57600), _datarate(1000), _datarate_events(500), + _rate_mult(0.0f), _mavlink_param_queue_index(0), mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), @@ -687,18 +690,23 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -bool -Mavlink::check_can_send(const int prio, const unsigned packet_len) +void +Mavlink::send_message(const uint8_t msgid, const void *msg) { + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + /* * Check if the OS buffer is full and disable HW * flow control if it continues to be full */ int buf_free = 0; + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); - if (get_flow_control_enabled() - && ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free) == 0) { - + if (get_flow_control_enabled() && buf_free == 0) { /* Disable hardware flow control: * if no successful write since a defined time * and if the last try was not the last successful write @@ -711,36 +719,22 @@ Mavlink::check_can_send(const int prio, const unsigned packet_len) } } - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (should_transmit()) { - _last_write_try_time = hrt_absolute_time(); - - /* check if there is space in the buffer, let it overflow else */ - ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); - - if (buf_free >= (prio <= 0 ? packet_len : TX_BUFFER_GAP)) { - warnx("\t\t\tSKIP %i bytes prio %i", packet_len, (int)prio); - /* we don't want to send anything just in half, so return */ - count_txerr(); - count_txerrbytes(packet_len); - return true; - } - } - return false; -} - -void -Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio) -{ - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - if (!check_can_send(prio, packet_len)) { + + _last_write_try_time = hrt_absolute_time(); + + /* check if there is space in the buffer, let it overflow else */ + if (buf_free >= TX_BUFFER_GAP) { + warnx("SKIP msgid %i, %i bytes", msgid, packet_len); + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); return; } + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + /* header */ buf[0] = MAVLINK_STX; buf[1] = payload_len; @@ -755,9 +749,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio) /* checksum */ uint16_t checksum; crc_init(&checksum); - crc_accumulate_buffer(&checksum, &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); #if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); + crc_accumulate(mavlink_message_crcs[msgid], &checksum); #endif buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); @@ -766,7 +760,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio) /* send message to UART */ ssize_t ret = write(_uart_fd, buf, packet_len); - if (ret != packet_len) { + if (ret != (int) packet_len) { count_txerr(); count_txerrbytes(packet_len); @@ -783,7 +777,7 @@ Mavlink::handle_message(const mavlink_message_t *msg) _mission_manager->handle_message(msg); /* handle packet with parameter component */ - mavlink_pm_message_handler(_channel, msg); + _parameters_manager->handle_message(msg); if (get_forwarding_on()) { /* forward any messages to other mavlink instances */ @@ -791,163 +785,6 @@ Mavlink::handle_message(const mavlink_message_t *msg) } } -int -Mavlink::mavlink_pm_queued_send() -{ - if (_mavlink_param_queue_index < param_count()) { - mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); - _mavlink_param_queue_index++; - return 0; - - } else { - return 1; - } -} - -void Mavlink::mavlink_pm_start_queued_send() -{ - _mavlink_param_queue_index = 0; -} - -int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) -{ - return mavlink_pm_send_param(param_for_index(index)); -} - -int Mavlink::mavlink_pm_send_param_for_name(const char *name) -{ - return mavlink_pm_send_param(param_find(name)); -} - -int Mavlink::mavlink_pm_send_param(param_t param) -{ - if (param == PARAM_INVALID) { return 1; } - - /* buffers for param transmission */ - char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; - float val_buf; - mavlink_message_t tx_msg; - - /* query parameter type */ - param_type_t type = param_type(param); - /* copy parameter name */ - strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - - /* - * Map onboard parameter type to MAVLink type, - * endianess matches (both little endian) - */ - uint8_t mavlink_type; - - if (type == PARAM_TYPE_INT32) { - mavlink_type = MAVLINK_TYPE_INT32_T; - - } else if (type == PARAM_TYPE_FLOAT) { - mavlink_type = MAVLINK_TYPE_FLOAT; - - } else { - mavlink_type = MAVLINK_TYPE_FLOAT; - } - - /* - * get param value, since MAVLink encodes float and int params in the same - * space during transmission, copy param onto float val_buf - */ - - int ret; - - if ((ret = param_get(param, &val_buf)) != OK) { - return ret; - } - - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, - mavlink_system.compid, - _channel, - &tx_msg, - name_buf, - val_buf, - mavlink_type, - param_count(), - param_get_index(param)); - send_message(&tx_msg); - return OK; -} - -void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) -{ - switch (msg->msgid) { - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - mavlink_param_request_list_t req; - mavlink_msg_param_request_list_decode(msg, &req); - - if (req.target_system == mavlink_system.sysid && - (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { - /* Start sending parameters */ - mavlink_pm_start_queued_send(); - send_statustext_info("[pm] sending list"); - } - } break; - - case MAVLINK_MSG_ID_PARAM_SET: { - - /* Handle parameter setting */ - - if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { - mavlink_param_set_t mavlink_param_set; - mavlink_msg_param_set_decode(msg, &mavlink_param_set); - - if (mavlink_param_set.target_system == mavlink_system.sysid - && ((mavlink_param_set.target_component == mavlink_system.compid) - || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter, set and send it */ - param_t param = param_find(name); - - if (param == PARAM_INVALID) { - char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[pm] unknown: %s", name); - send_statustext_info(buf); - - } else { - /* set and send parameter */ - param_set(param, &(mavlink_param_set.param_value)); - mavlink_pm_send_param(param); - } - } - } - } break; - - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { - mavlink_param_request_read_t mavlink_param_request_read; - mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - - if (mavlink_param_request_read.target_system == mavlink_system.sysid - && ((mavlink_param_request_read.target_component == mavlink_system.compid) - || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { - /* when no index is given, loop through string ids and compare them */ - if (mavlink_param_request_read.param_index == -1) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter and send it */ - mavlink_pm_send_param_for_name(name); - - } else { - /* when index is >= 0, send this parameter again */ - mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); - } - } - - } break; - } -} - int Mavlink::send_statustext_info(const char *string) { @@ -1031,11 +868,17 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) return sub_new; } +unsigned int +Mavlink::interval_from_rate(float rate) +{ + return (rate > 0.0f) ? (1000000.0f / rate) : 0; +} + int Mavlink::configure_stream(const char *stream_name, const float rate) { /* calculate interval in us, 0 means disabled stream */ - unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0; + unsigned int interval = interval_from_rate(rate); /* search if stream exists */ MavlinkStream *stream; @@ -1066,10 +909,9 @@ Mavlink::configure_stream(const char *stream_name, const float rate) if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { /* create new instance */ - stream = streams_list[i]->new_instance(); - stream->set_channel(get_channel()); + stream = streams_list[i]->new_instance(this); stream->set_interval(interval); - stream->subscribe(this); + stream->subscribe(); LL_APPEND(_streams, stream); return OK; @@ -1267,7 +1109,26 @@ Mavlink::pass_message(const mavlink_message_t *msg) float Mavlink::get_rate_mult() { - return _datarate / 1000.0f; + return _rate_mult; +} + +void +Mavlink::update_rate_mult() +{ + float const_rate = 0.0f; + float rate = 0.0f; + + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + if (stream->const_rate()) { + const_rate += stream->get_size() * 1000000.0f / stream->get_interval(); + + } else { + rate += stream->get_size() * 1000000.0f / stream->get_interval(); + } + } + + _rate_mult = ((float)_datarate - const_rate) / (rate - const_rate); } int @@ -1390,6 +1251,8 @@ Mavlink::task_main(int argc, char *argv[]) break; } + update_rate_mult(); + warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ @@ -1452,32 +1315,37 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkCommandsStream commands_stream(this, _channel); - /* add default streams depending on mode and intervals depending on datarate */ - float rate_mult = get_rate_mult(); + /* add default streams depending on mode */ + /* HEARTBEAT is constant rate stream, rate never adjusted */ configure_stream("HEARTBEAT", 1.0f); + /* PARAM_VALUE stream */ + _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); + _parameters_manager->set_interval(interval_from_rate(30.0f)); + LL_APPEND(_streams, _parameters_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); - configure_stream("HIGHRES_IMU", 1.0f * rate_mult); - configure_stream("ATTITUDE", 10.0f * rate_mult); - configure_stream("VFR_HUD", 8.0f * rate_mult); - configure_stream("GPS_RAW_INT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); - configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); - configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); - configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); - configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); + configure_stream("HIGHRES_IMU", 1.0f); + configure_stream("ATTITUDE", 10.0f); + configure_stream("VFR_HUD", 8.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 3.0f); + configure_stream("LOCAL_POSITION_NED", 3.0f); + configure_stream("RC_CHANNELS_RAW", 1.0f); + configure_stream("NAMED_VALUE_FLOAT", 1.0f); + configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); + configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); break; case MAVLINK_MODE_CAMERA: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult); + configure_stream("ATTITUDE", 15.0f); + configure_stream("GLOBAL_POSITION_INT", 15.0f); configure_stream("CAMERA_CAPTURE", 1.0f); break; @@ -1488,10 +1356,12 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); + float base_rate_mult = _datarate / 1000.0f; + + MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; + _main_loop_delay = MAIN_LOOP_DELAY / base_rate_mult; /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); @@ -1543,18 +1413,9 @@ Mavlink::task_main(int argc, char *argv[]) } if (fast_rate_limiter.check(t)) { + _mission_manager->eventloop(); - /* only send messages if they fit the buffer */ - if (check_can_send(0, MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { - mavlink_pm_queued_send(); - } - - if (check_can_send(0, MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { - _mission_manager->eventloop(); - } - - if (!mavlink_logbuffer_is_empty(&_logbuffer) && - check_can_send(0, MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); @@ -1630,6 +1491,7 @@ Mavlink::task_main(int argc, char *argv[]) } delete _mission_manager; + delete _parameters_manager; delete _subscribe_to_stream; _subscribe_to_stream = nullptr; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1c62b03d21..7fcbae72e8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -58,7 +58,7 @@ #include "mavlink_stream.h" #include "mavlink_messages.h" #include "mavlink_mission.h" - +#include "mavlink_parameters.h" class Mavlink { @@ -160,11 +160,7 @@ public: */ int set_hil_enabled(bool hil_enabled); - bool check_can_send(const int prio, const unsigned packet_len); - - void send_message(const uint8_t msgid, const void *msg, const int prio); - - void send_message(const mavlink_message_t *msg); + void send_message(const uint8_t msgid, const void *msg); void handle_message(const mavlink_message_t *msg); @@ -285,6 +281,7 @@ private: MavlinkStream *_streams; MavlinkMissionManager *_mission_manager; + MavlinkParametersManager *_parameters_manager; orb_advert_t _mission_pub; int _mission_result_sub; @@ -305,6 +302,7 @@ private: int _baudrate; int _datarate; ///< data rate for normal streams (attitude, position, etc.) int _datarate_events; ///< data rate for params, waypoints, text messages + float _rate_mult; /** * If the queue index is not at 0, the queue sending @@ -352,51 +350,12 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ - /** - * Send one parameter. - * - * @param param The parameter id to send. - * @return zero on success, nonzero on failure. - */ - int mavlink_pm_send_param(param_t param); - - /** - * Send one parameter identified by index. - * - * @param index The index of the parameter to send. - * @return zero on success, nonzero else. - */ - int mavlink_pm_send_param_for_index(uint16_t index); - - /** - * Send one parameter identified by name. - * - * @param name The index of the parameter to send. - * @return zero on success, nonzero else. - */ - int mavlink_pm_send_param_for_name(const char *name); - - /** - * Send a queue of parameters, one parameter per function call. - * - * @return zero on success, nonzero on failure - */ - int mavlink_pm_queued_send(void); - - /** - * Start sending the parameter queue. - * - * This function will not directly send parameters, but instead - * activate the sending of one parameter on each call of - * mavlink_pm_queued_send(). - * @see mavlink_pm_queued_send() - */ - void mavlink_pm_start_queued_send(); - void mavlink_update_system(); int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + static unsigned int interval_from_rate(float rate); + int configure_stream(const char *stream_name, const float rate); /** @@ -420,6 +379,11 @@ private: void pass_message(const mavlink_message_t *msg); + /** + * Update rate mult so total bitrate will be equal to _datarate. + */ + void update_rate_mult(); + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fe33985d20..7dc3ebac1d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -76,7 +76,7 @@ #include #include "mavlink_messages.h" - +#include "mavlink_main.h" static uint16_t cm_uint16_from_m_float(float m); static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, @@ -254,6 +254,15 @@ public: return new MavlinkStreamHeartbeat(mavlink); } + unsigned get_size() + { + return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + bool const_rate() { + return true; + } + private: MavlinkOrbSubscription *status_sub; MavlinkOrbSubscription *pos_sp_triplet_sub; @@ -291,6 +300,7 @@ protected: } mavlink_heartbeat_t msg; + msg.base_mode = 0; msg.custom_mode = 0; get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); @@ -298,7 +308,7 @@ protected: msg.autopilot = mavlink_system.type; msg.mavlink_version = 3; - _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 1); + _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg); } }; @@ -311,7 +321,7 @@ public: return MavlinkStreamSysStatus::get_name_static(); } - static const char *get_name_static () + static const char *get_name_static() { return "SYS_STATUS"; } @@ -321,9 +331,14 @@ public: return MAVLINK_MSG_ID_SYS_STATUS; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamSysStatus(); + return new MavlinkStreamSysStatus(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: @@ -334,13 +349,13 @@ private: MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); protected: - explicit MavlinkStreamSysStatus() : MavlinkStream(), + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), status_sub(nullptr) {} - void subscribe(Mavlink *mavlink) + void subscribe() { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); } void send(const hrt_abstime t) @@ -348,1591 +363,1594 @@ protected: struct vehicle_status_s status; if (status_sub->update(&status)) { - mavlink_msg_sys_status_send(_channel, - status.onboard_control_sensors_present, - status.onboard_control_sensors_enabled, - status.onboard_control_sensors_health, - status.load * 1000.0f, - status.battery_voltage * 1000.0f, - status.battery_current * 100.0f, - status.battery_remaining * 100.0f, - status.drop_rate_comm, - status.errors_comm, - status.errors_count1, - status.errors_count2, - status.errors_count3, - status.errors_count4); + mavlink_sys_status_t msg; + + msg.onboard_control_sensors_present = status.onboard_control_sensors_present; + msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; + msg.onboard_control_sensors_health = status.onboard_control_sensors_health; + msg.load = status.load * 1000.0f; + msg.voltage_battery = status.battery_voltage * 1000.0f; + msg.current_battery = status.battery_current * 100.0f; + msg.drop_rate_comm = status.drop_rate_comm; + msg.errors_comm = status.errors_comm; + msg.errors_count1 = status.errors_count1; + msg.errors_count2 = status.errors_count2; + msg.errors_count3 = status.errors_count3; + msg.errors_count4 = status.errors_count4; + msg.battery_remaining = status.battery_remaining * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); } } }; -class MavlinkStreamHighresIMU : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamHighresIMU::get_name_static(); - } - - static const char *get_name_static() - { - return "HIGHRES_IMU"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_HIGHRES_IMU; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamHighresIMU(); - } - -private: - MavlinkOrbSubscription *sensor_sub; - uint64_t sensor_time; - - uint64_t accel_timestamp; - uint64_t gyro_timestamp; - uint64_t mag_timestamp; - uint64_t baro_timestamp; - - /* do not allow top copying this class */ - MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); - MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); - -protected: - explicit MavlinkStreamHighresIMU() : MavlinkStream(), - sensor_sub(nullptr), - sensor_time(0), - accel_timestamp(0), - gyro_timestamp(0), - mag_timestamp(0), - baro_timestamp(0) - {} - - void subscribe(Mavlink *mavlink) - { - sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); - } - - void send(const hrt_abstime t) - { - struct sensor_combined_s sensor; - - if (sensor_sub->update(&sensor_time, &sensor)) { - uint16_t fields_updated = 0; - - if (accel_timestamp != sensor.accelerometer_timestamp) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_timestamp = sensor.accelerometer_timestamp; - } - - if (gyro_timestamp != sensor.timestamp) { - /* mark second group dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_timestamp = sensor.timestamp; - } - - if (mag_timestamp != sensor.magnetometer_timestamp) { - /* mark third group dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_timestamp = sensor.magnetometer_timestamp; - } - - if (baro_timestamp != sensor.baro_timestamp) { - /* mark last group dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_timestamp = sensor.baro_timestamp; - } - - mavlink_msg_highres_imu_send(_channel, - sensor.timestamp, - sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], - sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], - sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], - sensor.baro_pres_mbar, sensor.differential_pressure_pa, - sensor.baro_alt_meter, sensor.baro_temp_celcius, - fields_updated); - } - } -}; - - -class MavlinkStreamAttitude : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamAttitude::get_name_static(); - } - - static const char *get_name_static() - { - return "ATTITUDE"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ATTITUDE; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamAttitude(); - } - -private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; - - /* do not allow top copying this class */ - MavlinkStreamAttitude(MavlinkStreamAttitude &); - MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); - - -protected: - explicit MavlinkStreamAttitude() : MavlinkStream(), - att_sub(nullptr), - att_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - - void send(const hrt_abstime t) - { - struct vehicle_attitude_s att; - - if (att_sub->update(&att_time, &att)) { - mavlink_msg_attitude_send(_channel, - att.timestamp / 1000, - att.roll, att.pitch, att.yaw, - att.rollspeed, att.pitchspeed, att.yawspeed); - } - } -}; - - -class MavlinkStreamAttitudeQuaternion : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamAttitudeQuaternion::get_name_static(); - } - - static const char *get_name_static() - { - return "ATTITUDE_QUATERNION"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamAttitudeQuaternion(); - } - -private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; - - /* do not allow top copying this class */ - MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); - MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); - -protected: - explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), - att_sub(nullptr), - att_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - - void send(const hrt_abstime t) - { - struct vehicle_attitude_s att; - - if (att_sub->update(&att_time, &att)) { - mavlink_msg_attitude_quaternion_send(_channel, - att.timestamp / 1000, - att.q[0], - att.q[1], - att.q[2], - att.q[3], - att.rollspeed, - att.pitchspeed, - att.yawspeed); - } - } -}; - - -class MavlinkStreamVFRHUD : public MavlinkStream -{ -public: - - const char *get_name() const - { - return MavlinkStreamVFRHUD::get_name_static(); - } - - static const char *get_name_static() - { - return "VFR_HUD"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_VFR_HUD; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamVFRHUD(); - } - -private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; - - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; - - MavlinkOrbSubscription *armed_sub; - uint64_t armed_time; - - MavlinkOrbSubscription *act_sub; - uint64_t act_time; - - MavlinkOrbSubscription *airspeed_sub; - uint64_t airspeed_time; - - /* do not allow top copying this class */ - MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); - MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); - -protected: - explicit MavlinkStreamVFRHUD() : MavlinkStream(), - att_sub(nullptr), - att_time(0), - pos_sub(nullptr), - pos_time(0), - armed_sub(nullptr), - armed_time(0), - act_sub(nullptr), - act_time(0), - airspeed_sub(nullptr), - airspeed_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); - } - - void send(const hrt_abstime t) - { - struct vehicle_attitude_s att; - struct vehicle_global_position_s pos; - struct actuator_armed_s armed; - struct actuator_controls_s act; - struct airspeed_s airspeed; - - bool updated = att_sub->update(&att_time, &att); - updated |= pos_sub->update(&pos_time, &pos); - updated |= armed_sub->update(&armed_time, &armed); - updated |= act_sub->update(&act_time, &act); - updated |= airspeed_sub->update(&airspeed_time, &airspeed); - - if (updated) { - float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); - uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; - float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - - mavlink_msg_vfr_hud_send(_channel, - airspeed.true_airspeed_m_s, - groundspeed, - heading, - throttle, - pos.alt, - -pos.vel_d); - } - } -}; - - -class MavlinkStreamGPSRawInt : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamGPSRawInt::get_name_static(); - } - - static const char *get_name_static() - { - return "GPS_RAW_INT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_GPS_RAW_INT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamGPSRawInt(); - } - -private: - MavlinkOrbSubscription *gps_sub; - uint64_t gps_time; - - /* do not allow top copying this class */ - MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); - MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); - -protected: - explicit MavlinkStreamGPSRawInt() : MavlinkStream(), - gps_sub(nullptr), - gps_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); - } - - void send(const hrt_abstime t) - { - struct vehicle_gps_position_s gps; - - if (gps_sub->update(&gps_time, &gps)) { - mavlink_msg_gps_raw_int_send(_channel, - gps.timestamp_position, - gps.fix_type, - gps.lat, - gps.lon, - gps.alt, - cm_uint16_from_m_float(gps.eph), - cm_uint16_from_m_float(gps.epv), - gps.vel_m_s * 100.0f, - _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps.satellites_used); - } - } -}; - - -class MavlinkStreamGlobalPositionInt : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamGlobalPositionInt::get_name_static(); - } - - static const char *get_name_static() - { - return "GLOBAL_POSITION_INT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamGlobalPositionInt(); - } - -private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; - - MavlinkOrbSubscription *home_sub; - uint64_t home_time; - - /* do not allow top copying this class */ - MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); - MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); - -protected: - explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0), - home_sub(nullptr), - home_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } - - void send(const hrt_abstime t) - { - struct vehicle_global_position_s pos; - struct home_position_s home; - - bool updated = pos_sub->update(&pos_time, &pos); - updated |= home_sub->update(&home_time, &home); - - if (updated) { - mavlink_msg_global_position_int_send(_channel, - pos.timestamp / 1000, - pos.lat * 1e7, - pos.lon * 1e7, - pos.alt * 1000.0f, - (pos.alt - home.alt) * 1000.0f, - pos.vel_n * 100.0f, - pos.vel_e * 100.0f, - pos.vel_d * 100.0f, - _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); - } - } -}; - - -class MavlinkStreamLocalPositionNED : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamLocalPositionNED::get_name_static(); - } - - static const char *get_name_static() - { - return "LOCAL_POSITION_NED"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_LOCAL_POSITION_NED; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamLocalPositionNED(); - } - -private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; - - /* do not allow top copying this class */ - MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); - MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); - -protected: - explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); - } - - void send(const hrt_abstime t) - { - struct vehicle_local_position_s pos; - - if (pos_sub->update(&pos_time, &pos)) { - mavlink_msg_local_position_ned_send(_channel, - pos.timestamp / 1000, - pos.x, - pos.y, - pos.z, - pos.vx, - pos.vy, - pos.vz); - } - } -}; - - - -class MavlinkStreamViconPositionEstimate : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamViconPositionEstimate::get_name_static(); - } - - static const char *get_name_static() - { - return "VICON_POSITION_ESTIMATE"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamViconPositionEstimate(); - } - -private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; - - /* do not allow top copying this class */ - MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); - MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); - -protected: - explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - } - - void send(const hrt_abstime t) - { - struct vehicle_vicon_position_s pos; - - if (pos_sub->update(&pos_time, &pos)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos.timestamp / 1000, - pos.x, - pos.y, - pos.z, - pos.roll, - pos.pitch, - pos.yaw); - } - } -}; - - -class MavlinkStreamGPSGlobalOrigin : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamGPSGlobalOrigin::get_name_static(); - } - - static const char *get_name_static() - { - return "GPS_GLOBAL_ORIGIN"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamGPSGlobalOrigin(); - } - -private: - MavlinkOrbSubscription *home_sub; - - /* do not allow top copying this class */ - MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); - MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); - -protected: - explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), - home_sub(nullptr) - {} - - void subscribe(Mavlink *mavlink) - { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } - - void send(const hrt_abstime t) - { - /* we're sending the GPS home periodically to ensure the - * the GCS does pick it up at one point */ - if (home_sub->is_published()) { - struct home_position_s home; - - if (home_sub->update(&home)) { - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home.lat * 1e7), - (int32_t)(home.lon * 1e7), - (int32_t)(home.alt) * 1000.0f); - } - } - } -}; - -template -class MavlinkStreamServoOutputRaw : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamServoOutputRaw::get_name_static(); - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - } - - static const char *get_name_static() - { - switch (N) { - case 0: - return "SERVO_OUTPUT_RAW_0"; - - case 1: - return "SERVO_OUTPUT_RAW_1"; - - case 2: - return "SERVO_OUTPUT_RAW_2"; - - case 3: - return "SERVO_OUTPUT_RAW_3"; - } - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamServoOutputRaw(); - } - -private: - MavlinkOrbSubscription *act_sub; - uint64_t act_time; - - /* do not allow top copying this class */ - MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); - MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); - -protected: - explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), - act_sub(nullptr), - act_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - orb_id_t act_topics[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - act_sub = mavlink->add_orb_subscription(act_topics[N]); - } - - void send(const hrt_abstime t) - { - struct actuator_outputs_s act; - - if (act_sub->update(&act_time, &act)) { - mavlink_msg_servo_output_raw_send(_channel, - act.timestamp / 1000, - N, - act.output[0], - act.output[1], - act.output[2], - act.output[3], - act.output[4], - act.output[5], - act.output[6], - act.output[7]); - } - } -}; - - -class MavlinkStreamHILControls : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamHILControls::get_name_static(); - } - - static const char *get_name_static() - { - return "HIL_CONTROLS"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_HIL_CONTROLS; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamHILControls(); - } - -private: - MavlinkOrbSubscription *status_sub; - uint64_t status_time; - - MavlinkOrbSubscription *pos_sp_triplet_sub; - uint64_t pos_sp_triplet_time; - - MavlinkOrbSubscription *act_sub; - uint64_t act_time; - - /* do not allow top copying this class */ - MavlinkStreamHILControls(MavlinkStreamHILControls &); - MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); - -protected: - explicit MavlinkStreamHILControls() : MavlinkStream(), - status_sub(nullptr), - status_time(0), - pos_sp_triplet_sub(nullptr), - pos_sp_triplet_time(0), - act_sub(nullptr), - act_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); - } - - void send(const hrt_abstime t) - { - struct vehicle_status_s status; - struct position_setpoint_triplet_s pos_sp_triplet; - struct actuator_outputs_s act; - - bool updated = act_sub->update(&act_time, &act); - updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); - updated |= status_sub->update(&status_time, &status); - - if (updated && (status.arming_state == ARMING_STATE_ARMED)) { - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state; - uint8_t mavlink_base_mode; - uint32_t mavlink_custom_mode; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - float out[8]; - - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; - - /* scale outputs depending on system type */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR || - mavlink_system.type == MAV_TYPE_HEXAROTOR || - mavlink_system.type == MAV_TYPE_OCTOROTOR) { - /* multirotors: set number of rotor outputs depending on type */ - - unsigned n; - - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; - - case MAV_TYPE_HEXAROTOR: - n = 6; - break; - - default: - n = 8; - break; - } - - for (unsigned i = 0; i < 8; i++) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if (i < n) { - /* scale PWM out 900..2100 us to 0..1 for rotors */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - - } else { - /* scale PWM out 900..2100 us to -1..1 for other channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); - } - - } else { - /* send 0 when disarmed */ - out[i] = 0.0f; - } - } - - } else { - /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ - - for (unsigned i = 0; i < 8; i++) { - if (i != 3) { - /* scale PWM out 900..2100 us to -1..1 for normal channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); - - } else { - /* scale PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - } - - } - } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); - } - } -}; - - -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); - } - - static const char *get_name_static() - { - return "GLOBAL_POSITION_SETPOINT_INT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamGlobalPositionSetpointInt(); - } - -private: - MavlinkOrbSubscription *pos_sp_triplet_sub; - - /* do not allow top copying this class */ - MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); - MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); - -protected: - explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), - pos_sp_triplet_sub(nullptr) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } - - void send(const hrt_abstime t) - { - struct position_setpoint_triplet_s pos_sp_triplet; - - if (pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet.current.lat * 1e7), - (int32_t)(pos_sp_triplet.current.lon * 1e7), - (int32_t)(pos_sp_triplet.current.alt * 1000), - (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); - } - } -}; - - -class MavlinkStreamLocalPositionSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamLocalPositionSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "LOCAL_POSITION_SETPOINT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamLocalPositionSetpoint(); - } - -private: - MavlinkOrbSubscription *pos_sp_sub; - uint64_t pos_sp_time; - - /* do not allow top copying this class */ - MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); - MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); - -protected: - explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), - pos_sp_sub(nullptr), - pos_sp_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); - } - - void send(const hrt_abstime t) - { - struct vehicle_local_position_setpoint_s pos_sp; - - if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { - mavlink_msg_local_position_setpoint_send(_channel, - MAV_FRAME_LOCAL_NED, - pos_sp.x, - pos_sp.y, - pos_sp.z, - pos_sp.yaw); - } - } -}; - - -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "ROLL_PITCH_YAW_THRUST_SETPOINT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawThrustSetpoint(); - } - -private: - MavlinkOrbSubscription *att_sp_sub; - uint64_t att_sp_time; - - /* do not allow top copying this class */ - MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); - MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); - -protected: - explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), - att_sp_sub(nullptr), - att_sp_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); - } - - void send(const hrt_abstime t) - { - struct vehicle_attitude_setpoint_s att_sp; - - if (att_sp_sub->update(&att_sp_time, &att_sp)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, - att_sp.timestamp / 1000, - att_sp.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); - } - } -}; - - -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); - } - -private: - MavlinkOrbSubscription *att_rates_sp_sub; - uint64_t att_rates_sp_time; - - /* do not allow top copying this class */ - MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); - MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); - -protected: - explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), - att_rates_sp_sub(nullptr), - att_rates_sp_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); - } - - void send(const hrt_abstime t) - { - struct vehicle_rates_setpoint_s att_rates_sp; - - if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, - att_rates_sp.timestamp / 1000, - att_rates_sp.roll, - att_rates_sp.pitch, - att_rates_sp.yaw, - att_rates_sp.thrust); - } - } -}; - - -class MavlinkStreamRCChannelsRaw : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRCChannelsRaw::get_name_static(); - } - - static const char *get_name_static() - { - return "RC_CHANNELS_RAW"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_RC_CHANNELS_RAW; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamRCChannelsRaw(); - } - -private: - MavlinkOrbSubscription *rc_sub; - uint64_t rc_time; - - /* do not allow top copying this class */ - MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); - MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); - -protected: - explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), - rc_sub(nullptr), - rc_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); - } - - void send(const hrt_abstime t) - { - struct rc_input_values rc; - - if (rc_sub->update(&rc_time, &rc)) { - const unsigned port_width = 8; - - // Deprecated message (but still needed for compatibility!) - for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_channel, - rc.timestamp_publication / 1000, - i, - (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, - rc.rssi); - } - - // New message - mavlink_msg_rc_channels_send(_channel, - rc.timestamp_publication / 1000, - rc.channel_count, - ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), - ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), - ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), - ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), - ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), - ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), - ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), - ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), - ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), - ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), - ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), - ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), - ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), - ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), - ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), - ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), - ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), - ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), - rc.rssi); - } - } -}; - - -class MavlinkStreamManualControl : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamManualControl::get_name_static(); - } - - static const char *get_name_static() - { - return "MANUAL_CONTROL"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_MANUAL_CONTROL; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamManualControl(); - } - -private: - MavlinkOrbSubscription *manual_sub; - uint64_t manual_time; - - /* do not allow top copying this class */ - MavlinkStreamManualControl(MavlinkStreamManualControl &); - MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); - -protected: - explicit MavlinkStreamManualControl() : MavlinkStream(), - manual_sub(nullptr), - manual_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); - } - - void send(const hrt_abstime t) - { - struct manual_control_setpoint_s manual; - - if (manual_sub->update(&manual_time, &manual)) { - mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual.x * 1000, - manual.y * 1000, - manual.z * 1000, - manual.r * 1000, - 0); - } - } -}; - - -class MavlinkStreamOpticalFlow : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamOpticalFlow::get_name_static(); - } - - static const char *get_name_static() - { - return "OPTICAL_FLOW"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_OPTICAL_FLOW; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamOpticalFlow(); - } - -private: - MavlinkOrbSubscription *flow_sub; - uint64_t flow_time; - - /* do not allow top copying this class */ - MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); - MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); - -protected: - explicit MavlinkStreamOpticalFlow() : MavlinkStream(), - flow_sub(nullptr), - flow_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); - } - - void send(const hrt_abstime t) - { - struct optical_flow_s flow; - - if (flow_sub->update(&flow_time, &flow)) { - mavlink_msg_optical_flow_send(_channel, - flow.timestamp, - flow.sensor_id, - flow.flow_raw_x, flow.flow_raw_y, - flow.flow_comp_x_m, flow.flow_comp_y_m, - flow.quality, - flow.ground_distance_m); - } - } -}; - -class MavlinkStreamAttitudeControls : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamAttitudeControls::get_name_static(); - } - - static const char *get_name_static() - { - return "ATTITUDE_CONTROLS"; - } - - uint8_t get_id() - { - return 0; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamAttitudeControls(); - } - -private: - MavlinkOrbSubscription *att_ctrl_sub; - uint64_t att_ctrl_time; - - /* do not allow top copying this class */ - MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); - MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); - -protected: - explicit MavlinkStreamAttitudeControls() : MavlinkStream(), - att_ctrl_sub(nullptr), - att_ctrl_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - } - - void send(const hrt_abstime t) - { - struct actuator_controls_s att_ctrl; - - if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "rll ctrl ", - att_ctrl.control[0]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "ptch ctrl ", - att_ctrl.control[1]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "yaw ctrl ", - att_ctrl.control[2]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "thr ctrl ", - att_ctrl.control[3]); - } - } -}; - -class MavlinkStreamNamedValueFloat : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamNamedValueFloat::get_name_static(); - } - - static const char *get_name_static() - { - return "NAMED_VALUE_FLOAT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamNamedValueFloat(); - } - -private: - MavlinkOrbSubscription *debug_sub; - uint64_t debug_time; - - /* do not allow top copying this class */ - MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); - MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); - -protected: - explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), - debug_sub(nullptr), - debug_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); - } - - void send(const hrt_abstime t) - { - struct debug_key_value_s debug; - - if (debug_sub->update(&debug_time, &debug)) { - /* enforce null termination */ - debug.key[sizeof(debug.key) - 1] = '\0'; - - mavlink_msg_named_value_float_send(_channel, - debug.timestamp_ms, - debug.key, - debug.value); - } - } -}; - -class MavlinkStreamCameraCapture : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamCameraCapture::get_name_static(); - } - - static const char *get_name_static() - { - return "CAMERA_CAPTURE"; - } - - uint8_t get_id() - { - return 0; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamCameraCapture(); - } - -private: - MavlinkOrbSubscription *status_sub; - - /* do not allow top copying this class */ - MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); - MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); - -protected: - explicit MavlinkStreamCameraCapture() : MavlinkStream(), - status_sub(nullptr) - {} - - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } - - void send(const hrt_abstime t) - { - struct vehicle_status_s status; - (void)status_sub->update(&status); - - if (status.arming_state == ARMING_STATE_ARMED - || status.arming_state == ARMING_STATE_ARMED_ERROR) { - - /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); - - } else { - /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); - } - } -}; - -class MavlinkStreamDistanceSensor : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamDistanceSensor::get_name_static(); - } - - static const char *get_name_static() - { - return "DISTANCE_SENSOR"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_DISTANCE_SENSOR; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamDistanceSensor(); - } - -private: - MavlinkOrbSubscription *range_sub; - uint64_t range_time; - - /* do not allow top copying this class */ - MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); - MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); - -protected: - explicit MavlinkStreamDistanceSensor() : MavlinkStream(), - range_sub(nullptr), - range_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); - } - - void send(const hrt_abstime t) - { - struct range_finder_report range; - - if (range_sub->update(&range_time, &range)) { - - uint8_t type; - - switch (range.type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; - break; - } - - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; - - mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, - range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); - } - } -}; +//class MavlinkStreamHighresIMU : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamHighresIMU::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "HIGHRES_IMU"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_HIGHRES_IMU; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamHighresIMU(); +// } +// +//private: +// MavlinkOrbSubscription *sensor_sub; +// uint64_t sensor_time; +// +// uint64_t accel_timestamp; +// uint64_t gyro_timestamp; +// uint64_t mag_timestamp; +// uint64_t baro_timestamp; +// +// /* do not allow top copying this class */ +// MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); +// MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); +// +//protected: +// explicit MavlinkStreamHighresIMU() : MavlinkStream(), +// sensor_sub(nullptr), +// sensor_time(0), +// accel_timestamp(0), +// gyro_timestamp(0), +// mag_timestamp(0), +// baro_timestamp(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); +// } +// +// void send(const hrt_abstime t) +// { +// struct sensor_combined_s sensor; +// +// if (sensor_sub->update(&sensor_time, &sensor)) { +// uint16_t fields_updated = 0; +// +// if (accel_timestamp != sensor.accelerometer_timestamp) { +// /* mark first three dimensions as changed */ +// fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); +// accel_timestamp = sensor.accelerometer_timestamp; +// } +// +// if (gyro_timestamp != sensor.timestamp) { +// /* mark second group dimensions as changed */ +// fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); +// gyro_timestamp = sensor.timestamp; +// } +// +// if (mag_timestamp != sensor.magnetometer_timestamp) { +// /* mark third group dimensions as changed */ +// fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); +// mag_timestamp = sensor.magnetometer_timestamp; +// } +// +// if (baro_timestamp != sensor.baro_timestamp) { +// /* mark last group dimensions as changed */ +// fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); +// baro_timestamp = sensor.baro_timestamp; +// } +// +// mavlink_msg_highres_imu_send(_channel, +// sensor.timestamp, +// sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], +// sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], +// sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], +// sensor.baro_pres_mbar, sensor.differential_pressure_pa, +// sensor.baro_alt_meter, sensor.baro_temp_celcius, +// fields_updated); +// } +// } +//}; +// +// +//class MavlinkStreamAttitude : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamAttitude::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ATTITUDE"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_ATTITUDE; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamAttitude(); +// } +// +//private: +// MavlinkOrbSubscription *att_sub; +// uint64_t att_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamAttitude(MavlinkStreamAttitude &); +// MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); +// +// +//protected: +// explicit MavlinkStreamAttitude() : MavlinkStream(), +// att_sub(nullptr), +// att_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_attitude_s att; +// +// if (att_sub->update(&att_time, &att)) { +// mavlink_msg_attitude_send(_channel, +// att.timestamp / 1000, +// att.roll, att.pitch, att.yaw, +// att.rollspeed, att.pitchspeed, att.yawspeed); +// } +// } +//}; +// +// +//class MavlinkStreamAttitudeQuaternion : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamAttitudeQuaternion::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ATTITUDE_QUATERNION"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamAttitudeQuaternion(); +// } +// +//private: +// MavlinkOrbSubscription *att_sub; +// uint64_t att_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); +// MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); +// +//protected: +// explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), +// att_sub(nullptr), +// att_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_attitude_s att; +// +// if (att_sub->update(&att_time, &att)) { +// mavlink_msg_attitude_quaternion_send(_channel, +// att.timestamp / 1000, +// att.q[0], +// att.q[1], +// att.q[2], +// att.q[3], +// att.rollspeed, +// att.pitchspeed, +// att.yawspeed); +// } +// } +//}; +// +// +//class MavlinkStreamVFRHUD : public MavlinkStream +//{ +//public: +// +// const char *get_name() const +// { +// return MavlinkStreamVFRHUD::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "VFR_HUD"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_VFR_HUD; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamVFRHUD(); +// } +// +//private: +// MavlinkOrbSubscription *att_sub; +// uint64_t att_time; +// +// MavlinkOrbSubscription *pos_sub; +// uint64_t pos_time; +// +// MavlinkOrbSubscription *armed_sub; +// uint64_t armed_time; +// +// MavlinkOrbSubscription *act_sub; +// uint64_t act_time; +// +// MavlinkOrbSubscription *airspeed_sub; +// uint64_t airspeed_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); +// MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); +// +//protected: +// explicit MavlinkStreamVFRHUD() : MavlinkStream(), +// att_sub(nullptr), +// att_time(0), +// pos_sub(nullptr), +// pos_time(0), +// armed_sub(nullptr), +// armed_time(0), +// act_sub(nullptr), +// act_time(0), +// airspeed_sub(nullptr), +// airspeed_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); +// armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); +// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); +// airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_attitude_s att; +// struct vehicle_global_position_s pos; +// struct actuator_armed_s armed; +// struct actuator_controls_s act; +// struct airspeed_s airspeed; +// +// bool updated = att_sub->update(&att_time, &att); +// updated |= pos_sub->update(&pos_time, &pos); +// updated |= armed_sub->update(&armed_time, &armed); +// updated |= act_sub->update(&act_time, &act); +// updated |= airspeed_sub->update(&airspeed_time, &airspeed); +// +// if (updated) { +// float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); +// uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; +// float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; +// +// mavlink_msg_vfr_hud_send(_channel, +// airspeed.true_airspeed_m_s, +// groundspeed, +// heading, +// throttle, +// pos.alt, +// -pos.vel_d); +// } +// } +//}; +// +// +//class MavlinkStreamGPSRawInt : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamGPSRawInt::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "GPS_RAW_INT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_GPS_RAW_INT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamGPSRawInt(); +// } +// +//private: +// MavlinkOrbSubscription *gps_sub; +// uint64_t gps_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); +// MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); +// +//protected: +// explicit MavlinkStreamGPSRawInt() : MavlinkStream(), +// gps_sub(nullptr), +// gps_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_gps_position_s gps; +// +// if (gps_sub->update(&gps_time, &gps)) { +// mavlink_msg_gps_raw_int_send(_channel, +// gps.timestamp_position, +// gps.fix_type, +// gps.lat, +// gps.lon, +// gps.alt, +// cm_uint16_from_m_float(gps.eph), +// cm_uint16_from_m_float(gps.epv), +// gps.vel_m_s * 100.0f, +// _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, +// gps.satellites_used); +// } +// } +//}; +// +// +//class MavlinkStreamGlobalPositionInt : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamGlobalPositionInt::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "GLOBAL_POSITION_INT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamGlobalPositionInt(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sub; +// uint64_t pos_time; +// +// MavlinkOrbSubscription *home_sub; +// uint64_t home_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); +// MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); +// +//protected: +// explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), +// pos_sub(nullptr), +// pos_time(0), +// home_sub(nullptr), +// home_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); +// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_global_position_s pos; +// struct home_position_s home; +// +// bool updated = pos_sub->update(&pos_time, &pos); +// updated |= home_sub->update(&home_time, &home); +// +// if (updated) { +// mavlink_msg_global_position_int_send(_channel, +// pos.timestamp / 1000, +// pos.lat * 1e7, +// pos.lon * 1e7, +// pos.alt * 1000.0f, +// (pos.alt - home.alt) * 1000.0f, +// pos.vel_n * 100.0f, +// pos.vel_e * 100.0f, +// pos.vel_d * 100.0f, +// _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); +// } +// } +//}; +// +// +//class MavlinkStreamLocalPositionNED : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamLocalPositionNED::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "LOCAL_POSITION_NED"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_LOCAL_POSITION_NED; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamLocalPositionNED(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sub; +// uint64_t pos_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); +// MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); +// +//protected: +// explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), +// pos_sub(nullptr), +// pos_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_local_position_s pos; +// +// if (pos_sub->update(&pos_time, &pos)) { +// mavlink_msg_local_position_ned_send(_channel, +// pos.timestamp / 1000, +// pos.x, +// pos.y, +// pos.z, +// pos.vx, +// pos.vy, +// pos.vz); +// } +// } +//}; +// +// +// +//class MavlinkStreamViconPositionEstimate : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamViconPositionEstimate::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "VICON_POSITION_ESTIMATE"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamViconPositionEstimate(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sub; +// uint64_t pos_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); +// MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); +// +//protected: +// explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), +// pos_sub(nullptr), +// pos_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_vicon_position_s pos; +// +// if (pos_sub->update(&pos_time, &pos)) { +// mavlink_msg_vicon_position_estimate_send(_channel, +// pos.timestamp / 1000, +// pos.x, +// pos.y, +// pos.z, +// pos.roll, +// pos.pitch, +// pos.yaw); +// } +// } +//}; +// +// +//class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamGPSGlobalOrigin::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "GPS_GLOBAL_ORIGIN"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamGPSGlobalOrigin(); +// } +// +//private: +// MavlinkOrbSubscription *home_sub; +// +// /* do not allow top copying this class */ +// MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); +// MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); +// +//protected: +// explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), +// home_sub(nullptr) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// } +// +// void send(const hrt_abstime t) +// { +// /* we're sending the GPS home periodically to ensure the +// * the GCS does pick it up at one point */ +// if (home_sub->is_published()) { +// struct home_position_s home; +// +// if (home_sub->update(&home)) { +// mavlink_msg_gps_global_origin_send(_channel, +// (int32_t)(home.lat * 1e7), +// (int32_t)(home.lon * 1e7), +// (int32_t)(home.alt) * 1000.0f); +// } +// } +// } +//}; +// +//template +//class MavlinkStreamServoOutputRaw : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamServoOutputRaw::get_name_static(); +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; +// } +// +// static const char *get_name_static() +// { +// switch (N) { +// case 0: +// return "SERVO_OUTPUT_RAW_0"; +// +// case 1: +// return "SERVO_OUTPUT_RAW_1"; +// +// case 2: +// return "SERVO_OUTPUT_RAW_2"; +// +// case 3: +// return "SERVO_OUTPUT_RAW_3"; +// } +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamServoOutputRaw(); +// } +// +//private: +// MavlinkOrbSubscription *act_sub; +// uint64_t act_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); +// MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); +// +//protected: +// explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), +// act_sub(nullptr), +// act_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// orb_id_t act_topics[] = { +// ORB_ID(actuator_outputs_0), +// ORB_ID(actuator_outputs_1), +// ORB_ID(actuator_outputs_2), +// ORB_ID(actuator_outputs_3) +// }; +// +// act_sub = mavlink->add_orb_subscription(act_topics[N]); +// } +// +// void send(const hrt_abstime t) +// { +// struct actuator_outputs_s act; +// +// if (act_sub->update(&act_time, &act)) { +// mavlink_msg_servo_output_raw_send(_channel, +// act.timestamp / 1000, +// N, +// act.output[0], +// act.output[1], +// act.output[2], +// act.output[3], +// act.output[4], +// act.output[5], +// act.output[6], +// act.output[7]); +// } +// } +//}; +// +// +//class MavlinkStreamHILControls : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamHILControls::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "HIL_CONTROLS"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_HIL_CONTROLS; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamHILControls(); +// } +// +//private: +// MavlinkOrbSubscription *status_sub; +// uint64_t status_time; +// +// MavlinkOrbSubscription *pos_sp_triplet_sub; +// uint64_t pos_sp_triplet_time; +// +// MavlinkOrbSubscription *act_sub; +// uint64_t act_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamHILControls(MavlinkStreamHILControls &); +// MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); +// +//protected: +// explicit MavlinkStreamHILControls() : MavlinkStream(), +// status_sub(nullptr), +// status_time(0), +// pos_sp_triplet_sub(nullptr), +// pos_sp_triplet_time(0), +// act_sub(nullptr), +// act_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_status_s status; +// struct position_setpoint_triplet_s pos_sp_triplet; +// struct actuator_outputs_s act; +// +// bool updated = act_sub->update(&act_time, &act); +// updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); +// updated |= status_sub->update(&status_time, &status); +// +// if (updated && (status.arming_state == ARMING_STATE_ARMED)) { +// /* translate the current syste state to mavlink state and mode */ +// uint8_t mavlink_state; +// uint8_t mavlink_base_mode; +// uint32_t mavlink_custom_mode; +// get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); +// +// float out[8]; +// +// const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; +// +// /* scale outputs depending on system type */ +// if (mavlink_system.type == MAV_TYPE_QUADROTOR || +// mavlink_system.type == MAV_TYPE_HEXAROTOR || +// mavlink_system.type == MAV_TYPE_OCTOROTOR) { +// /* multirotors: set number of rotor outputs depending on type */ +// +// unsigned n; +// +// switch (mavlink_system.type) { +// case MAV_TYPE_QUADROTOR: +// n = 4; +// break; +// +// case MAV_TYPE_HEXAROTOR: +// n = 6; +// break; +// +// default: +// n = 8; +// break; +// } +// +// for (unsigned i = 0; i < 8; i++) { +// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { +// if (i < n) { +// /* scale PWM out 900..2100 us to 0..1 for rotors */ +// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); +// +// } else { +// /* scale PWM out 900..2100 us to -1..1 for other channels */ +// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); +// } +// +// } else { +// /* send 0 when disarmed */ +// out[i] = 0.0f; +// } +// } +// +// } else { +// /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ +// +// for (unsigned i = 0; i < 8; i++) { +// if (i != 3) { +// /* scale PWM out 900..2100 us to -1..1 for normal channels */ +// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); +// +// } else { +// /* scale PWM out 900..2100 us to 0..1 for throttle */ +// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); +// } +// +// } +// } +// +// mavlink_msg_hil_controls_send(_channel, +// hrt_absolute_time(), +// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], +// mavlink_base_mode, +// 0); +// } +// } +//}; +// +// +//class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "GLOBAL_POSITION_SETPOINT_INT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamGlobalPositionSetpointInt(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sp_triplet_sub; +// +// /* do not allow top copying this class */ +// MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); +// MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); +// +//protected: +// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), +// pos_sp_triplet_sub(nullptr) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// } +// +// void send(const hrt_abstime t) +// { +// struct position_setpoint_triplet_s pos_sp_triplet; +// +// if (pos_sp_triplet_sub->update(&pos_sp_triplet)) { +// mavlink_msg_global_position_setpoint_int_send(_channel, +// MAV_FRAME_GLOBAL, +// (int32_t)(pos_sp_triplet.current.lat * 1e7), +// (int32_t)(pos_sp_triplet.current.lon * 1e7), +// (int32_t)(pos_sp_triplet.current.alt * 1000), +// (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); +// } +// } +//}; +// +// +//class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamLocalPositionSetpoint::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "LOCAL_POSITION_SETPOINT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamLocalPositionSetpoint(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sp_sub; +// uint64_t pos_sp_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); +// MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); +// +//protected: +// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), +// pos_sp_sub(nullptr), +// pos_sp_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_local_position_setpoint_s pos_sp; +// +// if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { +// mavlink_msg_local_position_setpoint_send(_channel, +// MAV_FRAME_LOCAL_NED, +// pos_sp.x, +// pos_sp.y, +// pos_sp.z, +// pos_sp.yaw); +// } +// } +//}; +// +// +//class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ROLL_PITCH_YAW_THRUST_SETPOINT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamRollPitchYawThrustSetpoint(); +// } +// +//private: +// MavlinkOrbSubscription *att_sp_sub; +// uint64_t att_sp_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); +// MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); +// +//protected: +// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), +// att_sp_sub(nullptr), +// att_sp_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_attitude_setpoint_s att_sp; +// +// if (att_sp_sub->update(&att_sp_time, &att_sp)) { +// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, +// att_sp.timestamp / 1000, +// att_sp.roll_body, +// att_sp.pitch_body, +// att_sp.yaw_body, +// att_sp.thrust); +// } +// } +//}; +// +// +//class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); +// } +// +//private: +// MavlinkOrbSubscription *att_rates_sp_sub; +// uint64_t att_rates_sp_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); +// MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); +// +//protected: +// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), +// att_rates_sp_sub(nullptr), +// att_rates_sp_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_rates_setpoint_s att_rates_sp; +// +// if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { +// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, +// att_rates_sp.timestamp / 1000, +// att_rates_sp.roll, +// att_rates_sp.pitch, +// att_rates_sp.yaw, +// att_rates_sp.thrust); +// } +// } +//}; +// +// +//class MavlinkStreamRCChannelsRaw : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamRCChannelsRaw::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "RC_CHANNELS_RAW"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_RC_CHANNELS_RAW; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamRCChannelsRaw(); +// } +// +//private: +// MavlinkOrbSubscription *rc_sub; +// uint64_t rc_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); +// MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); +// +//protected: +// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), +// rc_sub(nullptr), +// rc_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); +// } +// +// void send(const hrt_abstime t) +// { +// struct rc_input_values rc; +// +// if (rc_sub->update(&rc_time, &rc)) { +// const unsigned port_width = 8; +// +// // Deprecated message (but still needed for compatibility!) +// for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { +// /* Channels are sent in MAVLink main loop at a fixed interval */ +// mavlink_msg_rc_channels_raw_send(_channel, +// rc.timestamp_publication / 1000, +// i, +// (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, +// rc.rssi); +// } +// +// // New message +// mavlink_msg_rc_channels_send(_channel, +// rc.timestamp_publication / 1000, +// rc.channel_count, +// ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), +// ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), +// ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), +// ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), +// ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), +// ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), +// ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), +// ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), +// ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), +// ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), +// ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), +// ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), +// ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), +// ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), +// ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), +// ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), +// ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), +// ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), +// rc.rssi); +// } +// } +//}; +// +// +//class MavlinkStreamManualControl : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamManualControl::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "MANUAL_CONTROL"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_MANUAL_CONTROL; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamManualControl(); +// } +// +//private: +// MavlinkOrbSubscription *manual_sub; +// uint64_t manual_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamManualControl(MavlinkStreamManualControl &); +// MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); +// +//protected: +// explicit MavlinkStreamManualControl() : MavlinkStream(), +// manual_sub(nullptr), +// manual_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); +// } +// +// void send(const hrt_abstime t) +// { +// struct manual_control_setpoint_s manual; +// +// if (manual_sub->update(&manual_time, &manual)) { +// mavlink_msg_manual_control_send(_channel, +// mavlink_system.sysid, +// manual.x * 1000, +// manual.y * 1000, +// manual.z * 1000, +// manual.r * 1000, +// 0); +// } +// } +//}; +// +// +//class MavlinkStreamOpticalFlow : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamOpticalFlow::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "OPTICAL_FLOW"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_OPTICAL_FLOW; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamOpticalFlow(); +// } +// +//private: +// MavlinkOrbSubscription *flow_sub; +// uint64_t flow_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); +// MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); +// +//protected: +// explicit MavlinkStreamOpticalFlow() : MavlinkStream(), +// flow_sub(nullptr), +// flow_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); +// } +// +// void send(const hrt_abstime t) +// { +// struct optical_flow_s flow; +// +// if (flow_sub->update(&flow_time, &flow)) { +// mavlink_msg_optical_flow_send(_channel, +// flow.timestamp, +// flow.sensor_id, +// flow.flow_raw_x, flow.flow_raw_y, +// flow.flow_comp_x_m, flow.flow_comp_y_m, +// flow.quality, +// flow.ground_distance_m); +// } +// } +//}; +// +//class MavlinkStreamAttitudeControls : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamAttitudeControls::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ATTITUDE_CONTROLS"; +// } +// +// uint8_t get_id() +// { +// return 0; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamAttitudeControls(); +// } +// +//private: +// MavlinkOrbSubscription *att_ctrl_sub; +// uint64_t att_ctrl_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); +// MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); +// +//protected: +// explicit MavlinkStreamAttitudeControls() : MavlinkStream(), +// att_ctrl_sub(nullptr), +// att_ctrl_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); +// } +// +// void send(const hrt_abstime t) +// { +// struct actuator_controls_s att_ctrl; +// +// if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { +// /* send, add spaces so that string buffer is at least 10 chars long */ +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl.timestamp / 1000, +// "rll ctrl ", +// att_ctrl.control[0]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl.timestamp / 1000, +// "ptch ctrl ", +// att_ctrl.control[1]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl.timestamp / 1000, +// "yaw ctrl ", +// att_ctrl.control[2]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl.timestamp / 1000, +// "thr ctrl ", +// att_ctrl.control[3]); +// } +// } +//}; +// +//class MavlinkStreamNamedValueFloat : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamNamedValueFloat::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "NAMED_VALUE_FLOAT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamNamedValueFloat(); +// } +// +//private: +// MavlinkOrbSubscription *debug_sub; +// uint64_t debug_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); +// MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); +// +//protected: +// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), +// debug_sub(nullptr), +// debug_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); +// } +// +// void send(const hrt_abstime t) +// { +// struct debug_key_value_s debug; +// +// if (debug_sub->update(&debug_time, &debug)) { +// /* enforce null termination */ +// debug.key[sizeof(debug.key) - 1] = '\0'; +// +// mavlink_msg_named_value_float_send(_channel, +// debug.timestamp_ms, +// debug.key, +// debug.value); +// } +// } +//}; +// +//class MavlinkStreamCameraCapture : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamCameraCapture::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "CAMERA_CAPTURE"; +// } +// +// uint8_t get_id() +// { +// return 0; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamCameraCapture(); +// } +// +//private: +// MavlinkOrbSubscription *status_sub; +// +// /* do not allow top copying this class */ +// MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); +// MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); +// +//protected: +// explicit MavlinkStreamCameraCapture() : MavlinkStream(), +// status_sub(nullptr) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_status_s status; +// (void)status_sub->update(&status); +// +// if (status.arming_state == ARMING_STATE_ARMED +// || status.arming_state == ARMING_STATE_ARMED_ERROR) { +// +// /* send camera capture on */ +// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); +// +// } else { +// /* send camera capture off */ +// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); +// } +// } +//}; +// +//class MavlinkStreamDistanceSensor : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamDistanceSensor::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "DISTANCE_SENSOR"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_DISTANCE_SENSOR; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamDistanceSensor(); +// } +// +//private: +// MavlinkOrbSubscription *range_sub; +// uint64_t range_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); +// MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); +// +//protected: +// explicit MavlinkStreamDistanceSensor() : MavlinkStream(), +// range_sub(nullptr), +// range_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); +// } +// +// void send(const hrt_abstime t) +// { +// struct range_finder_report range; +// +// if (range_sub->update(&range_time, &range)) { +// +// uint8_t type; +// +// switch (range.type) { +// case RANGE_FINDER_TYPE_LASER: +// type = MAV_DISTANCE_SENSOR_LASER; +// break; +// } +// +// uint8_t id = 0; +// uint8_t orientation = 0; +// uint8_t covariance = 20; +// +// mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, +// range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); +// } +// } +//}; StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), - new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), - new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), - new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), - new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), - new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), - new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), - new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), - new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), - new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), - new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), - new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), - new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), - new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), - new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), - new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), - new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), - new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), - new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), +// new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), +// new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), +// new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), +// new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), +// new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), +// new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), +// new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), +// new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), +// new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), +// new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), +// new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), +// new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), +// new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), +// new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), +// new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), +// new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), +// new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), +// new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), +// new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), +// new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), +// new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), +// new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), +// new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), +// new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), +// new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), nullptr }; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index ee64d0e424..7e44166099 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -46,10 +46,10 @@ class StreamListItem { public: - MavlinkStream* (*new_instance)(); + MavlinkStream* (*new_instance)(Mavlink *mavlink); const char* (*get_name)(); - StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) : + StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) : new_instance(inst), get_name(name) {}; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 068774c471..d17ccc6f9b 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -157,15 +157,13 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int void MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) { - mavlink_message_t msg; mavlink_mission_ack_t wpa; wpa.target_system = sysid; wpa.target_component = compid; wpa.type = type; - mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa); if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } } @@ -175,13 +173,11 @@ void MavlinkMissionManager::send_mission_current(uint16_t seq) { if (seq < _count) { - mavlink_message_t msg; mavlink_mission_current_t wpc; wpc.seq = seq; - mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc); } else if (seq == 0 && _count == 0) { /* don't broadcast if no WPs */ @@ -199,15 +195,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ { _time_last_sent = hrt_absolute_time(); - mavlink_message_t msg; mavlink_mission_count_t wpc; wpc.target_system = sysid; wpc.target_component = compid; wpc.count = _count; - mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc); if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } } @@ -226,13 +220,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t mavlink_mission_item_t wp; format_mavlink_mission_item(&mission_item, &wp); - mavlink_message_t msg; wp.target_system = sysid; wp.target_component = compid; wp.seq = seq; wp.current = (_current_seq == seq) ? 1 : 0; - mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp); - _mavlink->send_message(&msg); + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp); if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } @@ -251,13 +244,12 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 if (seq < _max_count) { _time_last_sent = hrt_absolute_time(); - mavlink_message_t msg; mavlink_mission_request_t wpr; wpr.target_system = sysid; wpr.target_component = compid; wpr.seq = seq; - mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr); - _mavlink->send_message(&msg); + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr); if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } @@ -272,13 +264,11 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 void MavlinkMissionManager::send_mission_item_reached(uint16_t seq) { - mavlink_message_t msg; mavlink_mission_item_reached_t wp_reached; wp_reached.seq = seq; - mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached); if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } } diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index b1dae918c4..4e8cd4ba47 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -38,11 +38,11 @@ * @author Anton Babushkin */ -//#include +#include #include "mavlink_parameters.h" +#include "mavlink_main.h" -explicit MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), _send_all_index(-1) { @@ -51,7 +51,7 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkSt unsigned MavlinkParametersManager::get_size() { - if (_send_all_index() >= 0) { + if (_send_all_index >= 0) { return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } else { diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 9b7a04a733..2a5a205e9e 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -40,6 +40,9 @@ #pragma once +#include + +#include "mavlink_bridge_header.h" #include "mavlink_stream.h" class MavlinkParametersManager : public MavlinkStream diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 1986ae3c8f..08faf102a2 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -40,6 +40,7 @@ SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ mavlink_mission.cpp \ + mavlink_parameters.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ From d70b21c51aacae1a3dae755dca4ba9c3fa7a0d88 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 15:37:56 +0200 Subject: [PATCH 397/600] mavlink: move commands stream to mavlink_messages.cpp, bugs fixed --- src/modules/mavlink/mavlink_bridge_header.h | 2 +- src/modules/mavlink/mavlink_commands.cpp | 73 --------------------- src/modules/mavlink/mavlink_commands.h | 65 ------------------ src/modules/mavlink/mavlink_main.cpp | 14 ++-- src/modules/mavlink/mavlink_messages.cpp | 70 ++++++++++++++++++++ src/modules/mavlink/mavlink_parameters.cpp | 2 +- src/modules/mavlink/module.mk | 1 - 7 files changed, 78 insertions(+), 149 deletions(-) delete mode 100644 src/modules/mavlink/mavlink_commands.cpp delete mode 100644 src/modules/mavlink/mavlink_commands.h diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 374d1511c1..d82c2dae7a 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -44,7 +44,7 @@ __BEGIN_DECLS -#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +//#define MAVLINK_USE_CONVENIENCE_FUNCTIONS /* use efficient approach, see mavlink_helpers.h */ #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp deleted file mode 100644 index b502c3c863..0000000000 --- a/src/modules/mavlink/mavlink_commands.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_commands.cpp - * Mavlink commands stream implementation. - * - * @author Anton Babushkin - */ - -#include "mavlink_commands.h" - -MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : - _cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))), - _cmd{}, - _channel(channel), - _cmd_time(0) -{ -} - -void -MavlinkCommandsStream::update(const hrt_abstime t) -{ - struct vehicle_command_s cmd; - - if (_cmd_sub->update(&_cmd_time, &cmd)) { - /* only send commands for other systems/components */ - if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { - mavlink_msg_command_long_send(_channel, - cmd.target_system, - cmd.target_component, - cmd.command, - cmd.confirmation, - cmd.param1, - cmd.param2, - cmd.param3, - cmd.param4, - cmd.param5, - cmd.param6, - cmd.param7); - } - } -} diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h deleted file mode 100644 index abdba34b98..0000000000 --- a/src/modules/mavlink/mavlink_commands.h +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_commands.h - * Mavlink commands stream definition. - * - * @author Anton Babushkin - */ - -#ifndef MAVLINK_COMMANDS_H_ -#define MAVLINK_COMMANDS_H_ - -#include -#include - -class Mavlink; -class MavlinkCommansStream; - -#include "mavlink_main.h" - -class MavlinkCommandsStream -{ -private: - MavlinkOrbSubscription *_cmd_sub; - struct vehicle_command_s *_cmd; - mavlink_channel_t _channel; - uint64_t _cmd_time; - -public: - MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); - void update(const hrt_abstime t); -}; - -#endif /* MAVLINK_COMMANDS_H_ */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index dca76c950f..4f4bf8691f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -78,7 +78,6 @@ #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" -#include "mavlink_commands.h" #ifndef MAVLINK_CRC_EXTRA #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems @@ -840,7 +839,7 @@ Mavlink::send_statustext(unsigned severity, const char *string) break; } - mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); + send_message(MAVLINK_MSG_ID_STATUSTEXT, &statustext); return OK; } else { @@ -1313,13 +1312,14 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s status; status_sub->update(&status_time, &status); - MavlinkCommandsStream commands_stream(this, _channel); - /* add default streams depending on mode */ /* HEARTBEAT is constant rate stream, rate never adjusted */ configure_stream("HEARTBEAT", 1.0f); + /* COMMAND_LONG stream: use high rate to avoid commands skipping */ + configure_stream("COMMAND_LONG", 100.0f); + /* PARAM_VALUE stream */ _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); _parameters_manager->set_interval(interval_from_rate(30.0f)); @@ -1384,9 +1384,6 @@ Mavlink::task_main(int argc, char *argv[]) set_hil_enabled(status.hil_state == HIL_STATE_ON); } - /* update commands stream */ - commands_stream.update(t); - /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { @@ -1469,7 +1466,8 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_unlock(&_message_buffer_mutex); - _mavlink_resend_uart(_channel, &msg); + // TODO implement message resending + //_mavlink_resend_uart(_channel, &msg); } } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7dc3ebac1d..a8b4d11ccd 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -312,6 +312,75 @@ protected: } }; +class MavlinkStreamCommandLong : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamCommandLong::get_name_static(); + } + + static const char *get_name_static() + { + return "COMMAND_LONG"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_COMMAND_LONG; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamCommandLong(mavlink); + } + + unsigned get_size() { return 0; } // commands stream is not regular and not predictable + +private: + MavlinkOrbSubscription *_cmd_sub; + uint64_t _cmd_time; + + /* do not allow top copying this class */ + MavlinkStreamCommandLong(MavlinkStreamCommandLong &); + MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &); + +protected: + explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), + _cmd_sub(nullptr), + _cmd_time(0) + {} + + void subscribe() { + _cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + } + + void send(const hrt_abstime t) + { + struct vehicle_command_s cmd; + + if (_cmd_sub->update(&_cmd_time, &cmd)) { + /* only send commands for other systems/components */ + if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { + mavlink_command_long_t mavcmd; + + mavcmd.target_system = cmd.target_system; + mavcmd.target_component = cmd.target_component; + mavcmd.command = cmd.command; + mavcmd.confirmation = cmd.confirmation; + mavcmd.param1 = cmd.param1; + mavcmd.param2 = cmd.param2; + mavcmd.param3 = cmd.param3; + mavcmd.param4 = cmd.param4; + mavcmd.param5 = cmd.param5; + mavcmd.param6 = cmd.param6; + mavcmd.param7 = cmd.param7; + + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &mavcmd); + } + } + } +}; class MavlinkStreamSysStatus : public MavlinkStream { @@ -1926,6 +1995,7 @@ protected: StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), + new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), // new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), // new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 4e8cd4ba47..cd5f53d88c 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -147,7 +147,7 @@ MavlinkParametersManager::send(const hrt_abstime t) if (_send_all_index >= 0) { send_param(param_for_index(_send_all_index)); _send_all_index++; - if (_send_all_index >= param_count()) { + if (_send_all_index >= (int) param_count()) { _send_all_index = -1; } } diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 08faf102a2..91fdd61547 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -45,7 +45,6 @@ SRCS += mavlink_main.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ mavlink_rate_limiter.cpp \ - mavlink_commands.cpp \ mavlink_ftp.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From 1fdc666bb0be393f048c85b1827494beedff0426 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jul 2014 16:53:19 +0200 Subject: [PATCH 398/600] Update NuttX to deal with parity --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 7e1b97bcf1..088146b90e 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 7e1b97bcf10d8495169eec355988ca5890bfd5df +Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172 From f7a40c5c6dcccf0ad14e97ef7fe1e426230e6e01 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jul 2014 16:55:42 +0200 Subject: [PATCH 399/600] Improve I2C address comment. Make filter much stronger to damp any non-realistic vehicle motion. --- src/drivers/meas_airspeed/meas_airspeed.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 6ab4377167..118c35960f 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -92,7 +92,7 @@ #include /* I2C bus address is 1010001x */ -#define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */ +#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */ #define PATH_MS4525 "/dev/ms4525" /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ #define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ @@ -102,9 +102,9 @@ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ /* Measurement rate is 100Hz */ -#define MEAS_RATE 100.0f -#define MEAS_DRIVER_FILTER_FREQ 3.0f -#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ +#define MEAS_RATE 100 +#define MEAS_DRIVER_FILTER_FREQ 0.5f +#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ class MEASAirspeed : public Airspeed { From 2313bf7cd5cbf14ffaec61116cce84b9c0c18653 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jul 2014 16:59:04 +0200 Subject: [PATCH 400/600] mTECS: Note better filter defaults to get airspeed response a bit smoother. Damp also ACC response with filter more than currently --- .../fw_pos_control_l1/mtecs/mTecs_params.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 4ca31fe201..7cfe63fbc2 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -241,7 +241,14 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); * * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f); +PARAM_DEFINE_FLOAT(MT_A_LP, 0.5f); + +/** + * Airspeed derivative calculation lowpass + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f); /** * P gain for the airspeed control @@ -268,7 +275,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f); * * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f); +PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 0.5f); /** * Minimal acceleration (air) @@ -286,13 +293,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); */ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); -/** - * Airspeed derivative calculation lowpass - * - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); - /** * Minimal throttle during takeoff * From 7ecf66c06d15fb9a8c04f96b5bd05fe1c93138fe Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 17:36:10 +0200 Subject: [PATCH 401/600] mavlink: bugs fixed --- src/modules/mavlink/mavlink_main.cpp | 22 ++++++++++------------ src/modules/mavlink/mavlink_messages.cpp | 4 ++-- src/modules/mavlink/mavlink_stream.cpp | 13 +++++++++++-- 3 files changed, 23 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4f4bf8691f..f3246c3809 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -146,7 +146,7 @@ Mavlink::Mavlink() : _baudrate(57600), _datarate(1000), _datarate_events(500), - _rate_mult(0.0f), + _rate_mult(1.0f), _mavlink_param_queue_index(0), mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), @@ -724,7 +724,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) _last_write_try_time = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ - if (buf_free >= TX_BUFFER_GAP) { + if (buf_free < TX_BUFFER_GAP) { warnx("SKIP msgid %i, %i bytes", msgid, packet_len); /* no enough space in buffer to send */ count_txerr(); @@ -749,9 +749,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) uint16_t checksum; crc_init(&checksum); crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); -#if MAVLINK_CRC_EXTRA crc_accumulate(mavlink_message_crcs[msgid], &checksum); -#endif buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); @@ -1127,7 +1125,7 @@ Mavlink::update_rate_mult() } } - _rate_mult = ((float)_datarate - const_rate) / (rate - const_rate); + _rate_mult = ((float)_datarate - const_rate) / rate; } int @@ -1250,8 +1248,6 @@ Mavlink::task_main(int argc, char *argv[]) break; } - update_rate_mult(); - warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ @@ -1328,7 +1324,7 @@ Mavlink::task_main(int argc, char *argv[]) switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); - configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); + /*configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); configure_stream("ATTITUDE", 10.0f); configure_stream("VFR_HUD", 8.0f); @@ -1340,6 +1336,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); + */ break; case MAVLINK_MODE_CAMERA: @@ -1353,9 +1350,6 @@ Mavlink::task_main(int argc, char *argv[]) break; } - /* don't send parameters on startup without request */ - _mavlink_param_queue_index = param_count(); - float base_rate_mult = _datarate / 1000.0f; MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult); @@ -1374,6 +1368,9 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); + update_rate_mult(); + warnx("rate mult %f", (double)_rate_mult); + if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ mavlink_update_system(); @@ -1410,7 +1407,8 @@ Mavlink::task_main(int argc, char *argv[]) } if (fast_rate_limiter.check(t)) { - _mission_manager->eventloop(); + // TODO missions + //_mission_manager->eventloop(); if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a8b4d11ccd..20b1a966ff 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -305,7 +305,7 @@ protected: msg.custom_mode = 0; get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); msg.type = mavlink_system.type; - msg.autopilot = mavlink_system.type; + msg.autopilot = MAV_AUTOPILOT_PX4; msg.mavlink_version = 3; _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg); @@ -352,7 +352,7 @@ protected: {} void subscribe() { - _cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + _cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_command)); } void send(const hrt_abstime t) diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index da3a551725..5b9e45d3ec 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -71,12 +71,21 @@ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; - unsigned int interval = _interval * _mavlink->get_rate_mult(); + unsigned int interval = _interval; + + if (!const_rate()) { + interval /= _mavlink->get_rate_mult(); + } if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); - _last_sent = t; + if (const_rate()) { + _last_sent = (t / _interval) * _interval; + + } else { + _last_sent = t; + } return 0; } From 20698c751c62ca6f11aa910b3c3f180fe30211ff Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 22:40:55 +0200 Subject: [PATCH 402/600] mavlink: HIGHRES_IMU stream added --- src/modules/mavlink/mavlink_main.cpp | 5 +- src/modules/mavlink/mavlink_messages.cpp | 246 ++++++++++++----------- 2 files changed, 133 insertions(+), 118 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f3246c3809..e49e99a9b6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1324,7 +1324,7 @@ Mavlink::task_main(int argc, char *argv[]) switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); - /*configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); configure_stream("ATTITUDE", 10.0f); configure_stream("VFR_HUD", 8.0f); @@ -1336,7 +1336,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - */ break; case MAVLINK_MODE_CAMERA: @@ -1369,7 +1368,7 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); update_rate_mult(); - warnx("rate mult %f", (double)_rate_mult); + warnx("rate mult %.2f", (double)_rate_mult); if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 20b1a966ff..a77d34c710 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -454,99 +454,115 @@ protected: }; -//class MavlinkStreamHighresIMU : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamHighresIMU::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "HIGHRES_IMU"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_HIGHRES_IMU; -// } -// -// static MavlinkStream *new_instance() -// { -// return new MavlinkStreamHighresIMU(); -// } -// -//private: -// MavlinkOrbSubscription *sensor_sub; -// uint64_t sensor_time; -// -// uint64_t accel_timestamp; -// uint64_t gyro_timestamp; -// uint64_t mag_timestamp; -// uint64_t baro_timestamp; -// -// /* do not allow top copying this class */ -// MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); -// MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); -// -//protected: -// explicit MavlinkStreamHighresIMU() : MavlinkStream(), -// sensor_sub(nullptr), -// sensor_time(0), -// accel_timestamp(0), -// gyro_timestamp(0), -// mag_timestamp(0), -// baro_timestamp(0) -// {} -// -// void subscribe(Mavlink *mavlink) -// { -// sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); -// } -// -// void send(const hrt_abstime t) -// { -// struct sensor_combined_s sensor; -// -// if (sensor_sub->update(&sensor_time, &sensor)) { -// uint16_t fields_updated = 0; -// -// if (accel_timestamp != sensor.accelerometer_timestamp) { -// /* mark first three dimensions as changed */ -// fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); -// accel_timestamp = sensor.accelerometer_timestamp; -// } -// -// if (gyro_timestamp != sensor.timestamp) { -// /* mark second group dimensions as changed */ -// fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); -// gyro_timestamp = sensor.timestamp; -// } -// -// if (mag_timestamp != sensor.magnetometer_timestamp) { -// /* mark third group dimensions as changed */ -// fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); -// mag_timestamp = sensor.magnetometer_timestamp; -// } -// -// if (baro_timestamp != sensor.baro_timestamp) { -// /* mark last group dimensions as changed */ -// fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); -// baro_timestamp = sensor.baro_timestamp; -// } -// -// mavlink_msg_highres_imu_send(_channel, -// sensor.timestamp, -// sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], -// sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], -// sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], -// sensor.baro_pres_mbar, sensor.differential_pressure_pa, -// sensor.baro_alt_meter, sensor.baro_temp_celcius, -// fields_updated); -// } -// } -//}; +class MavlinkStreamHighresIMU : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamHighresIMU::get_name_static(); + } + + static const char *get_name_static() + { + return "HIGHRES_IMU"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIGHRES_IMU; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamHighresIMU(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *sensor_sub; + uint64_t sensor_time; + + uint64_t accel_timestamp; + uint64_t gyro_timestamp; + uint64_t mag_timestamp; + uint64_t baro_timestamp; + + /* do not allow top copying this class */ + MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); + MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); + +protected: + explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), + sensor_sub(nullptr), + sensor_time(0), + accel_timestamp(0), + gyro_timestamp(0), + mag_timestamp(0), + baro_timestamp(0) + {} + + void subscribe() + { + sensor_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_combined)); + } + + void send(const hrt_abstime t) + { + struct sensor_combined_s sensor; + + if (sensor_sub->update(&sensor_time, &sensor)) { + uint16_t fields_updated = 0; + + if (accel_timestamp != sensor.accelerometer_timestamp) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_timestamp = sensor.accelerometer_timestamp; + } + + if (gyro_timestamp != sensor.timestamp) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_timestamp = sensor.timestamp; + } + + if (mag_timestamp != sensor.magnetometer_timestamp) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_timestamp = sensor.magnetometer_timestamp; + } + + if (baro_timestamp != sensor.baro_timestamp) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_timestamp = sensor.baro_timestamp; + } + + mavlink_highres_imu_t msg; + + msg.time_usec = sensor.timestamp; + msg.xacc = sensor.accelerometer_m_s2[0]; + msg.yacc = sensor.accelerometer_m_s2[1]; + msg.zacc = sensor.accelerometer_m_s2[2]; + msg.xgyro = sensor.gyro_rad_s[0]; + msg.ygyro = sensor.gyro_rad_s[1]; + msg.zgyro = sensor.gyro_rad_s[2]; + msg.xmag = sensor.magnetometer_ga[0]; + msg.ymag = sensor.magnetometer_ga[1]; + msg.zmag = sensor.magnetometer_ga[2]; + msg.abs_pressure = sensor.baro_pres_mbar; + msg.diff_pressure = sensor.differential_pressure_pa; + msg.pressure_alt = sensor.baro_alt_meter; + msg.temperature = sensor.baro_temp_celcius; + msg.fields_updated = fields_updated; + + _mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg); + } + } +}; // // //class MavlinkStreamAttitude : public MavlinkStream @@ -567,7 +583,7 @@ protected: // return MAVLINK_MSG_ID_ATTITUDE; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamAttitude(); // } @@ -624,7 +640,7 @@ protected: // return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamAttitudeQuaternion(); // } @@ -686,7 +702,7 @@ protected: // return MAVLINK_MSG_ID_VFR_HUD; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamVFRHUD(); // } @@ -783,7 +799,7 @@ protected: // return MAVLINK_MSG_ID_GPS_RAW_INT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamGPSRawInt(); // } @@ -846,7 +862,7 @@ protected: // return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamGlobalPositionInt(); // } @@ -918,7 +934,7 @@ protected: // return MAVLINK_MSG_ID_LOCAL_POSITION_NED; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamLocalPositionNED(); // } @@ -979,7 +995,7 @@ protected: // return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamViconPositionEstimate(); // } @@ -1039,7 +1055,7 @@ protected: // return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamGPSGlobalOrigin(); // } @@ -1109,7 +1125,7 @@ protected: // } // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamServoOutputRaw(); // } @@ -1179,7 +1195,7 @@ protected: // return MAVLINK_MSG_ID_HIL_CONTROLS; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamHILControls(); // } @@ -1319,7 +1335,7 @@ protected: // return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamGlobalPositionSetpointInt(); // } @@ -1375,7 +1391,7 @@ protected: // return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamLocalPositionSetpoint(); // } @@ -1433,7 +1449,7 @@ protected: // return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamRollPitchYawThrustSetpoint(); // } @@ -1491,7 +1507,7 @@ protected: // return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); // } @@ -1549,7 +1565,7 @@ protected: // return MAVLINK_MSG_ID_RC_CHANNELS_RAW; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamRCChannelsRaw(); // } @@ -1643,7 +1659,7 @@ protected: // return MAVLINK_MSG_ID_MANUAL_CONTROL; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamManualControl(); // } @@ -1702,7 +1718,7 @@ protected: // return MAVLINK_MSG_ID_OPTICAL_FLOW; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamOpticalFlow(); // } @@ -1760,7 +1776,7 @@ protected: // return 0; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamAttitudeControls(); // } @@ -1828,7 +1844,7 @@ protected: // return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamNamedValueFloat(); // } @@ -1886,7 +1902,7 @@ protected: // return 0; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamCameraCapture(); // } @@ -1944,7 +1960,7 @@ protected: // return MAVLINK_MSG_ID_DISTANCE_SENSOR; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamDistanceSensor(); // } @@ -1997,7 +2013,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), -// new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), + new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), // new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), // new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), // new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), From a65b7aee0bc07eeb7545f4a3206e860791b64a36 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 22:54:48 +0200 Subject: [PATCH 403/600] mavlink: ATTITUDE, ATTITUDE_QUATERNION, VFR_HUD streams added --- src/modules/mavlink/mavlink_messages.cpp | 544 ++++++++++++----------- 1 file changed, 284 insertions(+), 260 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a77d34c710..5752688146 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -563,224 +563,248 @@ protected: } } }; -// -// -//class MavlinkStreamAttitude : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamAttitude::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "ATTITUDE"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_ATTITUDE; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamAttitude(); -// } -// -//private: -// MavlinkOrbSubscription *att_sub; -// uint64_t att_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamAttitude(MavlinkStreamAttitude &); -// MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); -// -// -//protected: -// explicit MavlinkStreamAttitude() : MavlinkStream(), -// att_sub(nullptr), -// att_time(0) -// {} -// -// void subscribe(Mavlink *mavlink) -// { -// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_attitude_s att; -// -// if (att_sub->update(&att_time, &att)) { -// mavlink_msg_attitude_send(_channel, -// att.timestamp / 1000, -// att.roll, att.pitch, att.yaw, -// att.rollspeed, att.pitchspeed, att.yawspeed); -// } -// } -//}; -// -// -//class MavlinkStreamAttitudeQuaternion : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamAttitudeQuaternion::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "ATTITUDE_QUATERNION"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamAttitudeQuaternion(); -// } -// -//private: -// MavlinkOrbSubscription *att_sub; -// uint64_t att_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); -// MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); -// -//protected: -// explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), -// att_sub(nullptr), -// att_time(0) -// {} -// -// void subscribe(Mavlink *mavlink) -// { -// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_attitude_s att; -// -// if (att_sub->update(&att_time, &att)) { -// mavlink_msg_attitude_quaternion_send(_channel, -// att.timestamp / 1000, -// att.q[0], -// att.q[1], -// att.q[2], -// att.q[3], -// att.rollspeed, -// att.pitchspeed, -// att.yawspeed); -// } -// } -//}; -// -// -//class MavlinkStreamVFRHUD : public MavlinkStream -//{ -//public: -// -// const char *get_name() const -// { -// return MavlinkStreamVFRHUD::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "VFR_HUD"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_VFR_HUD; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamVFRHUD(); -// } -// -//private: -// MavlinkOrbSubscription *att_sub; -// uint64_t att_time; -// -// MavlinkOrbSubscription *pos_sub; -// uint64_t pos_time; -// -// MavlinkOrbSubscription *armed_sub; -// uint64_t armed_time; -// -// MavlinkOrbSubscription *act_sub; -// uint64_t act_time; -// -// MavlinkOrbSubscription *airspeed_sub; -// uint64_t airspeed_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); -// MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); -// -//protected: -// explicit MavlinkStreamVFRHUD() : MavlinkStream(), -// att_sub(nullptr), -// att_time(0), -// pos_sub(nullptr), -// pos_time(0), -// armed_sub(nullptr), -// armed_time(0), -// act_sub(nullptr), -// act_time(0), -// airspeed_sub(nullptr), -// airspeed_time(0) -// {} -// -// void subscribe(Mavlink *mavlink) -// { -// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); -// armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); -// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); -// airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_attitude_s att; -// struct vehicle_global_position_s pos; -// struct actuator_armed_s armed; -// struct actuator_controls_s act; -// struct airspeed_s airspeed; -// -// bool updated = att_sub->update(&att_time, &att); -// updated |= pos_sub->update(&pos_time, &pos); -// updated |= armed_sub->update(&armed_time, &armed); -// updated |= act_sub->update(&act_time, &act); -// updated |= airspeed_sub->update(&airspeed_time, &airspeed); -// -// if (updated) { -// float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); -// uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; -// float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; -// -// mavlink_msg_vfr_hud_send(_channel, -// airspeed.true_airspeed_m_s, -// groundspeed, -// heading, -// throttle, -// pos.alt, -// -pos.vel_d); -// } -// } -//}; -// -// + + +class MavlinkStreamAttitude : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAttitude::get_name_static(); + } + + static const char *get_name_static() + { + return "ATTITUDE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttitude(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *att_sub; + uint64_t att_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitude(MavlinkStreamAttitude &); + MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); + + +protected: + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), + att_sub(nullptr), + att_time(0) + {} + + void subscribe() + { + att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); + } + + void send(const hrt_abstime t) + { + struct vehicle_attitude_s att; + + if (att_sub->update(&att_time, &att)) { + mavlink_attitude_t msg; + + msg.time_boot_ms = att.timestamp / 1000; + msg.roll = att.roll; + msg.pitch = att.pitch; + msg.yaw = att.yaw; + msg.rollspeed = att.rollspeed; + msg.pitchspeed = att.pitchspeed; + msg.yawspeed = att.yawspeed; + + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg); + } + } +}; + + +class MavlinkStreamAttitudeQuaternion : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAttitudeQuaternion::get_name_static(); + } + + static const char *get_name_static() + { + return "ATTITUDE_QUATERNION"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttitudeQuaternion(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *att_sub; + uint64_t att_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); + MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); + +protected: + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), + att_sub(nullptr), + att_time(0) + {} + + void subscribe() + { + att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); + } + + void send(const hrt_abstime t) + { + struct vehicle_attitude_s att; + + if (att_sub->update(&att_time, &att)) { + mavlink_attitude_quaternion_t msg; + + msg.time_boot_ms = att.timestamp / 1000; + msg.q1 = att.q[0]; + msg.q2 = att.q[1]; + msg.q3 = att.q[2]; + msg.q4 = att.q[3]; + msg.rollspeed = att.rollspeed; + msg.pitchspeed = att.pitchspeed; + msg.yawspeed = att.yawspeed; + + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg); + } + } +}; + + +class MavlinkStreamVFRHUD : public MavlinkStream +{ +public: + + const char *get_name() const + { + return MavlinkStreamVFRHUD::get_name_static(); + } + + static const char *get_name_static() + { + return "VFR_HUD"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_VFR_HUD; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamVFRHUD(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *att_sub; + uint64_t att_time; + + MavlinkOrbSubscription *pos_sub; + uint64_t pos_time; + + MavlinkOrbSubscription *armed_sub; + uint64_t armed_time; + + MavlinkOrbSubscription *act_sub; + uint64_t act_time; + + MavlinkOrbSubscription *airspeed_sub; + uint64_t airspeed_time; + + /* do not allow top copying this class */ + MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); + MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); + +protected: + explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), + att_sub(nullptr), + att_time(0), + pos_sub(nullptr), + pos_time(0), + armed_sub(nullptr), + armed_time(0), + act_sub(nullptr), + act_time(0), + airspeed_sub(nullptr), + airspeed_time(0) + {} + + void subscribe() + { + att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); + pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); + armed_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_armed)); + act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); + airspeed_sub = _mavlink->add_orb_subscription(ORB_ID(airspeed)); + } + + void send(const hrt_abstime t) + { + struct vehicle_attitude_s att; + struct vehicle_global_position_s pos; + struct actuator_armed_s armed; + struct actuator_controls_s act; + struct airspeed_s airspeed; + + bool updated = att_sub->update(&att_time, &att); + updated |= pos_sub->update(&pos_time, &pos); + updated |= armed_sub->update(&armed_time, &armed); + updated |= act_sub->update(&act_time, &act); + updated |= airspeed_sub->update(&airspeed_time, &airspeed); + + if (updated) { + mavlink_vfr_hud_t msg; + + msg.airspeed = airspeed.true_airspeed_m_s; + msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); + msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; + msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; + msg.alt = pos.alt; + msg.climb = -pos.vel_d; + + _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); + } + } +}; + + //class MavlinkStreamGPSRawInt : public MavlinkStream //{ //public: @@ -818,9 +842,9 @@ protected: // gps_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); +// gps_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); // } // // void send(const hrt_abstime t) @@ -886,10 +910,10 @@ protected: // home_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); -// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); +// home_sub = _mavlink->add_orb_subscription(ORB_ID(home_position)); // } // // void send(const hrt_abstime t) @@ -953,9 +977,9 @@ protected: // pos_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); +// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); // } // // void send(const hrt_abstime t) @@ -1014,9 +1038,9 @@ protected: // pos_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); +// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); // } // // void send(const hrt_abstime t) @@ -1072,9 +1096,9 @@ protected: // home_sub(nullptr) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// home_sub = _mavlink->add_orb_subscription(ORB_ID(home_position)); // } // // void send(const hrt_abstime t) @@ -1144,7 +1168,7 @@ protected: // act_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { // orb_id_t act_topics[] = { // ORB_ID(actuator_outputs_0), @@ -1153,7 +1177,7 @@ protected: // ORB_ID(actuator_outputs_3) // }; // -// act_sub = mavlink->add_orb_subscription(act_topics[N]); +// act_sub = _mavlink->add_orb_subscription(act_topics[N]); // } // // void send(const hrt_abstime t) @@ -1224,11 +1248,11 @@ protected: // act_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); -// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); -// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); +// status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); // } // // void send(const hrt_abstime t) @@ -1352,9 +1376,9 @@ protected: // pos_sp_triplet_sub(nullptr) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); // } // // void send(const hrt_abstime t) @@ -1410,9 +1434,9 @@ protected: // pos_sp_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); +// pos_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); // } // // void send(const hrt_abstime t) @@ -1468,9 +1492,9 @@ protected: // att_sp_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); +// att_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); // } // // void send(const hrt_abstime t) @@ -1526,9 +1550,9 @@ protected: // att_rates_sp_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); +// att_rates_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); // } // // void send(const hrt_abstime t) @@ -1584,9 +1608,9 @@ protected: // rc_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); +// rc_sub = _mavlink->add_orb_subscription(ORB_ID(input_rc)); // } // // void send(const hrt_abstime t) @@ -1678,9 +1702,9 @@ protected: // manual_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); +// manual_sub = _mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); // } // // void send(const hrt_abstime t) @@ -1737,9 +1761,9 @@ protected: // flow_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); +// flow_sub = _mavlink->add_orb_subscription(ORB_ID(optical_flow)); // } // // void send(const hrt_abstime t) @@ -1795,9 +1819,9 @@ protected: // att_ctrl_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); +// att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); // } // // void send(const hrt_abstime t) @@ -1863,9 +1887,9 @@ protected: // debug_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); +// debug_sub = _mavlink->add_orb_subscription(ORB_ID(debug_key_value)); // } // // void send(const hrt_abstime t) @@ -1919,9 +1943,9 @@ protected: // status_sub(nullptr) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); // } // // void send(const hrt_abstime t) @@ -1979,9 +2003,9 @@ protected: // range_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); +// range_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); // } // // void send(const hrt_abstime t) @@ -2014,9 +2038,9 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), -// new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), -// new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), -// new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), + new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), + new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), + new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), // new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), // new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), // new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), From ea2dce39927b7afcdfae79c059cc57342c70904e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 23:10:10 +0200 Subject: [PATCH 404/600] mavlink: MavlinkStream API simplifyed --- src/modules/mavlink/mavlink_main.cpp | 1 - src/modules/mavlink/mavlink_messages.cpp | 171 +++++++++-------------- src/modules/mavlink/mavlink_parameters.h | 2 - src/modules/mavlink/mavlink_stream.h | 1 - 4 files changed, 66 insertions(+), 109 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e49e99a9b6..0cf7080177 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -908,7 +908,6 @@ Mavlink::configure_stream(const char *stream_name, const float rate) /* create new instance */ stream = streams_list[i]->new_instance(this); stream->set_interval(interval); - stream->subscribe(); LL_APPEND(_streams, stream); return OK; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 5752688146..863b7a1d4d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -264,8 +264,8 @@ public: } private: - MavlinkOrbSubscription *status_sub; - MavlinkOrbSubscription *pos_sp_triplet_sub; + MavlinkOrbSubscription *_status_sub; + MavlinkOrbSubscription *_pos_sp_triplet_sub; /* do not allow top copying this class */ MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); @@ -273,28 +273,22 @@ private: protected: explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), - status_sub(nullptr), - pos_sp_triplet_sub(nullptr) + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void subscribe() - { - status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; /* always send the heartbeat, independent of the update status of the topics */ - if (!status_sub->update(&status)) { + if (!_status_sub->update(&status)) { /* if topic update failed fill it with defaults */ memset(&status, 0, sizeof(status)); } - if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) { + if (!_pos_sp_triplet_sub->update(&pos_sp_triplet)) { /* if topic update failed fill it with defaults */ memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); } @@ -347,14 +341,10 @@ private: protected: explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), - _cmd_sub(nullptr), + _cmd_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_command))), _cmd_time(0) {} - void subscribe() { - _cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_command)); - } - void send(const hrt_abstime t) { struct vehicle_command_s cmd; @@ -411,7 +401,7 @@ public: } private: - MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *_status_sub; /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &); @@ -419,19 +409,14 @@ private: protected: explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), - status_sub(nullptr) + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void subscribe() - { - status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; - if (status_sub->update(&status)) { + if (_status_sub->update(&status)) { mavlink_sys_status_t msg; msg.onboard_control_sensors_present = status.onboard_control_sensors_present; @@ -483,13 +468,13 @@ public: } private: - MavlinkOrbSubscription *sensor_sub; - uint64_t sensor_time; + MavlinkOrbSubscription *_sensor_sub; + uint64_t _sensor_time; - uint64_t accel_timestamp; - uint64_t gyro_timestamp; - uint64_t mag_timestamp; - uint64_t baro_timestamp; + uint64_t _accel_timestamp; + uint64_t _gyro_timestamp; + uint64_t _mag_timestamp; + uint64_t _baro_timestamp; /* do not allow top copying this class */ MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); @@ -497,48 +482,43 @@ private: protected: explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), - sensor_sub(nullptr), - sensor_time(0), - accel_timestamp(0), - gyro_timestamp(0), - mag_timestamp(0), - baro_timestamp(0) + _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_time(0), + _accel_timestamp(0), + _gyro_timestamp(0), + _mag_timestamp(0), + _baro_timestamp(0) {} - void subscribe() - { - sensor_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_combined)); - } - void send(const hrt_abstime t) { struct sensor_combined_s sensor; - if (sensor_sub->update(&sensor_time, &sensor)) { + if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (accel_timestamp != sensor.accelerometer_timestamp) { + if (_accel_timestamp != sensor.accelerometer_timestamp) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_timestamp = sensor.accelerometer_timestamp; + _accel_timestamp = sensor.accelerometer_timestamp; } - if (gyro_timestamp != sensor.timestamp) { + if (_gyro_timestamp != sensor.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_timestamp = sensor.timestamp; + _gyro_timestamp = sensor.timestamp; } - if (mag_timestamp != sensor.magnetometer_timestamp) { + if (_mag_timestamp != sensor.magnetometer_timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_timestamp = sensor.magnetometer_timestamp; + _mag_timestamp = sensor.magnetometer_timestamp; } - if (baro_timestamp != sensor.baro_timestamp) { + if (_baro_timestamp != sensor.baro_timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_timestamp = sensor.baro_timestamp; + _baro_timestamp = sensor.baro_timestamp; } mavlink_highres_imu_t msg; @@ -594,8 +574,8 @@ public: } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &); @@ -604,20 +584,15 @@ private: protected: explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), - att_sub(nullptr), - att_time(0) + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0) {} - void subscribe() - { - att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; - if (att_sub->update(&att_time, &att)) { + if (_att_sub->update(&_att_time, &att)) { mavlink_attitude_t msg; msg.time_boot_ms = att.timestamp / 1000; @@ -663,8 +638,8 @@ public: } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); @@ -672,20 +647,15 @@ private: protected: explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), - att_sub(nullptr), - att_time(0) + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0) {} - void subscribe() - { - att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; - if (att_sub->update(&att_time, &att)) { + if (_att_sub->update(&_att_time, &att)) { mavlink_attitude_quaternion_t msg; msg.time_boot_ms = att.timestamp / 1000; @@ -733,20 +703,20 @@ public: } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; - MavlinkOrbSubscription *armed_sub; - uint64_t armed_time; + MavlinkOrbSubscription *_armed_sub; + uint64_t _armed_time; - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; - MavlinkOrbSubscription *airspeed_sub; - uint64_t airspeed_time; + MavlinkOrbSubscription *_airspeed_sub; + uint64_t _airspeed_time; /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); @@ -754,27 +724,18 @@ private: protected: explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), - att_sub(nullptr), - att_time(0), - pos_sub(nullptr), - pos_time(0), - armed_sub(nullptr), - armed_time(0), - act_sub(nullptr), - act_time(0), - airspeed_sub(nullptr), - airspeed_time(0) + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))), + _armed_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), + _act_time(0), + _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), + _airspeed_time(0) {} - void subscribe() - { - att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - armed_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_armed)); - act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - airspeed_sub = _mavlink->add_orb_subscription(ORB_ID(airspeed)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; @@ -783,11 +744,11 @@ protected: struct actuator_controls_s act; struct airspeed_s airspeed; - bool updated = att_sub->update(&att_time, &att); - updated |= pos_sub->update(&pos_time, &pos); - updated |= armed_sub->update(&armed_time, &armed); - updated |= act_sub->update(&act_time, &act); - updated |= airspeed_sub->update(&airspeed_time, &airspeed); + bool updated = _att_sub->update(&_att_time, &att); + updated |= _pos_sub->update(&_pos_time, &pos); + updated |= _armed_sub->update(&_armed_time, &armed); + updated |= _act_sub->update(&_act_time, &act); + updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); if (updated) { mavlink_vfr_hud_t msg; diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 2a5a205e9e..5576e6b84a 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -109,8 +109,6 @@ private: protected: explicit MavlinkParametersManager(Mavlink *mavlink); - void subscribe() {} - void send(const hrt_abstime t); void send_param(param_t param); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 646931c07b..ef22dc443a 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -75,7 +75,6 @@ public: int update(const hrt_abstime t); static MavlinkStream *new_instance(const Mavlink *mavlink); static const char *get_name_static(); - virtual void subscribe() = 0; virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; From 9df1da1b7cd140e9452c73c6307befeadd6ce4c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 12:10:21 +0200 Subject: [PATCH 405/600] mavlink: STATUSTEXT implemented via streams API --- src/modules/mavlink/mavlink_main.cpp | 72 ++++----------- src/modules/mavlink/mavlink_main.h | 18 ++-- src/modules/mavlink/mavlink_messages.cpp | 106 +++++++++++++++++++---- 3 files changed, 117 insertions(+), 79 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0cf7080177..7d3fb97e1d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -452,7 +452,6 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { const char *txt = (const char *)arg; -// printf("logmsg: %s\n", txt); struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); msg.severity = (unsigned char)cmd; @@ -782,67 +781,32 @@ Mavlink::handle_message(const mavlink_message_t *msg) } } -int +void Mavlink::send_statustext_info(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); + send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); } -int +void Mavlink::send_statustext_critical(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); + send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); } -int +void Mavlink::send_statustext_emergency(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); + send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); } -int +void Mavlink::send_statustext(unsigned severity, const char *string) { - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; + struct mavlink_logmessage logmsg; + strncpy(logmsg.text, string, sizeof(logmsg.text)); + logmsg.severity = severity; - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') { - break; - } - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; - - /* Map severity */ - switch (severity) { - case MAVLINK_IOC_SEND_TEXT_INFO: - statustext.severity = MAV_SEVERITY_INFO; - break; - - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - statustext.severity = MAV_SEVERITY_CRITICAL; - break; - - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - statustext.severity = MAV_SEVERITY_EMERGENCY; - break; - } - - send_message(MAVLINK_MSG_ID_STATUSTEXT, &statustext); - return OK; - - } else { - return ERROR; - } + mavlink_logbuffer_write(&_logbuffer, &logmsg); } MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) @@ -1312,6 +1276,9 @@ Mavlink::task_main(int argc, char *argv[]) /* HEARTBEAT is constant rate stream, rate never adjusted */ configure_stream("HEARTBEAT", 1.0f); + /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */ + configure_stream("STATUSTEXT", 20.0f); + /* COMMAND_LONG stream: use high rate to avoid commands skipping */ configure_stream("COMMAND_LONG", 100.0f); @@ -1367,7 +1334,7 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); update_rate_mult(); - warnx("rate mult %.2f", (double)_rate_mult); + warnx("rate mult %.2f rate %.3f err %.3f", (double)_rate_mult, (double)_rate_tx, (double)_rate_txerr); if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ @@ -1407,15 +1374,6 @@ Mavlink::task_main(int argc, char *argv[]) if (fast_rate_limiter.check(t)) { // TODO missions //_mission_manager->eventloop(); - - if (!mavlink_logbuffer_is_empty(&_logbuffer)) { - struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); - - if (lb_ret == OK) { - send_statustext(msg.severity, msg.text); - } - } } /* pass messages from other UARTs or FTP worker */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 7fcbae72e8..87bb9d9ad5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -188,29 +188,33 @@ public: * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_info(const char *string); + void send_statustext_info(const char *string); /** * Send a status text with loglevel CRITICAL * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_critical(const char *string); + void send_statustext_critical(const char *string); /** * Send a status text with loglevel EMERGENCY * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_emergency(const char *string); + void send_statustext_emergency(const char *string); /** - * Send a status text with loglevel + * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent + * only on this mavlink connection. Useful for reporting communication specific, not system-wide info + * only to client interested in it. Message will be not sent immediately but queued in buffer as + * for mavlink_log_xxx(). * * @param string the message to send (will be capped by mavlink max string length) - * @param severity the log level, one of + * @param severity the log level */ - int send_statustext(unsigned severity, const char *string); + void send_statustext(unsigned severity, const char *string); + MavlinkStream * get_streams() const { return _streams; } float get_rate_mult(); @@ -259,6 +263,8 @@ public: */ struct telemetry_status_s& get_rx_status() { return _rstatus; } + struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + protected: Mavlink *next; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 863b7a1d4d..4333369fef 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -40,9 +40,9 @@ */ #include + #include #include - #include #include #include @@ -72,8 +72,8 @@ #include #include #include - #include +#include #include "mavlink_messages.h" #include "mavlink_main.h" @@ -306,6 +306,77 @@ protected: } }; +class MavlinkStreamStatustext : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamStatustext::get_name_static(); + } + + static const char *get_name_static() + { + return "STATUSTEXT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_STATUSTEXT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamStatustext(mavlink); + } + + unsigned get_size() { + return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + } + +private: + /* do not allow top copying this class */ + MavlinkStreamStatustext(MavlinkStreamStatustext &); + MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); + +protected: + explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) + { + if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { + struct mavlink_logmessage logmsg; + int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg); + + if (lb_ret == OK) { + mavlink_statustext_t msg; + + /* map severity */ + switch (logmsg.severity) { + case MAVLINK_IOC_SEND_TEXT_INFO: + msg.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + msg.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + msg.severity = MAV_SEVERITY_EMERGENCY; + break; + + default: + msg.severity = MAV_SEVERITY_INFO; + break; + } + strncpy(msg.text, logmsg.text, sizeof(msg.text)); + + _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); + } + } + } +}; + class MavlinkStreamCommandLong : public MavlinkStream { public: @@ -329,7 +400,9 @@ public: return new MavlinkStreamCommandLong(mavlink); } - unsigned get_size() { return 0; } // commands stream is not regular and not predictable + unsigned get_size() { + return 0; // commands stream is not regular and not predictable + } private: MavlinkOrbSubscription *_cmd_sub; @@ -352,21 +425,21 @@ protected: if (_cmd_sub->update(&_cmd_time, &cmd)) { /* only send commands for other systems/components */ if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { - mavlink_command_long_t mavcmd; + mavlink_command_long_t msg; - mavcmd.target_system = cmd.target_system; - mavcmd.target_component = cmd.target_component; - mavcmd.command = cmd.command; - mavcmd.confirmation = cmd.confirmation; - mavcmd.param1 = cmd.param1; - mavcmd.param2 = cmd.param2; - mavcmd.param3 = cmd.param3; - mavcmd.param4 = cmd.param4; - mavcmd.param5 = cmd.param5; - mavcmd.param6 = cmd.param6; - mavcmd.param7 = cmd.param7; + msg.target_system = cmd.target_system; + msg.target_component = cmd.target_component; + msg.command = cmd.command; + msg.confirmation = cmd.confirmation; + msg.param1 = cmd.param1; + msg.param2 = cmd.param2; + msg.param3 = cmd.param3; + msg.param4 = cmd.param4; + msg.param5 = cmd.param5; + msg.param6 = cmd.param6; + msg.param7 = cmd.param7; - _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &mavcmd); + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); } } } @@ -1996,6 +2069,7 @@ protected: StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), + new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static), new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), From eb4015ced1b55a280fe93e10811c0dad5c719438 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 12:10:56 +0200 Subject: [PATCH 406/600] mavlink: strange FIXME comment removed --- src/modules/mavlink/mavlink_mission.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index b792b9aaf9..5bf80768cf 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -46,7 +46,6 @@ #include "mavlink_rate_limiter.h" #include -// FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { MAVLINK_WPM_STATE_IDLE = 0, MAVLINK_WPM_STATE_SENDLIST, From 897984c4f4a43136320c39f75fef106d3447ce47 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 12:28:02 +0200 Subject: [PATCH 407/600] mavlink: bug fixed, don't delete parameters stream --- src/modules/mavlink/mavlink_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7d3fb97e1d..44329e0d20 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1443,7 +1443,6 @@ Mavlink::task_main(int argc, char *argv[]) } delete _mission_manager; - delete _parameters_manager; delete _subscribe_to_stream; _subscribe_to_stream = nullptr; From 25ef4bc4a0557327af1f32d81c86e8981772a844 Mon Sep 17 00:00:00 2001 From: ggregory8 Date: Thu, 24 Jul 2014 22:31:45 +0800 Subject: [PATCH 408/600] Use current rotation matrix for vision instead of delayed rotation --- .../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 d3192fec21..06a81e3fee 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -912,7 +912,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float c = 0.0f; for (int j = 0; j < 3; j++) { - c += R_gps[j][i] * accel_bias_corr[j]; + c += att.R[j][i] * accel_bias_corr[j]; } if (isfinite(c)) { From 87334a987a5b571f6cc3d962df1194584de4fd7f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Jul 2014 17:42:45 +0200 Subject: [PATCH 409/600] Return 0 for a non-reset --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 768e0be350..3d504d3955 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2773,7 +2773,7 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) ResetHeight(); ResetStoredStates(); - ret = 3; + ret = 0; } // Reset the filter if gyro offsets are excessive From f03f6ddd74b1aca710f4675bbee18608eeea1f08 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Jul 2014 17:57:15 +0200 Subject: [PATCH 410/600] Improve user feedback on mission load fails --- src/modules/navigator/mission.cpp | 36 ++++++++++++++++++------------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ba766cd10f..12f6b9c211 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -314,30 +314,36 @@ Mission::set_mission_items() /* set previous position setpoint to current */ set_previous_pos_setpoint(); + /* get home distance state */ + bool home_dist_ok = check_dist_1wp(); + /* the home dist check provides user feedback, so we initialize it to this */ + bool user_feedback_done = !home_dist_ok; + /* try setting onboard mission item */ if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); } _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) { + } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; } else { - /* no mission available, switch to loiter */ + /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: mission finished"); - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: no mission available"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + } else if (!user_feedback_done) { + /* only tell users that we got no mission if there has not been any + * better, more specific feedback yet + */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available"); } _mission_type = MISSION_TYPE_NONE; @@ -397,7 +403,7 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); + mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; @@ -483,7 +489,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR waypoint could not be read"); + "ERROR waypoint could not be read"); return false; } @@ -502,7 +508,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP waypoint could not be written"); + "ERROR DO JUMP waypoint could not be written"); return false; } } @@ -511,8 +517,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: DO JUMP repetitions completed"); + mavlink_log_critical(_navigator->get_mavlink_fd(), + "DO JUMP repetitions completed"); /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } @@ -526,7 +532,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* we have given up, we don't want to cycle forever */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP is cycling, giving up"); + "ERROR DO JUMP is cycling, giving up"); return false; } From b4e6f535ea15055f39b122eb87004c97796eb584 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 24 Jul 2014 17:57:30 +0200 Subject: [PATCH 411/600] mavlink: onboard links should only pass on messages from the same system ID --- src/modules/mavlink/mavlink_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0c6f8c42f8..8cb0152fda 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -483,7 +483,12 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { - inst->pass_message(msg); + + /* if not in normal mode, we are an onboard link + * onboard links should only pass on messages from the same system ID */ + if(!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { + inst->pass_message(msg); + } } } } From 36d8d73aeb7ef214979a64960df727bf3bab048c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 24 Jul 2014 17:58:17 +0200 Subject: [PATCH 412/600] mavlink: use correct component ID --- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6885bebde5..92e469619d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1832,11 +1832,11 @@ protected: || status.arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, mavlink_system.compid, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, mavlink_system.compid, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; From 58aa9f28f060f7adf7955b39d72ac972c03f1348 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 24 Jul 2014 17:59:02 +0200 Subject: [PATCH 413/600] mavlink: when detecting spoof, be more verbose --- src/modules/mavlink/mavlink_receiver.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 54c412ce7c..69e3ef31d7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -241,7 +241,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD spoofed with same SYS/COMP ID"); + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); return; } From b31ddc193c0e3dff3a8e4b4c343848f0f64e61ed Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 19:31:25 +0200 Subject: [PATCH 414/600] mavlink: tix mission manager to use MavlinkStream API --- src/modules/mavlink/mavlink_main.cpp | 27 ++--- src/modules/mavlink/mavlink_main.h | 2 - src/modules/mavlink/mavlink_mission.cpp | 25 +++- src/modules/mavlink/mavlink_mission.h | 118 +++++++++++-------- src/modules/mavlink/mavlink_rate_limiter.cpp | 2 +- 5 files changed, 97 insertions(+), 77 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 44329e0d20..df078325a1 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -131,8 +131,6 @@ Mavlink::Mavlink() : _streams(nullptr), _mission_manager(nullptr), _parameters_manager(nullptr), - _mission_pub(-1), - _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _logbuffer {}, @@ -1255,12 +1253,6 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - - /* create mission manager */ - _mission_manager = new MavlinkMissionManager(this); - _mission_manager->set_verbose(_verbose); - _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); @@ -1287,6 +1279,14 @@ Mavlink::task_main(int argc, char *argv[]) _parameters_manager->set_interval(interval_from_rate(30.0f)); LL_APPEND(_streams, _parameters_manager); + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); @@ -1315,12 +1315,8 @@ Mavlink::task_main(int argc, char *argv[]) break; } - float base_rate_mult = _datarate / 1000.0f; - - MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult); - /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = MAIN_LOOP_DELAY / base_rate_mult; + _main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate; /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); @@ -1371,11 +1367,6 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - if (fast_rate_limiter.check(t)) { - // TODO missions - //_mission_manager->eventloop(); - } - /* pass messages from other UARTs or FTP worker */ if (_passing_on || _ftp_on) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 87bb9d9ad5..e3bad828f0 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -289,8 +289,6 @@ private: MavlinkMissionManager *_mission_manager; MavlinkParametersManager *_parameters_manager; - orb_advert_t _mission_pub; - int _mission_result_sub; MAVLINK_MODE _mode; mavlink_channel_t _channel; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index d17ccc6f9b..7a761588a9 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -59,8 +59,7 @@ static const int ERROR = -1; -MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : - _mavlink(mavlink), +MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), _state(MAVLINK_WPM_STATE_IDLE), _time_last_recv(0), _time_last_sent(0), @@ -79,9 +78,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _offboard_mission_sub(-1), _mission_result_sub(-1), _offboard_mission_pub(-1), - _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), + _slow_rate_limiter(_interval / 10.0f), _verbose(false), - _channel(mavlink->get_channel()), _comp_id(MAV_COMP_ID_MISSIONPLANNER) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); @@ -96,6 +94,20 @@ MavlinkMissionManager::~MavlinkMissionManager() close(_mission_result_sub); } +unsigned +MavlinkMissionManager::get_size() +{ + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + void MavlinkMissionManager::init_offboard_mission() { @@ -275,9 +287,10 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq) void -MavlinkMissionManager::eventloop() +MavlinkMissionManager::send(const hrt_abstime now) { - hrt_abstime now = hrt_absolute_time(); + /* update interval for slow rate limiter */ + _slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult()); bool updated = false; orb_check(_mission_result_sub, &updated); diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 5bf80768cf..8ff27f87d4 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -42,9 +42,11 @@ #pragma once +#include + #include "mavlink_bridge_header.h" #include "mavlink_rate_limiter.h" -#include +#include "mavlink_stream.h" enum MAVLINK_WPM_STATES { MAVLINK_WPM_STATE_IDLE = 0, @@ -65,20 +67,75 @@ enum MAVLINK_WPM_CODES { #define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds #define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds -class Mavlink; - -class MavlinkMissionManager { +class MavlinkMissionManager : public MavlinkStream { public: - MavlinkMissionManager(Mavlink *parent); - ~MavlinkMissionManager(); + const char *get_name() const + { + return MavlinkMissionManager::get_name_static(); + } + + static const char *get_name_static() + { + return "MISSION_ITEM"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_MISSION_ITEM; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkMissionManager(mavlink); + } + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + + void set_verbose(bool v) { _verbose = v; } + +private: + enum MAVLINK_WPM_STATES _state; ///< Current state + + uint64_t _time_last_recv; + uint64_t _time_last_sent; + + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximum number of mission items + + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission + + int _transfer_dataman_id; ///< Dataman storage ID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + unsigned _transfer_seq; ///< Item sequence in current transmission + unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission + unsigned _transfer_partner_compid; ///< Partner component ID for current transmission + + int _offboard_mission_sub; + int _mission_result_sub; + orb_advert_t _offboard_mission_pub; + + MavlinkRateLimiter _slow_rate_limiter; + + bool _verbose; + + uint8_t _comp_id; + + /* do not allow top copying this class */ + MavlinkMissionManager(MavlinkMissionManager &); + MavlinkMissionManager& operator = (const MavlinkMissionManager &); + void init_offboard_mission(); int update_active_mission(int dataman_id, unsigned count, int seq); - void send_message(mavlink_message_t *msg); - /** * @brief Sends an waypoint ack message */ @@ -110,10 +167,6 @@ public: */ void send_mission_item_reached(uint16_t seq); - void eventloop(); - - void handle_message(const mavlink_message_t *msg); - void handle_mission_ack(const mavlink_message_t *msg); void handle_mission_set_current(const mavlink_message_t *msg); @@ -138,43 +191,8 @@ public: */ int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - void set_verbose(bool v) { _verbose = v; } +protected: + explicit MavlinkMissionManager(Mavlink *mavlink); -private: - Mavlink* _mavlink; - - enum MAVLINK_WPM_STATES _state; ///< Current state - - uint64_t _time_last_recv; - uint64_t _time_last_sent; - - uint32_t _action_timeout; - uint32_t _retry_timeout; - unsigned _max_count; ///< Maximum number of mission items - - int _dataman_id; ///< Dataman storage ID for active mission - unsigned _count; ///< Count of items in active mission - int _current_seq; ///< Current item sequence in active mission - - int _transfer_dataman_id; ///< Dataman storage ID for current transmission - unsigned _transfer_count; ///< Items count in current transmission - unsigned _transfer_seq; ///< Item sequence in current transmission - unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) - unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission - unsigned _transfer_partner_compid; ///< Partner component ID for current transmission - - int _offboard_mission_sub; - int _mission_result_sub; - orb_advert_t _offboard_mission_pub; - - MavlinkRateLimiter _slow_rate_limiter; - - bool _verbose; - - mavlink_channel_t _channel; - uint8_t _comp_id; - - /* do not allow copying this class */ - MavlinkMissionManager(const MavlinkMissionManager&); - MavlinkMissionManager operator=(const MavlinkMissionManager&); + void send(const hrt_abstime t); }; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp index 9cdda8b17c..d3b3e1cde3 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.cpp +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t) uint64_t dt = t - _last_sent; if (dt > 0 && dt >= _interval) { - _last_sent = (t / _interval) * _interval; + _last_sent = t; return true; } From c486aa536f3c0169e1abb0f4c2b60baa1e2b053b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 19:34:19 +0200 Subject: [PATCH 415/600] mavlink: bug fixed, don't delete mission manages twice --- src/modules/mavlink/mavlink_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index df078325a1..e27a940cf8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1433,8 +1433,6 @@ Mavlink::task_main(int argc, char *argv[]) perf_end(_loop_perf); } - delete _mission_manager; - delete _subscribe_to_stream; _subscribe_to_stream = nullptr; From 53f1b3190215f373aa5d8d38264b4ba5771b1676 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Jul 2014 21:41:09 +0200 Subject: [PATCH 416/600] Do not abort on submodule init feedback --- Tools/check_submodules.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 0c03396e7b..3904a27753 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -73,8 +73,8 @@ then exit 1 fi else - git submodule init - git submodule update + git submodule init; + git submodule update; fi exit 0 From b72e89697c564c878c4e17c46b435e8fe94736c5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 25 Jul 2014 00:52:42 +0400 Subject: [PATCH 417/600] UAVCAN update for #1229 --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index dca2611c31..f51ee9cdec 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit dca2611c3186eaa1cac42557f07b013e2dc633d3 +Subproject commit f51ee9cdecd7841f6208bfb0b16228888daad663 From fd52e5561eaf14f9c6ca1abc00cb82b0e3d38ec8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 23:46:39 +0200 Subject: [PATCH 418/600] update comment about condition_global_position_valid --- src/modules/uORB/topics/vehicle_status.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b46c00b758..cbb148edaf 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -186,7 +186,7 @@ struct vehicle_status_s { bool condition_system_sensors_initialized; bool condition_system_returned_to_home; bool condition_auto_mission_available; - bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool condition_global_position_valid; /**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */ bool condition_launch_position_valid; /**< indicates a valid launch position */ bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; From e92a0cd10dc20ced072c77a0183a6112179f4639 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 25 Jul 2014 02:14:35 +0400 Subject: [PATCH 419/600] UAVCAN update for #1229 (windoze) --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index f51ee9cdec..d84fc8a846 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit f51ee9cdecd7841f6208bfb0b16228888daad663 +Subproject commit d84fc8a84678d93f97f93b240c81472797ca5889 From 8f0af1c5fe5a843c56fff2dc70acb2fc0e7e1b90 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Jul 2014 00:16:02 +0200 Subject: [PATCH 420/600] mavlink: forwarding and FTP fixed, flow control detector fixed --- src/modules/mavlink/mavlink_main.cpp | 97 +++++++++++++++++++--------- src/modules/mavlink/mavlink_main.h | 19 ++++-- 2 files changed, 77 insertions(+), 39 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e27a940cf8..aaee0455aa 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -93,7 +93,7 @@ static const int ERROR = -1; #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate -#define TX_BUFFER_GAP 256 +#define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN static Mavlink *_mavlink_instances = nullptr; @@ -252,25 +252,6 @@ Mavlink::count_txerr() perf_count(_txerr_perf); } -unsigned -Mavlink::get_free_tx_buf() -{ - unsigned buf_free; - - if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { - if (_rstatus.telem_time > 0 && - (hrt_elapsed_time(&_rstatus.telem_time) < (2 * 1000 * 1000))) { - - return (unsigned)(buf_free * 0.01f * _rstatus.txbuf); - - } else { - return buf_free; - } - } else { - return 0; - } -} - void Mavlink::set_mode(enum MAVLINK_MODE mode) { @@ -686,15 +667,9 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -void -Mavlink::send_message(const uint8_t msgid, const void *msg) +unsigned +Mavlink::get_free_tx_buf() { - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (!should_transmit()) { - return; - } - /* * Check if the OS buffer is full and disable HW * flow control if it continues to be full @@ -702,7 +677,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) int buf_free = 0; (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); - if (get_flow_control_enabled() && buf_free == 0) { + if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) { /* Disable hardware flow control: * if no successful write since a defined time * and if the last try was not the last successful write @@ -714,6 +689,19 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) enable_flow_control(false); } } + return buf_free; +} + +void +Mavlink::send_message(const uint8_t msgid, const void *msg) +{ + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + + int buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; @@ -722,7 +710,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* check if there is space in the buffer, let it overflow else */ if (buf_free < TX_BUFFER_GAP) { - warnx("SKIP msgid %i, %i bytes", msgid, packet_len); + warnx("SKIP msgid %i, %i bytes, free %i", msgid, packet_len, buf_free); /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -764,6 +752,52 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) } } +void +Mavlink::resend_message(mavlink_message_t *msg) +{ + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + + int buf_free = get_free_tx_buf(); + + _last_write_try_time = hrt_absolute_time(); + + unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + /* check if there is space in the buffer, let it overflow else */ + if (buf_free < TX_BUFFER_GAP) { + warnx("SKIP resent msgid %i, %i bytes, free %i", msg->msgid, packet_len, buf_free); + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + return; + } + + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header and payload */ + memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len); + + /* checksum */ + buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); + + /* send message to UART */ + ssize_t ret = write(_uart_fd, buf, packet_len); + + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); + + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); + } +} + void Mavlink::handle_message(const mavlink_message_t *msg) { @@ -1411,8 +1445,7 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_unlock(&_message_buffer_mutex); - // TODO implement message resending - //_mavlink_resend_uart(_channel, &msg); + resend_message(&msg); } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e3bad828f0..4121e286e0 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -162,6 +162,11 @@ public: void send_message(const uint8_t msgid, const void *msg); + /** + * Resend message as is, don't change sequence number and CRC. + */ + void resend_message(mavlink_message_t *msg); + void handle_message(const mavlink_message_t *msg); MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); @@ -251,13 +256,6 @@ public: */ void count_rxbytes(unsigned n) { _bytes_rx += n; }; - /** - * Get the free space in the transmit buffer - * - * @return free space in the UART TX buffer - */ - unsigned get_free_tx_buf(); - /** * Get the receive status of this MAVLink link */ @@ -358,6 +356,13 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + static unsigned int interval_from_rate(float rate); int configure_stream(const char *stream_name, const float rate); From 80a197f0fffb39037ce271bbc2e2107f2208eb01 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 25 Jul 2014 09:45:23 +0200 Subject: [PATCH 421/600] Revert "mavlink: use correct component ID" This reverts commit 36d8d73aeb7ef214979a64960df727bf3bab048c. --- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 92e469619d..6885bebde5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1832,11 +1832,11 @@ protected: || status.arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, mavlink_system.compid, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, mavlink_system.compid, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; From 1938ef16e39ff937a3076489f3eff0a2eb4bbb65 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Jul 2014 14:27:07 +0200 Subject: [PATCH 422/600] mavlink: don't scale up rates, debug output removed --- src/modules/mavlink/mavlink_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index aaee0455aa..572d5c19b7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -610,6 +610,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * if (enable_flow_control(true)) { warnx("hardware flow control not supported"); } + + } else { + _flow_control_enabled = false; } return _uart_fd; @@ -651,8 +654,7 @@ Mavlink::set_hil_enabled(bool hil_enabled) /* enable HIL */ if (hil_enabled && !_hil_enabled) { _hil_enabled = true; - float rate_mult = _datarate / 1000.0f; - configure_stream("HIL_CONTROLS", 15.0f * rate_mult); + configure_stream("HIL_CONTROLS", 150.0f); } /* disable HIL */ @@ -710,7 +712,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* check if there is space in the buffer, let it overflow else */ if (buf_free < TX_BUFFER_GAP) { - warnx("SKIP msgid %i, %i bytes, free %i", msgid, packet_len, buf_free); /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -769,7 +770,6 @@ Mavlink::resend_message(mavlink_message_t *msg) /* check if there is space in the buffer, let it overflow else */ if (buf_free < TX_BUFFER_GAP) { - warnx("SKIP resent msgid %i, %i bytes, free %i", msg->msgid, packet_len, buf_free); /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -1120,7 +1120,8 @@ Mavlink::update_rate_mult() } } - _rate_mult = ((float)_datarate - const_rate) / rate; + /* don't scale up rates, only scale down if needed */ + _rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate); } int @@ -1364,7 +1365,6 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); update_rate_mult(); - warnx("rate mult %.2f rate %.3f err %.3f", (double)_rate_mult, (double)_rate_tx, (double)_rate_txerr); if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ From 07c4144cde1026c4d966d043d12e36ba686c0781 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Jul 2014 14:30:06 +0200 Subject: [PATCH 423/600] mavlink: message buffer size fixed --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 572d5c19b7..408799be41 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1268,7 +1268,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) { + if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } From c5a623e15892d63eb3571bc450edea368b2894db Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Jul 2014 14:30:45 +0200 Subject: [PATCH 424/600] rcS: set rates for parameters and missions on USB connection --- ROMFS/px4fmu_common/init.d/rc.usb | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 1f8d8b8622..d31ef11cfd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,6 +5,10 @@ mavlink start -r 10000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB +mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200 +usleep 100000 +mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 +usleep 100000 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 usleep 100000 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 From 2de38d0628f3146caea28cd42b30840241269f41 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Fri, 25 Jul 2014 23:30:37 -0400 Subject: [PATCH 425/600] Improve update performance and clean up compiler warnings in px4io driver - Fix compiler warnings in px4io_serial.cpp - Fix compiler warnings in px4io_uploader.cpp - Rename confusing overloaded send method with nearly identical parameters in px4io_uploader.cpp - Improve update performance by using maximum size programming buffer since we are no longer limited by stack size. --- src/drivers/px4io/px4io_serial.cpp | 8 ++++++- src/drivers/px4io/px4io_uploader.cpp | 32 +++++++++++++++------------- src/drivers/px4io/uploader.h | 8 +++---- 3 files changed, 28 insertions(+), 20 deletions(-) diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index c39494fb0d..d227e15d57 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -157,6 +157,10 @@ private: perf_counter_t _pc_idle; perf_counter_t _pc_badidle; + /* do not allow top copying this class */ + PX4IO_serial(PX4IO_serial &); + PX4IO_serial& operator = (const PX4IO_serial &); + }; IOPacket PX4IO_serial::_dma_buffer; @@ -173,7 +177,9 @@ PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), _rx_dma_status(_dma_status_inactive), - _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), + _bus_semaphore(SEM_INITIALIZER(0)), + _completion_semaphore(SEM_INITIALIZER(0)), + _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), _pc_retries(perf_alloc(PC_COUNT, "io_retries ")), _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")), diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index bf6893a7e5..986e39dc88 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -65,7 +65,8 @@ PX4IO_Uploader::PX4IO_Uploader() : _io_fd(-1), - _fw_fd(-1) + _fw_fd(-1), + bl_rev(0) { } @@ -245,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[]) } int -PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) +PX4IO_Uploader::recv_byte_with_timeout(uint8_t *c, unsigned timeout) { struct pollfd fds[1]; @@ -262,22 +263,23 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) return -ETIMEDOUT; } - read(_io_fd, &c, 1); + read(_io_fd, c, 1); #ifdef UDEBUG - log("recv 0x%02x", c); + log("recv_bytes 0x%02x", c); #endif return OK; } int -PX4IO_Uploader::recv(uint8_t *p, unsigned count) +PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count) { - int ret; - while (count--) { - ret = recv(*p++, 5000); + int ret = OK; + while (count) { + ret = recv_byte_with_timeout(p++, 5000); if (ret != OK) break; + count--; } return ret; } @@ -289,10 +291,10 @@ PX4IO_Uploader::drain() int ret; do { - // the small recv timeout here is to allow for fast + // the small recv_bytes timeout here is to allow for fast // drain when rebooting the io board for a forced // update of the fw without using the safety switch - ret = recv(c, 40); + ret = recv_byte_with_timeout(&c, 40); #ifdef UDEBUG if (ret == OK) { @@ -331,12 +333,12 @@ PX4IO_Uploader::get_sync(unsigned timeout) uint8_t c[2]; int ret; - ret = recv(c[0], timeout); + ret = recv_byte_with_timeout(c, timeout); if (ret != OK) return ret; - ret = recv(c[1], timeout); + ret = recv_byte_with_timeout(c + 1, timeout); if (ret != OK) return ret; @@ -372,7 +374,7 @@ PX4IO_Uploader::get_info(int param, uint32_t &val) send(param); send(PROTO_EOC); - ret = recv((uint8_t *)&val, sizeof(val)); + ret = recv_bytes((uint8_t *)&val, sizeof(val)); if (ret != OK) return ret; @@ -513,7 +515,7 @@ PX4IO_Uploader::verify_rev2(size_t fw_size) for (ssize_t i = 0; i < count; i++) { uint8_t c; - ret = recv(c, 5000); + ret = recv_byte_with_timeout(&c, 5000); if (ret != OK) { log("%d: got %d waiting for bytes", sent + i, ret); @@ -600,7 +602,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) send(PROTO_GET_CRC); send(PROTO_EOC); - ret = recv((uint8_t*)(&crc), sizeof(crc)); + ret = recv_bytes((uint8_t*)(&crc), sizeof(crc)); if (ret != OK) { log("did not receive CRC checksum"); diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 3e2142cf28..e175234132 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -74,19 +74,19 @@ private: INFO_BOARD_REV = 3, /**< board revision */ INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ - PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */ + PROG_MULTI_MAX = 248, /**< protocol max is 255, must be multiple of 4 */ }; int _io_fd; int _fw_fd; - uint32_t bl_rev; /**< bootloader revision */ + uint32_t bl_rev; /**< bootloader revision */ void log(const char *fmt, ...); - int recv(uint8_t &c, unsigned timeout); - int recv(uint8_t *p, unsigned count); + int recv_byte_with_timeout(uint8_t *c, unsigned timeout); + int recv_bytes(uint8_t *p, unsigned count); void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); From 3a4da7c5fa827970a86777ee6f4dc201246f0d0d Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sat, 26 Jul 2014 09:05:44 -0400 Subject: [PATCH 426/600] Revert to original loop Original loop was correct, and slightly more efficient. Retain initialization of ret to handle the case where passed in count is 0. --- src/drivers/px4io/px4io_uploader.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 986e39dc88..fb16f891f6 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -274,12 +274,11 @@ int PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count) { int ret = OK; - while (count) { + while (count--) { ret = recv_byte_with_timeout(p++, 5000); if (ret != OK) break; - count--; } return ret; } From 54b9698d6560290e386b8dd2a7b9f0b6f4c5f57a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Jul 2014 17:48:45 +0200 Subject: [PATCH 427/600] circuit_breakers: added param to disable airspeed check --- src/modules/commander/commander.cpp | 2 ++ src/modules/commander/state_machine_helper.cpp | 4 +++- src/modules/systemlib/circuit_breaker.c | 12 ++++++++++++ src/modules/systemlib/circuit_breaker.h | 1 + src/modules/uORB/topics/vehicle_status.h | 1 + 5 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a0c8961788..daba4e7403 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -736,6 +736,7 @@ int commander_thread_main(int argc, char *argv[]) // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; + status.circuit_breaker_engaged_airspd_check = false; /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -977,6 +978,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_component_id, &(status.component_id)); status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); status_changed = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7b26e3e8cb..3c3d2f2335 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -669,7 +669,9 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) goto system_eval; } - if (!status->is_rotary_wing) { + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { /* accel done, close it */ close(fd); fd = orb_subscribe(ORB_ID(airspeed)); diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 8f697749ed..64317a18a8 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -83,6 +83,18 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); */ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); +/** + * Circuit breaker for airspeed sensor + * + * Setting this parameter to 162128 will disable the check for an airspeed sensor. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 162128 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 1175dbce86..b605846082 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -52,6 +52,7 @@ #define CBRK_SUPPLY_CHK_KEY 894281 #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 +#define CBRK_AIRSPD_CHK_KEY 162128 #include diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index cbb148edaf..b683bf98ad 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -226,6 +226,7 @@ struct vehicle_status_s { uint16_t errors_count4; bool circuit_breaker_engaged_power_check; + bool circuit_breaker_engaged_airspd_check; }; /** From 5d36381dc5f3bbb1eefc70158680f9622ac437b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Jul 2014 23:38:14 +0200 Subject: [PATCH 428/600] EKF: Fix wind publication, fix commented out flags --- .../ekf_att_pos_estimator_main.cpp | 27 +++---------------- 1 file changed, 4 insertions(+), 23 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index ccc497323c..91d17e787c 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -630,10 +630,10 @@ FixedwingEstimator::check_filter_state() rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); - // rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); - // rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); - // rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); - // rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); + rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); + rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); + rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); + rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); @@ -1465,25 +1465,6 @@ FixedwingEstimator::task_main() _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = 0.0f; // XXX get form filter - _wind.covariance_east = 0.0f; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - - } - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; From 8e12d79ef4b32da98dfb13af1321a6855ecbdc3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Jul 2014 23:43:39 +0200 Subject: [PATCH 429/600] Increase GPS position timeout to real-life timeouts. More work needed. --- 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 daba4e7403..6c24734e52 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -124,7 +124,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 (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ +#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 #define DL_TIMEOUT 5 * 1000* 1000 @@ -1111,7 +1111,7 @@ int commander_thread_main(int argc, char *argv[]) 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) { + if (global_position.eph > eph_epv_threshold * 2.5f || global_position.epv > eph_epv_threshold * 2.5f) { eph_epv_good = false; } else { From e1361fdc02a75d72849b52a0102e02e400eecd9e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 00:07:01 +0200 Subject: [PATCH 430/600] mavlink: GPS_RAW_INT stream added --- src/modules/mavlink/mavlink_messages.cpp | 134 ++++++++++++----------- 1 file changed, 71 insertions(+), 63 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4333369fef..8f0c15f5ae 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -839,69 +839,77 @@ protected: }; -//class MavlinkStreamGPSRawInt : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamGPSRawInt::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "GPS_RAW_INT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_GPS_RAW_INT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamGPSRawInt(); -// } -// -//private: -// MavlinkOrbSubscription *gps_sub; -// uint64_t gps_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); -// MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); -// -//protected: -// explicit MavlinkStreamGPSRawInt() : MavlinkStream(), -// gps_sub(nullptr), -// gps_time(0) -// {} -// -// void subscribe() -// { -// gps_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_gps_position_s gps; -// -// if (gps_sub->update(&gps_time, &gps)) { -// mavlink_msg_gps_raw_int_send(_channel, -// gps.timestamp_position, -// gps.fix_type, -// gps.lat, -// gps.lon, -// gps.alt, -// cm_uint16_from_m_float(gps.eph), -// cm_uint16_from_m_float(gps.epv), -// gps.vel_m_s * 100.0f, -// _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, -// gps.satellites_used); -// } -// } -//}; -// -// +class MavlinkStreamGPSRawInt : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGPSRawInt::get_name_static(); + } + + static const char *get_name_static() + { + return "GPS_RAW_INT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_RAW_INT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGPSRawInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_gps_sub; + uint64_t _gps_time; + + /* do not allow top copying this class */ + MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); + MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); + +protected: + explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _gps_sub(nullptr), + _gps_time(0) + {} + + void subscribe() + { + _gps_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); + } + + void send(const hrt_abstime t) + { + struct vehicle_gps_position_s gps; + + if (_gps_sub->update(&_gps_time, &gps)) { + mavlink_gps_raw_int_t msg; + + msg.time_usec = gps.timestamp_position; + msg.fix_type = gps.fix_type; + msg.lat = gps.lat; + msg.lon = gps.lon; + msg.alt = gps.alt; + msg.eph = cm_uint16_from_m_float(gps.eph); + msg.epv = cm_uint16_from_m_float(gps.epv); + msg.vel = cm_uint16_from_m_float(gps.vel_m_s), + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); + } + } +}; + + //class MavlinkStreamGlobalPositionInt : public MavlinkStream //{ //public: From f1b55e578ff17d59cba13193cca4c83e764854b6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 11:02:56 +0200 Subject: [PATCH 431/600] mavlink: more streams ported to new API --- src/modules/mavlink/mavlink_messages.cpp | 713 ++++++++++++----------- 1 file changed, 362 insertions(+), 351 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8f0c15f5ae..fe33c1bea6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -877,15 +877,10 @@ private: protected: explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), - _gps_sub(nullptr), + _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))), _gps_time(0) {} - void subscribe() - { - _gps_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); - } - void send(const hrt_abstime t) { struct vehicle_gps_position_s gps; @@ -910,339 +905,355 @@ protected: }; -//class MavlinkStreamGlobalPositionInt : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamGlobalPositionInt::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "GLOBAL_POSITION_INT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamGlobalPositionInt(); -// } -// -//private: -// MavlinkOrbSubscription *pos_sub; -// uint64_t pos_time; -// -// MavlinkOrbSubscription *home_sub; -// uint64_t home_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); -// MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); -// -//protected: -// explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), -// pos_sub(nullptr), -// pos_time(0), -// home_sub(nullptr), -// home_time(0) -// {} -// -// void subscribe() -// { -// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); -// home_sub = _mavlink->add_orb_subscription(ORB_ID(home_position)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_global_position_s pos; -// struct home_position_s home; -// -// bool updated = pos_sub->update(&pos_time, &pos); -// updated |= home_sub->update(&home_time, &home); -// -// if (updated) { -// mavlink_msg_global_position_int_send(_channel, -// pos.timestamp / 1000, -// pos.lat * 1e7, -// pos.lon * 1e7, -// pos.alt * 1000.0f, -// (pos.alt - home.alt) * 1000.0f, -// pos.vel_n * 100.0f, -// pos.vel_e * 100.0f, -// pos.vel_d * 100.0f, -// _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); -// } -// } -//}; -// -// -//class MavlinkStreamLocalPositionNED : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamLocalPositionNED::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "LOCAL_POSITION_NED"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_LOCAL_POSITION_NED; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamLocalPositionNED(); -// } -// -//private: -// MavlinkOrbSubscription *pos_sub; -// uint64_t pos_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); -// MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); -// -//protected: -// explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), -// pos_sub(nullptr), -// pos_time(0) -// {} -// -// void subscribe() -// { -// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_local_position_s pos; -// -// if (pos_sub->update(&pos_time, &pos)) { -// mavlink_msg_local_position_ned_send(_channel, -// pos.timestamp / 1000, -// pos.x, -// pos.y, -// pos.z, -// pos.vx, -// pos.vy, -// pos.vz); -// } -// } -//}; -// -// -// -//class MavlinkStreamViconPositionEstimate : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamViconPositionEstimate::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "VICON_POSITION_ESTIMATE"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamViconPositionEstimate(); -// } -// -//private: -// MavlinkOrbSubscription *pos_sub; -// uint64_t pos_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); -// MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); -// -//protected: -// explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), -// pos_sub(nullptr), -// pos_time(0) -// {} -// -// void subscribe() -// { -// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_vicon_position_s pos; -// -// if (pos_sub->update(&pos_time, &pos)) { -// mavlink_msg_vicon_position_estimate_send(_channel, -// pos.timestamp / 1000, -// pos.x, -// pos.y, -// pos.z, -// pos.roll, -// pos.pitch, -// pos.yaw); -// } -// } -//}; -// -// -//class MavlinkStreamGPSGlobalOrigin : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamGPSGlobalOrigin::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "GPS_GLOBAL_ORIGIN"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamGPSGlobalOrigin(); -// } -// -//private: -// MavlinkOrbSubscription *home_sub; -// -// /* do not allow top copying this class */ -// MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); -// MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); -// -//protected: -// explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), -// home_sub(nullptr) -// {} -// -// void subscribe() -// { -// home_sub = _mavlink->add_orb_subscription(ORB_ID(home_position)); -// } -// -// void send(const hrt_abstime t) -// { -// /* we're sending the GPS home periodically to ensure the -// * the GCS does pick it up at one point */ -// if (home_sub->is_published()) { -// struct home_position_s home; -// -// if (home_sub->update(&home)) { -// mavlink_msg_gps_global_origin_send(_channel, -// (int32_t)(home.lat * 1e7), -// (int32_t)(home.lon * 1e7), -// (int32_t)(home.alt) * 1000.0f); -// } -// } -// } -//}; -// -//template -//class MavlinkStreamServoOutputRaw : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamServoOutputRaw::get_name_static(); -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; -// } -// -// static const char *get_name_static() -// { -// switch (N) { -// case 0: -// return "SERVO_OUTPUT_RAW_0"; -// -// case 1: -// return "SERVO_OUTPUT_RAW_1"; -// -// case 2: -// return "SERVO_OUTPUT_RAW_2"; -// -// case 3: -// return "SERVO_OUTPUT_RAW_3"; -// } -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamServoOutputRaw(); -// } -// -//private: -// MavlinkOrbSubscription *act_sub; -// uint64_t act_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); -// MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); -// -//protected: -// explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), -// act_sub(nullptr), -// act_time(0) -// {} -// -// void subscribe() -// { -// orb_id_t act_topics[] = { -// ORB_ID(actuator_outputs_0), -// ORB_ID(actuator_outputs_1), -// ORB_ID(actuator_outputs_2), -// ORB_ID(actuator_outputs_3) -// }; -// -// act_sub = _mavlink->add_orb_subscription(act_topics[N]); -// } -// -// void send(const hrt_abstime t) -// { -// struct actuator_outputs_s act; -// -// if (act_sub->update(&act_time, &act)) { -// mavlink_msg_servo_output_raw_send(_channel, -// act.timestamp / 1000, -// N, -// act.output[0], -// act.output[1], -// act.output[2], -// act.output[3], -// act.output[4], -// act.output[5], -// act.output[6], -// act.output[7]); -// } -// } -//}; -// -// +class MavlinkStreamGlobalPositionInt : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGlobalPositionInt::get_name_static(); + } + + static const char *get_name_static() + { + return "GLOBAL_POSITION_INT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGlobalPositionInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + MavlinkOrbSubscription *_home_sub; + uint64_t _home_time; + + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); + MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); + +protected: + explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), + _home_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_global_position_s pos; + struct home_position_s home; + + bool updated = _pos_sub->update(&_pos_time, &pos); + updated |= _home_sub->update(&_home_time, &home); + + if (updated) { + mavlink_global_position_int_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.lat = pos.lat * 1e7; + msg.lon = pos.lon * 1e7; + msg.alt = pos.alt * 1000.0f; + msg.relative_alt = (pos.alt - home.alt) * 1000.0f; + msg.vx = pos.vel_n * 100.0f; + msg.vy = pos.vel_e * 100.0f; + msg.vz = pos.vel_d * 100.0f; + msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); + } + } +}; + + +class MavlinkStreamLocalPositionNED : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamLocalPositionNED::get_name_static(); + } + + static const char *get_name_static() + { + return "LOCAL_POSITION_NED"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionNED(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + /* do not allow top copying this class */ + MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); + MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); + +protected: + explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _pos_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_local_position_s pos; + + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_local_position_ned_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.vx = pos.vx; + msg.vy = pos.vy; + msg.vz = pos.vz; + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); + } + } +}; + + +class MavlinkStreamViconPositionEstimate : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamViconPositionEstimate::get_name_static(); + } + + static const char *get_name_static() + { + return "VICON_POSITION_ESTIMATE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamViconPositionEstimate(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + /* do not allow top copying this class */ + MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); + MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); + +protected: + explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))), + _pos_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_vicon_position_s pos; + + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_vicon_position_estimate_t msg; + + msg.usec = pos.timestamp; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.roll = pos.roll; + msg.pitch = pos.pitch; + msg.yaw = pos.yaw; + + _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); + } + } +}; + + +class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGPSGlobalOrigin::get_name_static(); + } + + static const char *get_name_static() + { + return "GPS_GLOBAL_ORIGIN"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGPSGlobalOrigin(mavlink); + } + + unsigned get_size() + { + return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + MavlinkOrbSubscription *_home_sub; + + /* do not allow top copying this class */ + MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); + +protected: + explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))) + {} + + void send(const hrt_abstime t) + { + /* we're sending the GPS home periodically to ensure the + * the GCS does pick it up at one point */ + if (_home_sub->is_published()) { + struct home_position_s home; + + if (_home_sub->update(&home)) { + mavlink_gps_global_origin_t msg; + + msg.latitude = home.lat * 1e7; + msg.longitude = home.lon * 1e7; + msg.altitude = home.alt * 1e3f; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg); + } + } + } +}; + + +template +class MavlinkStreamServoOutputRaw : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamServoOutputRaw::get_name_static(); + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + } + + static const char *get_name_static() + { + switch (N) { + case 0: + return "SERVO_OUTPUT_RAW_0"; + + case 1: + return "SERVO_OUTPUT_RAW_1"; + + case 2: + return "SERVO_OUTPUT_RAW_2"; + + case 3: + return "SERVO_OUTPUT_RAW_3"; + } + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamServoOutputRaw(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; + + /* do not allow top copying this class */ + MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); + MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); + +protected: + explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _act_sub(nullptr), + _act_time(0) + { + orb_id_t act_topics[] = { + ORB_ID(actuator_outputs_0), + ORB_ID(actuator_outputs_1), + ORB_ID(actuator_outputs_2), + ORB_ID(actuator_outputs_3) + }; + + _act_sub = _mavlink->add_orb_subscription(act_topics[N]); + } + + void send(const hrt_abstime t) + { + struct actuator_outputs_s act; + + if (_act_sub->update(&_act_time, &act)) { + mavlink_servo_output_raw_t msg; + + msg.time_usec = act.timestamp; + msg.port = N; + msg.servo1_raw = act.output[0]; + msg.servo2_raw = act.output[1]; + msg.servo3_raw = act.output[2]; + msg.servo4_raw = act.output[3]; + msg.servo5_raw = act.output[4]; + msg.servo6_raw = act.output[5]; + msg.servo7_raw = act.output[6]; + msg.servo8_raw = act.output[7]; + + _mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg); + } + } +}; + + //class MavlinkStreamHILControls : public MavlinkStream //{ //public: @@ -1281,7 +1292,7 @@ protected: // MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); // //protected: -// explicit MavlinkStreamHILControls() : MavlinkStream(), +// explicit MavlinkStreamHILControls() : MavlinkStream(mavlink), // status_sub(nullptr), // status_time(0), // pos_sp_triplet_sub(nullptr), @@ -1414,7 +1425,7 @@ protected: // MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); // //protected: -// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), +// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(mavlink), // pos_sp_triplet_sub(nullptr) // {} // @@ -1471,7 +1482,7 @@ protected: // MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); // //protected: -// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), +// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(mavlink), // pos_sp_sub(nullptr), // pos_sp_time(0) // {} @@ -1529,7 +1540,7 @@ protected: // MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); // //protected: -// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), +// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(mavlink), // att_sp_sub(nullptr), // att_sp_time(0) // {} @@ -1587,7 +1598,7 @@ protected: // MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); // //protected: -// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), +// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(mavlink), // att_rates_sp_sub(nullptr), // att_rates_sp_time(0) // {} @@ -1645,7 +1656,7 @@ protected: // MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); // //protected: -// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), +// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(mavlink), // rc_sub(nullptr), // rc_time(0) // {} @@ -1739,7 +1750,7 @@ protected: // MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); // //protected: -// explicit MavlinkStreamManualControl() : MavlinkStream(), +// explicit MavlinkStreamManualControl() : MavlinkStream(mavlink), // manual_sub(nullptr), // manual_time(0) // {} @@ -1798,7 +1809,7 @@ protected: // MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); // //protected: -// explicit MavlinkStreamOpticalFlow() : MavlinkStream(), +// explicit MavlinkStreamOpticalFlow() : MavlinkStream(mavlink), // flow_sub(nullptr), // flow_time(0) // {} @@ -1856,7 +1867,7 @@ protected: // MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); // //protected: -// explicit MavlinkStreamAttitudeControls() : MavlinkStream(), +// explicit MavlinkStreamAttitudeControls() : MavlinkStream(mavlink), // att_ctrl_sub(nullptr), // att_ctrl_time(0) // {} @@ -1924,7 +1935,7 @@ protected: // MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); // //protected: -// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), +// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(mavlink), // debug_sub(nullptr), // debug_time(0) // {} @@ -1981,7 +1992,7 @@ protected: // MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); // //protected: -// explicit MavlinkStreamCameraCapture() : MavlinkStream(), +// explicit MavlinkStreamCameraCapture() : MavlinkStream(mavlink), // status_sub(nullptr) // {} // @@ -2040,7 +2051,7 @@ protected: // MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); // //protected: -// explicit MavlinkStreamDistanceSensor() : MavlinkStream(), +// explicit MavlinkStreamDistanceSensor() : MavlinkStream(mavlink), // range_sub(nullptr), // range_time(0) // {} From e087ec81c396d7da8d5f7a006748062ee07b693f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 13:53:45 +0200 Subject: [PATCH 432/600] mavlink: more streams ported to new API --- src/modules/mavlink/mavlink_messages.cpp | 522 ++++++++++++----------- 1 file changed, 268 insertions(+), 254 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fe33c1bea6..14195b6a73 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1254,260 +1254,274 @@ protected: }; -//class MavlinkStreamHILControls : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamHILControls::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "HIL_CONTROLS"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_HIL_CONTROLS; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamHILControls(); -// } -// -//private: -// MavlinkOrbSubscription *status_sub; -// uint64_t status_time; -// -// MavlinkOrbSubscription *pos_sp_triplet_sub; -// uint64_t pos_sp_triplet_time; -// -// MavlinkOrbSubscription *act_sub; -// uint64_t act_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamHILControls(MavlinkStreamHILControls &); -// MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); -// -//protected: -// explicit MavlinkStreamHILControls() : MavlinkStream(mavlink), -// status_sub(nullptr), -// status_time(0), -// pos_sp_triplet_sub(nullptr), -// pos_sp_triplet_time(0), -// act_sub(nullptr), -// act_time(0) -// {} -// -// void subscribe() -// { -// status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); -// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); -// act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_status_s status; -// struct position_setpoint_triplet_s pos_sp_triplet; -// struct actuator_outputs_s act; -// -// bool updated = act_sub->update(&act_time, &act); -// updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); -// updated |= status_sub->update(&status_time, &status); -// -// if (updated && (status.arming_state == ARMING_STATE_ARMED)) { -// /* translate the current syste state to mavlink state and mode */ -// uint8_t mavlink_state; -// uint8_t mavlink_base_mode; -// uint32_t mavlink_custom_mode; -// get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); -// -// float out[8]; -// -// const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; -// -// /* scale outputs depending on system type */ -// if (mavlink_system.type == MAV_TYPE_QUADROTOR || -// mavlink_system.type == MAV_TYPE_HEXAROTOR || -// mavlink_system.type == MAV_TYPE_OCTOROTOR) { -// /* multirotors: set number of rotor outputs depending on type */ -// -// unsigned n; -// -// switch (mavlink_system.type) { -// case MAV_TYPE_QUADROTOR: -// n = 4; -// break; -// -// case MAV_TYPE_HEXAROTOR: -// n = 6; -// break; -// -// default: -// n = 8; -// break; -// } -// -// for (unsigned i = 0; i < 8; i++) { -// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { -// if (i < n) { -// /* scale PWM out 900..2100 us to 0..1 for rotors */ -// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); -// -// } else { -// /* scale PWM out 900..2100 us to -1..1 for other channels */ -// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); -// } -// -// } else { -// /* send 0 when disarmed */ -// out[i] = 0.0f; -// } -// } -// -// } else { -// /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ -// -// for (unsigned i = 0; i < 8; i++) { -// if (i != 3) { -// /* scale PWM out 900..2100 us to -1..1 for normal channels */ -// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); -// -// } else { -// /* scale PWM out 900..2100 us to 0..1 for throttle */ -// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); -// } -// -// } -// } -// -// mavlink_msg_hil_controls_send(_channel, -// hrt_absolute_time(), -// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], -// mavlink_base_mode, -// 0); -// } -// } -//}; -// -// -//class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "GLOBAL_POSITION_SETPOINT_INT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamGlobalPositionSetpointInt(); -// } -// -//private: -// MavlinkOrbSubscription *pos_sp_triplet_sub; -// -// /* do not allow top copying this class */ -// MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); -// MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); -// -//protected: -// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(mavlink), -// pos_sp_triplet_sub(nullptr) -// {} -// -// void subscribe() -// { -// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); -// } -// -// void send(const hrt_abstime t) -// { -// struct position_setpoint_triplet_s pos_sp_triplet; -// -// if (pos_sp_triplet_sub->update(&pos_sp_triplet)) { -// mavlink_msg_global_position_setpoint_int_send(_channel, -// MAV_FRAME_GLOBAL, -// (int32_t)(pos_sp_triplet.current.lat * 1e7), -// (int32_t)(pos_sp_triplet.current.lon * 1e7), -// (int32_t)(pos_sp_triplet.current.alt * 1000), -// (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); -// } -// } -//}; -// -// -//class MavlinkStreamLocalPositionSetpoint : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamLocalPositionSetpoint::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "LOCAL_POSITION_SETPOINT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamLocalPositionSetpoint(); -// } -// -//private: -// MavlinkOrbSubscription *pos_sp_sub; -// uint64_t pos_sp_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); -// MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); -// -//protected: -// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(mavlink), -// pos_sp_sub(nullptr), -// pos_sp_time(0) -// {} -// -// void subscribe() -// { -// pos_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_local_position_setpoint_s pos_sp; -// -// if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { -// mavlink_msg_local_position_setpoint_send(_channel, -// MAV_FRAME_LOCAL_NED, -// pos_sp.x, -// pos_sp.y, -// pos_sp.z, -// pos_sp.yaw); -// } -// } -//}; -// -// +class MavlinkStreamHILControls : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamHILControls::get_name_static(); + } + + static const char *get_name_static() + { + return "HIL_CONTROLS"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIL_CONTROLS; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamHILControls(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_status_sub; + uint64_t _status_time; + + MavlinkOrbSubscription *_pos_sp_triplet_sub; + uint64_t _pos_sp_triplet_time; + + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; + + /* do not allow top copying this class */ + MavlinkStreamHILControls(MavlinkStreamHILControls &); + MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); + +protected: + explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _status_time(0), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), + _pos_sp_triplet_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))), + _act_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_status_s status; + struct position_setpoint_triplet_s pos_sp_triplet; + struct actuator_outputs_s act; + + bool updated = _act_sub->update(&_act_time, &act); + updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet); + updated |= _status_sub->update(&_status_time, &status); + + if (updated && (status.arming_state == ARMING_STATE_ARMED)) { + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state; + uint8_t mavlink_base_mode; + uint32_t mavlink_custom_mode; + get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + /* scale outputs depending on system type */ + if (mavlink_system.type == MAV_TYPE_QUADROTOR || + mavlink_system.type == MAV_TYPE_HEXAROTOR || + mavlink_system.type == MAV_TYPE_OCTOROTOR) { + /* multirotors: set number of rotor outputs depending on type */ + + unsigned n; + + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; + + case MAV_TYPE_HEXAROTOR: + n = 6; + break; + + default: + n = 8; + break; + } + + for (unsigned i = 0; i < 8; i++) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (i < n) { + /* scale PWM out 900..2100 us to 0..1 for rotors */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + + } else { + /* scale PWM out 900..2100 us to -1..1 for other channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + } + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } + } + + } else { + /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ + + for (unsigned i = 0; i < 8; i++) { + if (i != 3) { + /* scale PWM out 900..2100 us to -1..1 for normal channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + + } else { + /* scale PWM out 900..2100 us to 0..1 for throttle */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + } + + } + } + + mavlink_hil_controls_t msg; + + msg.time_usec = hrt_absolute_time(); + msg.roll_ailerons = out[0]; + msg.pitch_elevator = out[1]; + msg.yaw_rudder = out[2]; + msg.throttle = out[3]; + msg.aux1 = out[4]; + msg.aux2 = out[5]; + msg.aux3 = out[6]; + msg.aux4 = out[7]; + msg.mode = mavlink_base_mode; + msg.nav_mode = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg); + } + } +}; + + +class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); + } + + static const char *get_name_static() + { + return "GLOBAL_POSITION_SETPOINT_INT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGlobalPositionSetpointInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sp_triplet_sub; + + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); + MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); + +protected: + explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) + {} + + void send(const hrt_abstime t) + { + struct position_setpoint_triplet_s pos_sp_triplet; + + if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { + mavlink_global_position_setpoint_int_t msg; + + msg.coordinate_frame = MAV_FRAME_GLOBAL; + msg.latitude = pos_sp_triplet.current.lat * 1e7; + msg.longitude = pos_sp_triplet.current.lon * 1e7; + msg.altitude = pos_sp_triplet.current.alt * 1000; + msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg); + } + } +}; + + +class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamLocalPositionSetpoint::get_name_static(); + } + + static const char *get_name_static() + { + return "LOCAL_POSITION_SETPOINT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionSetpoint(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sp_sub; + uint64_t _pos_sp_time; + + /* do not allow top copying this class */ + MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); + MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); + +protected: + explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint))), + _pos_sp_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_local_position_setpoint_s pos_sp; + + if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { + mavlink_local_position_setpoint_t msg; + + msg.coordinate_frame = MAV_FRAME_LOCAL_NED; + msg.x = pos_sp.x; + msg.y = pos_sp.y; + msg.z = pos_sp.z; + msg.yaw = pos_sp.yaw; + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg); + } + } +}; + + //class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream //{ //public: From 3b3e6f5aaafd1247447cad7070e3488e5798ce3c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Jul 2014 14:21:50 +0200 Subject: [PATCH 433/600] Increase filter pass-band --- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 118c35960f..03e50e5cb6 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -103,7 +103,7 @@ /* Measurement rate is 100Hz */ #define MEAS_RATE 100 -#define MEAS_DRIVER_FILTER_FREQ 0.5f +#define MEAS_DRIVER_FILTER_FREQ 0.8f #define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ class MEASAirspeed : public Airspeed From 67db8ee4f0908c31f17ed490c48ea21cc7ac3f86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Jul 2014 14:27:10 +0200 Subject: [PATCH 434/600] Add missing states, build fix for master --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 4 ++++ src/modules/ekf_att_pos_estimator/estimator_utilities.h | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 3d504d3955..ffdd29a5b7 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -3028,6 +3028,10 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) current_ekf_state.states[i] = states[i]; } current_ekf_state.n_states = n_states; + current_ekf_state.onGround = onGround; + current_ekf_state.staticMode = staticMode; + current_ekf_state.useCompass = useCompass; + current_ekf_state.useAirspeed = useAirspeed; memcpy(err, ¤t_ekf_state, sizeof(*err)); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 6d1f47b685..a6b670c4d2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -68,6 +68,10 @@ struct ekf_status_report { bool posTimeout; bool hgtTimeout; bool imuTimeout; + bool onGround; + bool staticMode; + bool useCompass; + bool useAirspeed; uint32_t velFailTime; uint32_t posFailTime; uint32_t hgtFailTime; From 241802a71f2af56a8dd4510827f8ab5a59f5d6b9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 17:40:53 +0200 Subject: [PATCH 435/600] mavlink: more streams ported to new API --- src/modules/mavlink/mavlink_messages.cpp | 584 ++++++++++++----------- 1 file changed, 300 insertions(+), 284 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 14195b6a73..a710cdf6a1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1522,275 +1522,291 @@ protected: }; -//class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "ROLL_PITCH_YAW_THRUST_SETPOINT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamRollPitchYawThrustSetpoint(); -// } -// -//private: -// MavlinkOrbSubscription *att_sp_sub; -// uint64_t att_sp_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); -// MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); -// -//protected: -// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(mavlink), -// att_sp_sub(nullptr), -// att_sp_time(0) -// {} -// -// void subscribe() -// { -// att_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_attitude_setpoint_s att_sp; -// -// if (att_sp_sub->update(&att_sp_time, &att_sp)) { -// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, -// att_sp.timestamp / 1000, -// att_sp.roll_body, -// att_sp.pitch_body, -// att_sp.yaw_body, -// att_sp.thrust); -// } -// } -//}; -// -// -//class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); -// } -// -//private: -// MavlinkOrbSubscription *att_rates_sp_sub; -// uint64_t att_rates_sp_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); -// MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); -// -//protected: -// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(mavlink), -// att_rates_sp_sub(nullptr), -// att_rates_sp_time(0) -// {} -// -// void subscribe() -// { -// att_rates_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_rates_setpoint_s att_rates_sp; -// -// if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { -// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, -// att_rates_sp.timestamp / 1000, -// att_rates_sp.roll, -// att_rates_sp.pitch, -// att_rates_sp.yaw, -// att_rates_sp.thrust); -// } -// } -//}; -// -// -//class MavlinkStreamRCChannelsRaw : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamRCChannelsRaw::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "RC_CHANNELS_RAW"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_RC_CHANNELS_RAW; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamRCChannelsRaw(); -// } -// -//private: -// MavlinkOrbSubscription *rc_sub; -// uint64_t rc_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); -// MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); -// -//protected: -// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(mavlink), -// rc_sub(nullptr), -// rc_time(0) -// {} -// -// void subscribe() -// { -// rc_sub = _mavlink->add_orb_subscription(ORB_ID(input_rc)); -// } -// -// void send(const hrt_abstime t) -// { -// struct rc_input_values rc; -// -// if (rc_sub->update(&rc_time, &rc)) { -// const unsigned port_width = 8; -// -// // Deprecated message (but still needed for compatibility!) -// for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { -// /* Channels are sent in MAVLink main loop at a fixed interval */ -// mavlink_msg_rc_channels_raw_send(_channel, -// rc.timestamp_publication / 1000, -// i, -// (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, -// (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, -// rc.rssi); -// } -// -// // New message -// mavlink_msg_rc_channels_send(_channel, -// rc.timestamp_publication / 1000, -// rc.channel_count, -// ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), -// ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), -// ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), -// ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), -// ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), -// ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), -// ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), -// ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), -// ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), -// ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), -// ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), -// ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), -// ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), -// ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), -// ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), -// ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), -// ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), -// ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), -// rc.rssi); -// } -// } -//}; -// -// -//class MavlinkStreamManualControl : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamManualControl::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "MANUAL_CONTROL"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_MANUAL_CONTROL; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamManualControl(); -// } -// -//private: -// MavlinkOrbSubscription *manual_sub; -// uint64_t manual_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamManualControl(MavlinkStreamManualControl &); -// MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); -// -//protected: -// explicit MavlinkStreamManualControl() : MavlinkStream(mavlink), -// manual_sub(nullptr), -// manual_time(0) -// {} -// -// void subscribe() -// { -// manual_sub = _mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); -// } -// -// void send(const hrt_abstime t) -// { -// struct manual_control_setpoint_s manual; -// -// if (manual_sub->update(&manual_time, &manual)) { -// mavlink_msg_manual_control_send(_channel, -// mavlink_system.sysid, -// manual.x * 1000, -// manual.y * 1000, -// manual.z * 1000, -// manual.r * 1000, -// 0); -// } -// } -//}; -// -// +class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); + } + + static const char *get_name_static() + { + return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *att_sp_sub; + uint64_t att_sp_time; + + /* do not allow top copying this class */ + MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); + MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); + +protected: + explicit MavlinkStreamRollPitchYawThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), + att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), + att_sp_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_attitude_setpoint_s att_sp; + + if (att_sp_sub->update(&att_sp_time, &att_sp)) { + mavlink_roll_pitch_yaw_thrust_setpoint_t msg; + + msg.time_boot_ms = att_sp.timestamp / 1000; + msg.roll = att_sp.roll_body; + msg.pitch = att_sp.pitch_body; + msg.yaw = att_sp.yaw_body; + msg.thrust = att_sp.thrust; + + _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg); + } + } +}; + + +class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); + } + + static const char *get_name_static() + { + return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_att_rates_sp_sub; + uint64_t _att_rates_sp_time; + + /* do not allow top copying this class */ + MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); + MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); + +protected: + explicit MavlinkStreamRollPitchYawRatesThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), + _att_rates_sp_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_rates_setpoint_s att_rates_sp; + + if (_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp)) { + mavlink_roll_pitch_yaw_rates_thrust_setpoint_t msg; + + msg.time_boot_ms = att_rates_sp.timestamp / 1000; + msg.roll_rate = att_rates_sp.roll; + msg.pitch_rate = att_rates_sp.pitch; + msg.yaw_rate = att_rates_sp.yaw; + msg.thrust = att_rates_sp.thrust; + + _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg); + } + } +}; + + +class MavlinkStreamRCChannelsRaw : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRCChannelsRaw::get_name_static(); + } + + static const char *get_name_static() + { + return "RC_CHANNELS_RAW"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_RC_CHANNELS_RAW; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRCChannelsRaw(mavlink); + } + + unsigned get_size() + { + return (RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + + MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_rc_sub; + uint64_t _rc_time; + + /* do not allow top copying this class */ + MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); + MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); + +protected: + explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))), + _rc_time(0) + {} + + void send(const hrt_abstime t) + { + struct rc_input_values rc; + + if (_rc_sub->update(&_rc_time, &rc)) { + const unsigned port_width = 8; + + /* deprecated message (but still needed for compatibility!) */ + for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { + /* channels are sent in MAVLink main loop at a fixed interval */ + mavlink_rc_channels_raw_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.port = i; + msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); + } + + /* new message */ + mavlink_rc_channels_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.chancount = rc.channel_count; + msg.chan1_raw = (rc.channel_count > 0) ? rc.values[0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > 1) ? rc.values[1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > 2) ? rc.values[2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > 3) ? rc.values[3] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > 4) ? rc.values[4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > 5) ? rc.values[5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > 6) ? rc.values[6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > 7) ? rc.values[7] : UINT16_MAX; + msg.chan9_raw = (rc.channel_count > 8) ? rc.values[8] : UINT16_MAX; + msg.chan10_raw = (rc.channel_count > 9) ? rc.values[9] : UINT16_MAX; + msg.chan11_raw = (rc.channel_count > 10) ? rc.values[10] : UINT16_MAX; + msg.chan12_raw = (rc.channel_count > 11) ? rc.values[11] : UINT16_MAX; + msg.chan13_raw = (rc.channel_count > 12) ? rc.values[12] : UINT16_MAX; + msg.chan14_raw = (rc.channel_count > 13) ? rc.values[13] : UINT16_MAX; + msg.chan15_raw = (rc.channel_count > 14) ? rc.values[14] : UINT16_MAX; + msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; + msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; + msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); + } + } +}; + + +class MavlinkStreamManualControl : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamManualControl::get_name_static(); + } + + static const char *get_name_static() + { + return "MANUAL_CONTROL"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_MANUAL_CONTROL; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamManualControl(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_manual_sub; + uint64_t _manual_time; + + /* do not allow top copying this class */ + MavlinkStreamManualControl(MavlinkStreamManualControl &); + MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); + +protected: + explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink), + _manual_sub(_mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint))), + _manual_time(0) + {} + + void send(const hrt_abstime t) + { + struct manual_control_setpoint_s manual; + + if (_manual_sub->update(&_manual_time, &manual)) { + mavlink_manual_control_t msg; + + msg.target = mavlink_system.sysid; + msg.x = manual.x * 1000; + msg.y = manual.y * 1000; + msg.z = manual.z * 1000; + msg.r = manual.r * 1000; + msg.buttons = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg); + } + } +}; + + //class MavlinkStreamOpticalFlow : public MavlinkStream //{ //public: @@ -2109,21 +2125,21 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), -// new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), -// new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), -// new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), -// new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), -// new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), -// new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), -// new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), -// new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), -// new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), -// new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), -// new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), -// new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), -// new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), -// new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), -// new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), + new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), + new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), + new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), + new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), + new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), + new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), + new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), // new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), // new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), // new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), From 019dc1b5264072c785433c53ca11995ed291f924 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 17:46:35 +0200 Subject: [PATCH 436/600] mavlink: log message severity fixed --- src/modules/mavlink/mavlink_main.cpp | 27 +++++++++++++++++++----- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/mavlink/mavlink_messages.cpp | 19 +---------------- 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 408799be41..f9c7fcedf2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -433,7 +433,24 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) const char *txt = (const char *)arg; struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); - msg.severity = (unsigned char)cmd; + + switch (cmd) { + case MAVLINK_IOC_SEND_TEXT_INFO: + msg.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + msg.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + msg.severity = MAV_SEVERITY_EMERGENCY; + break; + + default: + msg.severity = MAV_SEVERITY_INFO; + break; + } Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { @@ -816,23 +833,23 @@ Mavlink::handle_message(const mavlink_message_t *msg) void Mavlink::send_statustext_info(const char *string) { - send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); + send_statustext(MAV_SEVERITY_INFO, string); } void Mavlink::send_statustext_critical(const char *string) { - send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); + send_statustext(MAV_SEVERITY_CRITICAL, string); } void Mavlink::send_statustext_emergency(const char *string) { - send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); + send_statustext(MAV_SEVERITY_EMERGENCY, string); } void -Mavlink::send_statustext(unsigned severity, const char *string) +Mavlink::send_statustext(unsigned char severity, const char *string) { struct mavlink_logmessage logmsg; strncpy(logmsg.text, string, sizeof(logmsg.text)); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 4121e286e0..38149cf107 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -218,7 +218,7 @@ public: * @param string the message to send (will be capped by mavlink max string length) * @param severity the log level */ - void send_statustext(unsigned severity, const char *string); + void send_statustext(unsigned char severity, const char *string); MavlinkStream * get_streams() const { return _streams; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a710cdf6a1..083a6301a9 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -351,24 +351,7 @@ protected: if (lb_ret == OK) { mavlink_statustext_t msg; - /* map severity */ - switch (logmsg.severity) { - case MAVLINK_IOC_SEND_TEXT_INFO: - msg.severity = MAV_SEVERITY_INFO; - break; - - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - msg.severity = MAV_SEVERITY_CRITICAL; - break; - - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - msg.severity = MAV_SEVERITY_EMERGENCY; - break; - - default: - msg.severity = MAV_SEVERITY_INFO; - break; - } + msg.severity = logmsg.severity; strncpy(msg.text, logmsg.text, sizeof(msg.text)); _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); From f3ec46369b68d57489a5fa57a320ae99a1bda220 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 20:30:58 +0200 Subject: [PATCH 437/600] mavlink: all streams ported to new API --- src/modules/mavlink/mavlink_main.cpp | 1 - src/modules/mavlink/mavlink_messages.cpp | 663 ++++++++++++----------- 2 files changed, 347 insertions(+), 317 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f9c7fcedf2..d72fb85086 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1350,7 +1350,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("NAMED_VALUE_FLOAT", 1.0f); configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 083a6301a9..683f0f1e84 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1652,8 +1652,8 @@ public: unsigned get_size() { - return (RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + - MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + + MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1753,7 +1753,7 @@ public: unsigned get_size() { - return MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1790,313 +1790,344 @@ protected: }; -//class MavlinkStreamOpticalFlow : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamOpticalFlow::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "OPTICAL_FLOW"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_OPTICAL_FLOW; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamOpticalFlow(); -// } -// -//private: -// MavlinkOrbSubscription *flow_sub; -// uint64_t flow_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); -// MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); -// -//protected: -// explicit MavlinkStreamOpticalFlow() : MavlinkStream(mavlink), -// flow_sub(nullptr), -// flow_time(0) -// {} -// -// void subscribe() -// { -// flow_sub = _mavlink->add_orb_subscription(ORB_ID(optical_flow)); -// } -// -// void send(const hrt_abstime t) -// { -// struct optical_flow_s flow; -// -// if (flow_sub->update(&flow_time, &flow)) { -// mavlink_msg_optical_flow_send(_channel, -// flow.timestamp, -// flow.sensor_id, -// flow.flow_raw_x, flow.flow_raw_y, -// flow.flow_comp_x_m, flow.flow_comp_y_m, -// flow.quality, -// flow.ground_distance_m); -// } -// } -//}; -// -//class MavlinkStreamAttitudeControls : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamAttitudeControls::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "ATTITUDE_CONTROLS"; -// } -// -// uint8_t get_id() -// { -// return 0; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamAttitudeControls(); -// } -// -//private: -// MavlinkOrbSubscription *att_ctrl_sub; -// uint64_t att_ctrl_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); -// MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); -// -//protected: -// explicit MavlinkStreamAttitudeControls() : MavlinkStream(mavlink), -// att_ctrl_sub(nullptr), -// att_ctrl_time(0) -// {} -// -// void subscribe() -// { -// att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); -// } -// -// void send(const hrt_abstime t) -// { -// struct actuator_controls_s att_ctrl; -// -// if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { -// /* send, add spaces so that string buffer is at least 10 chars long */ -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl.timestamp / 1000, -// "rll ctrl ", -// att_ctrl.control[0]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl.timestamp / 1000, -// "ptch ctrl ", -// att_ctrl.control[1]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl.timestamp / 1000, -// "yaw ctrl ", -// att_ctrl.control[2]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl.timestamp / 1000, -// "thr ctrl ", -// att_ctrl.control[3]); -// } -// } -//}; -// -//class MavlinkStreamNamedValueFloat : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamNamedValueFloat::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "NAMED_VALUE_FLOAT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamNamedValueFloat(); -// } -// -//private: -// MavlinkOrbSubscription *debug_sub; -// uint64_t debug_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); -// MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); -// -//protected: -// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(mavlink), -// debug_sub(nullptr), -// debug_time(0) -// {} -// -// void subscribe() -// { -// debug_sub = _mavlink->add_orb_subscription(ORB_ID(debug_key_value)); -// } -// -// void send(const hrt_abstime t) -// { -// struct debug_key_value_s debug; -// -// if (debug_sub->update(&debug_time, &debug)) { -// /* enforce null termination */ -// debug.key[sizeof(debug.key) - 1] = '\0'; -// -// mavlink_msg_named_value_float_send(_channel, -// debug.timestamp_ms, -// debug.key, -// debug.value); -// } -// } -//}; -// -//class MavlinkStreamCameraCapture : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamCameraCapture::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "CAMERA_CAPTURE"; -// } -// -// uint8_t get_id() -// { -// return 0; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamCameraCapture(); -// } -// -//private: -// MavlinkOrbSubscription *status_sub; -// -// /* do not allow top copying this class */ -// MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); -// MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); -// -//protected: -// explicit MavlinkStreamCameraCapture() : MavlinkStream(mavlink), -// status_sub(nullptr) -// {} -// -// void subscribe() -// { -// status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_status_s status; -// (void)status_sub->update(&status); -// -// if (status.arming_state == ARMING_STATE_ARMED -// || status.arming_state == ARMING_STATE_ARMED_ERROR) { -// -// /* send camera capture on */ -// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); -// -// } else { -// /* send camera capture off */ -// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); -// } -// } -//}; -// -//class MavlinkStreamDistanceSensor : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamDistanceSensor::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "DISTANCE_SENSOR"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_DISTANCE_SENSOR; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamDistanceSensor(); -// } -// -//private: -// MavlinkOrbSubscription *range_sub; -// uint64_t range_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); -// MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); -// -//protected: -// explicit MavlinkStreamDistanceSensor() : MavlinkStream(mavlink), -// range_sub(nullptr), -// range_time(0) -// {} -// -// void subscribe() -// { -// range_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); -// } -// -// void send(const hrt_abstime t) -// { -// struct range_finder_report range; -// -// if (range_sub->update(&range_time, &range)) { -// -// uint8_t type; -// -// switch (range.type) { -// case RANGE_FINDER_TYPE_LASER: -// type = MAV_DISTANCE_SENSOR_LASER; -// break; -// } -// -// uint8_t id = 0; -// uint8_t orientation = 0; -// uint8_t covariance = 20; -// -// mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, -// range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); -// } -// } -//}; +class MavlinkStreamOpticalFlow : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamOpticalFlow::get_name_static(); + } + + static const char *get_name_static() + { + return "OPTICAL_FLOW"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_OPTICAL_FLOW; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamOpticalFlow(mavlink); + } + + unsigned get_size() + { + return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + MavlinkOrbSubscription *_flow_sub; + uint64_t _flow_time; + + /* do not allow top copying this class */ + MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); + MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); + +protected: + explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink), + _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))), + _flow_time(0) + {} + + void send(const hrt_abstime t) + { + struct optical_flow_s flow; + + if (_flow_sub->update(&_flow_time, &flow)) { + mavlink_optical_flow_t msg; + + msg.time_usec = flow.timestamp; + msg.sensor_id = flow.sensor_id; + msg.flow_x = flow.flow_raw_x; + msg.flow_y = flow.flow_raw_y; + msg.flow_comp_m_x = flow.flow_comp_x_m; + msg.flow_comp_m_y = flow.flow_comp_y_m; + msg.quality = flow.quality; + msg.ground_distance = flow.ground_distance_m; + + _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg); + } + } +}; + +class MavlinkStreamAttitudeControls : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAttitudeControls::get_name_static(); + } + + static const char *get_name_static() + { + return "ATTITUDE_CONTROLS"; + } + + uint8_t get_id() + { + return 0; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttitudeControls(mavlink); + } + + unsigned get_size() + { + return 4 * (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + } + +private: + MavlinkOrbSubscription *_att_ctrl_sub; + uint64_t _att_ctrl_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); + MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); + +protected: + explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_ctrl_sub(_mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS)), + _att_ctrl_time(0) + {} + + void send(const hrt_abstime t) + { + struct actuator_controls_s att_ctrl; + + if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_named_value_float_t msg; + + msg.time_boot_ms = att_ctrl.timestamp / 1000; + + snprintf(msg.name, sizeof(msg.name), "rll ctrl"); + msg.value = att_ctrl.control[0]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "ptch ctrl"); + msg.value = att_ctrl.control[1]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "yaw ctrl"); + msg.value = att_ctrl.control[2]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "thr ctrl"); + msg.value = att_ctrl.control[3]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + } + } +}; + + +class MavlinkStreamNamedValueFloat : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamNamedValueFloat::get_name_static(); + } + + static const char *get_name_static() + { + return "NAMED_VALUE_FLOAT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamNamedValueFloat(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_debug_sub; + uint64_t _debug_time; + + /* do not allow top copying this class */ + MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); + MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); + +protected: + explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink), + _debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_key_value))), + _debug_time(0) + {} + + void send(const hrt_abstime t) + { + struct debug_key_value_s debug; + + if (_debug_sub->update(&_debug_time, &debug)) { + mavlink_named_value_float_t msg; + + msg.time_boot_ms = debug.timestamp_ms; + memcpy(msg.name, debug.key, sizeof(msg.name)); + /* enforce null termination */ + msg.name[sizeof(msg.name) - 1] = '\0'; + msg.value = debug.value; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + } + } +}; + + +class MavlinkStreamCameraCapture : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamCameraCapture::get_name_static(); + } + + static const char *get_name_static() + { + return "CAMERA_CAPTURE"; + } + + uint8_t get_id() + { + return 0; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamCameraCapture(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_status_sub; + + /* do not allow top copying this class */ + MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); + MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); + +protected: + explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) + {} + + void send(const hrt_abstime t) + { + struct vehicle_status_s status; + (void)_status_sub->update(&status); + + mavlink_command_long_t msg; + + msg.target_system = mavlink_system.sysid; + msg.target_component = MAV_COMP_ID_ALL; + msg.command = MAV_CMD_DO_CONTROL_VIDEO; + msg.confirmation = 0; + msg.param1 = 0; + msg.param2 = 0; + msg.param3 = 0; + /* set camera capture ON/OFF depending on arming state */ + msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0; + msg.param5 = 0; + msg.param6 = 0; + msg.param7 = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); + } +}; + + +class MavlinkStreamDistanceSensor : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamDistanceSensor::get_name_static(); + } + + static const char *get_name_static() + { + return "DISTANCE_SENSOR"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_DISTANCE_SENSOR; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamDistanceSensor(mavlink); + } + + unsigned get_size() + { + return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + MavlinkOrbSubscription *_range_sub; + uint64_t _range_time; + + /* do not allow top copying this class */ + MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); + MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); + +protected: + explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), + _range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))), + _range_time(0) + {} + + void send(const hrt_abstime t) + { + struct range_finder_report range; + + if (_range_sub->update(&_range_time, &range)) { + + mavlink_distance_sensor_t msg; + + msg.time_boot_ms = range.timestamp / 1000; + + switch (range.type) { + case RANGE_FINDER_TYPE_LASER: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + + default: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + msg.id = 0; + msg.orientation = 0; + msg.min_distance = range.minimum_distance * 100; + msg.max_distance = range.minimum_distance * 100; + msg.current_distance = range.distance * 100; + msg.covariance = 20; + + _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); + } + } +}; StreamListItem *streams_list[] = { @@ -2111,6 +2142,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), @@ -2123,11 +2155,10 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), -// new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), -// new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), -// new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), -// new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), -// new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), -// new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), + new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), + new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), + new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), + new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), + new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), nullptr }; From 129c93f22f0717d9d85910de87fecece9fabbc79 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:11:58 +0200 Subject: [PATCH 438/600] Revert "Remove all unused TECS parameters" This reverts commit ce78b399691d43bd150c0a5928f08c98076a3892. Conflicts: src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c --- .../fw_pos_control_l1_main.cpp | 57 ++++++ .../fw_pos_control_l1_params.c | 176 ++++++++++++++++++ 2 files changed, 233 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0b111f7bd3..a1bc5c24ef 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -199,6 +199,19 @@ private: float l1_period; float l1_damping; + float time_const; + float min_sink_rate; + float max_sink_rate; + float max_climb_rate; + float throttle_damp; + float integrator_gain; + float vertical_accel_limit; + float height_comp_filter_omega; + float speed_comp_filter_omega; + float roll_throttle_compensation; + float speed_weight; + float pitch_damping; + float airspeed_min; float airspeed_trim; float airspeed_max; @@ -226,6 +239,19 @@ private: param_t l1_period; param_t l1_damping; + param_t time_const; + param_t min_sink_rate; + param_t max_sink_rate; + param_t max_climb_rate; + param_t throttle_damp; + param_t integrator_gain; + param_t vertical_accel_limit; + param_t height_comp_filter_omega; + param_t speed_comp_filter_omega; + param_t roll_throttle_compensation; + param_t speed_weight; + param_t pitch_damping; + param_t airspeed_min; param_t airspeed_trim; param_t airspeed_max; @@ -438,6 +464,21 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); + _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); + _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); + _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); + _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); + _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); + _parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA"); + _parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA"); + _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); + _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); + _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); + _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); + /* fetch initial parameter values */ parameters_update(); } @@ -488,6 +529,22 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); + param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); + param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); + param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); + param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain)); + param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit)); + param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega)); + param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega)); + param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation)); + param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); + param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); + param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + + param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); + param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 27039ff51f..52128e1b7a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -154,6 +154,182 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); +/** + * Maximum climb rate + * + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * FW_THR_MAX reduced. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); + +/** + * Minimum descent rate + * + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used + * to measure FW_T_CLMB_MAX. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); + +/** + * Maximum descent rate + * + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding + * the aircraft. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + +/** + * TECS time constant + * + * This is the time constant of the TECS control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); + +/** + * Throttle damping factor + * + * This is the damping gain for the throttle demand loop. + * Increase to add damping to correct for oscillations in speed and height. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); + +/** + * Integrator gain + * + * This is the integrator gain on the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and + * increases overshoot. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); + +/** + * Maximum vertical acceleration + * + * This is the maximum vertical acceleration (in metres/second square) + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover + * from under-speed conditions. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); + +/** + * Complementary filter "omega" parameter for height + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse vertical acceleration and barometric height to obtain + * an estimate of height rate and height. Increasing this frequency weights + * the solution more towards use of the barometer, whilst reducing it weights + * the solution more towards use of the accelerometer data. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); + +/** + * Complementary filter "omega" parameter for speed + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an + * improved airspeed estimate. Increasing this frequency weights the solution + * more towards use of the arispeed sensor, whilst reducing it weights the + * solution more towards use of the accelerometer data. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); + +/** + * Roll -> Throttle feedforward + * + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas + * inefficient low aspect-ratio models (eg delta wings) can use a higher value. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); + +/** + * Speed <--> Altitude priority + * + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots - set this parameter to 2.0 (The glider will + * adjust its pitch angle to maintain airspeed, ignoring changes in height). + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); + +/** + * Pitch damping factor + * + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned + * properly. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); + +/** + * Height rate P factor + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); + +/** + * Speed rate P factor + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); + /** * Landing slope angle * From af7385bdc268bf4f19e9d1291a3d753a2c634ef0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:15:23 +0200 Subject: [PATCH 439/600] Re-instate TECS default gains --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 3a50fcf56f..3c336f295f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -11,4 +11,8 @@ then param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 param set NAV_ACCEPT_RAD 50 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 fi \ No newline at end of file From ae2de675014c701b45710b646f3a816ad2222b73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:24:11 +0200 Subject: [PATCH 440/600] Revert "Remove old TECS implementation - we can really only decently flight-test and support one." This reverts commit 503ded05394767d58359834e73bc63672b701dbe. Conflicts: mavlink/include/mavlink/v1.0 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp --- makefiles/config_aerocore_default.mk | 1 + makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + .../fw_pos_control_l1_main.cpp | 74 +++++++++++++++---- 4 files changed, 61 insertions(+), 16 deletions(-) diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index 53a2ad1ab0..0d3934bd01 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -80,6 +80,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index cc0958c29a..a7c10f52f3 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -108,6 +108,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a498a1b402..d0a40445d6 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -121,6 +121,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a1bc5c24ef..78e91bbfde 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -42,8 +42,8 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Implementation for total energy control class: - * Thomas Gubler + * Original implementation for total energy control class: + * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) * * More details and acknowledgements in the referenced library headers. * @@ -87,6 +87,7 @@ #include #include #include +#include #include #include "landingslope.h" #include "mtecs/mTecs.h" @@ -192,6 +193,7 @@ private: math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; + TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; @@ -557,6 +559,23 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); + _tecs.set_time_const(_parameters.time_const); + _tecs.set_min_sink_rate(_parameters.min_sink_rate); + _tecs.set_max_sink_rate(_parameters.max_sink_rate); + _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_integrator_gain(_parameters.integrator_gain); + _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); + _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); + _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); + _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); + _tecs.set_speed_weight(_parameters.speed_weight); + _tecs.set_pitch_damping(_parameters.pitch_damping); + _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); + _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); + _tecs.set_max_climb_rate(_parameters.max_climb_rate); + _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_speedrate_p(_parameters.speedrate_p); + /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -618,6 +637,9 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); + return airspeed_updated; } @@ -790,6 +812,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements + /* filter speed and altitude for controller */ + math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); + math::Vector<3> accel_earth = _R_nb * accel_body; + + if (!_mTecs.getEnabled()) { + _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + } + /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -815,6 +845,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); + /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_parameters.speed_weight); + /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); @@ -1123,9 +1156,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getPitchSetpoint(); + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1305,20 +1338,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - } - fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + if (_mTecs.getEnabled()) { + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); } else { - limitOverride.disablePitchMinOverride(); + /* Using tecs library */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, + _airspeed.indicated_airspeed_m_s, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); } int From 2b55eb605ed2216f9f73f5745de7d7a34c851e1c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:27:59 +0200 Subject: [PATCH 441/600] Make airspeed filter a bit smoother, but do not induce a huge phase delay --- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 03e50e5cb6..9cfa7f3838 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -103,7 +103,7 @@ /* Measurement rate is 100Hz */ #define MEAS_RATE 100 -#define MEAS_DRIVER_FILTER_FREQ 0.8f +#define MEAS_DRIVER_FILTER_FREQ 1.5f #define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ class MEASAirspeed : public Airspeed From 5171286bbb7eef115bd7fc711f2c8fcebd432e4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:34:46 +0200 Subject: [PATCH 442/600] Re-add two params that fell off the truck before --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 78e91bbfde..f0e18d1bb4 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -205,6 +205,8 @@ private: float min_sink_rate; float max_sink_rate; float max_climb_rate; + float heightrate_p; + float speedrate_p; float throttle_damp; float integrator_gain; float vertical_accel_limit; @@ -245,6 +247,8 @@ private: param_t min_sink_rate; param_t max_sink_rate; param_t max_climb_rate; + param_t heightrate_p; + param_t speedrate_p; param_t throttle_damp; param_t integrator_gain; param_t vertical_accel_limit; From c1d3f592b4082333ac454ab62dbdf685d41b3bb6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:36:59 +0200 Subject: [PATCH 443/600] Make ext libs more flash efficient --- src/lib/external_lgpl/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk index 53f1629e39..29d3514f68 100644 --- a/src/lib/external_lgpl/module.mk +++ b/src/lib/external_lgpl/module.mk @@ -46,3 +46,5 @@ # SRCS = tecs/tecs.cpp + +MAXOPTIMIZATION = -Os From 1bd57f1dbfb9c9896fabf71cec292aa7024a33ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:37:12 +0200 Subject: [PATCH 444/600] Make ECL more flash efficient --- src/lib/ecl/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk index f2aa3db6a5..93a5b511f9 100644 --- a/src/lib/ecl/module.mk +++ b/src/lib/ecl/module.mk @@ -39,3 +39,5 @@ SRCS = attitude_fw/ecl_pitch_controller.cpp \ attitude_fw/ecl_roll_controller.cpp \ attitude_fw/ecl_yaw_controller.cpp \ l1/ecl_l1_pos_controller.cpp + +MAXOPTIMIZATION = -Os From 2cf688312a83d3c8ad5b813de0643901891886c0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Jul 2014 11:58:59 +0200 Subject: [PATCH 445/600] mavlink: show 'rate mult' in 'mavlink status' output --- src/modules/mavlink/mavlink_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4ae90c3442..69beb9cc95 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1660,6 +1660,7 @@ Mavlink::display_status() printf("\ttx: %.3f kB/s\n", (double)_rate_tx); printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); printf("\trx: %.3f kB/s\n", (double)_rate_rx); + printf("\trate mult: %.3f\n", (double)_rate_mult); } int From e3bc5571556d043ad5c8c4fcabd1f9b371599397 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Jul 2014 11:59:38 +0200 Subject: [PATCH 446/600] rc.usb: set RC_CHANNELS_RAW rate to 5Hz --- ROMFS/px4fmu_common/init.d/rc.usb | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index d31ef11cfd..eb6039b4c5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -19,6 +19,8 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 usleep 100000 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 usleep 100000 +mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 +usleep 100000 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 usleep 100000 mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20 From c9eea8fbfaad7bfb3eee36a49588c9ac3a42ddc6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Jul 2014 12:01:09 +0200 Subject: [PATCH 447/600] nshterm: increase stack size to fix crash on 'ls -l' --- src/systemcmds/nshterm/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index b22b446dae..e2fa0ff804 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1200 +MODULE_STACKSIZE = 1400 From 5a7a6bca777317cc69840c35b82c50486dd061e7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Jul 2014 16:02:58 +0200 Subject: [PATCH 448/600] fetch_log.py renamed to fetch_file.py and reworked, works with all files, not only logs, added recursive directory download --- Tools/fetch_file.py | 207 ++++++++++++++++++++++++++++++++++++++++++++ Tools/fetch_log.py | 133 ---------------------------- 2 files changed, 207 insertions(+), 133 deletions(-) create mode 100644 Tools/fetch_file.py delete mode 100644 Tools/fetch_log.py diff --git a/Tools/fetch_file.py b/Tools/fetch_file.py new file mode 100644 index 0000000000..7ad08192d2 --- /dev/null +++ b/Tools/fetch_file.py @@ -0,0 +1,207 @@ +#!/usr/bin/env python +############################################################################ +# +# 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +"""Fetch files via nsh console + +Usage: python fetch_file.py [-l] [-f] [-d device] [-s speed] [-o out_path] path +\t-l\tList files +\t-f\tOverwrite existing files +\t-d\tSerial device +\t-s\tSerial baudrate +\t-o\tOutput path +\tpath\tPath to list/fetch, if ends with "/" then directory will be fetched recursively""" + +__author__ = "Anton Babushkin" +__version__ = "1.1" + +import serial, time, sys, os + +def _wait_for_string(ser, s, timeout): + t0 = time.time() + buf = [] + res = [] + while (True): + c = ser.read() + buf.append(c) + if len(buf) > len(s): + res.append(buf.pop(0)) + if "".join(buf) == s: + break + if timeout > 0.0 and time.time() - t0 > timeout: + raise Exception("Timeout while waiting for: " + s) + return "".join(res) + +def _exec_cmd(ser, cmd, timeout): + ser.write(cmd + "\n") + ser.flush() + _wait_for_string(ser, cmd + "\r\n", timeout) + return _wait_for_string(ser, "nsh> \x1b[K", timeout) + +def _ls_dir_raw(ser, dir, timeout): + return _exec_cmd(ser, "ls -l " + dir, timeout) + +def _ls_dir(ser, dir, timeout): + res = [] + for line in _ls_dir_raw(ser, dir, timeout).splitlines(): + if line == dir + ":": + continue + if line.startswith("nsh: ls: no such directory:"): + raise Exception("No such file: " + dir) + res.append((line[20:], int(line[11:19].strip()), line[1] == "d")) + return res + +def _get_file(ser, fn, fn_out, force, timeout): + print "Get %s:" % fn, + if not force: + # Check if file already exists with the same size + try: + os.stat(fn_out) + except: + pass + else: + print "already fetched, skip" + return + + cmd = "dumpfile " + fn + ser.write(cmd + "\n") + ser.flush() + _wait_for_string(ser, cmd + "\r\n", timeout) + res = _wait_for_string(ser, "\n", timeout) + if res.startswith("OK"): + # Got correct responce, open temp file + fn_out_part = fn_out + ".part" + fout = open(fn_out_part, "wb") + + size = int(res.split()[1]) + sys.stdout.write(" [%i bytes] " % size) + n = 0 + while (n < size): + buf = ser.read(min(size - n, 8192)) + n += len(buf) + sys.stdout.write(".") + sys.stdout.flush() + fout.write(buf) + print " done" + fout.close() + os.rename(fn_out_part, fn_out) + else: + raise Exception("Error reading file") + _wait_for_string(ser, "nsh> \x1b[K", timeout) + +def _get_files_in_dir(ser, path, path_out, force, timeout): + try: + os.mkdir(path_out) + except: + pass + for fn in _ls_dir(ser, path, timeout): + path_fn = os.path.join(path, fn[0]) + path_fn_out = os.path.join(path_out, fn[0]) + if fn[2]: + _get_files_in_dir(ser, path_fn[:-1], path_fn_out[:-1], force, timeout) + else: + _get_file(ser, path_fn, path_fn_out, force, timeout) + +def _usage(): + print """Usage: python fetch_file.py [-l] [-f] [-d device] [-s speed] [-o out_path] path +\t-l\tList files +\t-f\tOverwrite existing files +\t-d\tSerial device +\t-s\tSerial baudrate +\t-o\tOutput path +\tpath\tPath to list/fetch, if ends with "/" then directory will be fetched recursively""" + +def _main(): + dev = "/dev/tty.usbmodem1" + speed = "57600" + cmd = "get" + path = None + path_out = None + force = False + + opt = None + for arg in sys.argv[1:]: + if opt != None: + if opt == "d": + dev = arg + elif opt == "s": + speed = arg + elif opt == "o": + path_out = arg + opt = None + else: + if arg == "-l": + cmd = "ls" + elif arg == "-f": + force = True + elif arg == "-d": + opt = "d" + elif arg == "-s": + opt = "s" + elif arg == "-o": + opt = "o" + elif path == None: + path = arg + + if path == None: + _usage() + exit(0) + + # Connect to serial port + ser = serial.Serial(dev, speed, timeout=0.2) + + timeout = 1.0 + + try: + if cmd == "ls": + # List directory + print _ls_dir_raw(ser, path, timeout) + elif cmd == "get": + # Get file(s) + if path.endswith("/"): + # Get all files from directory recursively + if path_out == None: + path_out = os.path.split(path[:-1])[1] + _get_files_in_dir(ser, path[:-1], path_out, force, timeout) + else: + # Get one file + if path_out == None: + path_out = os.path.split(path)[1] + _get_file(ser, path, os.path.split(path)[1], force, timeout) + except Exception as e: + print e + + ser.close() + +if __name__ == "__main__": + _main() diff --git a/Tools/fetch_log.py b/Tools/fetch_log.py deleted file mode 100644 index edcc6557c2..0000000000 --- a/Tools/fetch_log.py +++ /dev/null @@ -1,133 +0,0 @@ -#!/usr/bin/env python -############################################################################ -# -# Copyright (C) 2012, 2013 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 -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Log fetcher -# -# Print list of logs: -# python fetch_log.py -# -# Fetch log: -# python fetch_log.py sess001/log001.bin -# - -import serial, time, sys, os - -def wait_for_string(ser, s, timeout=1.0, debug=False): - t0 = time.time() - buf = [] - res = [] - n = 0 - while (True): - c = ser.read() - if debug: - sys.stderr.write(c) - buf.append(c) - if len(buf) > len(s): - res.append(buf.pop(0)) - n += 1 - if n % 10000 == 0: - sys.stderr.write(str(n) + "\n") - if "".join(buf) == s: - break - if timeout > 0.0 and time.time() - t0 > timeout: - raise Exception("Timeout while waiting for: " + s) - return "".join(res) - -def exec_cmd(ser, cmd, timeout): - ser.write(cmd + "\n") - ser.flush() - wait_for_string(ser, cmd + "\r\n", timeout) - return wait_for_string(ser, "nsh> \x1b[K", timeout) - -def ls_dir(ser, dir, timeout=1.0): - res = [] - for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]: - res.append((line[20:], int(line[11:19].strip()), line[1] == "d")) - return res - -def list_logs(ser): - logs_dir = "/fs/microsd/log" - res = [] - for d in ls_dir(ser, logs_dir): - if d[2]: - sess_dir = d[0][:-1] - for f in ls_dir(ser, logs_dir + "/" + sess_dir): - log_file = f[0] - log_size = f[1] - res.append(sess_dir + "/" + log_file + "\t" + str(log_size)) - return "\n".join(res) - -def fetch_log(ser, fn, timeout): - cmd = "dumpfile " + fn - ser.write(cmd + "\n") - ser.flush() - wait_for_string(ser, cmd + "\r\n", timeout, True) - res = wait_for_string(ser, "\n", timeout, True) - data = [] - if res.startswith("OK"): - size = int(res.split()[1]) - n = 0 - print "Reading data:" - while (n < size): - buf = ser.read(min(size - n, 8192)) - data.append(buf) - n += len(buf) - sys.stdout.write(".") - sys.stdout.flush() - print - else: - raise Exception("Error reading log") - wait_for_string(ser, "nsh> \x1b[K", timeout) - return "".join(data) - -def main(): - dev = "/dev/tty.usbmodem1" - ser = serial.Serial(dev, "115200", timeout=0.2) - if len(sys.argv) < 2: - print list_logs(ser) - else: - log_file = sys.argv[1] - data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0) - try: - os.mkdir(log_file.split("/")[0]) - except: - pass - fout = open(log_file, "wb") - fout.write(data) - fout.close() - ser.close() - -if __name__ == "__main__": - main() From 7f2799a78baf022db05484df2168b1b5a89923a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 17:49:51 +0200 Subject: [PATCH 449/600] global pos topic docs --- src/modules/uORB/topics/vehicle_global_position.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index e32529cb46..25137c1c64 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -70,8 +70,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 eph; - float epv; + float eph; /**< Standard deviation of position estimate horizontally */ + float epv; /**< Standard deviation of position vertically */ }; /** From 0cbd943d093f2cb69102e57ef7964b5dbebd9f86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 17:50:52 +0200 Subject: [PATCH 450/600] Differentiate between EPH and EPV. Do not declare position invalid because of EPV (because we use the baro anyway). No fundamental logic change / cleanup to ensure current approach and arming logic remains intact --- src/modules/commander/commander.cpp | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6c24734e52..dec5ab46c4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -163,7 +163,8 @@ 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 float eph_threshold = 5.0f; +static float epv_threshold = 10.0f; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -1108,32 +1109,32 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ - bool eph_epv_good; + bool eph_good; if (status.condition_global_position_valid) { - if (global_position.eph > eph_epv_threshold * 2.5f || global_position.epv > eph_epv_threshold * 2.5f) { - eph_epv_good = false; + if (global_position.eph > eph_threshold * 2.5f) { + eph_good = false; } else { - eph_epv_good = true; + eph_good = true; } } else { - if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { - eph_epv_good = true; + if (global_position.eph < eph_threshold) { + eph_good = true; } else { - eph_epv_good = false; + eph_good = false; } } - check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { home.lat = global_position.lat; home.lon = global_position.lon; @@ -1163,8 +1164,8 @@ int commander_thread_main(int argc, char *argv[]) /* hysteresis for EPH */ bool local_eph_good; - if (status.condition_global_position_valid) { - if (local_position.eph > eph_epv_threshold * 2.0f) { + if (status.condition_local_position_valid) { + if (local_position.eph > eph_threshold * 2.5f) { local_eph_good = false; } else { @@ -1172,7 +1173,7 @@ int commander_thread_main(int argc, char *argv[]) } } else { - if (local_position.eph < eph_epv_threshold) { + if (local_position.eph < eph_threshold) { local_eph_good = true; } else { @@ -1504,7 +1505,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 < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { // TODO remove code duplication home.lat = global_position.lat; From e3da7f564f1bc7746ba3be946fd02b95642cbf6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 21:36:55 +0200 Subject: [PATCH 451/600] NaN check and better init in lowpass --- src/modules/controllib/blocks.cpp | 3 +++ src/modules/controllib/blocks.hpp | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 0175acda99..f739446fa7 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -121,6 +121,9 @@ int blockLimitSymTest() float BlockLowPass::update(float input) { + if (!isfinite(getState())) { + setState(input); + } float b = 2 * float(M_PI) * getFCut() * getDt(); float a = b / (1 + b); setState(a * input + (1 - a)*getState()); diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 37d7832b33..bffc355a82 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -114,7 +114,7 @@ public: // methods BlockLowPass(SuperBlock *parent, const char *name) : Block(parent, name), - _state(0), + _state(0.0f/0.0f /* initialize to invalid val, force into is_finite() check on first call */), _fCut(this, "") // only one parameter, no need to name {}; virtual ~BlockLowPass() {}; From 1dc23d0c49d99fa93284a277a6bc4970ac0e7b3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 17:41:45 +0200 Subject: [PATCH 452/600] Disable mTECS until runtime error is better understood --- .../fw_pos_control_l1_main.cpp | 56 +++++++++---------- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 + 2 files changed, 29 insertions(+), 28 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f0e18d1bb4..2d6c5c121e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -194,7 +194,7 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; - fwPosctrl::mTecs _mTecs; + // fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; struct { @@ -443,7 +443,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _groundspeed_undershoot(0.0f), _global_pos_valid(false), _l1_control(), - _mTecs(), + // _mTecs(), _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; @@ -603,7 +603,7 @@ FixedwingPositionControl::parameters_update() launchDetector.updateParams(); /* Update the mTecs */ - _mTecs.updateParams(); + // _mTecs.updateParams(); return OK; } @@ -820,9 +820,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - if (!_mTecs.getEnabled()) { + // if (!_mTecs.getEnabled()) { _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - } + // } /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -838,10 +838,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_pos_control_mode) { /* reset integrators */ - if (_mTecs.getEnabled()) { - _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); - } + // if (_mTecs.getEnabled()) { + // _mTecs.resetIntegrators(); + // _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + // } } _was_pos_control_mode = true; @@ -1160,9 +1160,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(/*_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : */_tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + _att_sp.pitch_body = /*_mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : */_tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1342,29 +1342,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - if (_mTecs.getEnabled()) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - } - fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); - } else { - limitOverride.disablePitchMinOverride(); - } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); - } else { + // if (_mTecs.getEnabled()) { + // /* Using mtecs library: prepare arguments for mtecs call */ + // float flightPathAngle = 0.0f; + // float ground_speed_length = ground_speed.length(); + // if (ground_speed_length > FLT_EPSILON) { + // flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + // } + // fwPosctrl::LimitOverride limitOverride; + // if (climbout_mode) { + // limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + // } else { + // limitOverride.disablePitchMinOverride(); + // } + // _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + // limitOverride); + // } else { /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); - } + // } } int diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 749f57a2ba..afc411a60a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -76,6 +76,7 @@ mTecs::mTecs() : _counter(0), _debug(false) { + warnx("starting"); } mTecs::~mTecs() From 39aa9112baf0120538255cf44292ff8e1ff3387c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 17:56:17 +0200 Subject: [PATCH 453/600] Fix threading in FW pos controller --- .../fw_pos_control_l1_main.cpp | 87 +++++++++---------- 1 file changed, 43 insertions(+), 44 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 2d6c5c121e..eadb63f40d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -92,6 +92,8 @@ #include "landingslope.h" #include "mtecs/mTecs.h" +static int _control_task = -1; /**< task handle for sensor task */ + /** * L1 control app start / stop handling function @@ -116,9 +118,9 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ - int start(); + static int start(); /** * Task status @@ -132,7 +134,6 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_running; /**< if true, task is running in its mainloop */ - int _control_task; /**< task handle for sensor task */ int _global_pos_sub; int _pos_sp_triplet_sub; @@ -194,7 +195,7 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; - // fwPosctrl::mTecs _mTecs; + fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; struct { @@ -393,7 +394,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), _task_should_exit(false), _task_running(false), - _control_task(-1), /* subscriptions */ _global_pos_sub(-1), @@ -443,7 +443,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _groundspeed_undershoot(0.0f), _global_pos_valid(false), _l1_control(), - // _mTecs(), + _mTecs(), _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; @@ -603,7 +603,7 @@ FixedwingPositionControl::parameters_update() launchDetector.updateParams(); /* Update the mTecs */ - // _mTecs.updateParams(); + _mTecs.updateParams(); return OK; } @@ -704,7 +704,17 @@ FixedwingPositionControl::vehicle_setpoint_poll() void FixedwingPositionControl::task_main_trampoline(int argc, char *argv[]) { + l1_control::g_control = new FixedwingPositionControl(); + + if (l1_control::g_control == nullptr) { + warnx("OUT OF MEM"); + return; + } + + /* only returns on exit */ l1_control::g_control->task_main(); + delete l1_control::g_control; + l1_control::g_control = nullptr; } float @@ -820,9 +830,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - // if (!_mTecs.getEnabled()) { + if (!_mTecs.getEnabled()) { _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - // } + } /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -838,10 +848,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_pos_control_mode) { /* reset integrators */ - // if (_mTecs.getEnabled()) { - // _mTecs.resetIntegrators(); - // _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); - // } + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } } _was_pos_control_mode = true; @@ -1160,9 +1170,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(/*_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : */_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = /*_mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : */_tecs.get_pitch_demand(); + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1178,10 +1188,6 @@ void FixedwingPositionControl::task_main() { - /* inform about start */ - warnx("Initializing.."); - fflush(stdout); - /* * do subscriptions */ @@ -1342,29 +1348,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - // if (_mTecs.getEnabled()) { - // /* Using mtecs library: prepare arguments for mtecs call */ - // float flightPathAngle = 0.0f; - // float ground_speed_length = ground_speed.length(); - // if (ground_speed_length > FLT_EPSILON) { - // flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - // } - // fwPosctrl::LimitOverride limitOverride; - // if (climbout_mode) { - // limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); - // } else { - // limitOverride.disablePitchMinOverride(); - // } - // _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - // limitOverride); - // } else { + if (_mTecs.getEnabled()) { + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); + } else { /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); - // } + } } int @@ -1398,14 +1404,7 @@ int fw_pos_control_l1_main(int argc, char *argv[]) if (l1_control::g_control != nullptr) errx(1, "already running"); - l1_control::g_control = new FixedwingPositionControl; - - if (l1_control::g_control == nullptr) - errx(1, "alloc failed"); - - if (OK != l1_control::g_control->start()) { - delete l1_control::g_control; - l1_control::g_control = nullptr; + if (OK != FixedwingPositionControl::start()) { err(1, "start failed"); } From 05e253b357caca3cdeb274027c7627a8a59d190d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 19:49:01 +0200 Subject: [PATCH 454/600] Proper locking for message send calls --- src/modules/mavlink/mavlink_main.cpp | 22 ++++++++++++++++------ src/modules/mavlink/mavlink_main.h | 1 + 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 49781e8f8d..8c79a1fee0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -159,9 +159,10 @@ Mavlink::Mavlink() : _rate_tx(0.0f), _rate_txerr(0.0f), _rate_rx(0.0f), - _rstatus {}, - _message_buffer {}, - _message_buffer_mutex {}, + _rstatus{}, + _message_buffer{}, + _message_buffer_mutex{}, + _send_mutex{}, _param_initialized(false), _param_system_id(0), _param_component_id(0), @@ -716,7 +717,7 @@ Mavlink::get_free_tx_buf() * flow control if it continues to be full */ int buf_free = 0; - (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) { /* Disable hardware flow control: @@ -742,6 +743,8 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) return; } + pthread_mutex_lock(_send_mutex); + int buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; @@ -754,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); - return; + goto out; } uint8_t buf[MAVLINK_MAX_PACKET_LEN]; @@ -762,7 +765,8 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* header */ buf[0] = MAVLINK_STX; buf[1] = payload_len; - buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; // TODO use internal seq counter? + /* use mavlink's internal counter for the TX seq */ + buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; buf[3] = mavlink_system.sysid; buf[4] = mavlink_system.compid; buf[5] = msgid; @@ -790,6 +794,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } + +out: + pthread_mutex_unlock(_send_mutex); } void @@ -1298,6 +1305,9 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } + /* initialize send mutex */ + pthread_mutex_init(&_send_mutex, NULL); + /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 38149cf107..1e2e94cef6 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -342,6 +342,7 @@ private: mavlink_message_buffer _message_buffer; pthread_mutex_t _message_buffer_mutex; + pthread_mutex_t _send_mutex; bool _param_initialized; param_t _param_system_id; From 5bad95c92dc7b974a793074ca173fe129d258fb7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 19:52:02 +0200 Subject: [PATCH 455/600] make GCC happy --- src/modules/mavlink/mavlink_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8c79a1fee0..618b9fa29b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -743,7 +743,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) return; } - pthread_mutex_lock(_send_mutex); + pthread_mutex_lock(&_send_mutex); int buf_free = get_free_tx_buf(); @@ -757,7 +757,8 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); - goto out; + pthread_mutex_unlock(&_send_mutex); + return; } uint8_t buf[MAVLINK_MAX_PACKET_LEN]; @@ -795,8 +796,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) count_txbytes(packet_len); } -out: - pthread_mutex_unlock(_send_mutex); + pthread_mutex_unlock(&_send_mutex); } void From 391a5c82cbe6d02136e16528b472352e8461b02a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 19:54:40 +0200 Subject: [PATCH 456/600] Print avionics voltage --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index daba4e7403..409cf9d894 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -335,6 +335,7 @@ void print_status() { warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); /* read all relevant states */ int state_sub = orb_subscribe(ORB_ID(vehicle_status)); From edccc826a1834aa21171e2b5f88c97454955bb16 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 21:16:29 +0200 Subject: [PATCH 457/600] Improved comment of bridge header --- src/modules/mavlink/mavlink_bridge_header.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index d82c2dae7a..0cd08769e1 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -44,7 +44,12 @@ __BEGIN_DECLS -//#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +/* + * We are NOT using convenience functions, + * but instead send messages with a custom function. + * So we do NOT do this: + * #define MAVLINK_USE_CONVENIENCE_FUNCTIONS + */ /* use efficient approach, see mavlink_helpers.h */ #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes From fbbfe61d745202f7163d874ab61d4568f3a18a21 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 10:49:28 +0200 Subject: [PATCH 458/600] Add missing lock --- src/modules/mavlink/mavlink_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 618b9fa29b..84422c4aab 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -808,6 +808,8 @@ Mavlink::resend_message(mavlink_message_t *msg) return; } + pthread_mutex_lock(&_send_mutex); + int buf_free = get_free_tx_buf(); _last_write_try_time = hrt_absolute_time(); @@ -819,6 +821,7 @@ Mavlink::resend_message(mavlink_message_t *msg) /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); return; } @@ -842,6 +845,8 @@ Mavlink::resend_message(mavlink_message_t *msg) _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } + + pthread_mutex_unlock(&_send_mutex); } void From 9d7e3feffcfb34708cda5a726b7900101f41b205 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 11:18:59 +0200 Subject: [PATCH 459/600] airspeed: Filter slightly more, just a touch to not induce phase delay --- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 9cfa7f3838..1597062789 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -103,7 +103,7 @@ /* Measurement rate is 100Hz */ #define MEAS_RATE 100 -#define MEAS_DRIVER_FILTER_FREQ 1.5f +#define MEAS_DRIVER_FILTER_FREQ 1.2f #define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ class MEASAirspeed : public Airspeed From 2b71067618b2b4333c44072e27b960435407166d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 11:28:17 +0200 Subject: [PATCH 460/600] Code style fixes --- src/modules/mavlink/mavlink_main.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 84422c4aab..5589738c15 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -159,10 +159,10 @@ Mavlink::Mavlink() : _rate_tx(0.0f), _rate_txerr(0.0f), _rate_rx(0.0f), - _rstatus{}, - _message_buffer{}, - _message_buffer_mutex{}, - _send_mutex{}, + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _send_mutex {}, _param_initialized(false), _param_system_id(0), _param_component_id(0), @@ -387,7 +387,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) /* if not in normal mode, we are an onboard link * onboard links should only pass on messages from the same system ID */ - if(!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { + if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { inst->pass_message(msg); } } @@ -731,6 +731,7 @@ Mavlink::get_free_tx_buf() enable_flow_control(false); } } + return buf_free; } @@ -778,7 +779,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* checksum */ uint16_t checksum; crc_init(&checksum); - crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); crc_accumulate(mavlink_message_crcs[msgid], &checksum); buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); @@ -985,7 +986,7 @@ Mavlink::adjust_stream_rates(const float multiplier) /* allow max ~600 Hz */ if (interval < 1600) { interval = 1600; - } + } /* set new interval */ stream->set_interval(interval * multiplier); @@ -1513,6 +1514,7 @@ Mavlink::task_main(int argc, char *argv[]) _bytes_txerr = 0; _bytes_rx = 0; } + _bytes_timestamp = t; } @@ -1676,6 +1678,7 @@ Mavlink::display_status() } else { printf("\tno telem status.\n"); } + printf("\trates:\n"); printf("\ttx: %.3f kB/s\n", (double)_rate_tx); printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); From ee42d5f53d81580bee56b1cdcf69ebfdefb6ba09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 12:15:33 +0200 Subject: [PATCH 461/600] Comment fix --- .../position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 dff2768c77..cc0fb4155e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -269,7 +269,7 @@ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); /** * Disable vision input * - * Set to the appropriate key to disable input + * Set to the appropriate key (328754) to disable vision input. * * @min 0.0 * @max 1.0 From 39699670026f463df79a45c0dbf5cf7ceea8d5a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 12:16:11 +0200 Subject: [PATCH 462/600] Default to TECS --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 7cfe63fbc2..58a1e9f6b1 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -55,7 +55,7 @@ * @max 1 * @group mTECS */ -PARAM_DEFINE_INT32(MT_ENABLED, 1); +PARAM_DEFINE_INT32(MT_ENABLED, 0); /** * Total Energy Rate Control Feedforward From 92a4757971701bc1536db85f62590f9e864d96fe Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 31 Jul 2014 12:45:44 +0200 Subject: [PATCH 463/600] sdlog2_dump.py: minor fix allowing handling FMT messages in the middle of stream --- Tools/sdlog2/sdlog2_dump.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/sdlog2/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py index 5b1e55e22a..ca2efb021c 100644 --- a/Tools/sdlog2/sdlog2_dump.py +++ b/Tools/sdlog2/sdlog2_dump.py @@ -149,7 +149,8 @@ class SDLog2Parser: break if first_data_msg: # build CSV columns and init data map - self.__initCSV() + if not self.__debug_out: + self.__initCSV() first_data_msg = False self.__parseMsg(msg_descr) bytes_read += self.__ptr From fc2e0fad4731ef543be4c3da73de6b670d40d804 Mon Sep 17 00:00:00 2001 From: philipoe Date: Thu, 31 Jul 2014 15:23:30 +0200 Subject: [PATCH 464/600] TECS: Added separate gain / time constant for the throttle loop to allow independent tuning --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- src/lib/external_lgpl/tecs/tecs.h | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 6386e37a03..16c7e5ffad 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -298,7 +298,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } else { // Calculate gain scaler from specific energy error to throttle - float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min)); + float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min)); // Calculate feed-forward throttle float ff_throttle = 0; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 5cafb1c791..10c9e9344f 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -123,6 +123,10 @@ public: _timeConst = time_const; } + void set_time_const_throt(float time_const_throt) { + _timeConstThrot = time_const_throt; + } + void set_min_sink_rate(float rate) { _minSinkRate = rate; } @@ -204,6 +208,7 @@ private: float _minSinkRate; float _maxSinkRate; float _timeConst; + float _timeConstThrot; float _ptchDamp; float _thrDamp; float _integGain; From 7f293be7d77603768899aedb438821dd19b8b4d7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 31 Jul 2014 17:30:00 +0200 Subject: [PATCH 465/600] mavlink, rc.usb: increase HIL_CONTROLS rate and datarate on USB to allow HIL simulation @ 200Hz --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index eb6039b4c5..f1c21f295d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,7 +3,7 @@ # USB MAVLink start # -mavlink start -r 10000 -d /dev/ttyACM0 -x +mavlink start -r 20000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200 usleep 100000 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5589738c15..9a39d3bed2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" -#define MAX_DATA_RATE 10000 // max data rate in bytes/s +#define MAX_DATA_RATE 20000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate #define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN @@ -694,7 +694,7 @@ Mavlink::set_hil_enabled(bool hil_enabled) /* enable HIL */ if (hil_enabled && !_hil_enabled) { _hil_enabled = true; - configure_stream("HIL_CONTROLS", 150.0f); + configure_stream("HIL_CONTROLS", 200.0f); } /* disable HIL */ From b8c14417ce5c667920b32e3ba94ca8f496e0d10c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 31 Jul 2014 23:34:34 +0400 Subject: [PATCH 466/600] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index d84fc8a846..6980ee8240 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit d84fc8a84678d93f97f93b240c81472797ca5889 +Subproject commit 6980ee824074aa2f7a62221bf6040ee411119520 From 87273abc9aa0006baff53741e605c9992ec8c955 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 1 Aug 2014 00:56:44 +0400 Subject: [PATCH 467/600] UAVCAN node version info --- src/modules/uavcan/uavcan_main.cpp | 61 +++++++++++++++++++++++------- src/modules/uavcan/uavcan_main.hpp | 1 + 2 files changed, 48 insertions(+), 14 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 27e77e9c5a..ffd5990708 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -39,6 +39,8 @@ #include #include #include +#include +#include #include #include @@ -173,6 +175,44 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return OK; } +void UavcanNode::fill_node_info() +{ + /* software version */ + uavcan::protocol::SoftwareVersion swver; + + // Extracting the last 8 hex digits of FW_GIT and converting them to int + const unsigned fw_git_len = std::strlen(FW_GIT); + if (fw_git_len >= 8) { + char fw_git_short[9] = {}; + std::memmove(fw_git_short, FW_GIT + fw_git_len - 8, 8); + assert(fw_git_short[8] == '\0'); + char *end = nullptr; + swver.vcs_commit = std::strtol(fw_git_short, &end, 16); + swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; + } + + warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); + + _node.setSoftwareVersion(swver); + + /* hardware version */ + uavcan::protocol::HardwareVersion hwver; + + if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) { + hwver.major = 1; + } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) { + hwver.major = 2; + } else { + ; // All other values of HW_ARCH resolve to zero + } + + uint8_t udid[12] = {}; // Someone seems to love magic numbers + get_board_serial(udid); + uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); + + _node.setHardwareVersion(hwver); +} + int UavcanNode::init(uavcan::NodeID node_id) { int ret = -1; @@ -183,6 +223,13 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret != OK) return ret; + _node.setName("org.pixhawk.pixhawk"); + + _node.setNodeID(node_id); + + fill_node_info(); + + /* initializing the bridges UAVCAN <--> uORB */ ret = _esc_controller.init(); if (ret < 0) return ret; @@ -191,20 +238,6 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret < 0) return ret; - uavcan::protocol::SoftwareVersion swver; - swver.major = 12; // TODO fill version info - swver.minor = 34; - _node.setSoftwareVersion(swver); - - uavcan::protocol::HardwareVersion hwver; - hwver.major = 42; // TODO fill version info - hwver.minor = 42; - _node.setHardwareVersion(hwver); - - _node.setName("org.pixhawk"); // Huh? - - _node.setNodeID(node_id); - return _node.start(); } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 443525379e..05b66fd7b2 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -94,6 +94,7 @@ public: static UavcanNode* instance() { return _instance; } private: + void fill_node_info(); int init(uavcan::NodeID node_id); void node_spin_once(); int run(); From b606674fc77aa772fba66ad09dd3879a62a7f44c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 1 Aug 2014 01:04:24 +0400 Subject: [PATCH 468/600] Removed misleading comment from the UAVCAN module makefile --- src/modules/uavcan/module.mk | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 1ef6f0cfa8..3865f24681 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -65,10 +65,6 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM # # Invoke DSDL compiler -# TODO: Add make target for this, or invoke dsdlc manually. -# The second option assumes that the generated headers shall be saved -# under the version control, which may be undesirable. -# The first option requires any Python and the Python Mako library for the sources to be built. # $(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) INCLUDE_DIRS += dsdlc_generated From 2d4dd0d5c03a7ef3d696f40b6a6988e08e991034 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 28 Jul 2014 20:37:26 +0900 Subject: [PATCH 469/600] Tone alarm sound for barometer check fail --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 19f792d19c..307f7dbc78 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -150,6 +150,7 @@ enum { TONE_ARMING_FAILURE_TUNE, TONE_PARACHUTE_RELEASE_TUNE, TONE_EKF_WARNING_TUNE, + TONE_BARO_WARNING_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 03c7bd399f..8f523b3902 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -337,6 +337,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<< Date: Fri, 1 Aug 2014 13:04:28 +0200 Subject: [PATCH 470/600] Move the last throttle demand setting to a better position --- src/lib/external_lgpl/tecs/tecs.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 16c7e5ffad..579fb5d42c 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -327,9 +327,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM _throttle_dem = constrain(_throttle_dem, _last_throttle_dem - thrRateIncr, _last_throttle_dem + thrRateIncr); - _last_throttle_dem = _throttle_dem; } + // Ensure _last_throttle_dem is always initialized properly + // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!! + _last_throttle_dem = _throttle_dem; + // Calculate integrator state upper and lower limits // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand From 3b1c92e42f57277a12d40963c22fbea9820ee0ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 13:10:07 +0200 Subject: [PATCH 471/600] Make throttle time constant tunable separately, group TECS params correctly, make climbout alt configurable --- .../fw_pos_control_l1_main.cpp | 11 ++++- .../fw_pos_control_l1_params.c | 49 ++++++++++++++----- 2 files changed, 46 insertions(+), 14 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index eadb63f40d..f05f20cd00 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -203,9 +203,11 @@ private: float l1_damping; float time_const; + float time_const_throt; float min_sink_rate; float max_sink_rate; float max_climb_rate; + float climbout_diff; float heightrate_p; float speedrate_p; float throttle_damp; @@ -245,9 +247,11 @@ private: param_t l1_damping; param_t time_const; + param_t time_const_throt; param_t min_sink_rate; param_t max_sink_rate; param_t max_climb_rate; + param_t climbout_diff; param_t heightrate_p; param_t speedrate_p; param_t throttle_damp; @@ -471,9 +475,11 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); + _parameter_handles.climbout_diff = param_find("FW_CLMBOUT_DIFF"); _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); @@ -536,6 +542,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt)); param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); @@ -547,6 +554,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); @@ -564,6 +572,7 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); _tecs.set_time_const(_parameters.time_const); + _tecs.set_time_const_throt(_parameters.time_const_throt); _tecs.set_min_sink_rate(_parameters.min_sink_rate); _tecs.set_max_sink_rate(_parameters.max_sink_rate); _tecs.set_throttle_damp(_parameters.throttle_damp); @@ -1095,7 +1104,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi usePreTakeoffThrust = false; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 15.0f) { + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 52128e1b7a..a0d9309b92 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -154,6 +154,18 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); +/** + * Climbout Altitude difference + * + * If the altitude error exceeds this parameter, the system will climb out + * with maximum throttle and minimum airspeed until it is closer than this + * distance to the desired altitude. Mostly used for takeoff waypoints / modes. + * Set to zero to disable climbout mode (not recommended). + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); + /** * Maximum climb rate * @@ -181,7 +193,7 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); * set to THR_MIN and flown at the same airspeed as used * to measure FW_T_CLMB_MAX. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); @@ -194,7 +206,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); * exceeding the lower pitch angle limit and without over-speeding * the aircraft. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); @@ -205,17 +217,28 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); * Smaller values make it faster to respond, larger values make it slower * to respond. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); +/** + * TECS Throttle time constant + * + * This is the time constant of the TECS throttle control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); + /** * Throttle damping factor * * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); @@ -227,7 +250,7 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); * and height offsets are trimmed out, but reduces damping and * increases overshoot. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); @@ -240,7 +263,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * allows for reasonably aggressive pitch changes if required to recover * from under-speed conditions. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); @@ -253,7 +276,7 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); * the solution more towards use of the barometer, whilst reducing it weights * the solution more towards use of the accelerometer data. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); @@ -266,7 +289,7 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); @@ -282,7 +305,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); @@ -300,7 +323,7 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); * Note to Glider Pilots - set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); @@ -312,21 +335,21 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); * will work well provided the pitch to servo controller has been tuned * properly. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); /** * Height rate P factor * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); /** * Speed rate P factor * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); From 2e4dff3fea2177538f3549cedd8f84fbbbecd4ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 13:21:30 +0200 Subject: [PATCH 472/600] removed debug statement from mTECS --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index afc411a60a..749f57a2ba 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -76,7 +76,6 @@ mTecs::mTecs() : _counter(0), _debug(false) { - warnx("starting"); } mTecs::~mTecs() From 32e32d92bdad3eb4e92e9e53c112fcdcf848c7e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 14:39:24 +0200 Subject: [PATCH 473/600] Add throttle slew rate param --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++++ .../fw_pos_control_l1/fw_pos_control_l1_params.c | 11 +++++++++++ 2 files changed, 16 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f05f20cd00..189a19d48f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -229,6 +229,7 @@ private: float throttle_min; float throttle_max; float throttle_cruise; + float throttle_slew_max; float throttle_land_max; @@ -273,6 +274,7 @@ private: param_t throttle_min; param_t throttle_max; param_t throttle_cruise; + param_t throttle_slew_max; param_t throttle_land_max; @@ -464,6 +466,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.roll_limit = param_find("FW_R_LIM"); _parameter_handles.throttle_min = param_find("FW_THR_MIN"); _parameter_handles.throttle_max = param_find("FW_THR_MAX"); + _parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX"); _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); @@ -538,6 +541,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min)); param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max)); param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); @@ -576,6 +580,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_min_sink_rate(_parameters.min_sink_rate); _tecs.set_max_sink_rate(_parameters.max_sink_rate); _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_throttle_slewrate(_parameters.throttle_slew_max); _tecs.set_integrator_gain(_parameters.integrator_gain); _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index a0d9309b92..41c3744076 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -82,6 +82,17 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); */ PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); +/** + * Throttle max slew rate + * + * Maximum slew rate for the commanded throttle + * + * @min 0.0 + * @max 1.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); + /** * Negative pitch limit * From 491c6c29acef343819f32655e6e87334531af8cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 14:43:56 +0200 Subject: [PATCH 474/600] Init values --- src/lib/external_lgpl/tecs/tecs.h | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 10c9e9344f..752b0ddd9c 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,6 +28,26 @@ class __EXPORT TECS { public: TECS() : + _update_50hz_last_usec(0), + _update_speed_last_usec(0), + _update_pitch_throttle_last_usec(0), + // TECS tuning parameters + _hgtCompFiltOmega(0.0f), + _spdCompFiltOmega(0.0f), + _maxClimbRate(2.0f), + _minSinkRate(1.0f), + _maxSinkRate(2.0f), + _timeConst(5.0f), + _timeConstThrot(8.0f), + _ptchDamp(0.0f), + _thrDamp(0.0f), + _integGain(0.0f), + _vertAccLim(0.0f), + _rollComp(0.0f), + _spdWeight(0.5f), + _heightrate_p(0.0f), + _speedrate_p(0.0f), + _throttle_dem(0.0f), _pitch_dem(0.0f), _integ1_state(0.0f), _integ2_state(0.0f), From c3666a5504fc89b7e1861e25c9b6cb0c07453784 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 23:18:43 +0200 Subject: [PATCH 475/600] Widen default first WP radius --- src/modules/navigator/mission_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 881caa24e0..d15e1e771b 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -78,7 +78,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * waypoint is more distant than MIS_DIS_1WP from the current position. * * @min 0 - * @max 250 + * @max 1000 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175); +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); From 4d03202c4c8c322f9d93da0051dca015ab473cde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 23:42:26 +0200 Subject: [PATCH 476/600] Warn on far waypoints --- src/modules/navigator/mission.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 12f6b9c211..c0e37a3eda 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -273,6 +273,10 @@ Mission::check_dist_1wp() if (dist_to_1wp < _param_dist_1wp.get()) { _dist_1wp_ok = true; + if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp); + } return true; } else { From 9725a1635254ba6c0c1df0413f94398ec28e23f9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 2 Aug 2014 03:34:57 +0400 Subject: [PATCH 477/600] UAVCAN: Fixed short git hash computation --- src/modules/uavcan/uavcan_main.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ffd5990708..9d5404bafe 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -180,16 +180,13 @@ void UavcanNode::fill_node_info() /* software version */ uavcan::protocol::SoftwareVersion swver; - // Extracting the last 8 hex digits of FW_GIT and converting them to int - const unsigned fw_git_len = std::strlen(FW_GIT); - if (fw_git_len >= 8) { - char fw_git_short[9] = {}; - std::memmove(fw_git_short, FW_GIT + fw_git_len - 8, 8); - assert(fw_git_short[8] == '\0'); - char *end = nullptr; - swver.vcs_commit = std::strtol(fw_git_short, &end, 16); - swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; - } + // Extracting the first 8 hex digits of FW_GIT and converting them to int + char fw_git_short[9] = {}; + std::memmove(fw_git_short, FW_GIT, 8); + assert(fw_git_short[8] == '\0'); + char *end = nullptr; + swver.vcs_commit = std::strtol(fw_git_short, &end, 16); + swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); From 5824607c76ed38b5c08d1c9031814f741bce7c8b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 2 Aug 2014 13:01:42 +0200 Subject: [PATCH 478/600] uavcan: fix actuator groups subcriptions and poll() --- src/modules/uavcan/uavcan_main.cpp | 36 ++++++++++++++++-------------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 9d5404bafe..5b1539e3ed 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -253,7 +253,6 @@ int UavcanNode::run() // XXX figure out the output count _output_count = 2; - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); actuator_outputs_s outputs; @@ -273,21 +272,23 @@ int UavcanNode::run() _node.setStatusOk(); - while (!_task_should_exit) { + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds_num = 0; + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + while (!_task_should_exit) { + // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; } const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); @@ -301,7 +302,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 0; + unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { @@ -317,7 +318,7 @@ int UavcanNode::run() // XXX trigger failsafe } - //can we mix? + // can we mix? if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, @@ -417,7 +418,8 @@ UavcanNode::subscribe() // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; - _poll_fds_num = 0; + // the first fd used by CAN + _poll_fds_num = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); @@ -523,8 +525,8 @@ UavcanNode::print_info() warnx("not running, start first"); } - warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK"); + warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); } /* From 020ac409be62e6c459e7aa5cb3181941c9bbfd23 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Aug 2014 13:17:18 +0200 Subject: [PATCH 479/600] Remove airspeed and altitude raw data from TECS log, as we have these quantities already in the system log --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 6 ++---- src/modules/sdlog2/sdlog2.c | 6 ++---- src/modules/sdlog2/sdlog2_messages.h | 4 +--- src/modules/uORB/topics/tecs_status.h | 12 ++++++------ 4 files changed, 11 insertions(+), 17 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 749f57a2ba..bffeefc1ff 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -109,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* Write part of the status message */ _status.altitudeSp = altitudeSp; - _status.altitude = altitude; - _status.altitudeFiltered = altitudeFiltered; + _status.altitude_filtered = altitudeFiltered; /* use flightpath angle setpoint for total energy control */ @@ -146,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* Write part of the status message */ _status.airspeedSp = airspeedSp; - _status.airspeed = airspeed; - _status.airspeedFiltered = airspeedFiltered; + _status.airspeed_filtered = airspeedFiltered; /* use longitudinal acceleration setpoint for total energy control */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 31861c3fc8..6c4b494522 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1597,14 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[]) if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) { log_msg.msg_type = LOG_TECS_MSG; log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; - log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; - log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered; + log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; - log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; - log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; + log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 853a3811f6..fb7609435b 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -333,13 +333,11 @@ struct log_GS1B_s { #define LOG_TECS_MSG 30 struct log_TECS_s { float altitudeSp; - float altitude; float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; float airspeedSp; - float airspeed; float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; @@ -455,7 +453,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index 33055018ce..ddca19d61e 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -51,11 +51,13 @@ */ typedef enum { - TECS_MODE_NORMAL, + TECS_MODE_NORMAL = 0, TECS_MODE_UNDERSPEED, TECS_MODE_TAKEOFF, TECS_MODE_LAND, - TECS_MODE_LAND_THROTTLELIM + TECS_MODE_LAND_THROTTLELIM, + TECS_MODE_BAD_DESCENT, + TECS_MODE_CLIMBOUT } tecs_mode; /** @@ -65,14 +67,12 @@ struct tecs_status_s { uint64_t timestamp; /**< timestamp, in microseconds since system start */ float altitudeSp; - float altitude; - float altitudeFiltered; + float altitude_filtered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; float airspeedSp; - float airspeed; - float airspeedFiltered; + float airspeed_filtered; float airspeedDerivativeSp; float airspeedDerivative; From 43ef622725b1114beccb722bad0538c4c0cc3175 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Aug 2014 13:18:22 +0200 Subject: [PATCH 480/600] Reinstate TECS logging --- src/lib/external_lgpl/tecs/tecs.cpp | 40 +++++++++++++++--------- src/lib/external_lgpl/tecs/tecs.h | 47 ++++++++++++++++++----------- 2 files changed, 56 insertions(+), 31 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 579fb5d42c..a57a0481a6 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -554,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f // Calculate pitch demand _update_pitch(); -// // Write internal variables to the log_tuning structure. This -// // structure will be logged in dataflash at 10Hz - // log_tuning.hgt_dem = _hgt_dem_adj; - // log_tuning.hgt = _integ3_state; - // log_tuning.dhgt_dem = _hgt_rate_dem; - // log_tuning.dhgt = _integ2_state; - // log_tuning.spd_dem = _TAS_dem_adj; - // log_tuning.spd = _integ5_state; - // log_tuning.dspd = _vel_dot; - // log_tuning.ithr = _integ6_state; - // log_tuning.iptch = _integ7_state; - // log_tuning.thr = _throttle_dem; - // log_tuning.ptch = _pitch_dem; - // log_tuning.dspd_dem = _TAS_rate_dem; + _tecs_state.timestamp = now; + + if (_underspeed) { + _tecs_state.mode = ECL_TECS_MODE_UNDERSPEED; + } else if (_badDescent) { + _tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT; + } else if (_climbOutDem) { + _tecs_state.mode = ECL_TECS_MODE_CLIMBOUT; + } else { + // If no error flag applies, conclude normal + _tecs_state.mode = ECL_TECS_MODE_NORMAL; + } + + _tecs_state.hgt_dem = _hgt_dem_adj; + _tecs_state.hgt = _integ3_state; + _tecs_state.dhgt_dem = _hgt_rate_dem; + _tecs_state.dhgt = _integ2_state; + _tecs_state.spd_dem = _TAS_dem_adj; + _tecs_state.spd = _integ5_state; + _tecs_state.dspd = _vel_dot; + _tecs_state.ithr = _integ6_state; + _tecs_state.iptch = _integ7_state; + _tecs_state.thr = _throttle_dem; + _tecs_state.ptch = _pitch_dem; + _tecs_state.dspd_dem = _TAS_rate_dem; + } diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 752b0ddd9c..bcc2d90e5c 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,6 +28,7 @@ class __EXPORT TECS { public: TECS() : + _tecs_state {}, _update_50hz_last_usec(0), _update_speed_last_usec(0), _update_pitch_throttle_last_usec(0), @@ -120,24 +121,33 @@ public: return _spdWeight; } - // log data on internal state of the controller. Called at 10Hz - // void log_data(DataFlash_Class &dataflash, uint8_t msgid); + enum ECL_TECS_MODE { + ECL_TECS_MODE_NORMAL = 0, + ECL_TECS_MODE_UNDERSPEED, + ECL_TECS_MODE_BAD_DESCENT, + ECL_TECS_MODE_CLIMBOUT + }; - // struct PACKED log_TECS_Tuning { - // LOG_PACKET_HEADER; - // float hgt; - // float dhgt; - // float hgt_dem; - // float dhgt_dem; - // float spd_dem; - // float spd; - // float dspd; - // float ithr; - // float iptch; - // float thr; - // float ptch; - // float dspd_dem; - // } log_tuning; + struct tecs_state { + uint64_t timestamp; + float hgt; + float dhgt; + float hgt_dem; + float dhgt_dem; + float spd_dem; + float spd; + float dspd; + float ithr; + float iptch; + float thr; + float ptch; + float dspd_dem; + enum ECL_TECS_MODE mode; + }; + + void get_tecs_state(struct tecs_state& state) { + state = _tecs_state; + } void set_time_const(float time_const) { _timeConst = time_const; @@ -212,6 +222,9 @@ public: } private: + + struct tecs_state _tecs_state; + // Last time update_50Hz was called uint64_t _update_50hz_last_usec; From ff760d07b4d115ce0bc58ca8e8f98a915c4bbb67 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Aug 2014 13:19:10 +0200 Subject: [PATCH 481/600] Add TECS logging --- .../fw_pos_control_l1_main.cpp | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 189a19d48f..350ce6dec1 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -146,6 +146,7 @@ private: int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ @@ -414,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* publications */ _attitude_sp_pub(-1), + _tecs_status_pub(-1), _nav_capabilities_pub(-1), /* states */ @@ -1384,6 +1386,51 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); + + struct TECS::tecs_state s; + _tecs.get_tecs_state(s); + + struct tecs_status_s t; + + t.timestamp = s.timestamp; + + switch (s.mode) { + case TECS::ECL_TECS_MODE_NORMAL: + t.mode = TECS_MODE_NORMAL; + break; + case TECS::ECL_TECS_MODE_UNDERSPEED: + t.mode = TECS_MODE_UNDERSPEED; + break; + case TECS::ECL_TECS_MODE_BAD_DESCENT: + t.mode = TECS_MODE_BAD_DESCENT; + break; + case TECS::ECL_TECS_MODE_CLIMBOUT: + t.mode = TECS_MODE_CLIMBOUT; + break; + } + + t.altitudeSp = s.hgt_dem; + t.altitude_filtered = s.hgt; + t.airspeedSp = s.spd_dem; + t.airspeed_filtered = s.spd; + + t.flightPathAngleSp = s.dhgt_dem; + t.flightPathAngle = s.dhgt; + t.flightPathAngleFiltered = s.dhgt; + + t.airspeedDerivativeSp = s.dspd_dem; + t.airspeedDerivative = s.dspd; + + t.totalEnergyRateSp = s.thr; + t.totalEnergyRate = s.ithr; + t.energyDistributionRateSp = s.ptch; + t.energyDistributionRate = s.iptch; + + if (_tecs_status_pub > 0) { + orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); + } else { + _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); + } } } From 6fbeaeaacecfa532c1c4437ba51312e1c46d980d Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 4 Aug 2014 09:40:38 -0700 Subject: [PATCH 482/600] Update arming_state_transition calls Add new fRunPreArmChecks flag --- src/modules/commander/commander.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2b528eef66..4f976546e1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -393,7 +393,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -1085,7 +1085,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } @@ -1288,14 +1288,14 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1309,7 +1309,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1368,7 +1368,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -1394,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -2156,7 +2156,7 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2219,7 +2219,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); break; } From 014fd5f47baad2f49292216eba851100aa543fef Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 4 Aug 2014 09:41:29 -0700 Subject: [PATCH 483/600] Add fRunPreArmChecks flag This is to allow unit tests to be written which do not perform pre-arm checks --- src/modules/commander/state_machine_helper.cpp | 13 +++++++------ src/modules/commander/state_machine_helper.h | 2 +- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3c3d2f2335..f8589d24b8 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = { }; transition_result_t -arming_state_transition(struct vehicle_status_s *status, /// current vehicle status - const struct safety_s *safety, /// current safety settings - arming_state_t new_arming_state, /// arming state requested - struct actuator_armed_s *armed, /// current armed status - const int mavlink_fd) /// mavlink fd for error reporting, 0 for none +arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status + const struct safety_s *safety, ///< current safety settings + arming_state_t new_arming_state, ///< arming state requested + struct actuator_armed_s *armed, ///< current armed status + bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing + const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid ASSERT(ARMING_STATE_INIT == 0); @@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current int prearm_ret = OK; /* only perform the check if we have to */ - if (new_arming_state == ARMING_STATE_ARMED) { + if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) { prearm_ret = prearm_check(status, mavlink_fd); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bb1b87e712..69ce8bbcee 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,7 @@ typedef enum { bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); + arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd); transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); From cfe5c45e04f65f24110f222c3de02147146081e1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 4 Aug 2014 09:42:16 -0700 Subject: [PATCH 484/600] Fix broken arming unit test Also, update arming_state_transition calls to add new fRunPreArmChecks flag. --- .../commander_tests/state_machine_helper_test.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 2e18c42841..08dda2fabf 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) armed.ready_to_arm = test->current_state.ready_to_arm; // Attempt transition - transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */); + transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */); // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); @@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) MTT_ALL_NOT_VALID, MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, - { "transition: MANUAL to AUTO_MISSION - global position valid", - MTT_GLOBAL_POS_VALID, + { "transition: MANUAL to AUTO_MISSION - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, - { "transition: AUTO_MISSION to MANUAL - global position valid", - MTT_GLOBAL_POS_VALID, + { "transition: AUTO_MISSION to MANUAL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_LOITER - global position valid", From 33762ce861b06cfa521d75fc18df3c7237e3a423 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 6 Aug 2014 12:40:07 +0400 Subject: [PATCH 485/600] UAVCAN ESC mixer: removed the failsafe placeholder, it's no use here --- src/modules/uavcan/uavcan_main.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5b1539e3ed..4535b6d6ad 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -313,11 +313,6 @@ int UavcanNode::run() } } - if (!controls_updated) { - // timeout: no control data, switch to failsafe values - // XXX trigger failsafe - } - // can we mix? if (controls_updated && (_mixers != nullptr)) { From 13b9cd0cec2d10d6252d0e8015d000f4fcc1517f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Aug 2014 07:25:20 +0200 Subject: [PATCH 486/600] Do not fuse height after innovation consistency check fail --- .../ekf_att_pos_estimator/estimator_23states.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index ffdd29a5b7..c619735c61 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1070,10 +1070,18 @@ void AttPosEKF::FuseVelposNED() // apply a 10-sigma threshold current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; - if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout || staticMode) { current_ekf_state.hgtHealth = true; current_ekf_state.hgtFailTime = millis(); + + // if we just reset from a timeout, do not fuse + // the height data, but reset height and stored states + if (current_ekf_state.hgtTimeout) { + ResetHeight(); + ResetStoredStates(); + fuseHgtData = false; + } } else { From 20c3a329c72ca5950f7bb037473a812b4dd74b1b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Aug 2014 07:36:59 +0200 Subject: [PATCH 487/600] Introduce similar checks fo all other health checks in the filter --- .../estimator_23states.cpp | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index c619735c61..c7c7305b2a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1032,10 +1032,16 @@ void AttPosEKF::FuseVelposNED() // apply a 5-sigma threshold current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; - if (current_ekf_state.velHealth || current_ekf_state.velTimeout) - { + if (current_ekf_state.velHealth || staticMode) { current_ekf_state.velHealth = true; current_ekf_state.velFailTime = millis(); + } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) { + // XXX check + current_ekf_state.velHealth = true; + ResetVelocity(); + ResetStoredStates(); + // do not fuse bad data + fuseVelData = false; } else { @@ -1056,6 +1062,17 @@ void AttPosEKF::FuseVelposNED() { current_ekf_state.posHealth = true; current_ekf_state.posFailTime = millis(); + + if (current_ekf_state.posTimeout) { + ResetPosition(); + + // XXX cross-check the state reset + ResetStoredStates(); + + // do not fuse position data on this time + // step + fusePosData = false; + } } else { From faaeaeb11333598abd17db2189a2bc47216d0a98 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Aug 2014 00:22:57 +0200 Subject: [PATCH 488/600] mc_pos_control: manual and offboard control reorganization and cleanup --- .../mc_pos_control/mc_pos_control_main.cpp | 271 +++++++++--------- .../uORB/topics/position_setpoint_triplet.h | 8 +- 2 files changed, 140 insertions(+), 139 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 d22f43b5af..a0837a2ddc 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -105,6 +105,7 @@ public: int start(); private: + const float alt_ctl_dz = 0.2f; bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ @@ -184,6 +185,8 @@ private: math::Vector<3> _vel; math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ + math::Vector<3> _vel_ff; + math::Vector<3> _sp_move_rate; /** * Update our local parameter cache. @@ -216,6 +219,16 @@ private: */ void reset_alt_sp(); + /** + * Set position setpoint using manual control + */ + void control_manual(float dt); + + /** + * Set position setpoint using offboard control + */ + void control_offboard(float dt); + /** * Select between barometric and global (AMSL) altitudes */ @@ -297,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel.zero(); _vel_sp.zero(); _vel_prev.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); @@ -515,6 +530,120 @@ MulticopterPositionControl::reset_alt_sp() } } +void +MulticopterPositionControl::control_manual(float dt) +{ + _sp_move_rate.zero(); + + if (_control_mode.flag_control_altitude_enabled) { + /* move altitude setpoint with throttle stick */ + _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode.flag_control_position_enabled) { + /* move position setpoint with roll/pitch stick */ + _sp_move_rate(0) = _manual.x; + _sp_move_rate(1) = _manual.y; + } + + /* limit setpoint move rate */ + float sp_move_norm = _sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + _sp_move_rate /= sp_move_norm; + } + + /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); + + if (_control_mode.flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + } + + if (_control_mode.flag_control_position_enabled) { + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _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) { + 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) = (_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; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_offboard(float dt) +{ + bool updated; + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + } + + if (_pos_sp_triplet.current.valid) { + if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) { + /* control position */ + _pos_sp(0) = _pos_sp_triplet.current.x; + _pos_sp(1) = _pos_sp_triplet.current.y; + _pos_sp(2) = _pos_sp_triplet.current.z; + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + + } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { + /* control velocity */ + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + + /* set position setpoint move rate */ + _sp_move_rate(0) = _pos_sp_triplet.current.vx; + _sp_move_rate(1) = _pos_sp_triplet.current.vy; + _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; + } + + if (_control_mode.flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + + /* set altitude setpoint move rate */ + _sp_move_rate(2) = _pos_sp_triplet.current.vz; + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + } else { + reset_pos_sp(); + reset_alt_sp(); + } +} + void MulticopterPositionControl::task_main() { @@ -553,13 +682,6 @@ MulticopterPositionControl::task_main() hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - - math::Vector<3> sp_move_rate; - sp_move_rate.zero(); - - float yaw_sp_move_rate; - math::Vector<3> thrust_int; thrust_int.zero(); math::Matrix<3, 3> R; @@ -618,138 +740,17 @@ MulticopterPositionControl::task_main() _vel(1) = _local_pos.vy; _vel(2) = _local_pos.vz; - sp_move_rate.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ - if (_control_mode.flag_control_altitude_enabled) { - /* reset alt setpoint to current altitude if needed */ - reset_alt_sp(); - - /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); - } - - if (_control_mode.flag_control_position_enabled) { - /* reset position setpoint to current position if needed */ - reset_pos_sp(); - - /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = _manual.x; - sp_move_rate(1) = _manual.y; - } - - /* limit setpoint move rate */ - float sp_move_norm = sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - sp_move_rate /= sp_move_norm; - } - - /* scale to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); - - /* move position setpoint */ - _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) { - 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) = (_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; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); - } + control_manual(dt); } else if (_control_mode.flag_control_offboard_enabled) { - /* Offboard control */ - bool updated; - orb_check(_pos_sp_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - } - - if (_pos_sp_triplet.current.valid) { - - if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) { - - _pos_sp(0) = _pos_sp_triplet.current.x; - _pos_sp(1) = _pos_sp_triplet.current.y; - _pos_sp(2) = _pos_sp_triplet.current.z; - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; - - } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { - /* reset position setpoint to current position if needed */ - reset_pos_sp(); - /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = _pos_sp_triplet.current.vx; - sp_move_rate(1) = _pos_sp_triplet.current.vy; - yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed; - _att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt; - } - - if (_control_mode.flag_control_altitude_enabled) { - /* reset alt setpoint to current altitude if needed */ - reset_alt_sp(); - - /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);; - } - - /* limit setpoint move rate */ - float sp_move_norm = sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - sp_move_rate /= sp_move_norm; - } - - /* scale to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); - - /* move position setpoint */ - _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) { - 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) = (_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; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); - } - - } else { - reset_pos_sp(); - reset_alt_sp(); - } + /* offboard control */ + control_offboard(dt); } else { /* AUTO */ @@ -823,7 +824,7 @@ MulticopterPositionControl::task_main() /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; - _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; if (!_control_mode.flag_control_altitude_enabled) { _reset_alt_sp = true; @@ -903,7 +904,7 @@ MulticopterPositionControl::task_main() math::Vector<3> vel_err = _vel_sp - _vel; /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; _vel_prev = _vel; /* thrust vector in NED frame */ diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 4a19321809..ec2131abd8 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -60,7 +60,7 @@ enum SETPOINT_TYPE SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ - SETPOINT_TYPE_OFFBOARD, /**< setpoint set by offboard */ + SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */ }; struct position_setpoint_s @@ -71,9 +71,9 @@ struct position_setpoint_s float y; /**< local position setpoint in m in NED */ float z; /**< local position setpoint in m in NED */ bool position_valid; /**< true if local position setpoint valid */ - float vx; /**< local velocity setpoint in m in NED */ - float vy; /**< local velocity setpoint in m in NED */ - float vz; /**< local velocity setpoint in m in NED */ + float vx; /**< local velocity setpoint in m/s in NED */ + float vy; /**< local velocity setpoint in m/s in NED */ + float vz; /**< local velocity setpoint in m/s in NED */ bool velocity_valid; /**< true if local velocity setpoint valid */ double lat; /**< latitude, in deg */ double lon; /**< longitude, in deg */ From 03f839a27ac61178be8ef77526faa13a46c4cd6b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Aug 2014 00:24:18 +0200 Subject: [PATCH 489/600] mc_pos_control: more accurate position setpoint reset, keep attitude setpoint continuous --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 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 a0837a2ddc..0106af80aa 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -514,8 +514,11 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0) * (1.0f - _params.vel_ff(0)); - _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1) * (1.0f - _params.vel_ff(1)); + /* shift position setpoint to make attitude setpoint continuous */ + _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) + - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -525,7 +528,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2) * (1.0f - _params.vel_ff(2)); + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } From 5246d6b49211b7ba73a32c7b73967cabe5fd512a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Aug 2014 19:31:34 +0200 Subject: [PATCH 490/600] Updated mavlink version --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 04b1ad5b28..96425caae3 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 04b1ad5b284d5e916858ca9f928e93d97bbf6ad9 +Subproject commit 96425caae3de0e8cda81405c1a0e9d2fb14369a2 From 312f9e0ccce9e3f530f6fc222e371a48c3b1af0d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 9 Aug 2014 19:43:57 +0200 Subject: [PATCH 491/600] mixers: adapted FX79 mixer to have more pitch influence --- ROMFS/px4fmu_common/mixers/FMU_FX79.mix | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix index 0a1dca98d5..112d9b891c 100755 --- a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix @@ -27,12 +27,12 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 5000 8000 0 -10000 10000 +S: 0 0 7500 7500 0 -10000 10000 S: 0 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 8000 5000 0 -10000 10000 +S: 0 0 7500 7500 0 -10000 10000 S: 0 1 -8000 -8000 0 -10000 10000 Output 2 From 3fdf63d2b3cf25904371cf700e08ce4e8d214824 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 30 Mar 2014 15:00:37 +0200 Subject: [PATCH 492/600] Startup script: added viper script Conflicts: mavlink/include/mavlink/v1.0 --- ROMFS/px4fmu_common/init.d/3035_viper | 10 ++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 +++++ 2 files changed, 15 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/3035_viper diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper new file mode 100644 index 0000000000..9c8f8a585e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -0,0 +1,10 @@ +#!nsh +# +# Viper +# +# Simon Wilks +# + +sh /etc/init.d/rc.fw_defaults + +set MIXER FMU_FX79 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 17541e6800..9de7d9ecdb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -103,6 +103,11 @@ then sh /etc/init.d/3034_fx79 fi +if param compare SYS_AUTOSTART 3035 35 +then + sh /etc/init.d/3035_viper +fi + if param compare SYS_AUTOSTART 3100 then sh /etc/init.d/3100_tbs_caipirinha From e6210058a3527bc87a74781159631a1f59ad053d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 9 Aug 2014 19:52:30 +0200 Subject: [PATCH 493/600] mixers: added Viper mixer --- ROMFS/px4fmu_common/init.d/3035_viper | 2 +- ROMFS/px4fmu_common/mixers/Viper.mix | 72 +++++++++++++++++++++++++++ 2 files changed, 73 insertions(+), 1 deletion(-) create mode 100755 ROMFS/px4fmu_common/mixers/Viper.mix diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index 9c8f8a585e..a4c1e832dc 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -7,4 +7,4 @@ sh /etc/init.d/rc.fw_defaults -set MIXER FMU_FX79 \ No newline at end of file +set MIXER Viper diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix new file mode 100755 index 0000000000..79c4447bee --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -0,0 +1,72 @@ +Viper Delta-wing mixer +================================= + +Designed for Viper. + +TODO (sjwilks): Add mixers for flaps. + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 7500 7500 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 7500 7500 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 From 4b065a7e1d2a523ba40abbf63452bf89d1004cd8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 00:52:03 +0200 Subject: [PATCH 494/600] Updated MAVLink version --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 96425caae3..8cc21472cc 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 96425caae3de0e8cda81405c1a0e9d2fb14369a2 +Subproject commit 8cc21472cce09709d4c651e2855d891843a17e41 From 5225d87854bdea1b5e4367e3db6b41ece9e46e13 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 01:24:04 +0200 Subject: [PATCH 495/600] Updated MAVLink to latest revision --- src/modules/mavlink/mavlink_messages.cpp | 155 ++++++++--------------- src/modules/mavlink/mavlink_receiver.cpp | 34 ----- 2 files changed, 51 insertions(+), 138 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 683f0f1e84..7f67475f2e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1385,43 +1385,43 @@ protected: }; -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); + return MavlinkStreamPositionTargetGlobalInt::get_name_static(); } static const char *get_name_static() { - return "GLOBAL_POSITION_SETPOINT_INT"; + return "POSITION_TARGET_GLOBAL_INT"; } uint8_t get_id() { - return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGlobalPositionSetpointInt(mavlink); + return new MavlinkStreamPositionTargetGlobalInt(mavlink); } unsigned get_size() { - return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: MavlinkOrbSubscription *_pos_sp_triplet_sub; /* do not allow top copying this class */ - MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); - MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); + MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &); + MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &); protected: - explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(mavlink), + explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink), _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} @@ -1430,15 +1430,14 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_global_position_setpoint_int_t msg; + mavlink_position_target_global_int_t msg; msg.coordinate_frame = MAV_FRAME_GLOBAL; - msg.latitude = pos_sp_triplet.current.lat * 1e7; - msg.longitude = pos_sp_triplet.current.lon * 1e7; - msg.altitude = pos_sp_triplet.current.alt * 1000; - msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f; + msg.lat_int = pos_sp_triplet.current.lat * 1e7; + msg.lon_int = pos_sp_triplet.current.lon * 1e7; + msg.alt = pos_sp_triplet.current.alt; - _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg); } } }; @@ -1454,12 +1453,12 @@ public: static const char *get_name_static() { - return "LOCAL_POSITION_SETPOINT"; + return "POSITION_TARGET_LOCAL_NED"; } uint8_t get_id() { - return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; } static MavlinkStream *new_instance(Mavlink *mavlink) @@ -1469,7 +1468,7 @@ public: unsigned get_size() { - return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: @@ -1491,137 +1490,86 @@ protected: struct vehicle_local_position_setpoint_s pos_sp; if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { - mavlink_local_position_setpoint_t msg; + mavlink_position_target_local_ned_t msg; msg.coordinate_frame = MAV_FRAME_LOCAL_NED; msg.x = pos_sp.x; msg.y = pos_sp.y; msg.z = pos_sp.z; - msg.yaw = pos_sp.yaw; - _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); } } }; -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +class MavlinkStreamAttitudeTarget : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); + return MavlinkStreamAttitudeTarget::get_name_static(); } static const char *get_name_static() { - return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + return "ATTITUDE_TARGET"; } uint8_t get_id() { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + return MAVLINK_MSG_ID_ATTITUDE_TARGET; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink); + return new MavlinkStreamAttitudeTarget(mavlink); } unsigned get_size() { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sp_sub; - uint64_t att_sp_time; + MavlinkOrbSubscription *_att_sp_sub; + MavlinkOrbSubscription *_att_rates_sp_sub; + uint64_t _att_sp_time; + uint64_t _att_rates_sp_time; /* do not allow top copying this class */ - MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); - MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); + MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &); + MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &); protected: - explicit MavlinkStreamRollPitchYawThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), - att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), - att_sp_time(0) + explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), + _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), + _att_sp_time(0), + _att_rates_sp_time(0) {} void send(const hrt_abstime t) { struct vehicle_attitude_setpoint_s att_sp; - if (att_sp_sub->update(&att_sp_time, &att_sp)) { - mavlink_roll_pitch_yaw_thrust_setpoint_t msg; + if (_att_sp_sub->update(&_att_sp_time, &att_sp)) { + + struct vehicle_rates_setpoint_s att_rates_sp; + (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); + + mavlink_attitude_target_t msg; msg.time_boot_ms = att_sp.timestamp / 1000; - msg.roll = att_sp.roll_body; - msg.pitch = att_sp.pitch_body; - msg.yaw = att_sp.yaw_body; + mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); + + msg.body_roll_rate = att_rates_sp.roll; + msg.body_pitch_rate = att_rates_sp.pitch; + msg.body_yaw_rate = att_rates_sp.yaw; + msg.thrust = att_sp.thrust; - _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg); - } - } -}; - - -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink); - } - - unsigned get_size() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - -private: - MavlinkOrbSubscription *_att_rates_sp_sub; - uint64_t _att_rates_sp_time; - - /* do not allow top copying this class */ - MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); - MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); - -protected: - explicit MavlinkStreamRollPitchYawRatesThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), - _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), - _att_rates_sp_time(0) - {} - - void send(const hrt_abstime t) - { - struct vehicle_rates_setpoint_s att_rates_sp; - - if (_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp)) { - mavlink_roll_pitch_yaw_rates_thrust_setpoint_t msg; - - msg.time_boot_ms = att_rates_sp.timestamp / 1000; - msg.roll_rate = att_rates_sp.roll; - msg.pitch_rate = att_rates_sp.pitch; - msg.yaw_rate = att_rates_sp.yaw; - msg.thrust = att_rates_sp.thrust; - - _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg); } } }; @@ -2149,10 +2097,9 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), - new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), + new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 69e3ef31d7..63bac45c09 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -148,10 +148,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vicon_position_estimate(msg); break; - case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: - handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); - break; - case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -362,36 +358,6 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) } } -void -MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) -{ - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control); - - /* Only accept system IDs from 1 to 4 */ - if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) { - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - - /* Convert values * 1000 back */ - offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; - - offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; - - offboard_control_sp.timestamp = hrt_absolute_time(); - - if (_offboard_control_sp_pub < 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); - - } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); - } - } -} - void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { From 9ecec7fada7da3778c6214defcef68ea05352027 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 01:30:25 +0200 Subject: [PATCH 496/600] Add default initializers and timestamp in local position --- src/modules/mavlink/mavlink_messages.cpp | 7 ++++--- src/modules/mc_pos_control/mc_pos_control_main.cpp | 1 + src/modules/uORB/topics/vehicle_local_position_setpoint.h | 3 ++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7f67475f2e..c10be77b52 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1430,7 +1430,7 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_position_target_global_int_t msg; + mavlink_position_target_global_int_t msg{}; msg.coordinate_frame = MAV_FRAME_GLOBAL; msg.lat_int = pos_sp_triplet.current.lat * 1e7; @@ -1490,8 +1490,9 @@ protected: struct vehicle_local_position_setpoint_s pos_sp; if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { - mavlink_position_target_local_ned_t msg; + mavlink_position_target_local_ned_t msg{}; + msg.time_boot_ms = pos_sp.timestamp / 1000; msg.coordinate_frame = MAV_FRAME_LOCAL_NED; msg.x = pos_sp.x; msg.y = pos_sp.y; @@ -1558,7 +1559,7 @@ protected: struct vehicle_rates_setpoint_s att_rates_sp; (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); - mavlink_attitude_target_t msg; + mavlink_attitude_target_t msg{}; msg.time_boot_ms = att_sp.timestamp / 1000; mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); 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 0106af80aa..ecc40428de 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -788,6 +788,7 @@ MulticopterPositionControl::task_main() } /* fill local position setpoint */ + _local_pos_sp.timestamp = hrt_absolute_time(); _local_pos_sp.x = _pos_sp(0); _local_pos_sp.y = _pos_sp(1); _local_pos_sp.z = _pos_sp(2); diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h index 8988a0330d..6766bb58a9 100644 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h @@ -50,11 +50,12 @@ */ struct vehicle_local_position_setpoint_s { + uint64_t timestamp; /**< timestamp of the setpoint */ float x; /**< in meters NED */ float y; /**< in meters NED */ float z; /**< in meters NED */ float yaw; /**< in radians NED -PI..+PI */ -}; /**< Local position in NED frame to go to */ +}; /**< Local position in NED frame */ /** * @} From 1d3edfa62976bdf73eeef67309c67b154be07810 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 10 Aug 2014 12:02:10 -0700 Subject: [PATCH 497/600] More FTP support Added: - sequence numbers - directory list returns file sizes - open command returns file size in data --- src/modules/mavlink/mavlink_ftp.cpp | 48 ++++++++++++++++++++++++----- src/modules/mavlink/mavlink_ftp.h | 2 +- 2 files changed, 42 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 6a2c900afe..17d1babffe 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -190,6 +191,8 @@ void MavlinkFTP::_reply(Request *req) { auto hdr = req->header(); + + hdr->magic = kProtocolMagic; // message is assumed to be already constructed in the request buffer, so generate the CRC hdr->crc32 = 0; @@ -242,15 +245,18 @@ MavlinkFTP::_workList(Request *req) break; } - // name too big to fit? - if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) { - break; - } + uint32_t fileSize = 0; + char buf[256]; // store the type marker switch (entry.d_type) { case DTYPE_FILE: hdr->data[offset++] = kDirentFile; + snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name); + struct stat st; + if (stat(buf, &st) == 0) { + fileSize = st.st_size; + } break; case DTYPE_DIRECTORY: hdr->data[offset++] = kDirentDir; @@ -259,11 +265,24 @@ MavlinkFTP::_workList(Request *req) hdr->data[offset++] = kDirentUnknown; break; } + + if (entry.d_type == DTYPE_FILE) { + snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); + } else { + strncpy(buf, entry.d_name, sizeof(buf)); + buf[sizeof(buf)-1] = 0; + } + size_t nameLen = strlen(buf); + // name too big to fit? + if ((nameLen + offset + 2) > kMaxDataLength) { + break; + } + // copy the name, which we know will fit - strcpy((char *)&hdr->data[offset], entry.d_name); + strcpy((char *)&hdr->data[offset], buf); //printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); - offset += strlen(entry.d_name) + 1; + offset += nameLen + 1; } closedir(dp); @@ -282,6 +301,16 @@ MavlinkFTP::_workOpen(Request *req, bool create) return kErrNoSession; } + + uint32_t fileSize = 0; + if (!create) { + struct stat st; + if (stat(req->dataAsCString(), &st) != 0) { + return kErrNotFile; + } + fileSize = st.st_size; + } + int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; int fd = ::open(req->dataAsCString(), oflag); @@ -291,7 +320,12 @@ MavlinkFTP::_workOpen(Request *req, bool create) _session_fds[session_index] = fd; hdr->session = session_index; - hdr->size = 0; + if (create) { + hdr->size = 0; + } else { + hdr->size = sizeof(uint32_t); + *((uint32_t*)hdr->data) = fileSize; + } return kErrNone; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1bd1158fba..800c98b69f 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -146,7 +146,7 @@ private: mavlink_message_t msg; msg.checksum = 0; unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), - _mavlink->get_channel(), &msg, sequence(), rawData()); + _mavlink->get_channel(), &msg, sequence()+1, rawData()); _mavlink->lockMessageBufferMutex(); bool success = _mavlink->message_buffer_write(&msg, len); From 0633d7f601b38a60c2b0ed1ca98d57bd3f80e482 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 22:05:21 +0200 Subject: [PATCH 498/600] Updated mavlink version --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 8cc21472cc..4cd384001b 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 8cc21472cce09709d4c651e2855d891843a17e41 +Subproject commit 4cd384001b5373e1ecaa6c4cd66994855cec4742 From 89986cf3e5cc8a9e18d56fe971d3e089232e3beb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 11 Aug 2014 13:46:13 +0200 Subject: [PATCH 499/600] epv/eph threshold variable names changed --- 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 02571f56fb..6c2c030702 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1341,8 +1341,8 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() - && (gps_position.eph < eph_epv_threshold) - && (gps_position.epv < eph_epv_threshold) + && (gps_position.eph < eph_threshold) + && (gps_position.epv < epv_threshold) && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); From 258fb27e146ad175ca89a84ebc9c340a53db0884 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:54:18 +0200 Subject: [PATCH 500/600] Ensure that a failing dataman start does not abort boot --- ROMFS/px4fmu_common/init.d/rcS | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 24b2a299af..72cd4b0e1e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -280,9 +280,11 @@ then nshterm /dev/ttyACM0 & # - # Start the datamanager + # Start the datamanager (and do not abort boot if it fails) # - dataman start + if dataman start + then + fi # # Start the Commander (needs to be this early for in-air-restarts) From 2ca89032409d0519e242006fa34ab26548fdd7e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:56:13 +0200 Subject: [PATCH 501/600] If we failed loading params, reset them (if the file handle is valid the file is corrupted and there is no hope, if its just not present the reset command will not do anything) --- ROMFS/px4fmu_common/init.d/rcS | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 72cd4b0e1e..c9e6a27cac 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -83,9 +83,12 @@ then param select $PARAM_FILE if param load then - echo "[init] Params loaded: $PARAM_FILE" + echo "[param] Loaded: $PARAM_FILE" else - echo "[init] ERROR: Params loading failed: $PARAM_FILE" + echo "[param] FAILED loading $PARAM_FILE" + if param reset + then + fi fi # From 5baf9cea0d8fcf6e0c6b3c92b26167067e9cb0fa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 11 Aug 2014 15:29:59 +0200 Subject: [PATCH 502/600] geo: fix some warnings --- src/lib/geo/geo.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index ad1ff37e30..1c8d2a2a75 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -50,6 +50,7 @@ #include #include #include +#include #include #include @@ -147,7 +148,7 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref double cos_d_lon = cos(lon_rad - ref->lon_rad); 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)); + double k = (fabs(c) < DBL_EPSILON) ? 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_rad) * CONSTANTS_RADIUS_OF_EARTH; @@ -175,7 +176,7 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r double lat_rad; double lon_rad; - if (c != 0.0) { + if (fabs(c) > DBL_EPSILON) { lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); From 7d1cd7b994240d441a37b36f74598980f3f460db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 19:28:14 +0200 Subject: [PATCH 503/600] Fix NO RC handling (fixes #1263) --- src/drivers/px4io/px4io.cpp | 36 +++++++++++++++++++++++++++++------- 1 file changed, 29 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d93009c47d..bc0e6a221c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -135,6 +135,15 @@ public: */ virtual int init(); + /** + * Initialize the PX4IO class. + * + * Retrieve relevant initial system parameters. Initialize PX4IO registers. + * + * @param disable_rc_handling set to true to forbid override / RC handling on IO + */ + virtual int init(bool disable_rc_handling); + /** * Detect if a PX4IO is connected. * @@ -579,6 +588,12 @@ PX4IO::detect() return 0; } +int +PX4IO::init(bool rc_handling_disabled) { + _rc_handling_disabled = rc_handling_disabled; + init(); +} + int PX4IO::init() { @@ -778,6 +793,11 @@ PX4IO::init() if (_rc_handling_disabled) { ret = io_disable_rc_handling(); + if (ret != OK) { + log("failed disabling RC handling"); + return ret; + } + } else { /* publish RC config to IO */ ret = io_set_rc_config(); @@ -1175,6 +1195,7 @@ PX4IO::io_set_arming_state() int PX4IO::disable_rc_handling() { + _rc_handling_disabled = true; return io_disable_rc_handling(); } @@ -2613,24 +2634,25 @@ start(int argc, char *argv[]) errx(1, "driver alloc failed"); } - if (OK != g_dev->init()) { - delete g_dev; - g_dev = nullptr; - errx(1, "driver init failed"); - } + bool rc_handling_disabled = false; /* disable RC handling on request */ if (argc > 1) { if (!strcmp(argv[1], "norc")) { - if (g_dev->disable_rc_handling()) - warnx("Failed disabling RC handling"); + rc_handling_disabled = true; } else { warnx("unknown argument: %s", argv[1]); } } + if (OK != g_dev->init(rc_handling_disabled)) { + delete g_dev; + g_dev = nullptr; + errx(1, "driver init failed"); + } + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 int dsm_vcc_ctl; From 0c0c8e943a2ec70a91386b65c17c563b6e40a782 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 07:30:01 +0200 Subject: [PATCH 504/600] Fixed missing return value after last change set --- src/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bc0e6a221c..32069cf09b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -142,7 +142,7 @@ public: * * @param disable_rc_handling set to true to forbid override / RC handling on IO */ - virtual int init(bool disable_rc_handling); + int init(bool disable_rc_handling); /** * Detect if a PX4IO is connected. @@ -591,7 +591,7 @@ PX4IO::detect() int PX4IO::init(bool rc_handling_disabled) { _rc_handling_disabled = rc_handling_disabled; - init(); + return init(); } int From 3f4aef60c88b1e570dd30bc47a13d5340073e9a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 08:21:38 +0200 Subject: [PATCH 505/600] Increase timeout in an attempt to prevent timout python failure --- Tools/px_uploader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index cd7884f6d2..d8f4884bc0 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -180,7 +180,7 @@ class uploader(object): def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate, timeout=0.5) + self.port = serial.Serial(portname, baudrate, timeout=2.0) self.otp = b'' self.sn = b'' From c8ecf59ab70780efed6ac48967caa8e2358a48ba Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 12 Aug 2014 14:23:26 -0700 Subject: [PATCH 506/600] List command bug fix, easier debugging - List command was generating bad data size because it was adding directory entry char to buffer before testing buffer overflow. - Added MAVLINK_FTP_DEBUG define to easily turn on/off noisier debugging output --- src/modules/mavlink/mavlink_ftp.cpp | 42 ++++++++++++++++++++--------- src/modules/mavlink/mavlink_ftp.h | 20 +++++++------- 2 files changed, 41 insertions(+), 21 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 17d1babffe..00c8df18c5 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -39,6 +39,9 @@ #include "mavlink_ftp.h" +// Uncomment the line below to get better debug output. Never commit with this left on. +//#define MAVLINK_FTP_DEBUG + MavlinkFTP *MavlinkFTP::_server; MavlinkFTP * @@ -125,7 +128,9 @@ MavlinkFTP::_worker(Request *req) warnx("ftp: bad crc"); } - //printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); +#ifdef MAVLINK_FTP_DEBUG + printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); +#endif switch (hdr->opcode) { case kCmdNone: @@ -172,7 +177,9 @@ out: // handle success vs. error if (errorCode == kErrNone) { hdr->opcode = kRspAck; - //warnx("FTP: ack\n"); +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: ack\n"); +#endif } else { warnx("FTP: nak %u", errorCode); hdr->opcode = kRspNak; @@ -217,7 +224,9 @@ MavlinkFTP::_workList(Request *req) return kErrNotDir; } - //warnx("FTP: list %s offset %d", dirPath, hdr->offset); +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: list %s offset %d", dirPath, hdr->offset); +#endif ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -247,11 +256,13 @@ MavlinkFTP::_workList(Request *req) uint32_t fileSize = 0; char buf[256]; + char direntType; - // store the type marker + // Determine the directory entry type switch (entry.d_type) { case DTYPE_FILE: - hdr->data[offset++] = kDirentFile; + // For files we get the file size as well + direntType = kDirentFile; snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name); struct stat st; if (stat(buf, &st) == 0) { @@ -259,29 +270,34 @@ MavlinkFTP::_workList(Request *req) } break; case DTYPE_DIRECTORY: - hdr->data[offset++] = kDirentDir; + direntType = kDirentDir; break; default: - hdr->data[offset++] = kDirentUnknown; + direntType = kDirentUnknown; break; } if (entry.d_type == DTYPE_FILE) { + // Files send filename and file length snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); } else { + // Everything else just sends name strncpy(buf, entry.d_name, sizeof(buf)); buf[sizeof(buf)-1] = 0; } size_t nameLen = strlen(buf); - // name too big to fit? - if ((nameLen + offset + 2) > kMaxDataLength) { + // Do we have room for the name, the one char directory identifier and the null terminator? + if ((offset + nameLen + 2) > kMaxDataLength) { break; } - // copy the name, which we know will fit + // Move the data into the buffer + hdr->data[offset++] = direntType; strcpy((char *)&hdr->data[offset], buf); - //printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); +#ifdef MAVLINK_FTP_DEBUG + printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); +#endif offset += nameLen + 1; } @@ -342,7 +358,9 @@ MavlinkFTP::_workRead(Request *req) } // Seek to the specified position - //warnx("seek %d", hdr->offset); +#ifdef MAVLINK_FTP_DEBUG + warnx("seek %d", hdr->offset); +#endif if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { // Unable to see to the specified location warnx("seek fail"); diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 800c98b69f..43de89de91 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -154,15 +154,17 @@ private: if (!success) { warnx("FTP TX ERR"); - } - // else { - // warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", - // _mavlink->get_system_id(), - // _mavlink->get_component_id(), - // _mavlink->get_channel(), - // len, - // msg.checksum); - // } + } +#ifdef MAVLINK_FTP_DEBUG + else { + warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", + _mavlink->get_system_id(), + _mavlink->get_component_id(), + _mavlink->get_channel(), + len, + msg.checksum); + } +#endif } uint8_t *rawData() { return &_message.data[0]; } From 2cc5c6e84f2fed508a358d2b2fc9c38085c446db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Aug 2014 09:14:43 +0200 Subject: [PATCH 507/600] INAV: Add braces to ensure statements are evaluated correctly --- .../position_estimator_inav/position_estimator_inav_main.c | 6 +++--- 1 file changed, 3 insertions(+), 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 06a81e3fee..81bbaa6587 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -788,21 +788,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* check for timeout on GPS topic */ - if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) { + if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) { gps_valid = false; warnx("GPS timeout"); mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); } /* check for timeout on vision topic */ - if (vision_valid && t > vision.timestamp_boot + vision_topic_timeout) { + if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) { vision_valid = false; warnx("VISION timeout"); mavlink_log_info(mavlink_fd, "[inav] VISION timeout"); } /* check for sonar measurement timeout */ - if (sonar_valid && t > sonar_time + sonar_timeout) { + if (sonar_valid && (t > (sonar_time + sonar_timeout))) { corr_sonar = 0.0f; sonar_valid = false; } From ae4339ce1e3046e44387f9c9e7a66827b859a653 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Aug 2014 09:42:41 +0200 Subject: [PATCH 508/600] Update MAVLink version, remove unneeded INT frames --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 4cd384001b..4d7487c2bc 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 4cd384001b5373e1ecaa6c4cd66994855cec4742 +Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50 From 54fc6aa6788a125b387926a45023844daa42ec48 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Aug 2014 10:33:46 +0200 Subject: [PATCH 509/600] Hotfix: Optimize shell commands for size - we do not need massive performance there --- src/modules/gpio_led/module.mk | 2 ++ src/modules/unit_test/module.mk | 1 + src/systemcmds/esc_calib/module.mk | 2 ++ src/systemcmds/mtd/module.mk | 2 ++ src/systemcmds/nshterm/module.mk | 2 ++ src/systemcmds/pwm/module.mk | 2 ++ 6 files changed, 11 insertions(+) diff --git a/src/modules/gpio_led/module.mk b/src/modules/gpio_led/module.mk index 3b8553489d..70c75b10cb 100644 --- a/src/modules/gpio_led/module.mk +++ b/src/modules/gpio_led/module.mk @@ -37,3 +37,5 @@ MODULE_COMMAND = gpio_led SRCS = gpio_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/modules/unit_test/module.mk b/src/modules/unit_test/module.mk index f00b0f5926..5000790a59 100644 --- a/src/modules/unit_test/module.mk +++ b/src/modules/unit_test/module.mk @@ -37,3 +37,4 @@ SRCS = unit_test.cpp +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/esc_calib/module.mk b/src/systemcmds/esc_calib/module.mk index 990c567681..ce87eb3e2c 100644 --- a/src/systemcmds/esc_calib/module.mk +++ b/src/systemcmds/esc_calib/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = esc_calib SRCS = esc_calib.c MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk index b3fceceb5c..1bc4f414ec 100644 --- a/src/systemcmds/mtd/module.mk +++ b/src/systemcmds/mtd/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = mtd SRCS = mtd.c 24xxxx_mtd.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index e2fa0ff804..7d2c59f913 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c MODULE_STACKSIZE = 1400 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/pwm/module.mk b/src/systemcmds/pwm/module.mk index 13a24150f2..a51ac8e0cf 100644 --- a/src/systemcmds/pwm/module.mk +++ b/src/systemcmds/pwm/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = pwm SRCS = pwm.c MODULE_STACKSIZE = 1800 + +MAXOPTIMIZATION = -Os From 54a6e66adafb7cca710bab3dc9e9c78c8b2390e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Aug 2014 19:46:35 +0200 Subject: [PATCH 510/600] mavlink: in HIL_CONTROLS send 0 for disabled channels --- src/modules/mavlink/mavlink_messages.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c10be77b52..f292605c9b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1333,7 +1333,7 @@ protected: } for (unsigned i = 0; i < 8; i++) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (act.output[i] > PWM_LOWEST_MIN / 2) { if (i < n) { /* scale PWM out 900..2100 us to 0..1 for rotors */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); @@ -1344,7 +1344,7 @@ protected: } } else { - /* send 0 when disarmed */ + /* send 0 when disarmed and for disabled channels */ out[i] = 0.0f; } } @@ -1353,15 +1353,20 @@ protected: /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) { - if (i != 3) { - /* scale PWM out 900..2100 us to -1..1 for normal channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + if (act.output[i] > PWM_LOWEST_MIN / 2) { + if (i != 3) { + /* scale PWM out 900..2100 us to -1..1 for normal channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + + } else { + /* scale PWM out 900..2100 us to 0..1 for throttle */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + } } else { - /* scale PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + /* set 0 for disabled channels */ + out[i] = 0.0f; } - } } From 73b4c6eba16d57550c4e745f99428b3580cbe4aa Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Aug 2014 20:09:57 +0200 Subject: [PATCH 511/600] mavlink: stream names updated in configuration --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index f1c21f295d..ee040a706d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -23,7 +23,7 @@ mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 usleep 100000 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 usleep 100000 -mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20 +mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 usleep 100000 # Exit shell to make it available to MAVLink diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9a39d3bed2..4086059398 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1388,8 +1388,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); - configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); + configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); break; From e03c4d4b65d956a90a9e95b35718ebc323ff7539 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 Aug 2014 12:34:47 +0200 Subject: [PATCH 512/600] Fixed copy & paste error on RC output --- src/modules/mavlink/mavlink_messages.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c10be77b52..0d0896353e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1637,10 +1637,10 @@ protected: msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; msg.rssi = rc.rssi; _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); From 1525341cad376c04a1e495b1ac4ac2cd870b30a2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 14 Aug 2014 13:12:02 +0200 Subject: [PATCH 513/600] Disable CONFIG_ARCH_IRQPRIO in all NuttX configs --- NuttX | 2 +- nuttx-configs/aerocore/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- nuttx-configs/px4io-v1/nsh/defconfig | 1 - nuttx-configs/px4io-v2/nsh/defconfig | 1 - 6 files changed, 4 insertions(+), 6 deletions(-) diff --git a/NuttX b/NuttX index 088146b90e..41fffa0df1 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172 +Subproject commit 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index 8d0bae7b9f..317194f685 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -314,7 +314,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500 # CONFIG_ARCH_NOINTC is not set # CONFIG_ARCH_VECNOTIRQ is not set CONFIG_ARCH_DMA=y -CONFIG_ARCH_IRQPRIO=y +# CONFIG_ARCH_IRQPRIO is not set # CONFIG_CUSTOM_STACK is not set # CONFIG_ADDRENV is not set CONFIG_ARCH_HAVE_VFORK=y diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a651faffab..d7697cd673 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -309,7 +309,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500 # CONFIG_ARCH_NOINTC is not set # CONFIG_ARCH_VECNOTIRQ is not set CONFIG_ARCH_DMA=y -CONFIG_ARCH_IRQPRIO=y +# CONFIG_ARCH_IRQPRIO is not set # CONFIG_CUSTOM_STACK is not set # CONFIG_ADDRENV is not set CONFIG_ARCH_HAVE_VFORK=y diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index a55c95a291..bd956f7bf5 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -350,7 +350,7 @@ CONFIG_SDIO_PRI=128 # CONFIG_ARCH_NOINTC is not set # CONFIG_ARCH_VECNOTIRQ is not set CONFIG_ARCH_DMA=y -CONFIG_ARCH_IRQPRIO=y +# CONFIG_ARCH_IRQPRIO is not set # CONFIG_CUSTOM_STACK is not set # CONFIG_ADDRENV is not set CONFIG_ARCH_HAVE_VFORK=y diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig index 3785e03678..512c6a0f23 100755 --- a/nuttx-configs/px4io-v1/nsh/defconfig +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -83,7 +83,6 @@ CONFIG_ARCH_BOARD="px4io-v1" CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_DRAM_SIZE=0x00002000 CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_IRQPRIO=y CONFIG_ARCH_INTERRUPTSTACK=n CONFIG_ARCH_STACKDUMP=y CONFIG_ARCH_BOOTLOADER=n diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index e6563e5878..02b51e3d73 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -79,7 +79,6 @@ CONFIG_ARCH_BOARD_PX4IO_V2=y CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_DRAM_SIZE=0x00002000 CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_IRQPRIO=y CONFIG_ARCH_INTERRUPTSTACK=n CONFIG_ARCH_STACKDUMP=y CONFIG_ARCH_BOOTLOADER=n From da0e3169f2796d3223b4f7857da067e9aac2fea4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 18:32:51 +0200 Subject: [PATCH 514/600] fw att control: change control surface deflection scaling --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 6 ++++-- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 46db788a6b..926a8db2ac 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = _bodyrate_setpoint * _k_ff * scaler + + _rate_error * _k_p * scaler * scaler + + integrator_constrained; //scaler is proportional to 1/airspeed // warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); // warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 9894a34d75..94bd26f03d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch, if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = _bodyrate_setpoint * _k_ff * scaler + + _rate_error * _k_p * scaler * scaler + + integrator_constrained; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); } From 5b1e4db396425ea6dc559734b785753c54449235 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Sat, 16 Aug 2014 21:43:07 +0200 Subject: [PATCH 515/600] px4io driver: fix actuator output format --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 32069cf09b..97919538ff 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1968,7 +1968,7 @@ PX4IO::print_status(bool extended_status) printf("actuators"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); + printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i))); printf("\n"); printf("servos"); From dd0916bd7d537f831838925cbe86e7ff3d933e9c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Aug 2014 10:31:56 +0200 Subject: [PATCH 516/600] code style fix for mavlink app --- src/modules/mavlink/mavlink_main.cpp | 74 ++++++++++++++-------------- 1 file changed, 37 insertions(+), 37 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4086059398..2faf8ab769 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -134,44 +134,44 @@ Mavlink::Mavlink() : _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _logbuffer {}, - _total_counter(0), - _receive_thread {}, - _verbose(false), - _forwarding_on(false), - _passing_on(false), - _ftp_on(false), - _uart_fd(-1), - _baudrate(57600), - _datarate(1000), - _datarate_events(500), - _rate_mult(1.0f), - _mavlink_param_queue_index(0), - mavlink_link_termination_allowed(false), - _subscribe_to_stream(nullptr), - _subscribe_to_stream_rate(0.0f), - _flow_control_enabled(true), - _last_write_success_time(0), - _last_write_try_time(0), - _bytes_tx(0), - _bytes_txerr(0), - _bytes_rx(0), - _bytes_timestamp(0), - _rate_tx(0.0f), - _rate_txerr(0.0f), - _rate_rx(0.0f), - _rstatus {}, - _message_buffer {}, - _message_buffer_mutex {}, - _send_mutex {}, - _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_system_type(0), - _param_use_hil_gps(0), + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(1000), + _datarate_events(500), + _rate_mult(1.0f), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _last_write_success_time(0), + _last_write_try_time(0), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _send_mutex {}, + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), - _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; From 1913ae19f166d61038e436eff73efa42ac4d8f13 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 10:45:30 +0200 Subject: [PATCH 517/600] add param for qnh --- src/drivers/ms5611/ms5611.cpp | 4 +-- src/modules/sensors/sensor_params.c | 10 ++++++++ src/modules/sensors/sensors.cpp | 40 +++++++++++++++++++++++------ 3 files changed, 44 insertions(+), 10 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 873fa62c44..889643d0df 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -130,7 +130,7 @@ protected: float _T; /* altitude conversion calibration */ - unsigned _msl_pressure; /* in kPa */ + unsigned _msl_pressure; /* in Pa */ orb_advert_t _baro_topic; @@ -466,7 +466,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) irqrestore(flags); return -ENOMEM; } - irqrestore(flags); + irqrestore(flags); return OK; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7ce6ef5ef6..229bfe3ce1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); */ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); +/** + * QNH for barometer + * + * @min 500 + * @max 1500 + * @group Sensor Calibration + * @unit hPa + */ +PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); + /** * Board rotation diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4e8a8c01d3..d8e1fe9c8c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -235,7 +235,7 @@ private: math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ - + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -258,7 +258,7 @@ private: int board_rotation; int external_mag_rotation; - + float board_offset[3]; int rc_map_roll; @@ -301,6 +301,8 @@ private: float battery_voltage_scaling; float battery_current_scaling; + float baro_qnh; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -354,9 +356,11 @@ private: param_t board_rotation; param_t external_mag_rotation; - + param_t board_offset[3]; + param_t baro_qnh; + } _parameter_handles; /**< handles for interesting parameters */ @@ -611,12 +615,15 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); - + /* rotation offsets */ _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); + /* Barometer QNH */ + _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); + /* fetch initial parameter values */ parameters_update(); } @@ -828,19 +835,36 @@ Sensors::parameters_update() get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); - + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); - + /** fine tune board offset on parameter update **/ - math::Matrix<3, 3> board_rotation_offset; + math::Matrix<3, 3> board_rotation_offset; board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], M_DEG_TO_RAD_F * _parameters.board_offset[1], M_DEG_TO_RAD_F * _parameters.board_offset[2]); - + _board_rotation = _board_rotation * board_rotation_offset; + /* update barometer qnh setting */ + param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); + int fd; + fd = open(BARO_DEVICE_PATH, 0); + if (fd < 0) { + warn("%s", BARO_DEVICE_PATH); + errx(1, "FATAL: no barometer found"); + + } else { + warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100)); + int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (ret) { + errx(ret, "qnh could not be set"); + } + } + close(fd); + return OK; } From c3522f85928c16d55e54dedc73eb4ed1420d527f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Aug 2014 10:58:28 +0200 Subject: [PATCH 518/600] Publish telemetry status on telemetry update and on heartbeat update events to avoid inducing heartbeat update latencies resulting in spurious telemetry link dropped detections. Makes overall state handling simpler --- src/modules/mavlink/mavlink_main.cpp | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 30 +++++++----------------- src/modules/mavlink/mavlink_receiver.h | 1 - 3 files changed, 11 insertions(+), 22 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2faf8ab769..c27716f74f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -217,6 +217,8 @@ Mavlink::Mavlink() : errx(1, "instance ID is out of range"); break; } + + _rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; } Mavlink::~Mavlink() diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c0fae0a2f5..e9a9ff1332 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -430,9 +429,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } - - /* this means that heartbeats alone won't be published to the radio status no more */ - _radio_status_available = true; } } @@ -474,25 +470,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); - hrt_abstime tnow = hrt_absolute_time(); + /* set heartbeat time and topic time and publish - + * the telem status also gets updated on telemetry events + */ + tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = tstatus.timestamp; - /* always set heartbeat, publish only if telemetry link not up */ - tstatus.heartbeat_time = tnow; + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { - - tstatus.timestamp = tnow; - /* telem_time indicates the timestamp of a telemetry status packet and we got none */ - tstatus.telem_time = 0; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; - - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - - } else { - orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); - } + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 014193100b..3999b8b01b 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -151,7 +151,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - bool _radio_status_available; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; From 91d8cb2edcda1fde3c1765a659b2445592ed03f1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 13:45:51 +0200 Subject: [PATCH 519/600] sensors: move close to correct position --- 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 d8e1fe9c8c..0d51667d05 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -862,8 +862,8 @@ Sensors::parameters_update() if (ret) { errx(ret, "qnh could not be set"); } + close(fd); } - close(fd); return OK; } From 86ae2e489fdd3656faa38202f44e4b53342e450b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 15:21:27 +0200 Subject: [PATCH 520/600] sensors: do not exit task on error --- src/modules/sensors/sensors.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 0d51667d05..aac297ef8d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -143,6 +143,12 @@ #define STICK_ON_OFF_LIMIT 0.75f +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + /** * Sensor app start / stop handling function * @@ -466,12 +472,6 @@ private: namespace sensors { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - Sensors *g_sensors = nullptr; } @@ -860,7 +860,9 @@ Sensors::parameters_update() warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100)); int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); if (ret) { - errx(ret, "qnh could not be set"); + warnx("qnh could not be set"); + close(fd); + return ERROR; } close(fd); } From 01f1c90c2673754c10d031796b0dbba5e7a6fd55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Aug 2014 20:48:38 +0200 Subject: [PATCH 521/600] Make some space on FMUv1 --- makefiles/config_px4fmu-v1_default.mk | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a7c10f52f3..97eddfdd2b 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -25,7 +25,6 @@ MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -MODULES += drivers/ll40ls MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry @@ -44,7 +43,6 @@ MODULES += modules/sensors # MODULES += systemcmds/mtd MODULES += systemcmds/bl_update -MODULES += systemcmds/i2c MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf @@ -152,5 +150,4 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) + $(call _B, serdis, , 2048, serdis_main ) From 16694051c927b35a853c6ee41a00ad69c81f0bd0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 13:16:58 +0200 Subject: [PATCH 522/600] Require a digit ahead of the dot for a valid number for the SF0x output --- src/drivers/sf0x/sf0x.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index bca1715fa1..80ecab2ee1 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -598,7 +598,8 @@ SF0X::collect() memcpy(_linebuf, buf, (lend + 1) - (i + 1)); } - if (_linebuf[i] == '.') { + /* we need a digit before the dot and a dot for a valid number */ + if (i > 0 && _linebuf[i] == '.') { valid = true; } } From 024c6d572706b3ae7755b5c022e198d7db7c5d34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 16:49:12 +0200 Subject: [PATCH 523/600] Airspeed fix attempt --- src/drivers/airspeed/airspeed.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 41942aacd5..29110373f1 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -364,6 +364,7 @@ Airspeed::update_status() _sensor_ok, SUBSYSTEM_TYPE_DIFFPRESSURE }; + warnx("airspeed ok state changed"); if (_subsys_pub > 0) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); @@ -381,7 +382,10 @@ Airspeed::cycle_trampoline(void *arg) Airspeed *dev = (Airspeed *)arg; dev->cycle(); - dev->update_status(); + // XXX we do not know if this is + // really helping - do not update the + // subsys state right now + //dev->update_status(); } void From 0d87dc992312526bb96269d706e2869d540926f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 16:50:13 +0200 Subject: [PATCH 524/600] mavlink: Handle command int packets --- src/modules/mavlink/mavlink_main.cpp | 2 + src/modules/mavlink/mavlink_receiver.cpp | 60 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + 3 files changed, 63 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c27716f74f..976b6d4d5f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1655,6 +1655,8 @@ Mavlink::display_status() printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } + printf("\tmavlink chan: #%u\n", _channel); + if (_rstatus.timestamp > 0) { printf("\ttype:\t\t"); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e9a9ff1332..a602344fd2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -136,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_command_long(msg); break; + case MAVLINK_MSG_ID_COMMAND_INT: + handle_message_command_int(msg); + break; + case MAVLINK_MSG_ID_OPTICAL_FLOW: handle_message_optical_flow(msg); break; @@ -275,6 +279,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_int_t cmd_mavlink; + mavlink_msg_command_int_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + warnx("terminated by remote command"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; + + } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); + return; + } + + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ + vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; + vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; + vcmd.param7 = cmd_mavlink.z; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + + if (_cmd_pub < 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } + } + } +} + void MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 3999b8b01b..91125736c6 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -110,6 +110,7 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); + void handle_message_command_int(mavlink_message_t *msg); void handle_message_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); From 9e82f14ad805dab3be7fcba701430db54c4b339f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 18:53:37 +0200 Subject: [PATCH 525/600] Mag scale check: only test if the scale roughly makes sense, do not judge the environment --- src/systemcmds/tests/test_sensors.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index a4f17eebde..e005bf9c12 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -331,7 +331,7 @@ mag(int argc, char *argv[]) float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); - if (len < 1.0f || len > 3.0f) { + if (len < 0.25f || len > 3.0f) { warnx("MAG scale error!"); return ERROR; } From 8d7a12218ca4305e3ca36320584113773e550091 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 20:06:30 +0200 Subject: [PATCH 526/600] Time out after a reasonable interval (10 seconds, as e.g. OBC rules prescribe). Experiments show the SiK radios to time out ~4-7 seconds if they loose sync --- 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 6c2c030702..74deda8cca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -129,7 +129,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 -#define DL_TIMEOUT 5 * 1000* 1000 +#define DL_TIMEOUT (10 * 1000 * 1000) #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 From 7eb521b6d4643fbf36d20e923faadca4e8f61dc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 20:07:55 +0200 Subject: [PATCH 527/600] Do not perform retries in airspeed driver - this is too much load for the HRT queue --- src/drivers/airspeed/airspeed.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 29110373f1..293690d274 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -165,7 +165,7 @@ Airspeed::probe() */ _retries = 4; int ret = measure(); - _retries = 2; + _retries = 0; return ret; } @@ -364,7 +364,6 @@ Airspeed::update_status() _sensor_ok, SUBSYSTEM_TYPE_DIFFPRESSURE }; - warnx("airspeed ok state changed"); if (_subsys_pub > 0) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); From 09a9ea87e77d2627b20bd6893a39627f57421f4d Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Fri, 15 Aug 2014 14:22:41 +0200 Subject: [PATCH 528/600] uavcan: increased thread prio, reduces roundtrip latency by a factor of 5..7 --- src/modules/uavcan/uavcan_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4535b6d6ad..f4d3cc77ba 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -164,7 +165,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * Start the task. Normally it should never exit. */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, + _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, static_cast(run_trampoline), nullptr); if (_instance->_task < 0) { From 690843c53aa03283688e04fbe2894fc986f31b6b Mon Sep 17 00:00:00 2001 From: muharred Date: Wed, 20 Aug 2014 13:06:39 +0300 Subject: [PATCH 529/600] Reset excitement mode for hmc5883 sensor before applying a new one to avoid getting reserved 11 bits set in case of a set_excitement(1) call following a set_excitement(-1) call. --- src/drivers/hmc5883/hmc5883.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 0e9a961ac7..e4ecfa6b5d 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1283,14 +1283,13 @@ int HMC5883::set_excitement(unsigned enable) if (OK != ret) perf_count(_comms_errors); + _conf_reg &= ~0x03; // reset previous excitement mode if (((int)enable) < 0) { _conf_reg |= 0x01; } else if (enable > 0) { _conf_reg |= 0x02; - } else { - _conf_reg &= ~0x03; } // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); From bb1a45ac42001d96cbc4a4a4fbe69c989c261094 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 15:22:15 +0200 Subject: [PATCH 530/600] skeleton for adafruiti2cpwm driver --- src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp | 362 ++++++++++++++++++ ...fruit_PWM_Servo_Driver_Library_license.txt | 29 ++ src/drivers/adafruiti2cpwm/module.mk | 41 ++ 3 files changed, 432 insertions(+) create mode 100644 src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp create mode 100644 src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt create mode 100644 src/drivers/adafruiti2cpwm/module.mk diff --git a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp new file mode 100644 index 0000000000..959b90db86 --- /dev/null +++ b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp @@ -0,0 +1,362 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file adafruiti2cpwm.cpp + * + * Driver for the adafruit I2C PWM converter based on the PCA9685 + * https://www.adafruit.com/product/815 + * + * some code is adapted from the arduino library for the board + * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library + * see the arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file + * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors + * + * @author Thomas Gubler + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#define PCA9685_SUBADR1 0x2 +#define PCA9685_SUBADR2 0x3 +#define PCA9685_SUBADR3 0x4 + +#define PCA9685_MODE1 0x0 +#define PCA9685_PRESCALE 0xFE + +#define LED0_ON_L 0x6 +#define LED0_ON_H 0x7 +#define LED0_OFF_L 0x8 +#define LED0_OFF_H 0x9 + +#define ALLLED_ON_L 0xFA +#define ALLLED_ON_H 0xFB +#define ALLLED_OFF_L 0xFC +#define ALLLED_OFF_H 0xFD + +#define ADDR PCA9685_SUBADR1 ///< I2C adress + +#define ADAFRUITI2CPWM_DEVICE_PATH "/dev/adafruiti2cpwm" +#define ADAFRUITI2CPWM_BUS PX4_I2C_BUS_EXPANSION + +class ADAFRUITI2CPWM : public device::I2C +{ +public: + ADAFRUITI2CPWM(int bus=ADAFRUITI2CPWM_BUS, uint8_t address=ADDR); + virtual ~ADAFRUITI2CPWM(); + + + virtual int init(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + bool is_running() { return _running; } + +private: + work_s _work; + + + enum IOX_MODE _mode; + bool _running; + int _i2cpwm_interval; + bool _should_run; + + static void i2cpwm_trampoline(void *arg); + void i2cpwm(); + +}; + +/* for now, we only support one board */ +namespace +{ +ADAFRUITI2CPWM *g_adafruiti2cpwm; +} + +void adafruiti2cpwm_usage(); + +extern "C" __EXPORT int adafruiti2cpwm_main(int argc, char *argv[]); + +ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : + I2C("adafruiti2cpwm", ADAFRUITI2CPWM_DEVICE_PATH, bus, address, 100000), + _mode(IOX_MODE_OFF), + _running(false), + _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), + _should_run(false) +{ + memset(&_work, 0, sizeof(_work)); +} + +ADAFRUITI2CPWM::~ADAFRUITI2CPWM() +{ +} + +int +ADAFRUITI2CPWM::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + return OK; +} + +int +ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -EINVAL; + switch (cmd) { + + case IOX_SET_MODE: + + if (_mode != (IOX_MODE)arg) { + + switch ((IOX_MODE)arg) { + case IOX_MODE_OFF: + case IOX_MODE_ON: + case IOX_MODE_TEST_OUT: + break; + + default: + return -1; + } + + _mode = (IOX_MODE)arg; + } + + return OK; + + default: + // see if the parent class can make any use of it + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + +void +ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) +{ + ADAFRUITI2CPWM *i2cpwm = reinterpret_cast(arg); + + i2cpwm->i2cpwm(); +} + +/** + * Main loop function + */ +void +ADAFRUITI2CPWM::i2cpwm() +{ + + if (_mode == IOX_MODE_TEST_OUT) { + + _should_run = true; + } else if (_mode == IOX_MODE_OFF) { + _should_run = false; + } else { + + } + + + // check if any activity remains, else stop + if (!_should_run) { + _running = false; + return; + } + + // re-queue ourselves to run again later + _running = true; + work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, _i2cpwm_interval); +} + +void +print_usage() +{ + warnx("missing command: try 'start', 'test', 'stop'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION); + warnx(" -a addr (0x%x)", ADDR); +} + +int +adafruiti2cpwm_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int i2caddr = ADDR; // 7bit + + int ch; + + // jump over start/off/etc and look at options first + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + i2caddr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + print_usage(); + exit(0); + } + } + + if (optind >= argc) { + print_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_adafruiti2cpwm != nullptr) { + errx(1, "already started"); + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_adafruiti2cpwm = new ADAFRUITI2CPWM(PX4_I2C_BUS_EXPANSION, i2caddr); + + if (g_adafruiti2cpwm != nullptr && OK != g_adafruiti2cpwm->init()) { + delete g_adafruiti2cpwm; + g_adafruiti2cpwm = nullptr; + } + + if (g_adafruiti2cpwm == nullptr) { + errx(1, "init failed"); + } + } + + if (g_adafruiti2cpwm == nullptr) { + g_adafruiti2cpwm = new ADAFRUITI2CPWM(i2cdevice, i2caddr); + + if (g_adafruiti2cpwm == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_adafruiti2cpwm->init()) { + delete g_adafruiti2cpwm; + g_adafruiti2cpwm = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + // need the driver past this point + if (g_adafruiti2cpwm == nullptr) { + warnx("not started, run adafruiti2cpwm start"); + exit(1); + } + + if (!strcmp(verb, "test")) { + fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "stop")) { + fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + + // wait until we're not running any more + for (unsigned i = 0; i < 15; i++) { + if (!g_adafruiti2cpwm->is_running()) { + break; + } + + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + fflush(stdout); + + if (!g_adafruiti2cpwm->is_running()) { + delete g_adafruiti2cpwm; + g_adafruiti2cpwm= nullptr; + warnx("stopped, exiting"); + exit(0); + } else { + warnx("stop failed."); + exit(1); + } + } + + print_usage(); + exit(0); +} diff --git a/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt b/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt new file mode 100644 index 0000000000..9c5eb42eb6 --- /dev/null +++ b/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt @@ -0,0 +1,29 @@ +The following license agreement covers re-used code from the arduino driver +for the Adafruit I2C to PWM converter. + +Software License Agreement (BSD License) + +Copyright (c) 2012, Adafruit Industries +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright +notice, this list of conditions and the following disclaimer in the +documentation and/or other materials provided with the distribution. +3. Neither the name of the copyright holders nor the +names of its contributors may be used to endorse or promote products +derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/drivers/adafruiti2cpwm/module.mk b/src/drivers/adafruiti2cpwm/module.mk new file mode 100644 index 0000000000..1f24591cfd --- /dev/null +++ b/src/drivers/adafruiti2cpwm/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Driver for the adafruit I2C PWM converter, +# which allow to control servos via I2C. +# https://www.adafruit.com/product/815 + +MODULE_COMMAND = adafruiti2cpwm + +SRCS = adafruiti2cpwm.cpp From 66991f9bb1493437a05ef4b4a704b43889a3413b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 11:19:18 +0200 Subject: [PATCH 531/600] bring up adafruit i2c pwm driver includes the test function, fucntionality verified with scope and servo --- src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp | 271 ++++++++++++++++-- 1 file changed, 255 insertions(+), 16 deletions(-) diff --git a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp index 959b90db86..6fc4f21aa2 100644 --- a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp +++ b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp @@ -37,9 +37,10 @@ * Driver for the adafruit I2C PWM converter based on the PCA9685 * https://www.adafruit.com/product/815 * - * some code is adapted from the arduino library for the board + * Parts of the code are adapted from the arduino library for the board * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library - * see the arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file + * for the license of these parts see the + * arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors * * @author Thomas Gubler @@ -84,12 +85,25 @@ #define ALLLED_ON_L 0xFA #define ALLLED_ON_H 0xFB #define ALLLED_OFF_L 0xFC -#define ALLLED_OFF_H 0xFD +#define ALLLED_OF -#define ADDR PCA9685_SUBADR1 ///< I2C adress +#define ADDR 0x40 // I2C adress #define ADAFRUITI2CPWM_DEVICE_PATH "/dev/adafruiti2cpwm" #define ADAFRUITI2CPWM_BUS PX4_I2C_BUS_EXPANSION +#define ADAFRUITI2CPWM_PWMFREQ 60.0f + +#define ADAFRUITI2CPWM_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) +#define ADAFRUITI2CPWM_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f + +#define ADAFRUITI2CPWM_CENTER (ADAFRUITI2CPWM_SERVOMAX - ADAFRUITI2CPWM_SERVOMIN)/2 +#define ADAFRUITI2CPWM_SCALE ADAFRUITI2CPWM_CENTER + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; class ADAFRUITI2CPWM : public device::I2C { @@ -100,6 +114,8 @@ public: virtual int init(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int info(); + virtual int reset(); bool is_running() { return _running; } private: @@ -110,10 +126,40 @@ private: bool _running; int _i2cpwm_interval; bool _should_run; + perf_counter_t _comms_errors; + + uint8_t _msg[6]; static void i2cpwm_trampoline(void *arg); void i2cpwm(); + /** + * Helper function to set the pwm frequency + */ + int setPWMFreq(float freq); + + /** + * Helper function to set the demanded pwm value + * @param num pwm output number + */ + int setPWM(uint8_t num, uint16_t on, uint16_t off); + + /** + * Sets pin without having to deal with on/off tick placement and properly handles + * a zero value as completely off. Optional invert parameter supports inverting + * the pulse for sinking to ground. + * @param num pwm output number + * @param val should be a value from 0 to 4095 inclusive. + */ + int setPin(uint8_t num, uint16_t val, bool invert = false); + + + /* Wrapper to read a byte from addr */ + int read8(uint8_t addr, uint8_t &value); + + /* Wrapper to wite a byte to addr */ + int write8(uint8_t addr, uint8_t value); + }; /* for now, we only support one board */ @@ -131,9 +177,11 @@ ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : _mode(IOX_MODE_OFF), _running(false), _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), - _should_run(false) + _should_run(false), + _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")) { memset(&_work, 0, sizeof(_work)); + memset(_msg, 0, sizeof(_msg)); } ADAFRUITI2CPWM::~ADAFRUITI2CPWM() @@ -145,12 +193,18 @@ ADAFRUITI2CPWM::init() { int ret; ret = I2C::init(); - if (ret != OK) { return ret; } - return OK; + ret = reset(); + if (ret != OK) { + return ret; + } + + ret = setPWMFreq(ADAFRUITI2CPWM_PWMFREQ); + + return ret; } int @@ -165,8 +219,13 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) switch ((IOX_MODE)arg) { case IOX_MODE_OFF: + warnx("shutting down"); + break; case IOX_MODE_ON: + warnx("starting"); + break; case IOX_MODE_TEST_OUT: + warnx("test starting"); break; default: @@ -176,6 +235,13 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) _mode = (IOX_MODE)arg; } + // if not active, kick it + if (!_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, 1); + } + + return OK; default: @@ -187,6 +253,20 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; } +int +ADAFRUITI2CPWM::info() +{ + int ret = OK; + + if (is_running()) { + warnx("Driver is running, mode: %u", _mode); + } else { + warnx("Driver started but not running"); + } + + return ret; +} + void ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) { @@ -201,17 +281,16 @@ ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) void ADAFRUITI2CPWM::i2cpwm() { - if (_mode == IOX_MODE_TEST_OUT) { - + setPin(0, ADAFRUITI2CPWM_CENTER); _should_run = true; } else if (_mode == IOX_MODE_OFF) { _should_run = false; } else { + _should_run = true; } - // check if any activity remains, else stop if (!_should_run) { _running = false; @@ -223,10 +302,152 @@ ADAFRUITI2CPWM::i2cpwm() work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, _i2cpwm_interval); } -void -print_usage() +int +ADAFRUITI2CPWM::setPWM(uint8_t num, uint16_t on, uint16_t off) { - warnx("missing command: try 'start', 'test', 'stop'"); + int ret; + /* convert to correct message */ + _msg[0] = LED0_ON_L + 4 * num; + _msg[1] = on; + _msg[2] = on >> 8; + _msg[3] = off; + _msg[4] = off >> 8; + + /* try i2c transfer */ + ret = transfer(_msg, 5, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + } + + return ret; +} + +int +ADAFRUITI2CPWM::setPin(uint8_t num, uint16_t val, bool invert) +{ + // Clamp value between 0 and 4095 inclusive. + if (val > 4095) { + val = 4095; + } + if (invert) { + if (val == 0) { + // Special value for signal fully on. + return setPWM(num, 4096, 0); + } else if (val == 4095) { + // Special value for signal fully off. + return setPWM(num, 0, 4096); + } else { + return setPWM(num, 0, 4095-val); + } + } else { + if (val == 4095) { + // Special value for signal fully on. + return setPWM(num, 4096, 0); + } else if (val == 0) { + // Special value for signal fully off. + return setPWM(num, 0, 4096); + } else { + return setPWM(num, 0, val); + } + } + + return ERROR; +} + +int +ADAFRUITI2CPWM::setPWMFreq(float freq) +{ + int ret = OK; + freq *= 0.9f; /* Correct for overshoot in the frequency setting (see issue + https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */ + float prescaleval = 25000000; + prescaleval /= 4096; + prescaleval /= freq; + prescaleval -= 1; + uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor() + uint8_t oldmode; + ret = read8(PCA9685_MODE1, oldmode); + if (ret != OK) { + return ret; + } + uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep + + ret = write8(PCA9685_MODE1, newmode); // go to sleep + if (ret != OK) { + return ret; + } + ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler + if (ret != OK) { + return ret; + } + ret = write8(PCA9685_MODE1, oldmode); + if (ret != OK) { + return ret; + } + + usleep(5000); //5ms delay (from arduino driver) + + ret = write8(PCA9685_MODE1, oldmode | 0xa1); // This sets the MODE1 register to turn on auto increment. + if (ret != OK) { + return ret; + } + + return ret; +} + +/* Wrapper to read a byte from addr */ +int +ADAFRUITI2CPWM::read8(uint8_t addr, uint8_t &value) +{ + int ret = OK; + + /* send addr */ + ret = transfer(&addr, sizeof(addr), nullptr, 0); + if (ret != OK) { + goto fail_read; + } + + /* get value */ + ret = transfer(nullptr, 0, &value, 1); + if (ret != OK) { + goto fail_read; + } + + return ret; + +fail_read: + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + + return ret; +} + +int ADAFRUITI2CPWM::reset(void) { + warnx("resetting"); + return write8(PCA9685_MODE1, 0x0); +} + +/* Wrapper to wite a byte to addr */ +int +ADAFRUITI2CPWM::write8(uint8_t addr, uint8_t value) { + int ret = OK; + _msg[0] = addr; + _msg[1] = value; + /* send addr and value */ + ret = transfer(_msg, 2, nullptr, 0); + if (ret != OK) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + } + return ret; +} + +void +adafruiti2cpwm_usage() +{ + warnx("missing command: try 'start', 'test', 'stop', 'info'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION); warnx(" -a addr (0x%x)", ADDR); @@ -252,13 +473,13 @@ adafruiti2cpwm_main(int argc, char *argv[]) break; default: - print_usage(); + adafruiti2cpwm_usage(); exit(0); } } if (optind >= argc) { - print_usage(); + adafruiti2cpwm_usage(); exit(1); } @@ -300,6 +521,13 @@ adafruiti2cpwm_main(int argc, char *argv[]) errx(1, "init failed"); } } + fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + } + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON); + close(fd); + exit(0); } @@ -310,6 +538,17 @@ adafruiti2cpwm_main(int argc, char *argv[]) exit(1); } + if (!strcmp(verb, "info")) { + g_adafruiti2cpwm->info(); + exit(0); + } + + if (!strcmp(verb, "reset")) { + g_adafruiti2cpwm->reset(); + exit(0); + } + + if (!strcmp(verb, "test")) { fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); @@ -357,6 +596,6 @@ adafruiti2cpwm_main(int argc, char *argv[]) } } - print_usage(); + adafruiti2cpwm_usage(); exit(0); } From dd85c0407cbf552c7044d425d81ed346997e8572 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 15:04:24 +0200 Subject: [PATCH 532/600] adafruit i2c pwm: listen to uorb for setpoints --- src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp | 52 +++++++++++++++++-- 1 file changed, 49 insertions(+), 3 deletions(-) diff --git a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp index 6fc4f21aa2..6d3359999c 100644 --- a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp +++ b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include @@ -67,6 +68,9 @@ #include #include +#include +#include + #include #include @@ -92,12 +96,15 @@ #define ADAFRUITI2CPWM_DEVICE_PATH "/dev/adafruiti2cpwm" #define ADAFRUITI2CPWM_BUS PX4_I2C_BUS_EXPANSION #define ADAFRUITI2CPWM_PWMFREQ 60.0f +#define ADAFRUITI2CPWM_NCHANS 16 // total amount of pwm outputs #define ADAFRUITI2CPWM_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) #define ADAFRUITI2CPWM_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f -#define ADAFRUITI2CPWM_CENTER (ADAFRUITI2CPWM_SERVOMAX - ADAFRUITI2CPWM_SERVOMIN)/2 -#define ADAFRUITI2CPWM_SCALE ADAFRUITI2CPWM_CENTER +#define ADAFRUITI2CPWM_HALFRANGE ((ADAFRUITI2CPWM_SERVOMAX - ADAFRUITI2CPWM_SERVOMIN)/2) +#define ADAFRUITI2CPWM_CENTER (ADAFRUITI2CPWM_SERVOMIN + ADAFRUITI2CPWM_HALFRANGE) +#define ADAFRUITI2CPWM_MAXSERVODEG 45 //maximal defelction in degrees +#define ADAFRUITI2CPWM_SCALE (ADAFRUITI2CPWM_HALFRANGE / (M_DEG_TO_RAD_F * ADAFRUITI2CPWM_MAXSERVODEG)) /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -130,6 +137,13 @@ private: uint8_t _msg[6]; + int _actuator_controls_sub; + struct actuator_controls_s _actuator_controls; + uint16_t _current_values[NUM_ACTUATOR_CONTROLS]; /**< stores the current pwm output + values as sent to the setPin() */ + + bool _mode_on_initialized; /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */ + static void i2cpwm_trampoline(void *arg); void i2cpwm(); @@ -178,10 +192,14 @@ ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : _running(false), _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), _should_run(false), - _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")) + _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")), + _actuator_controls_sub(-1), + _actuator_controls(), + _mode_on_initialized(false) { memset(&_work, 0, sizeof(_work)); memset(_msg, 0, sizeof(_msg)); + memset(_current_values, 0, sizeof(_current_values)); } ADAFRUITI2CPWM::~ADAFRUITI2CPWM() @@ -287,7 +305,35 @@ ADAFRUITI2CPWM::i2cpwm() } else if (_mode == IOX_MODE_OFF) { _should_run = false; } else { + if (!_mode_on_initialized) { + /* Subscribe to actuator control 2 (payload group for gimbal) */ + _actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2)); + /* set the uorb update interval lower than the driver pwm interval */ + orb_set_interval(_actuator_controls_sub, 1000.0f / ADAFRUITI2CPWM_PWMFREQ - 5); + _mode_on_initialized = true; + } + + /* Read the servo setpoints from the actuator control topics (gimbal) */ + bool updated; + orb_check(_actuator_controls_sub, &updated); + if (updated) { + orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); + for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + uint16_t new_value = ADAFRUITI2CPWM_CENTER + + (_actuator_controls.control[i] * ADAFRUITI2CPWM_SCALE); + debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, + (double)_actuator_controls.control[i]); + if (new_value != _current_values[i] && + isfinite(new_value) && + new_value >= 0 && + new_value <= ADAFRUITI2CPWM_SERVOMAX) { + /* This value was updated, send the command to adjust the PWM value */ + setPin(i, new_value); + _current_values[i] = new_value; + } + } + } _should_run = true; } From c414417cf8801f690582876cd723eca7273cbe6b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 16:21:02 +0200 Subject: [PATCH 533/600] rename adafruiti2cpwm to pca9685 --- ...fruit_PWM_Servo_Driver_Library_license.txt | 0 .../{adafruiti2cpwm => pca9685}/module.mk | 9 +- .../pca9685.cpp} | 146 +++++++++--------- 3 files changed, 78 insertions(+), 77 deletions(-) rename src/drivers/{adafruiti2cpwm => pca9685}/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt (100%) rename src/drivers/{adafruiti2cpwm => pca9685}/module.mk (90%) rename src/drivers/{adafruiti2cpwm/adafruiti2cpwm.cpp => pca9685/pca9685.cpp} (76%) diff --git a/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt b/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt similarity index 100% rename from src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt rename to src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt diff --git a/src/drivers/adafruiti2cpwm/module.mk b/src/drivers/pca9685/module.mk similarity index 90% rename from src/drivers/adafruiti2cpwm/module.mk rename to src/drivers/pca9685/module.mk index 1f24591cfd..7a5c7996ec 100644 --- a/src/drivers/adafruiti2cpwm/module.mk +++ b/src/drivers/pca9685/module.mk @@ -32,10 +32,11 @@ ############################################################################ # -# Driver for the adafruit I2C PWM converter, -# which allow to control servos via I2C. +# Driver for the PCA9685 I2C PWM controller +# The chip is used on the adafruit I2C PWM converter, +# which allows to control servos via I2C. # https://www.adafruit.com/product/815 -MODULE_COMMAND = adafruiti2cpwm +MODULE_COMMAND = pca9685 -SRCS = adafruiti2cpwm.cpp +SRCS = pca9685.cpp diff --git a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp b/src/drivers/pca9685/pca9685.cpp similarity index 76% rename from src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp rename to src/drivers/pca9685/pca9685.cpp index 6d3359999c..c97d1e9abe 100644 --- a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -32,10 +32,10 @@ ****************************************************************************/ /** - * @file adafruiti2cpwm.cpp + * @file pca9685.cpp * - * Driver for the adafruit I2C PWM converter based on the PCA9685 - * https://www.adafruit.com/product/815 + * Driver for the PCA9685 I2C PWM module + * The chip is used on the Adafruit I2C/PWM converter https://www.adafruit.com/product/815 * * Parts of the code are adapted from the arduino library for the board * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library @@ -93,18 +93,18 @@ #define ADDR 0x40 // I2C adress -#define ADAFRUITI2CPWM_DEVICE_PATH "/dev/adafruiti2cpwm" -#define ADAFRUITI2CPWM_BUS PX4_I2C_BUS_EXPANSION -#define ADAFRUITI2CPWM_PWMFREQ 60.0f -#define ADAFRUITI2CPWM_NCHANS 16 // total amount of pwm outputs +#define PCA9685_DEVICE_PATH "/dev/pca9685" +#define PCA9685_BUS PX4_I2C_BUS_EXPANSION +#define PCA9685_PWMFREQ 60.0f +#define PCA9685_NCHANS 16 // total amount of pwm outputs -#define ADAFRUITI2CPWM_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) -#define ADAFRUITI2CPWM_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f +#define PCA9685_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) +#define PCA9685_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f -#define ADAFRUITI2CPWM_HALFRANGE ((ADAFRUITI2CPWM_SERVOMAX - ADAFRUITI2CPWM_SERVOMIN)/2) -#define ADAFRUITI2CPWM_CENTER (ADAFRUITI2CPWM_SERVOMIN + ADAFRUITI2CPWM_HALFRANGE) -#define ADAFRUITI2CPWM_MAXSERVODEG 45 //maximal defelction in degrees -#define ADAFRUITI2CPWM_SCALE (ADAFRUITI2CPWM_HALFRANGE / (M_DEG_TO_RAD_F * ADAFRUITI2CPWM_MAXSERVODEG)) +#define PCA9685_HALFRANGE ((PCA9685_SERVOMAX - PCA9685_SERVOMIN)/2) +#define PCA9685_CENTER (PCA9685_SERVOMIN + PCA9685_HALFRANGE) +#define PCA9685_MAXSERVODEG 45 //maximal defelction in degrees +#define PCA9685_SCALE (PCA9685_HALFRANGE / (M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -112,11 +112,11 @@ #endif static const int ERROR = -1; -class ADAFRUITI2CPWM : public device::I2C +class PCA9685 : public device::I2C { public: - ADAFRUITI2CPWM(int bus=ADAFRUITI2CPWM_BUS, uint8_t address=ADDR); - virtual ~ADAFRUITI2CPWM(); + PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR); + virtual ~PCA9685(); virtual int init(); @@ -179,15 +179,15 @@ private: /* for now, we only support one board */ namespace { -ADAFRUITI2CPWM *g_adafruiti2cpwm; +PCA9685 *g_pca9685; } -void adafruiti2cpwm_usage(); +void pca9685_usage(); -extern "C" __EXPORT int adafruiti2cpwm_main(int argc, char *argv[]); +extern "C" __EXPORT int pca9685_main(int argc, char *argv[]); -ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : - I2C("adafruiti2cpwm", ADAFRUITI2CPWM_DEVICE_PATH, bus, address, 100000), +PCA9685::PCA9685(int bus, uint8_t address) : + I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000), _mode(IOX_MODE_OFF), _running(false), _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), @@ -202,12 +202,12 @@ ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : memset(_current_values, 0, sizeof(_current_values)); } -ADAFRUITI2CPWM::~ADAFRUITI2CPWM() +PCA9685::~PCA9685() { } int -ADAFRUITI2CPWM::init() +PCA9685::init() { int ret; ret = I2C::init(); @@ -220,13 +220,13 @@ ADAFRUITI2CPWM::init() return ret; } - ret = setPWMFreq(ADAFRUITI2CPWM_PWMFREQ); + ret = setPWMFreq(PCA9685_PWMFREQ); return ret; } int -ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) +PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = -EINVAL; switch (cmd) { @@ -256,7 +256,7 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) // if not active, kick it if (!_running) { _running = true; - work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, 1); + work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, 1); } @@ -272,7 +272,7 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) } int -ADAFRUITI2CPWM::info() +PCA9685::info() { int ret = OK; @@ -286,9 +286,9 @@ ADAFRUITI2CPWM::info() } void -ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) +PCA9685::i2cpwm_trampoline(void *arg) { - ADAFRUITI2CPWM *i2cpwm = reinterpret_cast(arg); + PCA9685 *i2cpwm = reinterpret_cast(arg); i2cpwm->i2cpwm(); } @@ -297,10 +297,10 @@ ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) * Main loop function */ void -ADAFRUITI2CPWM::i2cpwm() +PCA9685::i2cpwm() { if (_mode == IOX_MODE_TEST_OUT) { - setPin(0, ADAFRUITI2CPWM_CENTER); + setPin(0, PCA9685_CENTER); _should_run = true; } else if (_mode == IOX_MODE_OFF) { _should_run = false; @@ -309,7 +309,7 @@ ADAFRUITI2CPWM::i2cpwm() /* Subscribe to actuator control 2 (payload group for gimbal) */ _actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2)); /* set the uorb update interval lower than the driver pwm interval */ - orb_set_interval(_actuator_controls_sub, 1000.0f / ADAFRUITI2CPWM_PWMFREQ - 5); + orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5); _mode_on_initialized = true; } @@ -320,14 +320,14 @@ ADAFRUITI2CPWM::i2cpwm() if (updated) { orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - uint16_t new_value = ADAFRUITI2CPWM_CENTER + - (_actuator_controls.control[i] * ADAFRUITI2CPWM_SCALE); + uint16_t new_value = PCA9685_CENTER + + (_actuator_controls.control[i] * PCA9685_SCALE); debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, (double)_actuator_controls.control[i]); if (new_value != _current_values[i] && isfinite(new_value) && new_value >= 0 && - new_value <= ADAFRUITI2CPWM_SERVOMAX) { + new_value <= PCA9685_SERVOMAX) { /* This value was updated, send the command to adjust the PWM value */ setPin(i, new_value); _current_values[i] = new_value; @@ -345,11 +345,11 @@ ADAFRUITI2CPWM::i2cpwm() // re-queue ourselves to run again later _running = true; - work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, _i2cpwm_interval); + work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval); } int -ADAFRUITI2CPWM::setPWM(uint8_t num, uint16_t on, uint16_t off) +PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off) { int ret; /* convert to correct message */ @@ -371,7 +371,7 @@ ADAFRUITI2CPWM::setPWM(uint8_t num, uint16_t on, uint16_t off) } int -ADAFRUITI2CPWM::setPin(uint8_t num, uint16_t val, bool invert) +PCA9685::setPin(uint8_t num, uint16_t val, bool invert) { // Clamp value between 0 and 4095 inclusive. if (val > 4095) { @@ -403,7 +403,7 @@ ADAFRUITI2CPWM::setPin(uint8_t num, uint16_t val, bool invert) } int -ADAFRUITI2CPWM::setPWMFreq(float freq) +PCA9685::setPWMFreq(float freq) { int ret = OK; freq *= 0.9f; /* Correct for overshoot in the frequency setting (see issue @@ -445,7 +445,7 @@ ADAFRUITI2CPWM::setPWMFreq(float freq) /* Wrapper to read a byte from addr */ int -ADAFRUITI2CPWM::read8(uint8_t addr, uint8_t &value) +PCA9685::read8(uint8_t addr, uint8_t &value) { int ret = OK; @@ -470,14 +470,14 @@ fail_read: return ret; } -int ADAFRUITI2CPWM::reset(void) { +int PCA9685::reset(void) { warnx("resetting"); return write8(PCA9685_MODE1, 0x0); } /* Wrapper to wite a byte to addr */ int -ADAFRUITI2CPWM::write8(uint8_t addr, uint8_t value) { +PCA9685::write8(uint8_t addr, uint8_t value) { int ret = OK; _msg[0] = addr; _msg[1] = value; @@ -491,7 +491,7 @@ ADAFRUITI2CPWM::write8(uint8_t addr, uint8_t value) { } void -adafruiti2cpwm_usage() +pca9685_usage() { warnx("missing command: try 'start', 'test', 'stop', 'info'"); warnx("options:"); @@ -500,7 +500,7 @@ adafruiti2cpwm_usage() } int -adafruiti2cpwm_main(int argc, char *argv[]) +pca9685_main(int argc, char *argv[]) { int i2cdevice = -1; int i2caddr = ADDR; // 7bit @@ -519,13 +519,13 @@ adafruiti2cpwm_main(int argc, char *argv[]) break; default: - adafruiti2cpwm_usage(); + pca9685_usage(); exit(0); } } if (optind >= argc) { - adafruiti2cpwm_usage(); + pca9685_usage(); exit(1); } @@ -535,41 +535,41 @@ adafruiti2cpwm_main(int argc, char *argv[]) int ret; if (!strcmp(verb, "start")) { - if (g_adafruiti2cpwm != nullptr) { + if (g_pca9685 != nullptr) { errx(1, "already started"); } if (i2cdevice == -1) { // try the external bus first i2cdevice = PX4_I2C_BUS_EXPANSION; - g_adafruiti2cpwm = new ADAFRUITI2CPWM(PX4_I2C_BUS_EXPANSION, i2caddr); + g_pca9685 = new PCA9685(PX4_I2C_BUS_EXPANSION, i2caddr); - if (g_adafruiti2cpwm != nullptr && OK != g_adafruiti2cpwm->init()) { - delete g_adafruiti2cpwm; - g_adafruiti2cpwm = nullptr; + if (g_pca9685 != nullptr && OK != g_pca9685->init()) { + delete g_pca9685; + g_pca9685 = nullptr; } - if (g_adafruiti2cpwm == nullptr) { + if (g_pca9685 == nullptr) { errx(1, "init failed"); } } - if (g_adafruiti2cpwm == nullptr) { - g_adafruiti2cpwm = new ADAFRUITI2CPWM(i2cdevice, i2caddr); + if (g_pca9685 == nullptr) { + g_pca9685 = new PCA9685(i2cdevice, i2caddr); - if (g_adafruiti2cpwm == nullptr) { + if (g_pca9685 == nullptr) { errx(1, "new failed"); } - if (OK != g_adafruiti2cpwm->init()) { - delete g_adafruiti2cpwm; - g_adafruiti2cpwm = nullptr; + if (OK != g_pca9685->init()) { + delete g_pca9685; + g_pca9685 = nullptr; errx(1, "init failed"); } } - fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + fd = open(PCA9685_DEVICE_PATH, 0); if (fd == -1) { - errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + errx(1, "Unable to open " PCA9685_DEVICE_PATH); } ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON); close(fd); @@ -579,27 +579,27 @@ adafruiti2cpwm_main(int argc, char *argv[]) } // need the driver past this point - if (g_adafruiti2cpwm == nullptr) { - warnx("not started, run adafruiti2cpwm start"); + if (g_pca9685 == nullptr) { + warnx("not started, run pca9685 start"); exit(1); } if (!strcmp(verb, "info")) { - g_adafruiti2cpwm->info(); + g_pca9685->info(); exit(0); } if (!strcmp(verb, "reset")) { - g_adafruiti2cpwm->reset(); + g_pca9685->reset(); exit(0); } if (!strcmp(verb, "test")) { - fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + fd = open(PCA9685_DEVICE_PATH, 0); if (fd == -1) { - errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + errx(1, "Unable to open " PCA9685_DEVICE_PATH); } ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); @@ -609,10 +609,10 @@ adafruiti2cpwm_main(int argc, char *argv[]) } if (!strcmp(verb, "stop")) { - fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + fd = open(PCA9685_DEVICE_PATH, 0); if (fd == -1) { - errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + errx(1, "Unable to open " PCA9685_DEVICE_PATH); } ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); @@ -620,7 +620,7 @@ adafruiti2cpwm_main(int argc, char *argv[]) // wait until we're not running any more for (unsigned i = 0; i < 15; i++) { - if (!g_adafruiti2cpwm->is_running()) { + if (!g_pca9685->is_running()) { break; } @@ -631,9 +631,9 @@ adafruiti2cpwm_main(int argc, char *argv[]) printf("\n"); fflush(stdout); - if (!g_adafruiti2cpwm->is_running()) { - delete g_adafruiti2cpwm; - g_adafruiti2cpwm= nullptr; + if (!g_pca9685->is_running()) { + delete g_pca9685; + g_pca9685= nullptr; warnx("stopped, exiting"); exit(0); } else { @@ -642,6 +642,6 @@ adafruiti2cpwm_main(int argc, char *argv[]) } } - adafruiti2cpwm_usage(); + pca9685_usage(); exit(0); } From 3cbfe989c793377cc5eac627c5f12573655b3ad2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 20 Aug 2014 21:54:38 +0200 Subject: [PATCH 534/600] mavlink: missions manager compID cleanup, use the common compID, stict compID checks for incoming messages --- src/modules/mavlink/mavlink_mission.cpp | 18 +++++++++--------- src/modules/mavlink/mavlink_mission.h | 2 -- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 7a761588a9..90a8417873 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -58,6 +58,7 @@ #endif static const int ERROR = -1; +#define CHECK_SYSID_COMPID(_msg) (_msg.target_system == mavlink_system.sysid && _msg.target_component == mavlink_system.compid) MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), _state(MAVLINK_WPM_STATE_IDLE), @@ -79,8 +80,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m _mission_result_sub(-1), _offboard_mission_pub(-1), _slow_rate_limiter(_interval / 10.0f), - _verbose(false), - _comp_id(MAV_COMP_ID_MISSIONPLANNER) + _verbose(false) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); @@ -384,7 +384,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); - if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID(wpa)) { if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -416,7 +416,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -451,7 +451,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID(wprl)) { if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -487,7 +487,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); - if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID(wpr)) { if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -558,7 +558,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -624,7 +624,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); - if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) { + if (CHECK_SYSID_COMPID(wp)) { if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); @@ -710,7 +710,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { + if (CHECK_SYSID_COMPID(wpca)) { if (_state == MAVLINK_WPM_STATE_IDLE) { /* don't touch mission items storage itself, but only items count in mission state */ diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 8ff27f87d4..a7bb94c22d 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -126,8 +126,6 @@ private: bool _verbose; - uint8_t _comp_id; - /* do not allow top copying this class */ MavlinkMissionManager(MavlinkMissionManager &); MavlinkMissionManager& operator = (const MavlinkMissionManager &); From 5bc81c8561bbbad3f63d630a7adb40405623a626 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 09:28:46 +0200 Subject: [PATCH 535/600] pca9685: correct input scaling --- src/drivers/pca9685/pca9685.cpp | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp index c97d1e9abe..6f69ce8a1f 100644 --- a/src/drivers/pca9685/pca9685.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -98,13 +98,15 @@ #define PCA9685_PWMFREQ 60.0f #define PCA9685_NCHANS 16 // total amount of pwm outputs -#define PCA9685_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) -#define PCA9685_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f +#define PCA9685_PWMMIN 150 // this is the 'minimum' pulse length count (out of 4096) +#define PCA9685_PWMMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f -#define PCA9685_HALFRANGE ((PCA9685_SERVOMAX - PCA9685_SERVOMIN)/2) -#define PCA9685_CENTER (PCA9685_SERVOMIN + PCA9685_HALFRANGE) -#define PCA9685_MAXSERVODEG 45 //maximal defelction in degrees -#define PCA9685_SCALE (PCA9685_HALFRANGE / (M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) +#define PCA9685_PWMCENTER ((PCA9685_PWMMAX + PCA9685_PWMMIN)/2) +#define PCA9685_MAXSERVODEG 90.0f /* maximal servo deflection in degrees + PCA9685_PWMMIN <--> -PCA9685_MAXSERVODEG + PCA9685_PWMMAX <--> PCA9685_MAXSERVODEG + */ +#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -300,7 +302,7 @@ void PCA9685::i2cpwm() { if (_mode == IOX_MODE_TEST_OUT) { - setPin(0, PCA9685_CENTER); + setPin(0, PCA9685_PWMCENTER); _should_run = true; } else if (_mode == IOX_MODE_OFF) { _should_run = false; @@ -320,14 +322,16 @@ PCA9685::i2cpwm() if (updated) { orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - uint16_t new_value = PCA9685_CENTER + - (_actuator_controls.control[i] * PCA9685_SCALE); + /* Scale the controls to PWM, first multiply by pi to get rad, + * the control[i] values are on the range -1 ... 1 */ + uint16_t new_value = PCA9685_PWMCENTER + + (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE); debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, (double)_actuator_controls.control[i]); if (new_value != _current_values[i] && isfinite(new_value) && - new_value >= 0 && - new_value <= PCA9685_SERVOMAX) { + new_value >= PCA9685_PWMMIN && + new_value <= PCA9685_PWMMAX) { /* This value was updated, send the command to adjust the PWM value */ setPin(i, new_value); _current_values[i] = new_value; From 3e1de713af500bfa97e5e27f588d76b2ddaafc01 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 09:39:43 +0200 Subject: [PATCH 536/600] navigator: correct mode for land and termination --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 331a9a728f..042c46afda 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -353,6 +353,8 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; @@ -368,8 +370,6 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: _navigation_mode = &_offboard; break; From f7dbc340275a79aafbdd14d10c1fb721a4c1e276 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 12:43:34 +0200 Subject: [PATCH 537/600] sensors: remove warnx --- src/modules/sensors/sensors.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index aac297ef8d..f40034d79c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -857,7 +857,6 @@ Sensors::parameters_update() errx(1, "FATAL: no barometer found"); } else { - warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100)); int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); if (ret) { warnx("qnh could not be set"); From 9c75b9562e904210389a497c8baa46374cbc9927 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 14:39:17 +0200 Subject: [PATCH 538/600] Be more permissive with mission component IDs, renamed camera to onboard link but still accepting same commandline syntax --- src/modules/mavlink/mavlink_main.cpp | 12 ++++++++---- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/mavlink/mavlink_mission.cpp | 19 +++++++++++-------- 3 files changed, 20 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c27716f74f..d702179f00 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1229,7 +1229,10 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MAVLINK_MODE_CUSTOM; } else if (strcmp(optarg, "camera") == 0) { - _mode = MAVLINK_MODE_CAMERA; + // left in here for compatibility + _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(optarg, "onboard") == 0) { + _mode = MAVLINK_MODE_ONBOARD; } break; @@ -1289,8 +1292,8 @@ Mavlink::task_main(int argc, char *argv[]) warnx("mode: CUSTOM"); break; - case MAVLINK_MODE_CAMERA: - warnx("mode: CAMERA"); + case MAVLINK_MODE_ONBOARD: + warnx("mode: ONBOARD"); break; default: @@ -1393,9 +1396,10 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); + configure_stream("OPTICAL_FLOW", 0.5f); break; - case MAVLINK_MODE_CAMERA: + case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 15.0f); configure_stream("GLOBAL_POSITION_INT", 15.0f); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1e2e94cef6..7f9d225bb9 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -127,7 +127,7 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_CAMERA + MAVLINK_MODE_ONBOARD }; void set_mode(enum MAVLINK_MODE); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 90a8417873..d436c95e90 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -58,7 +58,10 @@ #endif static const int ERROR = -1; -#define CHECK_SYSID_COMPID(_msg) (_msg.target_system == mavlink_system.sysid && _msg.target_component == mavlink_system.compid) +#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ + ((_msg.target_component == mavlink_system.compid) || \ + (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ + (_msg.target_component == MAV_COMP_ID_ALL))) MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), _state(MAVLINK_WPM_STATE_IDLE), @@ -384,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); - if (CHECK_SYSID_COMPID(wpa)) { + if (CHECK_SYSID_COMPID_MISSION(wpa)) { if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -416,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); - if (CHECK_SYSID_COMPID(wpc)) { + if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -451,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); - if (CHECK_SYSID_COMPID(wprl)) { + if (CHECK_SYSID_COMPID_MISSION(wprl)) { if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -487,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); - if (CHECK_SYSID_COMPID(wpr)) { + if (CHECK_SYSID_COMPID_MISSION(wpr)) { if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -558,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); - if (CHECK_SYSID_COMPID(wpc)) { + if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -624,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); - if (CHECK_SYSID_COMPID(wp)) { + if (CHECK_SYSID_COMPID_MISSION(wp)) { if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); @@ -710,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); - if (CHECK_SYSID_COMPID(wpca)) { + if (CHECK_SYSID_COMPID_MISSION(wpca)) { if (_state == MAVLINK_WPM_STATE_IDLE) { /* don't touch mission items storage itself, but only items count in mission state */ From f87860bc96a61c538de50b5b74aba0452f9b5784 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 21 Aug 2014 23:23:35 +0400 Subject: [PATCH 539/600] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 6980ee8240..446577e4fb 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 6980ee824074aa2f7a62221bf6040ee411119520 +Subproject commit 446577e4fb979ee4c629081368233eaa5683d086 From 0d9f6b6e2ec86f7fa95bc2e7650256e1392544b2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 01:52:23 +0400 Subject: [PATCH 540/600] UAVCAN: Refactored and generalized sensor bridge support --- .../{esc_controller.cpp => actuators/esc.cpp} | 4 +- .../{esc_controller.hpp => actuators/esc.hpp} | 2 +- src/modules/uavcan/module.mk | 14 +- .../{gnss_receiver.cpp => sensors/gnss.cpp} | 15 ++- .../{gnss_receiver.hpp => sensors/gnss.hpp} | 18 ++- src/modules/uavcan/sensors/sensor_bridge.cpp | 48 +++++++ src/modules/uavcan/sensors/sensor_bridge.hpp | 70 ++++++++++ src/modules/uavcan/uavcan_main.cpp | 121 +++++++++++++++--- src/modules/uavcan/uavcan_main.hpp | 21 +-- 9 files changed, 265 insertions(+), 48 deletions(-) rename src/modules/uavcan/{esc_controller.cpp => actuators/esc.cpp} (98%) rename src/modules/uavcan/{esc_controller.hpp => actuators/esc.hpp} (99%) rename src/modules/uavcan/{gnss_receiver.cpp => sensors/gnss.cpp} (92%) rename src/modules/uavcan/{gnss_receiver.hpp => sensors/gnss.hpp} (87%) create mode 100644 src/modules/uavcan/sensors/sensor_bridge.cpp create mode 100644 src/modules/uavcan/sensors/sensor_bridge.hpp diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/actuators/esc.cpp similarity index 98% rename from src/modules/uavcan/esc_controller.cpp rename to src/modules/uavcan/actuators/esc.cpp index 406eba88c4..223d947312 100644 --- a/src/modules/uavcan/esc_controller.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -32,12 +32,12 @@ ****************************************************************************/ /** - * @file esc_controller.cpp + * @file esc.cpp * * @author Pavel Kirienko */ -#include "esc_controller.hpp" +#include "esc.hpp" #include UavcanEscController::UavcanEscController(uavcan::INode &node) : diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/actuators/esc.hpp similarity index 99% rename from src/modules/uavcan/esc_controller.hpp rename to src/modules/uavcan/actuators/esc.hpp index 559ede561e..cf09882104 100644 --- a/src/modules/uavcan/esc_controller.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file esc_controller.hpp + * @file esc.hpp * * UAVCAN <--> ORB bridge for ESC messages: * uavcan.equipment.esc.RawCommand diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 3865f24681..2b97290456 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,10 +40,16 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os -SRCS += uavcan_main.cpp \ - uavcan_clock.cpp \ - esc_controller.cpp \ - gnss_receiver.cpp +# Main +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp + +# Actuators +SRCS += actuators/esc.cpp + +# Sensors +SRCS += sensors/sensor_bridge.cpp \ + sensors/gnss.cpp # # libuavcan diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/sensors/gnss.cpp similarity index 92% rename from src/modules/uavcan/gnss_receiver.cpp rename to src/modules/uavcan/sensors/gnss.cpp index ba1fe5e499..4fc0743fff 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -32,32 +32,34 @@ ****************************************************************************/ /** - * @file gnss_receiver.cpp + * @file gnss.cpp * * @author Pavel Kirienko * @author Andrew Chambers * */ -#include "gnss_receiver.hpp" +#include "gnss.hpp" #include #include #define MM_PER_CM 10 // Millimeters per centimeter -UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : +UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _node(node), _uavcan_sub_status(node), _report_pub(-1) { } -int UavcanGnssReceiver::init() +const char *UavcanGnssBridge::get_name() const { return "gnss"; } + +int UavcanGnssBridge::init() { int res = -1; // GNSS fix subscription - res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb)); + res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); if (res < 0) { warnx("GNSS fix sub failed %i", res); @@ -67,10 +69,11 @@ int UavcanGnssReceiver::init() // Clear the uORB GPS report memset(&_report, 0, sizeof(_report)); + warnx("gnss sensor bridge init ok"); return res; } -void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) +void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { _report.timestamp_position = hrt_absolute_time(); _report.lat = msg.lat_1e7; diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/sensors/gnss.hpp similarity index 87% rename from src/modules/uavcan/gnss_receiver.hpp rename to src/modules/uavcan/sensors/gnss.hpp index 18df8da2f5..eec595ad10 100644 --- a/src/modules/uavcan/gnss_receiver.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file gnss_receiver.hpp + * @file gnss.hpp * * UAVCAN --> ORB bridge for GNSS messages: * uavcan.equipment.gnss.Fix @@ -51,12 +51,16 @@ #include #include -class UavcanGnssReceiver +#include "sensor_bridge.hpp" + +class UavcanGnssBridge : public IUavcanSensorBridge { public: - UavcanGnssReceiver(uavcan::INode& node); + UavcanGnssBridge(uavcan::INode& node); - int init(); + const char *get_name() const override; + + int init() override; private: /** @@ -65,14 +69,14 @@ private: void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); - typedef uavcan::MethodBinder&)> + typedef uavcan::MethodBinder&)> FixCbBinder; /* * libuavcan related things */ - uavcan::INode &_node; + uavcan::INode &_node; uavcan::Subscriber _uavcan_sub_status; /* diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp new file mode 100644 index 0000000000..ba27d6c71c --- /dev/null +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#include "sensor_bridge.hpp" +#include "gnss.hpp" + +IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) +{ + if (!std::strncmp("gnss", bridge_name, MaxNameLen)) { + return new UavcanGnssBridge(node); + } else { + return nullptr; + } +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp new file mode 100644 index 0000000000..1667c2b57d --- /dev/null +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include + +/** + * A sensor bridge class must implement this interface. + */ +class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode +{ +public: + static constexpr unsigned MaxNameLen = 20; + + virtual ~IUavcanSensorBridge() { } + + /** + * Returns ASCII name of the bridge. + */ + virtual const char *get_name() const = 0; + + /** + * Starts the bridge. + * @return Non-negative value on success, negative on error. + */ + virtual int init() = 0; + + /** + * Sensor bridge factory. + * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". + * @return nullptr if such bridge can't be created. + */ + static IUavcanSensorBridge* make(uavcan::INode &node, const char *bridge_name); +}; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4535b6d6ad..fc5521aa7e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -65,16 +65,18 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), - _esc_controller(_node), - _gnss_receiver(_node) + _node_mutex(), + _esc_controller(_node) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); - // memset(_controls, 0, sizeof(_controls)); - // memset(_poll_fds, 0, sizeof(_poll_fds)); + const int res = pthread_mutex_init(&_node_mutex, nullptr); + if (res < 0) { + std::abort(); + } } UavcanNode::~UavcanNode() @@ -99,7 +101,7 @@ UavcanNode::~UavcanNode() } /* clean up the alternate device node */ - // unregister_driver(PWM_OUTPUT_DEVICE_PATH); + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); ::close(_armed_sub); @@ -231,10 +233,6 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret < 0) return ret; - ret = _gnss_receiver.init(); - if (ret < 0) - return ret; - return _node.start(); } @@ -248,6 +246,8 @@ void UavcanNode::node_spin_once() int UavcanNode::run() { + (void)pthread_mutex_lock(&_node_mutex); + const unsigned PollTimeoutMs = 50; // XXX figure out the output count @@ -291,8 +291,13 @@ int UavcanNode::run() _groups_subscribed = _groups_required; } + // Mutex is unlocked while the thread is blocked on IO multiplexing + (void)pthread_mutex_unlock(&_node_mutex); + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + (void)pthread_mutex_lock(&_node_mutex); + node_spin_once(); // Non-blocking // this would be bad... @@ -352,7 +357,6 @@ int UavcanNode::run() // Output to the bus _esc_controller.update_outputs(outputs.output, outputs.noutputs); } - } // Check arming state @@ -376,10 +380,7 @@ int UavcanNode::run() } int -UavcanNode::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) +UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -524,12 +525,69 @@ UavcanNode::print_info() warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); } +int UavcanNode::sensor_enable(const char *bridge_name) +{ + int retval = -1; + + (void)pthread_mutex_lock(&_node_mutex); + + // Checking if such bridge already exists + { + auto pos = _sensor_bridges.getHead(); + while (pos != nullptr) { + if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MaxNameLen)) { + warnx("sensor bridge '%s' already exists", bridge_name); + retval = -1; + goto leave; + } + pos = pos->getSibling(); + } + } + + // Creating and initing + { + auto bridge = IUavcanSensorBridge::make(get_node(), bridge_name); + if (bridge == nullptr) { + warnx("cannot create sensor bridge '%s'", bridge_name); + retval = -1; + goto leave; + } + + assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MaxNameLen)); + + retval = bridge->init(); + if (retval >= 0) { + _sensor_bridges.add(bridge); + } + } + +leave: + (void)pthread_mutex_unlock(&_node_mutex); + return retval; +} + +void UavcanNode::sensor_print_enabled() +{ + (void)pthread_mutex_lock(&_node_mutex); + + auto pos = _sensor_bridges.getHead(); + while (pos != nullptr) { + warnx("%s", pos->get_name()); + pos = pos->getSibling(); + } + + (void)pthread_mutex_unlock(&_node_mutex); +} + /* * App entry point */ static void print_usage() { - warnx("usage: uavcan start [can_bitrate]"); + warnx("usage: \n" + "\tuavcan start [can_bitrate]\n" + "\tuavcan sensor enable \n" + "\tuavcan sensor list"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -585,7 +643,7 @@ int uavcan_main(int argc, char *argv[]) } /* commands below require the app to be started */ - UavcanNode *inst = UavcanNode::instance(); + UavcanNode *const inst = UavcanNode::instance(); if (!inst) { errx(1, "application not running"); @@ -594,14 +652,37 @@ int uavcan_main(int argc, char *argv[]) if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { inst->print_info(); - return OK; + ::exit(0); } if (!std::strcmp(argv[1], "stop")) { - delete inst; - inst = nullptr; - return OK; + ::exit(0); + } + + if (!std::strcmp(argv[1], "sensor")) { + if (argc < 3) { + print_usage(); + ::exit(1); + } + + if (!std::strcmp(argv[2], "list")) { + inst->sensor_print_enabled(); + ::exit(0); + } + + if (argc < 4) { + print_usage(); + ::exit(1); + } + + if (!std::strcmp(argv[2], "enable")) { + const int res = inst->sensor_enable(argv[3]); + if (res < 0) { + warnx("failed to enable sensor '%s': error %d", argv[3], res); + } + ::exit(0); + } } print_usage(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 05b66fd7b2..bca1aa530d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -42,8 +42,8 @@ #include #include -#include "esc_controller.hpp" -#include "gnss_receiver.hpp" +#include "actuators/esc.hpp" +#include "sensors/sensor_bridge.hpp" /** * @file uavcan_main.hpp @@ -77,12 +77,10 @@ public: static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node& getNode() { return _node; } + Node& get_node() { return _node; } - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + // TODO: move the actuator mixing stuff into the ESC controller class + static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); void subscribe(); @@ -91,6 +89,10 @@ public: void print_info(); + int sensor_enable(const char *bridge_name); + + void sensor_print_enabled(); + static UavcanNode* instance() { return _instance; } private: @@ -109,8 +111,11 @@ private: static UavcanNode *_instance; ///< singleton pointer Node _node; ///< library instance + pthread_mutex_t _node_mutex; + UavcanEscController _esc_controller; - UavcanGnssReceiver _gnss_receiver; + + List _sensor_bridges; ///< Append-only list of active sensor bridges MixerGroup *_mixers = nullptr; From f820010a2b51d03f0099d3b4853e0620593721e6 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 13:24:31 +0400 Subject: [PATCH 541/600] UAVCAN GNSS subscription name fix --- src/modules/uavcan/sensors/gnss.cpp | 4 ++-- src/modules/uavcan/sensors/gnss.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 4fc0743fff..fb89b8365f 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -47,7 +47,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _node(node), -_uavcan_sub_status(node), +_sub_fix(node), _report_pub(-1) { } @@ -59,7 +59,7 @@ int UavcanGnssBridge::init() int res = -1; // GNSS fix subscription - res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); + res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); if (res < 0) { warnx("GNSS fix sub failed %i", res); diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index eec595ad10..6bdae8de34 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -77,7 +77,7 @@ private: * libuavcan related things */ uavcan::INode &_node; - uavcan::Subscriber _uavcan_sub_status; + uavcan::Subscriber _sub_fix; /* * uORB From 54affaf633216c3aef65164c7e43674c8c26f178 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 13:58:05 +0400 Subject: [PATCH 542/600] UAVCAN sensor enable command fix --- src/modules/uavcan/uavcan_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index fc5521aa7e..71302d9282 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -558,6 +558,8 @@ int UavcanNode::sensor_enable(const char *bridge_name) retval = bridge->init(); if (retval >= 0) { _sensor_bridges.add(bridge); + } else { + delete bridge; } } From 29dbe8aed582f833f2994daaaf7ab227f7a7cf45 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 14:27:32 +0400 Subject: [PATCH 543/600] UAVCAN magnetometer driver --- src/modules/uavcan/module.mk | 3 +- src/modules/uavcan/sensors/mag.cpp | 155 +++++++++++++++++++ src/modules/uavcan/sensors/mag.hpp | 72 +++++++++ src/modules/uavcan/sensors/sensor_bridge.cpp | 3 + 4 files changed, 232 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/sensors/mag.cpp create mode 100644 src/modules/uavcan/sensors/mag.hpp diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 2b97290456..de411e1e24 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -49,7 +49,8 @@ SRCS += actuators/esc.cpp # Sensors SRCS += sensors/sensor_bridge.cpp \ - sensors/gnss.cpp + sensors/gnss.cpp \ + sensors/mag.cpp # # libuavcan diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp new file mode 100644 index 0000000000..41a1a5270e --- /dev/null +++ b/src/modules/uavcan/sensors/mag.cpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#include "mag.hpp" + +UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : +device::CDev("uavcan_mag", "/dev/uavcan/mag"), +_sub_mag(node) +{ + _scale.x_scale = 1.0F; + _scale.y_scale = 1.0F; + _scale.z_scale = 1.0F; +} + +UavcanMagnetometerBridge::~UavcanMagnetometerBridge() +{ + if (_class_instance > 0) { + (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); + } +} + +const char *UavcanMagnetometerBridge::get_name() const { return "mag"; } + +int UavcanMagnetometerBridge::init() +{ + // Init the libuavcan subscription + int res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + + // Detect our device class + _class_instance = register_class_devname(MAG_DEVICE_PATH); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: { + _orb_id = ORB_ID(sensor_mag0); + break; + } + case CLASS_DEVICE_SECONDARY: { + _orb_id = ORB_ID(sensor_mag1); + break; + } + case CLASS_DEVICE_TERTIARY: { + _orb_id = ORB_ID(sensor_mag2); + break; + } + default: { + log("invalid class instance: %d", _class_instance); + (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); + return -1; + } + } + + log("inited with class instance %d", _class_instance); + return 0; +} + +int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case MAGIOCSSAMPLERATE: + case MAGIOCGSAMPLERATE: + case MAGIOCSRANGE: + case MAGIOCGRANGE: + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: { + return -EINVAL; + } + case MAGIOCSSCALE: { + std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); + log("new scale/offset: x: %f/%f y: %f/%f z: %f/%f", + double(_scale.x_scale), double(_scale.x_offset), + double(_scale.y_scale), double(_scale.y_offset), + double(_scale.z_scale), double(_scale.z_offset)); + return 0; + } + case MAGIOCGSCALE: { + std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); + return 0; + } + case MAGIOCCALIBRATE: + case MAGIOCEXSTRAP: + case MAGIOCSELFTEST: { + return -EINVAL; + } + case MAGIOCGEXTERNAL: { + return 1; + } + default: { + return CDev::ioctl(filp, cmd, arg); + } + } +} + +void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + auto report = ::mag_report(); + + report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything + + report.timestamp = msg.getUtcTimestamp().toUSec(); + if (report.timestamp == 0) { + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + } + + report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; + report.y = (msg.magnetic_field[1] - _scale.x_offset) * _scale.x_scale; + report.z = (msg.magnetic_field[2] - _scale.x_offset) * _scale.x_scale; + + if (_orb_advert >= 0) { + orb_publish(_orb_id, _orb_advert, &report); + } else { + _orb_advert = orb_advertise(_orb_id, &report); + if (_orb_advert < 0) { + log("ADVERT FAIL"); + } else { + log("advertised"); + } + } +} diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp new file mode 100644 index 0000000000..7f23a0b8f9 --- /dev/null +++ b/src/modules/uavcan/sensors/mag.hpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include +#include + +#include + +class UavcanMagnetometerBridge : public IUavcanSensorBridge, public device::CDev +{ +public: + UavcanMagnetometerBridge(uavcan::INode& node); + ~UavcanMagnetometerBridge() override; + + const char *get_name() const override; + + int init() override; + +private: + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder&)> + MagCbBinder; + + + uavcan::Subscriber _sub_mag; + mag_scale _scale = {}; + orb_id_t _orb_id = nullptr; + orb_advert_t _orb_advert = -1; + int _class_instance = -1; +}; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index ba27d6c71c..08fca73c53 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -37,11 +37,14 @@ #include "sensor_bridge.hpp" #include "gnss.hpp" +#include "mag.hpp" IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { if (!std::strncmp("gnss", bridge_name, MaxNameLen)) { return new UavcanGnssBridge(node); + } else if (!std::strncmp("mag", bridge_name, MaxNameLen)) { + return new UavcanMagnetometerBridge(node); } else { return nullptr; } From e32ff6004be5d7bbd4b94b437b6deaa770618259 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 15:31:08 +0400 Subject: [PATCH 544/600] UAVCAN mag driver fix --- src/modules/uavcan/sensors/mag.cpp | 30 ++++++++++++++---------------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 41a1a5270e..ac43ea1e3b 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -93,33 +93,31 @@ int UavcanMagnetometerBridge::init() int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case MAGIOCSSAMPLERATE: - case MAGIOCGSAMPLERATE: - case MAGIOCSRANGE: - case MAGIOCGRANGE: - case MAGIOCSLOWPASS: - case MAGIOCGLOWPASS: { - return -EINVAL; - } case MAGIOCSSCALE: { std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); - log("new scale/offset: x: %f/%f y: %f/%f z: %f/%f", - double(_scale.x_scale), double(_scale.x_offset), - double(_scale.y_scale), double(_scale.y_offset), - double(_scale.z_scale), double(_scale.z_offset)); return 0; } case MAGIOCGSCALE: { std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); return 0; } - case MAGIOCCALIBRATE: - case MAGIOCEXSTRAP: case MAGIOCSELFTEST: { - return -EINVAL; + return 0; // Nothing to do } case MAGIOCGEXTERNAL: { - return 1; + return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard + } + case MAGIOCSSAMPLERATE: { + return 0; // Pretend that this stuff is supported to keep the sensor app happy + } + case MAGIOCCALIBRATE: + case MAGIOCGSAMPLERATE: + case MAGIOCSRANGE: + case MAGIOCGRANGE: + case MAGIOCSLOWPASS: + case MAGIOCEXSTRAP: + case MAGIOCGLOWPASS: { + return -EINVAL; } default: { return CDev::ioctl(filp, cmd, arg); From bdc2ecd9f6d0ae3e66feb8a8e94391b606ee451e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 15:41:21 +0400 Subject: [PATCH 545/600] Too much Ctrl+C Ctrl+V --- src/modules/uavcan/sensors/mag.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index ac43ea1e3b..6be9e9bac8 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -137,8 +137,8 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure= 0) { orb_publish(_orb_id, _orb_advert, &report); From 6ebd59c633db0d610f63eeb06c5c867da34740e0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 15:52:35 +0400 Subject: [PATCH 546/600] UAVCAN: improved sensor bridge factory --- src/modules/uavcan/sensors/gnss.cpp | 4 ++-- src/modules/uavcan/sensors/gnss.hpp | 4 +++- src/modules/uavcan/sensors/mag.cpp | 4 ++-- src/modules/uavcan/sensors/mag.hpp | 7 ++++--- src/modules/uavcan/sensors/sensor_bridge.cpp | 4 ++-- src/modules/uavcan/sensors/sensor_bridge.hpp | 2 +- src/modules/uavcan/uavcan_main.cpp | 4 ++-- 7 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index fb89b8365f..6b69d163f3 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -45,6 +45,8 @@ #define MM_PER_CM 10 // Millimeters per centimeter +const char *const UavcanGnssBridge::NAME = "gnss"; + UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _node(node), _sub_fix(node), @@ -52,8 +54,6 @@ _report_pub(-1) { } -const char *UavcanGnssBridge::get_name() const { return "gnss"; } - int UavcanGnssBridge::init() { int res = -1; diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 6bdae8de34..db3a515fa7 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -56,9 +56,11 @@ class UavcanGnssBridge : public IUavcanSensorBridge { public: + static const char *const NAME; + UavcanGnssBridge(uavcan::INode& node); - const char *get_name() const override; + const char *get_name() const override { return NAME; } int init() override; diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 6be9e9bac8..4f8a5e1046 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,6 +37,8 @@ #include "mag.hpp" +const char *const UavcanMagnetometerBridge::NAME = "mag"; + UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : device::CDev("uavcan_mag", "/dev/uavcan/mag"), _sub_mag(node) @@ -53,8 +55,6 @@ UavcanMagnetometerBridge::~UavcanMagnetometerBridge() } } -const char *UavcanMagnetometerBridge::get_name() const { return "mag"; } - int UavcanMagnetometerBridge::init() { // Init the libuavcan subscription diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 7f23a0b8f9..4bc5129a2f 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -46,10 +46,12 @@ class UavcanMagnetometerBridge : public IUavcanSensorBridge, public device::CDev { public: + static const char *const NAME; + UavcanMagnetometerBridge(uavcan::INode& node); ~UavcanMagnetometerBridge() override; - const char *get_name() const override; + const char *get_name() const override { return NAME; } int init() override; @@ -63,8 +65,7 @@ private: (const uavcan::ReceivedDataStructure&)> MagCbBinder; - - uavcan::Subscriber _sub_mag; + uavcan::Subscriber _sub_mag; mag_scale _scale = {}; orb_id_t _orb_id = nullptr; orb_advert_t _orb_advert = -1; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 08fca73c53..2bd662d5ce 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -41,9 +41,9 @@ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { - if (!std::strncmp("gnss", bridge_name, MaxNameLen)) { + if (!std::strncmp(UavcanGnssBridge::NAME, bridge_name, MAX_NAME_LEN)) { return new UavcanGnssBridge(node); - } else if (!std::strncmp("mag", bridge_name, MaxNameLen)) { + } else if (!std::strncmp(UavcanMagnetometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { return new UavcanMagnetometerBridge(node); } else { return nullptr; diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 1667c2b57d..7bd8118133 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -46,7 +46,7 @@ class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode { public: - static constexpr unsigned MaxNameLen = 20; + static constexpr unsigned MAX_NAME_LEN = 20; virtual ~IUavcanSensorBridge() { } diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 71302d9282..aca4587fff 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -535,7 +535,7 @@ int UavcanNode::sensor_enable(const char *bridge_name) { auto pos = _sensor_bridges.getHead(); while (pos != nullptr) { - if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MaxNameLen)) { + if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN)) { warnx("sensor bridge '%s' already exists", bridge_name); retval = -1; goto leave; @@ -553,7 +553,7 @@ int UavcanNode::sensor_enable(const char *bridge_name) goto leave; } - assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MaxNameLen)); + assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN)); retval = bridge->init(); if (retval >= 0) { From 2a6ab537b2f8687fdb125d9e5d46338ff85220ad Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 20:04:31 +0400 Subject: [PATCH 547/600] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 446577e4fb..6c070852d7 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 446577e4fb979ee4c629081368233eaa5683d086 +Subproject commit 6c070852d76b18c2d57612f66b5bd00e63118c94 From 6870cd4d3d68941945d303b707c4b05bd5d1e6e4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 20:15:04 +0400 Subject: [PATCH 548/600] UAVCAN baro driver --- src/modules/uavcan/module.mk | 3 +- src/modules/uavcan/sensors/baro.cpp | 147 +++++++++++++++++++ src/modules/uavcan/sensors/baro.hpp | 73 +++++++++ src/modules/uavcan/sensors/sensor_bridge.cpp | 3 + 4 files changed, 225 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/sensors/baro.cpp create mode 100644 src/modules/uavcan/sensors/baro.hpp diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index de411e1e24..26ff7102db 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -50,7 +50,8 @@ SRCS += actuators/esc.cpp # Sensors SRCS += sensors/sensor_bridge.cpp \ sensors/gnss.cpp \ - sensors/mag.cpp + sensors/mag.cpp \ + sensors/baro.cpp # # libuavcan diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp new file mode 100644 index 0000000000..3afcc3f1cc --- /dev/null +++ b/src/modules/uavcan/sensors/baro.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#include "baro.hpp" +#include + +const char *const UavcanBarometerBridge::NAME = "baro"; + +UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : +device::CDev("uavcan_baro", "/dev/uavcan/baro"), +_sub_air_data(node) +{ +} + +UavcanBarometerBridge::~UavcanBarometerBridge() +{ + if (_class_instance > 0) { + (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); + } +} + +int UavcanBarometerBridge::init() +{ + // Init the libuavcan subscription + int res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + + // Detect our device class + _class_instance = register_class_devname(BARO_DEVICE_PATH); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: { + _orb_id = ORB_ID(sensor_baro0); + break; + } + case CLASS_DEVICE_SECONDARY: { + _orb_id = ORB_ID(sensor_baro1); + break; + } + default: { + log("invalid class instance: %d", _class_instance); + (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); + return -1; + } + } + + log("inited with class instance %d", _class_instance); + return 0; +} + +int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case BAROIOCSMSLPRESSURE: { + if ((arg < 80000) || (arg > 120000)) { + return -EINVAL; + } else { + log("new msl pressure %u", _msl_pressure); + _msl_pressure = arg; + return OK; + } + } + case BAROIOCGMSLPRESSURE: { + return _msl_pressure; + } + default: { + return CDev::ioctl(filp, cmd, arg); + } + } +} + +void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + auto report = ::baro_report(); + + report.timestamp = msg.getUtcTimestamp().toUSec(); + if (report.timestamp == 0) { + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + } + + report.temperature = msg.static_temperature; + report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + + /* + * Altitude computation + * Refer to the MS5611 driver for details + */ + const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin + const double a = -6.5 / 1000; // temperature gradient in degrees per metre + const double g = 9.80665; // gravity constant in m/s/s + const double R = 287.05; // ideal gas constant in J/kg/K + + const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa + const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa + + report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + /* + * Publish + */ + if (_orb_advert >= 0) { + orb_publish(_orb_id, _orb_advert, &report); + } else { + _orb_advert = orb_advertise(_orb_id, &report); + if (_orb_advert < 0) { + log("ADVERT FAIL"); + } else { + log("advertised"); + } + } +} diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp new file mode 100644 index 0000000000..f6aa01216a --- /dev/null +++ b/src/modules/uavcan/sensors/baro.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include +#include + +#include + +class UavcanBarometerBridge : public IUavcanSensorBridge, public device::CDev +{ +public: + static const char *const NAME; + + UavcanBarometerBridge(uavcan::INode& node); + ~UavcanBarometerBridge() override; + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + void air_data_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder&)> + AirDataCbBinder; + + uavcan::Subscriber _sub_air_data; + unsigned _msl_pressure = 101325; + orb_id_t _orb_id = nullptr; + orb_advert_t _orb_advert = -1; + int _class_instance = -1; +}; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 2bd662d5ce..f826c8fd24 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -38,6 +38,7 @@ #include "sensor_bridge.hpp" #include "gnss.hpp" #include "mag.hpp" +#include "baro.hpp" IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { @@ -45,6 +46,8 @@ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char * return new UavcanGnssBridge(node); } else if (!std::strncmp(UavcanMagnetometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { return new UavcanMagnetometerBridge(node); + } else if (!std::strncmp(UavcanBarometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { + return new UavcanBarometerBridge(node); } else { return nullptr; } From 7132141cc478e1b9cdde41207c03f2c622f7831a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 20:33:35 +0400 Subject: [PATCH 549/600] UAVCAN: Printing all known sensor bridge names with usage info --- src/modules/uavcan/sensors/sensor_bridge.cpp | 12 ++++++++++++ src/modules/uavcan/sensors/sensor_bridge.hpp | 5 +++++ src/modules/uavcan/uavcan_main.cpp | 3 +++ 3 files changed, 20 insertions(+) diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index f826c8fd24..5752d5524d 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -36,12 +36,17 @@ */ #include "sensor_bridge.hpp" +#include + #include "gnss.hpp" #include "mag.hpp" #include "baro.hpp" IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { + /* + * TODO: make a linked list of known implementations at startup? + */ if (!std::strncmp(UavcanGnssBridge::NAME, bridge_name, MAX_NAME_LEN)) { return new UavcanGnssBridge(node); } else if (!std::strncmp(UavcanMagnetometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { @@ -52,3 +57,10 @@ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char * return nullptr; } } + +void IUavcanSensorBridge::print_known_names(const char *prefix) +{ + printf("%s%s\n", prefix, UavcanGnssBridge::NAME); + printf("%s%s\n", prefix, UavcanMagnetometerBridge::NAME); + printf("%s%s\n", prefix, UavcanBarometerBridge::NAME); +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 7bd8118133..976d9a03d4 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -67,4 +67,9 @@ public: * @return nullptr if such bridge can't be created. */ static IUavcanSensorBridge* make(uavcan::INode &node, const char *bridge_name); + + /** + * Prints all valid bridge names into stdout via printf(), one name per line with prefix. + */ + static void print_known_names(const char *prefix); }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index aca4587fff..8607af1456 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -590,6 +590,9 @@ static void print_usage() "\tuavcan start [can_bitrate]\n" "\tuavcan sensor enable \n" "\tuavcan sensor list"); + + warnx("known sensor bridges:"); + IUavcanSensorBridge::print_known_names("\t"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); From 8d3dd7363dbf9a29d88d2abb3364739749894684 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 23:15:11 +0200 Subject: [PATCH 550/600] catapult launch detection: fix integration logic --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index c555a0a692..9d479832f8 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -66,10 +66,10 @@ void CatapultLaunchMethod::update(float accel_x) last_timestamp = hrt_absolute_time(); if (accel_x > threshold_accel.get()) { - integrator += accel_x * dt; + integrator += dt; // warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", // (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); - if (integrator > threshold_accel.get() * threshold_time.get()) { + if (integrator > threshold_time.get()) { launchDetected = true; } From 6a8971e28f492073a951d96065df30034853bea7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 17:23:59 +0400 Subject: [PATCH 551/600] New UAVCAN initialization logic --- ROMFS/px4fmu_common/init.d/rc.uavcan | 16 ++++++ ROMFS/px4fmu_common/init.d/rcS | 12 +++-- src/modules/uavcan/module.mk | 3 +- src/modules/uavcan/uavcan_main.cpp | 50 ++++++------------- src/modules/uavcan/uavcan_params.c | 73 ++++++++++++++++++++++++++++ 5 files changed, 114 insertions(+), 40 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.uavcan create mode 100644 src/modules/uavcan/uavcan_params.c diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan new file mode 100644 index 0000000000..abb76b4001 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -0,0 +1,16 @@ +#!nsh +# +# UAVCAN initialization script. +# + +if param compare UAVCAN_ENABLE 1 +then + if uavcan start + then + sleep 1 # Sensor autodetection delay + echo "[init] UAVCAN started" + else + echo "[init] ERROR: Could not start UAVCAN" + tone_alarm $TUNE_OUT_ERROR + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c9e6a27cac..1957719057 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -304,11 +304,10 @@ then then if [ $OUTPUT_MODE == uavcan_esc ] then - if uavcan start 1 + if param compare UAVCAN_ENABLE 0 then - echo "CAN UP" - else - echo "CAN ERR" + echo "[init] OVERRIDING UAVCAN_ENABLE = 1" + param set UAVCAN_ENABLE 1 fi fi @@ -447,6 +446,11 @@ then mavlink start $MAVLINK_FLAGS + # + # UAVCAN + # + sh /etc/init.d/rc.uavcan + # # Sensors, Logging, GPS # diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 26ff7102db..93a1bf96fd 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -42,7 +42,8 @@ MAXOPTIMIZATION = -Os # Main SRCS += uavcan_main.cpp \ - uavcan_clock.cpp + uavcan_clock.cpp \ + uavcan_params.c # Actuators SRCS += actuators/esc.cpp diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8607af1456..a15f836964 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -587,7 +588,7 @@ void UavcanNode::sensor_print_enabled() static void print_usage() { warnx("usage: \n" - "\tuavcan start [can_bitrate]\n" + "\tuavcan start\n" "\tuavcan sensor enable \n" "\tuavcan sensor list"); @@ -599,52 +600,32 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); int uavcan_main(int argc, char *argv[]) { - constexpr unsigned DEFAULT_CAN_BITRATE = 1000000; - if (argc < 2) { print_usage(); ::exit(1); } if (!std::strcmp(argv[1], "start")) { - if (argc < 3) { - print_usage(); - ::exit(1); + if (UavcanNode::instance()) { + errx(1, "already started"); } - /* - * Node ID - */ - const int node_id = atoi(argv[2]); + // Node ID + int32_t node_id = 0; + (void)param_get(param_find("UAVCAN_NODE_ID"), &node_id); if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { warnx("Invalid Node ID %i", node_id); ::exit(1); } - /* - * CAN bitrate - */ - unsigned bitrate = 0; + // CAN bitrate + int32_t bitrate = 0; + (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate); - if (argc > 3) { - bitrate = atol(argv[3]); - } - - if (bitrate <= 0) { - bitrate = DEFAULT_CAN_BITRATE; - } - - if (UavcanNode::instance()) { - errx(1, "already started"); - } - - /* - * Start - */ + // Start warnx("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); - } /* commands below require the app to be started */ @@ -655,14 +636,13 @@ int uavcan_main(int argc, char *argv[]) } if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { - - inst->print_info(); - ::exit(0); + inst->print_info(); + ::exit(0); } if (!std::strcmp(argv[1], "stop")) { - delete inst; - ::exit(0); + delete inst; + ::exit(0); } if (!std::strcmp(argv[1], "sensor")) { diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c new file mode 100644 index 0000000000..e6ea8a8fb7 --- /dev/null +++ b/src/modules/uavcan/uavcan_params.c @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#include +#include + +/** + * Enable UAVCAN. + * + * Enables support for UAVCAN-interfaced actuators and sensors. + * + * @min 0 + * @max 1 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); + +/** + * UAVCAN Node ID. + * + * Read the specs at http://uavcan.org to learn more about Node ID. + * + * @min 1 + * @max 125 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1); + +/** + * UAVCAN CAN bus bitrate. + * + * @min 20000 + * @max 1000000 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000); + + + From 40fe9ab969247315e3065d0423034554cf66b885 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 23 Aug 2014 18:03:01 +0200 Subject: [PATCH 552/600] meas_airspeed: don't take the aboslute value --- src/drivers/meas_airspeed/meas_airspeed.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 1597062789..c136c66419 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -228,8 +228,10 @@ MEASAirspeed::collect() // the raw value still should be compensated for the known offset diff_press_pa_raw -= _diff_pres_offset; - float diff_press_pa = fabsf(diff_press_pa_raw); - + /* don't take the absolute value because the calibration takes this into account and warns the user if the + * tubes are connected backwards */ + float diff_press_pa = diff_press_pa_raw; + /* note that we return both the absolute value with offset applied and a raw value without the offset applied. This @@ -241,14 +243,8 @@ MEASAirspeed::collect() With the above calculation the MS4525 sensor will produce a positive number when the top port is used as a dynamic port and bottom port is used as the static port - - Also note that the _diff_pres_offset is applied before the - fabsf() not afterwards. It needs to be done this way to - prevent a bias at low speeds, but this also means that when - setting a offset you must set it based on the raw value, not - the offset value */ - + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ @@ -345,7 +341,7 @@ MEASAirspeed::cycle() /** correct for 5V rail voltage if the system_power ORB topic is available - + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of offset versus voltage for 3 sensors */ @@ -394,7 +390,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) if (voltage_diff < -1.0f) { voltage_diff = -1.0f; } - temperature -= voltage_diff * temp_slope; + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } From c6fb75f66fa94444d07056f271980e3f05008f94 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 23 Aug 2014 18:44:09 +0200 Subject: [PATCH 553/600] airspeed_calibration: stop talking about Pa and and hashtags --- src/drivers/meas_airspeed/meas_airspeed.cpp | 8 +++++--- src/modules/commander/airspeed_calibration.cpp | 18 ++++++++++-------- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index c136c66419..bcdf4670a9 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -232,6 +232,8 @@ MEASAirspeed::collect() * tubes are connected backwards */ float diff_press_pa = diff_press_pa_raw; + warnx("diff preasure: %.4f", (double)diff_press_pa); + /* note that we return both the absolute value with offset applied and a raw value without the offset applied. This @@ -259,9 +261,9 @@ MEASAirspeed::collect() report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); /* the dynamics of the filter can make it overshoot into the negative range */ - if (report.differential_pressure_filtered_pa < 0.0f) { - report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); - } + //if (report.differential_pressure_filtered_pa < 0.0f) { + // report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); + //} report.differential_pressure_raw_pa = diff_press_pa_raw; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 0e58c68b60..339b11bbe6 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,11 +180,13 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); + mavlink_log_critical(mavlink_fd, "Create airflow now"); + calibration_counter = 0; const unsigned maxcount = 3000; @@ -204,18 +206,18 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { - if (calibration_counter % 100 == 0) { - mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", - (int)diff_pres.differential_pressure_raw_pa); + if (calibration_counter % 500 == 0) { + mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", - (int)diff_pres.differential_pressure_raw_pa); - mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); + mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -236,7 +238,7 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { - mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } From 6480747c69203f70b4f28d3a357f49e05df89c85 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 23 Aug 2014 18:49:00 +0200 Subject: [PATCH 554/600] Revert "airspeed_calibration: stop talking about Pa and and hashtags" This reverts commit c6fb75f66fa94444d07056f271980e3f05008f94. --- src/drivers/meas_airspeed/meas_airspeed.cpp | 8 +++----- src/modules/commander/airspeed_calibration.cpp | 18 ++++++++---------- 2 files changed, 11 insertions(+), 15 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index bcdf4670a9..c136c66419 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -232,8 +232,6 @@ MEASAirspeed::collect() * tubes are connected backwards */ float diff_press_pa = diff_press_pa_raw; - warnx("diff preasure: %.4f", (double)diff_press_pa); - /* note that we return both the absolute value with offset applied and a raw value without the offset applied. This @@ -261,9 +259,9 @@ MEASAirspeed::collect() report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); /* the dynamics of the filter can make it overshoot into the negative range */ - //if (report.differential_pressure_filtered_pa < 0.0f) { - // report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); - //} + if (report.differential_pressure_filtered_pa < 0.0f) { + report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); + } report.differential_pressure_raw_pa = diff_press_pa_raw; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 339b11bbe6..0e58c68b60 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,13 +180,11 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - mavlink_log_critical(mavlink_fd, "Create airflow now"); - calibration_counter = 0; const unsigned maxcount = 3000; @@ -206,18 +204,18 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { - if (calibration_counter % 500 == 0) { - mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", - (int)diff_pres.differential_pressure_raw_pa); + if (calibration_counter % 100 == 0) { + mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); - mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", - (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -238,7 +236,7 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { - mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", + mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } From a1b4d72d1f8bd3cd25f9fcfd4dfd4c4ec5bcad01 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 23 Aug 2014 18:52:56 +0200 Subject: [PATCH 555/600] airspeed_calibration: stop talking about Pa and and hashtags (now the correct files) --- src/modules/commander/airspeed_calibration.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 0e58c68b60..339b11bbe6 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,11 +180,13 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); + mavlink_log_critical(mavlink_fd, "Create airflow now"); + calibration_counter = 0; const unsigned maxcount = 3000; @@ -204,18 +206,18 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { - if (calibration_counter % 100 == 0) { - mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", - (int)diff_pres.differential_pressure_raw_pa); + if (calibration_counter % 500 == 0) { + mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", - (int)diff_pres.differential_pressure_raw_pa); - mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); + mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -236,7 +238,7 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { - mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } From e09bc02c37d5e36a3c7d3feffca83e9ddfe1ed79 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 23 Aug 2014 18:53:45 +0200 Subject: [PATCH 556/600] meas_airspeed: don't reset the filter below 0 --- src/drivers/meas_airspeed/meas_airspeed.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index c136c66419..479fa3ccf4 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -258,11 +258,6 @@ MEASAirspeed::collect() report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); - /* the dynamics of the filter can make it overshoot into the negative range */ - if (report.differential_pressure_filtered_pa < 0.0f) { - report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); - } - report.differential_pressure_raw_pa = diff_press_pa_raw; report.max_differential_pressure_pa = _max_differential_pressure_pa; From 4e0d7c6b0e52d3eecba65f4415d4c7372dfd8a49 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 23:14:59 +0400 Subject: [PATCH 557/600] UAVCAN: redundant sensors support --- src/modules/uavcan/module.mk | 6 +- src/modules/uavcan/sensors/baro.cpp | 49 ++--------- src/modules/uavcan/sensors/baro.hpp | 7 +- src/modules/uavcan/sensors/gnss.cpp | 15 ++++ src/modules/uavcan/sensors/gnss.hpp | 18 ++-- src/modules/uavcan/sensors/mag.cpp | 51 ++--------- src/modules/uavcan/sensors/mag.hpp | 7 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 93 +++++++++++++++++++- src/modules/uavcan/sensors/sensor_bridge.hpp | 54 ++++++++++++ 9 files changed, 188 insertions(+), 112 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 93a1bf96fd..f92bc754f0 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -49,9 +49,9 @@ SRCS += uavcan_main.cpp \ SRCS += actuators/esc.cpp # Sensors -SRCS += sensors/sensor_bridge.cpp \ - sensors/gnss.cpp \ - sensors/mag.cpp \ +SRCS += sensors/sensor_bridge.cpp \ + sensors/gnss.cpp \ + sensors/mag.cpp \ sensors/baro.cpp # diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 3afcc3f1cc..ef4f0dbbab 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -38,49 +38,26 @@ #include "baro.hpp" #include +static const orb_id_t BARO_TOPICS[2] = { + ORB_ID(sensor_baro0), + ORB_ID(sensor_baro1) +}; + const char *const UavcanBarometerBridge::NAME = "baro"; UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : -device::CDev("uavcan_baro", "/dev/uavcan/baro"), +UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS), _sub_air_data(node) { } -UavcanBarometerBridge::~UavcanBarometerBridge() -{ - if (_class_instance > 0) { - (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); - } -} - int UavcanBarometerBridge::init() { - // Init the libuavcan subscription int res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); return res; } - - // Detect our device class - _class_instance = register_class_devname(BARO_DEVICE_PATH); - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: { - _orb_id = ORB_ID(sensor_baro0); - break; - } - case CLASS_DEVICE_SECONDARY: { - _orb_id = ORB_ID(sensor_baro1); - break; - } - default: { - log("invalid class instance: %d", _class_instance); - (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); - return -1; - } - } - - log("inited with class instance %d", _class_instance); return 0; } @@ -131,17 +108,5 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure< report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - /* - * Publish - */ - if (_orb_advert >= 0) { - orb_publish(_orb_id, _orb_advert, &report); - } else { - _orb_advert = orb_advertise(_orb_id, &report); - if (_orb_advert < 0) { - log("ADVERT FAIL"); - } else { - log("advertised"); - } - } + publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index f6aa01216a..9d470219ed 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -39,17 +39,15 @@ #include "sensor_bridge.hpp" #include -#include #include -class UavcanBarometerBridge : public IUavcanSensorBridge, public device::CDev +class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase { public: static const char *const NAME; UavcanBarometerBridge(uavcan::INode& node); - ~UavcanBarometerBridge() override; const char *get_name() const override { return NAME; } @@ -67,7 +65,4 @@ private: uavcan::Subscriber _sub_air_data; unsigned _msl_pressure = 101325; - orb_id_t _orb_id = nullptr; - orb_advert_t _orb_advert = -1; - int _class_instance = -1; }; diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 6b69d163f3..f2bb280878 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -73,8 +73,23 @@ int UavcanGnssBridge::init() return res; } +unsigned UavcanGnssBridge::get_num_redundant_channels() const +{ + return (_receiver_node_id < 0) ? 0 : 1; +} + void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { + // This bridge does not support redundant GNSS receivers yet. + if (_receiver_node_id < 0) { + _receiver_node_id = msg.getSrcNodeID().get(); + warnx("GNSS receiver node ID: %d", _receiver_node_id); + } else { + if (_receiver_node_id != msg.getSrcNodeID().get()) { + return; // This GNSS receiver is the redundant one, ignore it. + } + } + _report.timestamp_position = hrt_absolute_time(); _report.lat = msg.lat_1e7; _report.lon = msg.lon_1e7; diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index db3a515fa7..9488c5fe52 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -64,27 +64,23 @@ public: int init() override; + unsigned get_num_redundant_channels() const override; + private: /** * GNSS fix message will be reported via this callback. */ void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); - typedef uavcan::MethodBinder&)> FixCbBinder; - /* - * libuavcan related things - */ - uavcan::INode &_node; - uavcan::Subscriber _sub_fix; + uavcan::INode &_node; + uavcan::Subscriber _sub_fix; + int _receiver_node_id = -1; - /* - * uORB - */ - struct vehicle_gps_position_s _report; ///< uORB topic for gnss position - orb_advert_t _report_pub; ///< uORB pub for gnss position + struct vehicle_gps_position_s _report; ///< uORB topic for gnss position + orb_advert_t _report_pub; ///< uORB pub for gnss position }; diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 4f8a5e1046..aaa3a44632 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,10 +37,16 @@ #include "mag.hpp" +static const orb_id_t MAG_TOPICS[3] = { + ORB_ID(sensor_mag0), + ORB_ID(sensor_mag1), + ORB_ID(sensor_mag2) +}; + const char *const UavcanMagnetometerBridge::NAME = "mag"; UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : -device::CDev("uavcan_mag", "/dev/uavcan/mag"), +UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), _sub_mag(node) { _scale.x_scale = 1.0F; @@ -48,45 +54,13 @@ _sub_mag(node) _scale.z_scale = 1.0F; } -UavcanMagnetometerBridge::~UavcanMagnetometerBridge() -{ - if (_class_instance > 0) { - (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); - } -} - int UavcanMagnetometerBridge::init() { - // Init the libuavcan subscription int res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); return res; } - - // Detect our device class - _class_instance = register_class_devname(MAG_DEVICE_PATH); - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: { - _orb_id = ORB_ID(sensor_mag0); - break; - } - case CLASS_DEVICE_SECONDARY: { - _orb_id = ORB_ID(sensor_mag1); - break; - } - case CLASS_DEVICE_TERTIARY: { - _orb_id = ORB_ID(sensor_mag2); - break; - } - default: { - log("invalid class instance: %d", _class_instance); - (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); - return -1; - } - } - - log("inited with class instance %d", _class_instance); return 0; } @@ -140,14 +114,5 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure= 0) { - orb_publish(_orb_id, _orb_advert, &report); - } else { - _orb_advert = orb_advertise(_orb_id, &report); - if (_orb_advert < 0) { - log("ADVERT FAIL"); - } else { - log("advertised"); - } - } + publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 4bc5129a2f..6d413a8f71 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -38,18 +38,16 @@ #pragma once #include "sensor_bridge.hpp" -#include #include #include -class UavcanMagnetometerBridge : public IUavcanSensorBridge, public device::CDev +class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase { public: static const char *const NAME; UavcanMagnetometerBridge(uavcan::INode& node); - ~UavcanMagnetometerBridge() override; const char *get_name() const override { return NAME; } @@ -67,7 +65,4 @@ private: uavcan::Subscriber _sub_mag; mag_scale _scale = {}; - orb_id_t _orb_id = nullptr; - orb_advert_t _orb_advert = -1; - int _class_instance = -1; }; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 5752d5524d..05d589b580 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -36,12 +36,15 @@ */ #include "sensor_bridge.hpp" -#include +#include #include "gnss.hpp" #include "mag.hpp" #include "baro.hpp" +/* + * IUavcanSensorBridge + */ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { /* @@ -64,3 +67,91 @@ void IUavcanSensorBridge::print_known_names(const char *prefix) printf("%s%s\n", prefix, UavcanMagnetometerBridge::NAME); printf("%s%s\n", prefix, UavcanBarometerBridge::NAME); } + +/* + * UavcanCDevSensorBridgeBase + */ +UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() +{ + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].redunancy_channel_id >= 0) { + (void)unregister_class_devname(_class_devname, _channels[i].class_instance); + } + } + delete [] _orb_topics; + delete [] _channels; +} + +void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report) +{ + Channel *channel = nullptr; + + // Checking if such channel already exists + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].redunancy_channel_id == redundancy_channel_id) { + channel = _channels + i; + break; + } + } + + // No such channel - try to create one + if (channel == nullptr) { + if (_out_of_channels) { + return; // Give up immediately - saves some CPU time + } + + log("adding channel %d...", redundancy_channel_id); + + // Search for the first free channel + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].redunancy_channel_id < 0) { + channel = _channels + i; + break; + } + } + + // No free channels left + if (channel == nullptr) { + _out_of_channels = true; + log("out of channels"); + return; + } + + // Ask the CDev helper which class instance we can take + const int class_instance = register_class_devname(_class_devname); + if (class_instance < 0 || class_instance >= int(_max_channels)) { + _out_of_channels = true; + log("out of class instances"); + (void)unregister_class_devname(_class_devname, class_instance); + return; + } + + // Publish to the appropriate topic, abort on failure + channel->orb_id = _orb_topics[class_instance]; + channel->redunancy_channel_id = redundancy_channel_id; + channel->class_instance = class_instance; + + channel->orb_advert = orb_advertise(channel->orb_id, report); + if (channel->orb_advert < 0) { + log("ADVERTISE FAILED"); + *channel = Channel(); + return; + } + + log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance); + } + assert(channel != nullptr); + + (void)orb_publish(channel->orb_id, channel->orb_advert, report); +} + +unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const +{ + unsigned out = 0; + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].redunancy_channel_id >= 0) { + out += 1; + } + } + return out; +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 976d9a03d4..d27ccb8a09 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -39,6 +39,8 @@ #include #include +#include +#include /** * A sensor bridge class must implement this interface. @@ -61,6 +63,11 @@ public: */ virtual int init() = 0; + /** + * Returns number of active redundancy channels. + */ + virtual unsigned get_num_redundant_channels() const = 0; + /** * Sensor bridge factory. * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". @@ -73,3 +80,50 @@ public: */ static void print_known_names(const char *prefix); }; + +/** + * This is the base class for redundant sensors with an independent ORB topic per each redundancy channel. + * For example, sensor_mag0, sensor_mag1, etc. + */ +class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev +{ + struct Channel + { + int redunancy_channel_id = -1; + orb_id_t orb_id = nullptr; + orb_advert_t orb_advert = -1; + int class_instance = -1; + }; + + const unsigned _max_channels; + const char *const _class_devname; + orb_id_t *const _orb_topics; + Channel *const _channels; + bool _out_of_channels = false; + +protected: + template + UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, + const orb_id_t (&orb_topics)[MaxChannels]) : + device::CDev(name, devname), + _max_channels(MaxChannels), + _class_devname(class_devname), + _orb_topics(new orb_id_t[MaxChannels]), + _channels(new Channel[MaxChannels]) + { + memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); + } + + /** + * Sends one measurement into appropriate ORB topic. + * New redundancy channels will be registered automatically. + * @param redundancy_channel_id Redundant sensor identifier (e.g. UAVCAN Node ID) + * @param report ORB message object + */ + void publish(const int redundancy_channel_id, const void *report); + +public: + virtual ~UavcanCDevSensorBridgeBase(); + + unsigned get_num_redundant_channels() const override; +}; From ce73be514e0add0356c120c19e43f6818af236ad Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 23:30:49 +0400 Subject: [PATCH 558/600] UAVCAN: Proper CDev initialization from sensor bridges --- src/modules/uavcan/sensors/baro.cpp | 7 ++++++- src/modules/uavcan/sensors/mag.cpp | 7 ++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index ef4f0dbbab..80c5e3828e 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -53,7 +53,12 @@ _sub_air_data(node) int UavcanBarometerBridge::init() { - int res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); + int res = device::CDev::init(); + if (res < 0) { + return res; + } + + res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); return res; diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index aaa3a44632..8e6a9a22f0 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -56,7 +56,12 @@ _sub_mag(node) int UavcanMagnetometerBridge::init() { - int res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); + int res = device::CDev::init(); + if (res < 0) { + return res; + } + + res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); return res; From 0f124963d4f0c07afa94d96b779a0d28b0fbd66f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 23:43:01 +0400 Subject: [PATCH 559/600] UAVCAN: Minor improvement of the GNSS bridge --- src/modules/uavcan/sensors/gnss.cpp | 57 ++++++++++++++--------------- src/modules/uavcan/sensors/gnss.hpp | 1 - 2 files changed, 28 insertions(+), 30 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index f2bb280878..b3a9cb99b3 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -66,9 +66,6 @@ int UavcanGnssBridge::init() return res; } - // Clear the uORB GPS report - memset(&_report, 0, sizeof(_report)); - warnx("gnss sensor bridge init ok"); return res; } @@ -90,12 +87,14 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) ? sqrtf(horizontal_pos_variance) : -1.0F; + report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F; // Vertical position uncertainty - _report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; + report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; } else { - _report.eph = -1.0F; - _report.epv = -1.0F; + report.eph = -1.0F; + report.epv = -1.0F; } if (valid_velocity_covariance) { float vel_cov[9]; msg.velocity_covariance.unpackSquareMatrix(vel_cov); - _report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); + report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); /* There is a nonlinear relationship between the velocity vector and the heading. * Use Jacobian to transform velocity covariance to heading covariance @@ -136,36 +135,36 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &report); } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &report); } } diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 9488c5fe52..c2b6e41956 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -80,7 +80,6 @@ private: uavcan::Subscriber _sub_fix; int _receiver_node_id = -1; - struct vehicle_gps_position_s _report; ///< uORB topic for gnss position orb_advert_t _report_pub; ///< uORB pub for gnss position }; From e9da8303161a1e1f012b7c5d770249c3d606fcbf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 00:06:47 +0400 Subject: [PATCH 560/600] UAVCAN: initializing all bridges by default --- src/modules/uavcan/sensors/gnss.cpp | 7 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 24 +--- src/modules/uavcan/sensors/sensor_bridge.hpp | 7 +- src/modules/uavcan/uavcan_main.cpp | 111 ++++--------------- src/modules/uavcan/uavcan_main.hpp | 6 +- 5 files changed, 29 insertions(+), 126 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index b3a9cb99b3..8548660fe1 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -56,17 +56,12 @@ _report_pub(-1) int UavcanGnssBridge::init() { - int res = -1; - - // GNSS fix subscription - res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); + int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); if (res < 0) { warnx("GNSS fix sub failed %i", res); return res; } - - warnx("gnss sensor bridge init ok"); return res; } diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 05d589b580..a69514d77e 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -45,27 +45,11 @@ /* * IUavcanSensorBridge */ -IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) +void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) { - /* - * TODO: make a linked list of known implementations at startup? - */ - if (!std::strncmp(UavcanGnssBridge::NAME, bridge_name, MAX_NAME_LEN)) { - return new UavcanGnssBridge(node); - } else if (!std::strncmp(UavcanMagnetometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { - return new UavcanMagnetometerBridge(node); - } else if (!std::strncmp(UavcanBarometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { - return new UavcanBarometerBridge(node); - } else { - return nullptr; - } -} - -void IUavcanSensorBridge::print_known_names(const char *prefix) -{ - printf("%s%s\n", prefix, UavcanGnssBridge::NAME); - printf("%s%s\n", prefix, UavcanMagnetometerBridge::NAME); - printf("%s%s\n", prefix, UavcanBarometerBridge::NAME); + list.add(new UavcanBarometerBridge(node)); + list.add(new UavcanMagnetometerBridge(node)); + list.add(new UavcanGnssBridge(node)); } /* diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index d27ccb8a09..a13d0ab35d 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -73,12 +73,7 @@ public: * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". * @return nullptr if such bridge can't be created. */ - static IUavcanSensorBridge* make(uavcan::INode &node, const char *bridge_name); - - /** - * Prints all valid bridge names into stdout via printf(), one name per line with prefix. - */ - static void print_known_names(const char *prefix); + static void make_all(uavcan::INode &node, List &list); }; /** diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a15f836964..19b54b9cf3 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -217,11 +217,12 @@ int UavcanNode::init(uavcan::NodeID node_id) { int ret = -1; - /* do regular cdev init */ + // Do regular cdev init ret = CDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } _node.setName("org.pixhawk.pixhawk"); @@ -229,10 +230,24 @@ int UavcanNode::init(uavcan::NodeID node_id) fill_node_info(); - /* initializing the bridges UAVCAN <--> uORB */ + // Actuators ret = _esc_controller.init(); - if (ret < 0) + if (ret < 0) { return ret; + } + + // Sensor bridges + IUavcanSensorBridge::make_all(_node, _sensor_bridges); + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + ret = br->init(); + if (ret < 0) { + warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret); + return ret; + } + warnx("sensor bridge '%s' init ok", br->get_name()); + br = br->getSibling(); + } return _node.start(); } @@ -522,62 +537,10 @@ UavcanNode::print_info() warnx("not running, start first"); } + (void)pthread_mutex_lock(&_node_mutex); + warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); -} - -int UavcanNode::sensor_enable(const char *bridge_name) -{ - int retval = -1; - - (void)pthread_mutex_lock(&_node_mutex); - - // Checking if such bridge already exists - { - auto pos = _sensor_bridges.getHead(); - while (pos != nullptr) { - if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN)) { - warnx("sensor bridge '%s' already exists", bridge_name); - retval = -1; - goto leave; - } - pos = pos->getSibling(); - } - } - - // Creating and initing - { - auto bridge = IUavcanSensorBridge::make(get_node(), bridge_name); - if (bridge == nullptr) { - warnx("cannot create sensor bridge '%s'", bridge_name); - retval = -1; - goto leave; - } - - assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN)); - - retval = bridge->init(); - if (retval >= 0) { - _sensor_bridges.add(bridge); - } else { - delete bridge; - } - } - -leave: - (void)pthread_mutex_unlock(&_node_mutex); - return retval; -} - -void UavcanNode::sensor_print_enabled() -{ - (void)pthread_mutex_lock(&_node_mutex); - - auto pos = _sensor_bridges.getHead(); - while (pos != nullptr) { - warnx("%s", pos->get_name()); - pos = pos->getSibling(); - } (void)pthread_mutex_unlock(&_node_mutex); } @@ -588,12 +551,7 @@ void UavcanNode::sensor_print_enabled() static void print_usage() { warnx("usage: \n" - "\tuavcan start\n" - "\tuavcan sensor enable \n" - "\tuavcan sensor list"); - - warnx("known sensor bridges:"); - IUavcanSensorBridge::print_known_names("\t"); + "\tuavcan {start|status|stop}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -645,31 +603,6 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } - if (!std::strcmp(argv[1], "sensor")) { - if (argc < 3) { - print_usage(); - ::exit(1); - } - - if (!std::strcmp(argv[2], "list")) { - inst->sensor_print_enabled(); - ::exit(0); - } - - if (argc < 4) { - print_usage(); - ::exit(1); - } - - if (!std::strcmp(argv[2], "enable")) { - const int res = inst->sensor_enable(argv[3]); - if (res < 0) { - warnx("failed to enable sensor '%s': error %d", argv[3], res); - } - ::exit(0); - } - } - print_usage(); ::exit(1); } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index bca1aa530d..be7db97411 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -89,10 +89,6 @@ public: void print_info(); - int sensor_enable(const char *bridge_name); - - void sensor_print_enabled(); - static UavcanNode* instance() { return _instance; } private: @@ -115,7 +111,7 @@ private: UavcanEscController _esc_controller; - List _sensor_bridges; ///< Append-only list of active sensor bridges + List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; From 701bd803ce2b7b212b53704453ee9a493d473d2d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 00:20:57 +0400 Subject: [PATCH 561/600] UAVCAN status reporting and proper termination --- src/modules/uavcan/uavcan_main.cpp | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 19b54b9cf3..482fec1e0a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -106,6 +106,14 @@ UavcanNode::~UavcanNode() ::close(_armed_sub); + // Removing the sensor bridges + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + auto next = br->getSibling(); + delete br; + br = next; + } + _instance = nullptr; } @@ -539,8 +547,17 @@ UavcanNode::print_info() (void)pthread_mutex_lock(&_node_mutex); - warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); + // ESC mixer status + warnx("ESC actuators control groups: sub: %u / req: %u / fds: %u", + (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + warnx("ESC mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); + + // Sensor bridges + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + warnx("Sensor '%s': channels: %u", br->get_name(), br->get_num_redundant_channels()); + br = br->getSibling(); + } (void)pthread_mutex_unlock(&_node_mutex); } From 3a029926b432d531f8e85ff73c0578750c171d75 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 22:36:40 +0200 Subject: [PATCH 562/600] vfr_hud mavlink msg: use baro alt The vfr_hud message demands the AMSL altitude and not the wgs84 altitude. Use the baro altitude for now. This can be changed to an output of the position estimator later. --- src/modules/mavlink/mavlink_messages.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index af4d46a362..5a92004a61 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -774,6 +774,9 @@ private: MavlinkOrbSubscription *_airspeed_sub; uint64_t _airspeed_time; + MavlinkOrbSubscription *_sensor_combined_sub; + uint64_t _sensor_combined_time; + /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); @@ -789,7 +792,9 @@ protected: _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), _act_time(0), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), - _airspeed_time(0) + _airspeed_time(0), + _sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_combined_time(0) {} void send(const hrt_abstime t) @@ -799,12 +804,14 @@ protected: struct actuator_armed_s armed; struct actuator_controls_s act; struct airspeed_s airspeed; + struct sensor_combined_s sensor_combined; bool updated = _att_sub->update(&_att_time, &att); updated |= _pos_sub->update(&_pos_time, &pos); updated |= _armed_sub->update(&_armed_time, &armed); updated |= _act_sub->update(&_act_time, &act); updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); + updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined); if (updated) { mavlink_vfr_hud_t msg; @@ -813,7 +820,7 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - msg.alt = pos.alt; + msg.alt = sensor_combined.baro_alt_meter; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); From 1fa49aaea98c18a00ec5bc6e227b46ac19fe66a1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 01:41:54 +0400 Subject: [PATCH 563/600] UAVCAN: clarification --- ROMFS/px4fmu_common/init.d/rc.uavcan | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index abb76b4001..9a470a6b8d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -7,7 +7,9 @@ if param compare UAVCAN_ENABLE 1 then if uavcan start then - sleep 1 # Sensor autodetection delay + # First sensor publisher to initialize takes lowest instance ID + # This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs + sleep 1 echo "[init] UAVCAN started" else echo "[init] ERROR: Could not start UAVCAN" From 3866b5a5fe1c6bc9d8b56ef2d603b8c88f1a295d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 03:02:52 +0400 Subject: [PATCH 564/600] Resource leak fix --- src/modules/uavcan/sensors/sensor_bridge.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index a69514d77e..a98596f9c5 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -118,6 +118,7 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const channel->orb_advert = orb_advertise(channel->orb_id, report); if (channel->orb_advert < 0) { log("ADVERTISE FAILED"); + (void)unregister_class_devname(_class_devname, class_instance); *channel = Channel(); return; } From 127948f32f24e36c0c03a047b57fcd64287d9967 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 11:49:45 +0200 Subject: [PATCH 565/600] Remove absolute pressure field as its not useful and confusing anywary --- src/drivers/ets_airspeed/ets_airspeed.cpp | 20 +++++------------ src/drivers/meas_airspeed/meas_airspeed.cpp | 22 +++++-------------- src/modules/sensors/sensors.cpp | 14 ++++++------ .../uORB/topics/differential_pressure.h | 1 - 4 files changed, 18 insertions(+), 39 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index f98d615a2c..0f77bb8050 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -155,7 +155,6 @@ ETSAirspeed::collect() } uint16_t diff_pres_pa_raw = val[1] << 8 | val[0]; - uint16_t diff_pres_pa; if (diff_pres_pa_raw == 0) { // a zero value means the pressure sensor cannot give us a // value. We need to return, and not report a value or the @@ -166,28 +165,21 @@ ETSAirspeed::collect() return -1; } - if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { - diff_pres_pa = 0; - } else { - diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset; - } - // The raw value still should be compensated for the known offset diff_pres_pa_raw -= _diff_pres_offset; // Track maximum differential pressure measured (so we can work out top speed). - if (diff_pres_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_pres_pa; + if (diff_pres_pa_raw > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_pres_pa_raw; } differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - report.differential_pressure_pa = (float)diff_pres_pa; // XXX we may want to smooth out the readings to remove noise. - report.differential_pressure_filtered_pa = (float)diff_pres_pa; - report.differential_pressure_raw_pa = (float)diff_pres_pa_raw; + report.differential_pressure_filtered_pa = diff_pres_pa_raw; + report.differential_pressure_raw_pa = diff_pres_pa_raw; report.temperature = -1000.0f; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -369,7 +361,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -394,7 +386,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); } /* reset the sensor polling to its default rate */ diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 479fa3ccf4..1d9a463ad3 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -228,18 +228,7 @@ MEASAirspeed::collect() // the raw value still should be compensated for the known offset diff_press_pa_raw -= _diff_pres_offset; - /* don't take the absolute value because the calibration takes this into account and warns the user if the - * tubes are connected backwards */ - float diff_press_pa = diff_press_pa_raw; - /* - note that we return both the absolute value with offset - applied and a raw value without the offset applied. This - makes it possible for higher level code to detect if the - user has the tubes connected backwards, and also makes it - possible to correctly use offsets calculated by a higher - level airspeed driver. - With the above calculation the MS4525 sensor will produce a positive number when the top port is used as a dynamic port and bottom port is used as the static port @@ -248,15 +237,14 @@ MEASAirspeed::collect() struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ - if (diff_press_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_press_pa; + if (diff_press_pa_raw > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_press_pa_raw; } report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; - report.differential_pressure_pa = diff_press_pa; - report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); report.differential_pressure_raw_pa = diff_press_pa_raw; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -514,7 +502,7 @@ test() } warnx("single read"); - warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -542,7 +530,7 @@ test() } warnx("periodic read %u", i); - warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f40034d79c..cdcb428dd1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1229,16 +1229,18 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) if (updated) { orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); - raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; + raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa; raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; - _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa); - _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius); + + /* don't risk to feed negative airspeed into the system */ + _airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); + _airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ @@ -1457,12 +1459,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; - float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f; _diff_pres.timestamp = t; - _diff_pres.differential_pressure_pa = diff_pres_pa; _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; - _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f); + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f); _diff_pres.temperature = -1000.0f; /* announce the airspeed if needed, just publish else */ diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index cd48d3cb2f..7342fcf04f 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -54,7 +54,6 @@ struct differential_pressure_s { uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */ uint64_t error_count; /**< Number of errors detected by driver */ - float differential_pressure_pa; /**< Differential pressure reading */ float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ From bf8956d2e87b29af25392b74a24ea0da8d422d4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 13:26:28 +0200 Subject: [PATCH 566/600] Be only reasonably strict on avionics supply voltage. --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f8589d24b8..c9fc9218c1 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -183,9 +183,9 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Fail transition if power levels on the avionics rail // are measured but are insufficient if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.9f))) { + (status->avionics_power_rail_voltage < 4.75f))) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; } From 64d3c48770860586d4755b1d2e865e607af4b25e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 13:32:46 +0200 Subject: [PATCH 567/600] Add warning for non-standard avionics rail voltages --- .../commander/state_machine_helper.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c9fc9218c1..684c61a173 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -182,12 +182,19 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Fail transition if power levels on the avionics rail // are measured but are insufficient - if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.75f))) { - - mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - valid_transition = false; + if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { + // Check avionics rail voltages + if (status->avionics_power_rail_voltage < 4.75f) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + valid_transition = false; + } else if (status->avionics_power_rail_voltage < 4.9f) { + mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + } else if (status->avionics_power_rail_voltage > 5.4f) { + mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + } } } From 2418969b8705a859e49480c338770fc476cd0c24 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 15:35:02 +0400 Subject: [PATCH 568/600] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 6c070852d7..5bfa1999f4 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 6c070852d76b18c2d57612f66b5bd00e63118c94 +Subproject commit 5bfa1999f41e4983a947efa0029efd2de6873beb From 163224eda20657b867142df380cb7e5311c82e97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 13:37:00 +0200 Subject: [PATCH 569/600] Updated MAVLink --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 4d7487c2bc..2423e47b4f 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50 +Subproject commit 2423e47b4f9169e77f7194b36fa2118e018c94e2 From 1a582e8be0993dadb3b1a70d8ccab8ed3ededfce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 16:36:35 +0400 Subject: [PATCH 570/600] UAVCAN update for #1306 --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 5bfa1999f4..75153eb643 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 5bfa1999f41e4983a947efa0029efd2de6873beb +Subproject commit 75153eb6436d0cc00679056ff8e916c6d99057ad From 04ccf5d8c955d1074bede184eeb35ebed897b55a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 19:44:54 +0400 Subject: [PATCH 571/600] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 75153eb643..c7872def16 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 75153eb6436d0cc00679056ff8e916c6d99057ad +Subproject commit c7872def16e82a8b318d571829fe9622e2d35ff0 From 2f5c0cbd133deb492102ea8515563f471531acce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 23:05:28 +0200 Subject: [PATCH 572/600] Deal with zero airspeed measurements --- .../fw_att_control/fw_att_control_main.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 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 0cea13cc4d..ad873203eb 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -145,6 +145,7 @@ private: perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ struct { float tconst; @@ -324,7 +325,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ - _setpoint_valid(false) + _setpoint_valid(false), + _debug(false) { /* safely initialize structs */ _att = {}; @@ -700,7 +702,8 @@ FixedwingAttitudeControl::task_main() perf_count(_nonfinite_input_perf); } } else { - airspeed = _airspeed.true_airspeed_m_s; + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); } /* @@ -785,7 +788,7 @@ FixedwingAttitudeControl::task_main() speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } } @@ -808,7 +811,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("roll_u %.4f", (double)roll_u); } } @@ -821,7 +824,7 @@ FixedwingAttitudeControl::task_main() if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," " roll_sp %.4f, pitch_sp %.4f," @@ -845,7 +848,7 @@ FixedwingAttitudeControl::task_main() if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } } @@ -853,13 +856,13 @@ FixedwingAttitudeControl::task_main() /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } } else { perf_count(_nonfinite_input_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } } From b150c3092c2bfe532618678b91c1eab42095c486 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Aug 2014 09:33:39 +0200 Subject: [PATCH 573/600] mavlink_main: raise rates of onboard mode --- src/modules/mavlink/mavlink_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ed7e879d3d..93f4fec92e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1401,9 +1401,9 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f); - configure_stream("GLOBAL_POSITION_INT", 15.0f); - configure_stream("CAMERA_CAPTURE", 1.0f); + configure_stream("ATTITUDE", 50.0f); + configure_stream("GLOBAL_POSITION_INT", 50.0f); + configure_stream("CAMERA_CAPTURE", 2.0f); break; default: From 9825ed8f3cd037b8fd131a911ba350a29203ef0c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Aug 2014 10:21:26 +0200 Subject: [PATCH 574/600] Attempt at fixing programming timeouts --- Tools/px_uploader.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d8f4884bc0..b46db00b57 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -178,9 +178,9 @@ class uploader(object): MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0') MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac') - def __init__(self, portname, baudrate): + def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5): # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate, timeout=2.0) + self.port = serial.Serial(portname, baudrate) self.otp = b'' self.sn = b'' @@ -195,7 +195,7 @@ class uploader(object): def __recv(self, count=1): c = self.port.read(count) if len(c) < 1: - raise RuntimeError("timeout waiting for data") + raise RuntimeError("timeout waiting for data (%u bytes)", count) # print("recv " + binascii.hexlify(c)) return c From 73ecbbe13d1f231bf2a9c2ccaafe29065352d75c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Aug 2014 11:12:01 +0200 Subject: [PATCH 575/600] config_px4fmu-v2_default: include px4flow driver by default --- makefiles/config_px4fmu-v2_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d0a40445d6..5f146686c8 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -41,6 +41,7 @@ MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl MODULES += drivers/pca8574 +MODULES += drivers/px4flow # Needs to be burned to the ground and re-written; for now, From 60799e51558e6259a7a2d20d765a4f8d29b88cc5 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 11:07:30 +0100 Subject: [PATCH 576/600] sdlog2: add vision log struct --- src/modules/sdlog2/sdlog2_messages.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fb7609435b..82d83b5c1e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -391,6 +391,16 @@ struct log_TEL_s { uint64_t heartbeat_time; }; +/* --- VISN - VISION POSITION --- */ +#define LOG_VISN_MSG 38 +struct log_VISN_s { + float x; + float y; + float z; + float roll; + float pitch; + float yaw; +}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -449,6 +459,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(VISN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), From ec8438bdcab31c566f91869140505b506a4fcaf8 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 11:20:55 +0100 Subject: [PATCH 577/600] sdlog2: added vision estimate logging --- src/modules/sdlog2/sdlog2.c | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6c4b494522..59765415a6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -934,6 +934,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; + struct vehicle_vision_position_s vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -984,6 +985,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; + struct log_VISN_s log_VISN; struct log_GS0A_s log_GS0A; struct log_GS0B_s log_GS0B; struct log_GS1A_s log_GS1A; @@ -1013,6 +1015,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int gps_pos_sub; int sat_info_sub; int vicon_pos_sub; + int vision_pos_sub; int flow_sub; int rc_sub; int airspeed_sub; @@ -1043,6 +1046,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + subs.vision_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position)); subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); @@ -1459,6 +1463,18 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; LOGBUFFER_WRITE_AND_COUNT(VICN); } + + /* --- VISION POSITION --- */ + if (copy_if_updated(ORB_ID(vehicle_vision_position), subs.vision_pos_sub, &buf.vision_pos)) { + log_msg.msg_type = LOG_VISN_MSG; + log_msg.body.log_VISN.x = buf.vision_pos.x; + log_msg.body.log_VISN.y = buf.vision_pos.y; + log_msg.body.log_VISN.z = buf.vision_pos.z; + log_msg.body.log_VISN.pitch = buf.vision_pos.pitch; + log_msg.body.log_VISN.roll = buf.vision_pos.roll; + log_msg.body.log_VISN.yaw = buf.vision_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VISN); + } /* --- FLOW --- */ if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { @@ -1565,14 +1581,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - /* --- BOTTOM DISTANCE --- */ - if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { - log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.range_finder.distance; - log_msg.body.log_DIST.bottom_rate = 0.0f; - log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); - LOGBUFFER_WRITE_AND_COUNT(DIST); - } + /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { From d0f5eca5be3d3257b1350337b58f020063b65eb9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Aug 2014 13:13:07 +0200 Subject: [PATCH 578/600] px4flow: removed flow report in driver, just use uORB topic --- src/drivers/drv_px4flow.h | 31 ------------- src/drivers/px4flow/px4flow.cpp | 68 ++++++++++++++-------------- src/modules/mavlink/mavlink_main.cpp | 2 +- 3 files changed, 35 insertions(+), 66 deletions(-) diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index 76ec55c3e8..ab640837bb 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -46,37 +46,6 @@ #define PX4FLOW_DEVICE_PATH "/dev/px4flow" -/** - * @addtogroup topics - * @{ - */ - -/** - * Optical flow in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - * - * @warning If possible the usage of the raw flow and performing rotation-compensation - * using the autopilot angular rate estimate is recommended. - */ -struct px4flow_report { - - uint64_t timestamp; /**< in microseconds since system start */ - - 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 per second, rotation-compensated */ - float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ - uint8_t sensor_id; /**< id of the sensor emitting the flow value */ - -}; - -/** - * @} - */ - /* * ObjDev tag for px4flow data. */ diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index f214b5964f..60ad3c1aff 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -37,7 +37,7 @@ * * Driver for the PX4FLOW module connected via I2C. */ - + #include #include @@ -68,7 +68,7 @@ #include #include -//#include +#include #include @@ -80,7 +80,7 @@ /* PX4FLOW Registers addresses */ #define PX4FLOW_REG 0x00 /* Measure Register */ -#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz +#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -115,17 +115,17 @@ class PX4FLOW : public device::I2C public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); - + virtual int init(); - + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - + /** * Diagnostics - print some basic information about the driver. */ void print_info(); - + protected: virtual int probe(); @@ -136,13 +136,13 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - + orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - + /** * Test whether the device supported by the driver is present at a * specific address. @@ -151,7 +151,7 @@ private: * @return True if the device is present. */ int probe_address(uint8_t address); - + /** * Initialise the automatic measurement state machine and start it. * @@ -159,12 +159,12 @@ private: * to make it more aggressive about resetting the bus in case of errors. */ void start(); - + /** * Stop the automatic measurement state machine. */ void stop(); - + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -179,8 +179,8 @@ private: * @param arg Instance pointer for the driver that is polling. */ static void cycle_trampoline(void *arg); - - + + }; /* @@ -201,7 +201,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : { // enable debug() calls _debug_enabled = true; - + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -226,13 +226,13 @@ PX4FLOW::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(px4flow_report)); + _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); if (_reports == nullptr) goto out; /* get a publish handle on the px4flow topic */ - struct px4flow_report zero_report; + struct optical_flow_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); @@ -323,24 +323,24 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) return -EINVAL; - + irqstate_t flags = irqsave(); if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); - + return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); - + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; - + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -350,8 +350,8 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct px4flow_report); - struct px4flow_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct optical_flow_s); + struct optical_flow_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -425,7 +425,7 @@ PX4FLOW::measure() return ret; } ret = OK; - + return ret; } @@ -433,14 +433,14 @@ int PX4FLOW::collect() { int ret = -EIO; - + /* read from the sensor */ uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; - + perf_begin(_sample_perf); - + ret = transfer(nullptr, 0, &val[0], 22); - + if (ret < 0) { log("error reading from sensor: %d", ret); @@ -448,7 +448,7 @@ PX4FLOW::collect() perf_end(_sample_perf); return ret; } - + // f.frame_count = val[1] << 8 | val[0]; // f.pixel_flow_x_sum= val[3] << 8 | val[2]; // f.pixel_flow_y_sum= val[5] << 8 | val[4]; @@ -466,7 +466,7 @@ PX4FLOW::collect() int16_t flowcy = val[9] << 8 | val[8]; int16_t gdist = val[21] << 8 | val[20]; - struct px4flow_report report; + struct optical_flow_s report; report.flow_comp_x_m = float(flowcx)/1000.0f; report.flow_comp_y_m = float(flowcy)/1000.0f; report.flow_raw_x= val[3] << 8 | val[2]; @@ -503,7 +503,7 @@ PX4FLOW::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); - + /* notify about state change */ struct subsystem_info_s info = { true, @@ -644,7 +644,7 @@ start() fail: - if (g_dev != nullptr) + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; @@ -678,7 +678,7 @@ void stop() void test() { - struct px4flow_report report; + struct optical_flow_s report; ssize_t sz; int ret; @@ -777,7 +777,7 @@ px4flow_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "start")) px4flow::start(); - + /* * Stop the driver */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 93f4fec92e..940e641445 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1396,7 +1396,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW", 0.5f); + configure_stream("OPTICAL_FLOW", 20.0f); break; case MAVLINK_MODE_ONBOARD: From 0994006f96f2c030acd31bb14b32d2fd2127c6dd Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 14:16:13 +0100 Subject: [PATCH 579/600] sdlog2: update vision log fields --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++------ src/modules/sdlog2/sdlog2_messages.h | 12 ++++++++---- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 59765415a6..ec4197e79f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -934,7 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; - struct vehicle_vision_position_s vision_pos; + struct vision_position_estimate_s vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -1046,7 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - subs.vision_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position)); + subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate)); subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); @@ -1465,14 +1466,18 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- VISION POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_vision_position), subs.vision_pos_sub, &buf.vision_pos)) { + if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) { log_msg.msg_type = LOG_VISN_MSG; log_msg.body.log_VISN.x = buf.vision_pos.x; log_msg.body.log_VISN.y = buf.vision_pos.y; log_msg.body.log_VISN.z = buf.vision_pos.z; - log_msg.body.log_VISN.pitch = buf.vision_pos.pitch; - log_msg.body.log_VISN.roll = buf.vision_pos.roll; - log_msg.body.log_VISN.yaw = buf.vision_pos.yaw; + log_msg.body.log_VISN.vx = buf.vision_pos.vx; + log_msg.body.log_VISN.vy = buf.vision_pos.vy; + log_msg.body.log_VISN.vz = buf.vision_pos.vz; + log_msg.body.log_VISN.qx = buf.vision_pos.q[0]; + log_msg.body.log_VISN.qy = buf.vision_pos.q[1]; + log_msg.body.log_VISN.qz = buf.vision_pos.q[2]; + log_msg.body.log_VISN.qw = buf.vision_pos.q[3]; LOGBUFFER_WRITE_AND_COUNT(VISN); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 82d83b5c1e..6741c7e258 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -397,9 +397,13 @@ struct log_VISN_s { float x; float y; float z; - float roll; - float pitch; - float yaw; + float vx; + float vy; + float vz; + float qx; + float qy; + float qz; + float qw; }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -459,7 +463,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), - LOG_FORMAT(VISN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), From 3ef374c4264969928ce958ff3053f6238909479b Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 14:20:17 +0100 Subject: [PATCH 580/600] sdlog2: added BOTTOM_DISTANCE again --- src/modules/sdlog2/sdlog2.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec4197e79f..02cc39dfc0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1586,7 +1586,14 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - + /* --- BOTTOM DISTANCE --- */ +- if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { +- log_msg.msg_type = LOG_DIST_MSG; +- log_msg.body.log_DIST.bottom = buf.range_finder.distance; +- log_msg.body.log_DIST.bottom_rate = 0.0f; +- log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); +- LOGBUFFER_WRITE_AND_COUNT(DIST); +- } /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { From ebd56aa2c12c3c55e4eb349429a765355afe6360 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 15:07:14 +0100 Subject: [PATCH 581/600] sdlog2: minor corrections --- src/modules/sdlog2/sdlog2.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 02cc39dfc0..07655808c0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -935,7 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; - struct vision_position_estimate_s vision_pos; + struct vision_position_estimate vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -1587,13 +1587,13 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- BOTTOM DISTANCE --- */ -- if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { -- log_msg.msg_type = LOG_DIST_MSG; -- log_msg.body.log_DIST.bottom = buf.range_finder.distance; -- log_msg.body.log_DIST.bottom_rate = 0.0f; -- log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); -- LOGBUFFER_WRITE_AND_COUNT(DIST); -- } + if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { + log_msg.msg_type = LOG_DIST_MSG; + log_msg.body.log_DIST.bottom = buf.range_finder.distance; + log_msg.body.log_DIST.bottom_rate = 0.0f; + log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); + LOGBUFFER_WRITE_AND_COUNT(DIST); + } /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { From d6810ae1f8e2b8c86d54acfe80094265e97a3232 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 15:37:51 +0100 Subject: [PATCH 582/600] sdlog2: minor improvements --- src/modules/sdlog2/sdlog2.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 07655808c0..dbda8ea6f3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1589,11 +1589,11 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- BOTTOM DISTANCE --- */ if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.range_finder.distance; + log_msg.body.log_DIST.bottom = buf.range_finder.distance; log_msg.body.log_DIST.bottom_rate = 0.0f; log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); - LOGBUFFER_WRITE_AND_COUNT(DIST); - } + LOGBUFFER_WRITE_AND_COUNT(DIST); + } /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { From eab701b896fa316132aff78a34362ca77549e581 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 Aug 2014 00:50:19 +0400 Subject: [PATCH 583/600] Improved UAVCAN status reporting --- src/modules/uavcan/sensors/gnss.cpp | 10 ++++++ src/modules/uavcan/sensors/gnss.hpp | 2 ++ src/modules/uavcan/sensors/sensor_bridge.cpp | 36 ++++++++++++++------ src/modules/uavcan/sensors/sensor_bridge.hpp | 15 +++++--- src/modules/uavcan/uavcan_main.cpp | 10 +++--- 5 files changed, 55 insertions(+), 18 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 8548660fe1..0d67aad476 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -70,6 +70,16 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const return (_receiver_node_id < 0) ? 0 : 1; } +void UavcanGnssBridge::print_status() const +{ + printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount()); + if (_receiver_node_id < 0) { + printf("N/A\n"); + } else { + printf("%d\n", _receiver_node_id); + } +} + void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { // This bridge does not support redundant GNSS receivers yet. diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index c2b6e41956..e8466b4010 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -66,6 +66,8 @@ public: unsigned get_num_redundant_channels() const override; + void print_status() const override; + private: /** * GNSS fix message will be reported via this callback. diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index a98596f9c5..9608ce6804 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -58,7 +58,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List= 0) { + if (_channels[i].node_id >= 0) { (void)unregister_class_devname(_class_devname, _channels[i].class_instance); } } @@ -66,13 +66,15 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() delete [] _channels; } -void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report) +void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) { + assert(report != nullptr); + Channel *channel = nullptr; // Checking if such channel already exists for (unsigned i = 0; i < _max_channels; i++) { - if (_channels[i].redunancy_channel_id == redundancy_channel_id) { + if (_channels[i].node_id == node_id) { channel = _channels + i; break; } @@ -84,11 +86,11 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const return; // Give up immediately - saves some CPU time } - log("adding channel %d...", redundancy_channel_id); + log("adding channel %d...", node_id); // Search for the first free channel for (unsigned i = 0; i < _max_channels; i++) { - if (_channels[i].redunancy_channel_id < 0) { + if (_channels[i].node_id < 0) { channel = _channels + i; break; } @@ -111,9 +113,9 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const } // Publish to the appropriate topic, abort on failure - channel->orb_id = _orb_topics[class_instance]; - channel->redunancy_channel_id = redundancy_channel_id; - channel->class_instance = class_instance; + channel->orb_id = _orb_topics[class_instance]; + channel->node_id = node_id; + channel->class_instance = class_instance; channel->orb_advert = orb_advertise(channel->orb_id, report); if (channel->orb_advert < 0) { @@ -123,7 +125,7 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const return; } - log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance); + log("channel %d class instance %d ok", channel->node_id, channel->class_instance); } assert(channel != nullptr); @@ -134,9 +136,23 @@ unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const { unsigned out = 0; for (unsigned i = 0; i < _max_channels; i++) { - if (_channels[i].redunancy_channel_id >= 0) { + if (_channels[i].node_id >= 0) { out += 1; } } return out; } + +void UavcanCDevSensorBridgeBase::print_status() const +{ + printf("devname: %s\n", _class_devname); + + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id >= 0) { + printf("channel %d: node id %d --> class instance %d\n", + i, _channels[i].node_id, _channels[i].class_instance); + } else { + printf("channel %d: empty\n", i); + } + } +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index a13d0ab35d..1316f7ecc5 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -68,6 +68,11 @@ public: */ virtual unsigned get_num_redundant_channels() const = 0; + /** + * Prints current status in a human readable format to stdout. + */ + virtual void print_status() const = 0; + /** * Sensor bridge factory. * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". @@ -84,7 +89,7 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD { struct Channel { - int redunancy_channel_id = -1; + int node_id = -1; orb_id_t orb_id = nullptr; orb_advert_t orb_advert = -1; int class_instance = -1; @@ -112,13 +117,15 @@ protected: /** * Sends one measurement into appropriate ORB topic. * New redundancy channels will be registered automatically. - * @param redundancy_channel_id Redundant sensor identifier (e.g. UAVCAN Node ID) - * @param report ORB message object + * @param node_id Sensor's Node ID + * @param report Pointer to ORB message object */ - void publish(const int redundancy_channel_id, const void *report); + void publish(const int node_id, const void *report); public: virtual ~UavcanCDevSensorBridgeBase(); unsigned get_num_redundant_channels() const override; + + void print_status() const override; }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 482fec1e0a..95c6ba13e0 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -548,14 +548,16 @@ UavcanNode::print_info() (void)pthread_mutex_lock(&_node_mutex); // ESC mixer status - warnx("ESC actuators control groups: sub: %u / req: %u / fds: %u", - (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("ESC mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); + printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", + (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK"); // Sensor bridges auto br = _sensor_bridges.getHead(); while (br != nullptr) { - warnx("Sensor '%s': channels: %u", br->get_name(), br->get_num_redundant_channels()); + printf("Sensor '%s':\n", br->get_name()); + br->print_status(); + printf("\n"); br = br->getSibling(); } From a54ef70a207cd892a9ef406df6f1aa0732035537 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Aug 2014 08:14:52 +0200 Subject: [PATCH 584/600] Decrease I2C timeout in config so it matches the previous 500 us timeout as close as possible. This is necessary after fixing the NuttX I2C timeout logic --- nuttx-configs/px4fmu-v1/nsh/defconfig | 3 +-- nuttx-configs/px4fmu-v2/nsh/defconfig | 3 +-- nuttx-configs/px4io-v1/nsh/defconfig | 2 ++ 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index d7697cd673..cd9c05b136 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -287,8 +287,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=500 +CONFIG_STM32_I2CTIMEOMS=1 # CONFIG_STM32_I2C_DUTY16_9 is not set # diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index bd956f7bf5..e31f40cd2a 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -323,8 +323,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=500 +CONFIG_STM32_I2CTIMEOMS=1 # CONFIG_STM32_I2C_DUTY16_9 is not set # diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig index 512c6a0f23..7c76be7ec0 100755 --- a/nuttx-configs/px4io-v1/nsh/defconfig +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -133,6 +133,8 @@ CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y CONFIG_STM32_I2C1=y CONFIG_STM32_I2C2=n +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=1 CONFIG_STM32_BKP=n CONFIG_STM32_PWR=n CONFIG_STM32_DAC=n From 65dab36910ed44acbfd589c52bfe8d308f0199e5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Aug 2014 10:13:52 +0200 Subject: [PATCH 585/600] Improve startup and payload handling --- ROMFS/px4fmu_common/init.d/3035_viper | 2 ++ ROMFS/px4fmu_common/init.d/rc.interface | 5 +++++ ROMFS/px4fmu_common/init.d/rcS | 9 ++++----- ROMFS/px4fmu_common/mixers/FMU_AERT.mix | 11 ++++++----- ROMFS/px4fmu_common/mixers/Viper.mix | 13 ++++++------- 5 files changed, 23 insertions(+), 17 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index a4c1e832dc..3714b612fc 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -8,3 +8,5 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper + +set FAILSAFE "-c567 -p 1000" diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 1de0abb58d..e44cd0953d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -77,4 +77,9 @@ then pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi + + if [ $FAILSAFE != none ] + then + pwm failsafe -d $OUTPUT_DEV $FAILSAFE + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1957719057..ea04ece34d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -66,6 +66,9 @@ then # sercon + # Try to get an USB console + nshterm /dev/ttyACM0 & + # # Start the ORB (first app to start) # @@ -96,11 +99,9 @@ then # if rgbled start then - echo "[init] RGB Led" else if blinkm start then - echo "[init] BlinkM" blinkm systemstate fi fi @@ -129,6 +130,7 @@ then set LOAD_DEFAULT_APPS yes set GPS yes set GPS_FAKE no + set FAILSAFE none # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -279,9 +281,6 @@ then fi fi - # Try to get an USB console - nshterm /dev/ttyACM0 & - # # Start the datamanager (and do not abort boot if it fails) # diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix index 0ec663e35e..7fed488af1 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix @@ -64,21 +64,22 @@ O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Gimbal / flaps / payload mixer for last four channels, +using the payload control group ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 +S: 2 3 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix index 79c4447bee..5a0381bd87 100755 --- a/ROMFS/px4fmu_common/mixers/Viper.mix +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -52,21 +52,20 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Inputs to the mixer come from channel group 2 (payload), channels 0 +(bay servo 1), 1 (bay servo 2) and 3 (drop release). ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 2 2 -10000 -10000 0 -10000 10000 + -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 From 4af4e4e1e5d7209819c1fb43508ddd1ebed4f9c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Aug 2014 10:14:36 +0200 Subject: [PATCH 586/600] Support additional payload commands and let commander ignore them --- src/modules/commander/commander.cpp | 5 +++++ src/modules/uORB/topics/vehicle_command.h | 19 ++++++++++++------- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 74deda8cca..a5a7728252 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -615,6 +615,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case VEHICLE_CMD_PREFLIGHT_STORAGE: + case VEHICLE_CMD_CUSTOM_0: + case VEHICLE_CMD_CUSTOM_1: + case VEHICLE_CMD_CUSTOM_2: + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: /* ignore commands that handled in low prio loop */ break; diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 7db33d98b3..44aa505721 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author 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 @@ -37,6 +34,10 @@ /** * @file vehicle_command.h * Definition of the vehicle command uORB topic. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier */ #ifndef TOPIC_VEHICLE_COMMAND_H_ @@ -52,6 +53,9 @@ * but can contain additional ones. */ enum VEHICLE_CMD { + VEHICLE_CMD_CUSTOM_0 = 0, /* test command */ + VEHICLE_CMD_CUSTOM_1 = 1, /* test command */ + VEHICLE_CMD_CUSTOM_2 = 2, /* test command */ VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ @@ -87,7 +91,8 @@ enum VEHICLE_CMD { VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - VEHICLE_CMD_ENUM_END = 501, /* | */ + VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */ + VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */ }; /** @@ -115,8 +120,8 @@ struct vehicle_command_s { float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */ - float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ - float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ + double param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ + double param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ uint8_t target_system; /**< System which should execute the command */ From 87b2375be436aa92d361e0131be9ff63ae43fe36 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 14:34:14 +0200 Subject: [PATCH 587/600] Ignore single channels during PWM output --- src/drivers/drv_pwm_output.h | 5 +++++ src/drivers/px4fmu/fmu.cpp | 4 +++- src/modules/px4iofirmware/registers.c | 4 +++- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 84815fdfbb..5aff6825be 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -94,6 +94,11 @@ __BEGIN_DECLS */ #define PWM_LOWEST_MAX 1700 +/** + * Do not output a channel with this value + */ +#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX + /** * Servo output signal type, value is actual servo output pulse * width in microseconds. diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 82977a032f..122a3cd174 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1272,7 +1272,9 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) memcpy(values, buffer, count * 2); for (uint8_t i = 0; i < count; i++) { - up_pwm_servo_set(i, values[i]); + if (values[i] != PWM_IGNORE_THIS_CHANNEL) { + up_pwm_servo_set(i, values[i]); + } } return count * 2; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 43161aa70e..0da778b6f6 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -285,7 +285,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { /* XXX range-check value? */ - r_page_servos[offset] = *values; + if (*values != PWM_IGNORE_THIS_CHANNEL) { + r_page_servos[offset] = *values; + } offset++; num_values--; From b928897ab525a79eb2fad202fc28ef0235adeb50 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 07:58:36 +0200 Subject: [PATCH 588/600] mavlink: code style only fix --- src/modules/mavlink/mavlink_messages.cpp | 26 ++++++++++++------------ 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 5a92004a61..a2f3828ffa 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -886,8 +886,8 @@ protected: msg.eph = cm_uint16_from_m_float(gps.eph); msg.epv = cm_uint16_from_m_float(gps.epv); msg.vel = cm_uint16_from_m_float(gps.vel_m_s), - msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - msg.satellites_visible = gps.satellites_used; + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); } @@ -957,11 +957,11 @@ protected: msg.lat = pos.lat * 1e7; msg.lon = pos.lon * 1e7; msg.alt = pos.alt * 1000.0f; - msg.relative_alt = (pos.alt - home.alt) * 1000.0f; - msg.vx = pos.vel_n * 100.0f; - msg.vy = pos.vel_e * 100.0f; - msg.vz = pos.vel_d * 100.0f; - msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; + msg.relative_alt = (pos.alt - home.alt) * 1000.0f; + msg.vx = pos.vel_n * 100.0f; + msg.vy = pos.vel_e * 100.0f; + msg.vz = pos.vel_d * 100.0f; + msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); } @@ -1022,9 +1022,9 @@ protected: msg.x = pos.x; msg.y = pos.y; msg.z = pos.z; - msg.vx = pos.vx; - msg.vy = pos.vy; - msg.vz = pos.vz; + msg.vx = pos.vx; + msg.vy = pos.vy; + msg.vz = pos.vz; _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); } @@ -1085,9 +1085,9 @@ protected: msg.x = pos.x; msg.y = pos.y; msg.z = pos.z; - msg.roll = pos.roll; - msg.pitch = pos.pitch; - msg.yaw = pos.yaw; + msg.roll = pos.roll; + msg.pitch = pos.pitch; + msg.yaw = pos.yaw; _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); } From 0eea110f6fb4e95238a496f3a71b1cb6741625f7 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 27 Aug 2014 17:14:49 -0700 Subject: [PATCH 589/600] Modified to use new FILE_TRANSFER_PROTOCOL message - Also corrected system/component id transmit/check --- src/modules/mavlink/mavlink_ftp.cpp | 4 +- src/modules/mavlink/mavlink_ftp.h | 49 +++++++++++++----------- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 30 insertions(+), 25 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 00c8df18c5..f4a7bc1012 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -114,7 +114,7 @@ MavlinkFTP::_worker(Request *req) uint32_t messageCRC; // basic sanity checks; must validate length before use - if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) { + if (hdr->size > kMaxDataLength) { errorCode = kErrNoRequest; goto out; } @@ -199,7 +199,7 @@ MavlinkFTP::_reply(Request *req) { auto hdr = req->header(); - hdr->magic = kProtocolMagic; + hdr->seqNumber = req->header()->seqNumber + 1; // message is assumed to be already constructed in the request buffer, so generate the CRC hdr->crc32 = 0; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 43de89de91..cf9ea0c071 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -38,9 +38,6 @@ * * MAVLink remote file server. * - * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes - * a session ID and sequence number. - * * A limited number of requests (currently 2) may be outstanding at a time. * Additional messages will be discarded. * @@ -74,16 +71,18 @@ private: static MavlinkFTP *_server; + /// @brief This structure is sent in the payload portion of the file transfer protocol mavlink message. + /// This structure is layed out such that it should not require any special compiler packing to not consume extra space. struct RequestHeader - { - uint8_t magic; - uint8_t session; - uint8_t opcode; - uint8_t size; - uint32_t crc32; - uint32_t offset; + { + uint16_t seqNumber; ///< sequence number for message + unsigned int session:4; ///< Session id for read and write commands + unsigned int opcode:4; ///< Command opcode + uint8_t size; ///< Size of data + uint32_t crc32; ///< CRC for entire Request structure, with crc32 set to 0 + uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; - }; + }; enum Opcode : uint8_t { @@ -131,10 +130,11 @@ private: }; bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { - if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) { + if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + _systemId = fromMessage->sysid; _mavlink = mavlink; - mavlink_msg_encapsulated_data_decode(fromMessage, &_message); - return true; + mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message); + return _message.target_system == _mavlink->get_system_id(); } return false; } @@ -145,8 +145,14 @@ private: // flat memory architecture, as we're operating between threads here. mavlink_message_t msg; msg.checksum = 0; - unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), - _mavlink->get_channel(), &msg, sequence()+1, rawData()); + unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id + _mavlink->get_component_id(), // Sender component id + _mavlink->get_channel(), // Channel to send on + &msg, // Message to pack payload into + 0, // Target network + _systemId, // Target system id + 0, // Target component id + rawData()); // Payload to pack into message _mavlink->lockMessageBufferMutex(); bool success = _mavlink->message_buffer_write(&msg, len); @@ -167,26 +173,25 @@ private: #endif } - uint8_t *rawData() { return &_message.data[0]; } - RequestHeader *header() { return reinterpret_cast(&_message.data[0]); } + uint8_t *rawData() { return &_message.payload[0]; } + RequestHeader *header() { return reinterpret_cast(&_message.payload[0]); } uint8_t *requestData() { return &(header()->data[0]); } unsigned dataSize() { return header()->size + sizeof(RequestHeader); } - uint16_t sequence() const { return _message.seqnr; } mavlink_channel_t channel() { return _mavlink->get_channel(); } char *dataAsCString(); private: Mavlink *_mavlink; - mavlink_encapsulated_data_t _message; + mavlink_file_transfer_protocol_t _message; + uint8_t _systemId; }; - static const uint8_t kProtocolMagic = 'f'; static const char kDirentFile = 'F'; static const char kDirentDir = 'D'; static const char kDirentUnknown = 'U'; - static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader); + static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader); /// Request worker; runs on the low-priority work queue to service /// remote requests. diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a602344fd2..bed1bd7898 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -172,7 +172,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_request_data_stream(msg); break; - case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: MavlinkFTP::getServer()->handle_message(_mavlink, msg); break; From fce0a3b728f0aca12f9afb678f36cacde865e976 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 27 Aug 2014 21:39:55 -0700 Subject: [PATCH 590/600] Gave up on using bitfields --- src/modules/mavlink/mavlink_ftp.cpp | 6 ++++++ src/modules/mavlink/mavlink_ftp.h | 17 +++++++++-------- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index f4a7bc1012..5b65dc369d 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -122,6 +122,9 @@ MavlinkFTP::_worker(Request *req) // check request CRC to make sure this is one of ours messageCRC = hdr->crc32; hdr->crc32 = 0; + hdr->padding[0] = 0; + hdr->padding[1] = 0; + hdr->padding[2] = 0; if (crc32(req->rawData(), req->dataSize()) != messageCRC) { errorCode = kErrNoRequest; goto out; @@ -203,6 +206,9 @@ MavlinkFTP::_reply(Request *req) // message is assumed to be already constructed in the request buffer, so generate the CRC hdr->crc32 = 0; + hdr->padding[0] = 0; + hdr->padding[1] = 0; + hdr->padding[2] = 0; hdr->crc32 = crc32(req->rawData(), req->dataSize()); // then pack and send the reply back to the request source diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index cf9ea0c071..1dd8f102e7 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -71,16 +71,17 @@ private: static MavlinkFTP *_server; - /// @brief This structure is sent in the payload portion of the file transfer protocol mavlink message. - /// This structure is layed out such that it should not require any special compiler packing to not consume extra space. + /// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the + /// structure ourselves to 32 bit alignment which should get us what we want. struct RequestHeader { - uint16_t seqNumber; ///< sequence number for message - unsigned int session:4; ///< Session id for read and write commands - unsigned int opcode:4; ///< Command opcode - uint8_t size; ///< Size of data - uint32_t crc32; ///< CRC for entire Request structure, with crc32 set to 0 - uint32_t offset; ///< Offsets for List and Read commands + uint16_t seqNumber; ///< sequence number for message + uint8_t session; ///< Session id for read and write commands + uint8_t opcode; ///< Command opcode + uint8_t size; ///< Size of data + uint8_t padding[3]; + uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 + uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; }; From a87a2644156e1fc105d151887489cb8eba8f881b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 Aug 2014 03:19:49 +0400 Subject: [PATCH 591/600] UAVCAN update (lots of warning fixes) --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index c7872def16..c4c14c60fb 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit c7872def16e82a8b318d571829fe9622e2d35ff0 +Subproject commit c4c14c60fbbd9acd281ee97d5bb2a4027d0ae2d9 From 580b726536bee9b4383e984ab40352a0c67ab850 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 Aug 2014 14:34:29 +0400 Subject: [PATCH 592/600] UAVCAN: missing declaration warning fix --- src/modules/uavcan/uavcan_clock.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp index e41d5f953b..fe8ba406a5 100644 --- a/src/modules/uavcan/uavcan_clock.cpp +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -62,6 +62,8 @@ void adjustUtc(uavcan::UtcDuration adjustment) (void)adjustment; } +uavcan::uint64_t getUtcUSecFromCanInterrupt(); + uavcan::uint64_t getUtcUSecFromCanInterrupt() { return 0; From ab022d51338c30207379048c913715a0b8d601c3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 31 Aug 2014 16:23:37 +0200 Subject: [PATCH 593/600] disable underspeed protection when landing also in tecs --- src/lib/external_lgpl/tecs/tecs.cpp | 5 +++++ src/lib/external_lgpl/tecs/tecs.h | 10 ++++++++++ .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 +++ 3 files changed, 18 insertions(+) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a57a0481a6..d27bf776fb 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -252,6 +252,11 @@ void TECS::_update_height_demand(float demand, float state) void TECS::_detect_underspeed(void) { + if (!_detect_underspeed_enabled) { + _underspeed = false; + return; + } + if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) { _underspeed = true; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index bcc2d90e5c..36ae4ecaf2 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -66,6 +66,9 @@ public: _hgt_dem_prev(0.0f), _TAS_dem_adj(0.0f), _STEdotErrLast(0.0f), + _underspeed(false), + _detect_underspeed_enabled(true), + _badDescent(false), _climbOutDem(false), _SPE_dem(0.0f), _SKE_dem(0.0f), @@ -221,6 +224,10 @@ public: _speedrate_p = speedrate_p; } + void set_detect_underspeed_enabled(bool enabled) { + _detect_underspeed_enabled = enabled; + } + private: struct tecs_state _tecs_state; @@ -323,6 +330,9 @@ private: // Underspeed condition bool _underspeed; + // Underspeed detection enabled + bool _detect_underspeed_enabled; + // Bad descent condition caused by unachievable airspeed demand bool _badDescent; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 350ce6dec1..522f5caca0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1380,6 +1380,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, limitOverride); } else { + /* No underspeed protection in landing mode */ + _tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM)); + /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, From 0e954b30c2ef3e31d60e5ef7a5fa985882baef8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 17:58:43 +0200 Subject: [PATCH 594/600] Rely on laser altitude once valid --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 522f5caca0..c983ac6f9c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -823,7 +823,7 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, * the measurement is valid * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt */ - if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) { + if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) { return rel_alt_estimated; } From 7a253d50f8da7a6f8176f784c196b85bcae3c8f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 17:59:21 +0200 Subject: [PATCH 595/600] Add more paranoid checks to sf0x altitude parsing --- src/drivers/sf0x/sf0x.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 80ecab2ee1..d7780e9e81 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -593,13 +593,19 @@ SF0X::collect() /* wipe out partially read content from last cycle(s), check for dot */ for (unsigned i = 0; i < (lend - 2); i++) { if (_linebuf[i] == '\n') { + /* allocate temporary buffer */ char buf[sizeof(_linebuf)]; + /* copy remainder of buffer (2nd measurement) to temporary buffer */ memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); + /* copy temporary buffer to beginning of line buffer, + * effectively overwriting a previous temporary + * measurement + */ memcpy(_linebuf, buf, (lend + 1) - (i + 1)); } /* we need a digit before the dot and a dot for a valid number */ - if (i > 0 && _linebuf[i] == '.') { + if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) { valid = true; } } From ee913202991f2a015904b1570bd9f5a62865e5c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 19:53:01 +0200 Subject: [PATCH 596/600] SF02/F driver: Improve parsing --- src/drivers/sf0x/sf0x.cpp | 40 ++++++++++++++++----------------------- 1 file changed, 16 insertions(+), 24 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index d7780e9e81..cf04190756 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -520,11 +520,9 @@ SF0X::collect() /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + /* timed out - retry */ if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; - } else if (_linebuf_index > 0) { - /* increment to next read position */ - _linebuf_index++; } /* the buffer for read chars is buflen minus null termination */ @@ -550,18 +548,19 @@ SF0X::collect() return -EAGAIN; } - /* we did increment the index to the next position already, so just add the additional fields */ - _linebuf_index += (ret - 1); + /* let the write pointer point to the next free entry */ + _linebuf_index += ret; _last_read = hrt_absolute_time(); - if (_linebuf_index < 1) { - /* we need at least the two end bytes to make sense of this string */ + /* require a reasonable amount of minimum bytes */ + if (_linebuf_index < 6) { + /* we need at this format: x.xx\r\n */ return -EAGAIN; - } else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') { + } else if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - if (_linebuf_index >= readlen - 1) { + if (_linebuf_index == readlen) { /* we have a full buffer, but no line ending - abort */ _linebuf_index = 0; perf_count(_comms_errors); @@ -577,9 +576,7 @@ SF0X::collect() bool valid; /* enforce line ending */ - unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1); - - _linebuf[lend] = '\0'; + _linebuf[_linebuf_index] = '\0'; if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; @@ -591,17 +588,12 @@ SF0X::collect() valid = false; /* wipe out partially read content from last cycle(s), check for dot */ - for (unsigned i = 0; i < (lend - 2); i++) { + for (unsigned i = 0; i < (_linebuf_index - 2); i++) { if (_linebuf[i] == '\n') { - /* allocate temporary buffer */ - char buf[sizeof(_linebuf)]; - /* copy remainder of buffer (2nd measurement) to temporary buffer */ - memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); - /* copy temporary buffer to beginning of line buffer, - * effectively overwriting a previous temporary - * measurement - */ - memcpy(_linebuf, buf, (lend + 1) - (i + 1)); + /* wipe out any partial measurements */ + for (unsigned j = 0; j <= i; j++) { + _linebuf[j] = ' '; + } } /* we need a digit before the dot and a dot for a valid number */ @@ -613,8 +605,8 @@ SF0X::collect() if (valid) { si_units = strtod(_linebuf, &end); - /* we require at least 3 characters for a valid number */ - if (end > _linebuf + 3) { + /* we require at least four characters for a valid number */ + if (end > _linebuf + 4) { valid = true; } else { si_units = -1.0f; From a46e1aa60efcaf8b52db02f783fc4c0a9dc05878 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 20:55:29 +0200 Subject: [PATCH 597/600] Fix reschedule logic for SF02/F driver --- src/drivers/sf0x/sf0x.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index cf04190756..b690938e71 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -707,12 +707,12 @@ SF0X::cycle() int collect_ret = collect(); if (collect_ret == -EAGAIN) { - /* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */ + /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, - USEC2TICK(1100)); + USEC2TICK(1042 * 8)); return; } From af3b3680e090ad80ffeb6581f1f58f2dd4ae5195 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 21:10:04 +0200 Subject: [PATCH 598/600] Fixed SF02/F driver, tested ok --- src/drivers/sf0x/sf0x.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index b690938e71..d382d08d09 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -606,7 +606,7 @@ SF0X::collect() si_units = strtod(_linebuf, &end); /* we require at least four characters for a valid number */ - if (end > _linebuf + 4) { + if (end > _linebuf + 3) { valid = true; } else { si_units = -1.0f; @@ -615,7 +615,7 @@ SF0X::collect() } } - debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); + debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); /* done with this chunk, resetting - even if invalid */ _linebuf_index = 0; From 84d26185df1aa3a727d0db7b06b262e756eb5248 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 21:11:20 +0200 Subject: [PATCH 599/600] Better initial config for Phantom --- ROMFS/px4fmu_common/init.d/3031_phantom | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 31dfe71007..53c48d8aac 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -30,6 +30,9 @@ then param set FW_RR_P 0.08 param set FW_R_LIM 50 param set FW_R_RMAX 0 + # Bottom of bay and nominal zero-pitch attitude differ + # the payload bay is pitched up about 7 degrees + param set SENS_BOARD_Y_OFF 7.0 fi set MIXER phantom From 22d2e26b9cab22fbe3f418b22e74fb05048083e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 21:55:31 +0200 Subject: [PATCH 600/600] Simply throttle limiting on approach - limit throttle still defaults to 1 --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++++++ .../fw_pos_control_l1/fw_pos_control_l1_params.c | 13 +++++++++---- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index c983ac6f9c..deccab4820 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -569,6 +569,12 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); + + /* check if negative value for 2/3 of flare altitude is set for throttle cut */ + if (_parameters.land_thrust_lim_alt_relative < 0.0f) { + _parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative; + } + param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt)); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 41c3744076..890ab3bad2 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -379,18 +379,23 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); /** - * Landing flare altitude (relative) + * Landing flare altitude (relative to landing altitude) * + * @unit meter * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f); /** - * Landing throttle limit altitude (relative) + * Landing throttle limit altitude (relative landing altitude) * + * Default of -1.0f lets the system default to applying throttle + * limiting at 2/3 of the flare altitude. + * + * @unit meter * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f); /** * Landing heading hold horizontal distance