diff options
| -rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.cpp | 5 | ||||
| -rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 67 |
2 files changed, 48 insertions, 24 deletions
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 0a95b46f6..d5c759b17 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -39,6 +39,7 @@ */ #include "CatapultLaunchMethod.h" +#include <systemlib/err.h> CatapultLaunchMethod::CatapultLaunchMethod() : last_timestamp(0), @@ -61,11 +62,15 @@ void CatapultLaunchMethod::update(float accel_x) if (accel_x > threshold_accel.get()) { integrator += accel_x * 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()) { launchDetected = true; } } else { +// 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); /* reset integrator */ integrator = 0.0f; launchDetected = false; 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 26f6768cc..5056bcdc1 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 @@ -76,6 +76,7 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/navigation_capabilities.h> +#include <uORB/topics/sensor_combined.h> #include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/err.h> @@ -132,7 +133,7 @@ private: int _control_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ - int _accel_sub; /**< body frame accelerations */ + int _sensor_combined_sub; /**< for body frame accelerations */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ @@ -145,7 +146,7 @@ private: struct vehicle_control_mode_s _control_mode; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ - struct accel_report _accel; /**< body frame accelerations */ + struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -171,6 +172,10 @@ private: bool land_motor_lim; bool land_onslope; + /* takeoff/launch states */ + bool launch_detected; + bool launch_detection_message_sent; + /* Landingslope object */ Landingslope landingslope; @@ -311,7 +316,7 @@ private: /** * Check for accel updates. */ - void vehicle_accel_poll(); + void vehicle_sensor_combined_poll(); /** * Check for set triplet updates. @@ -389,7 +394,9 @@ FixedwingPositionControl::FixedwingPositionControl() : land_onslope(false), flare_curve_alt_last(0.0f), _mavlink_fd(-1), - launchDetector() + launchDetector(), + launch_detected(false), + launch_detection_message_sent(false) { /* safely initialize structs */ vehicle_attitude_s _att = {0}; @@ -400,7 +407,7 @@ FixedwingPositionControl::FixedwingPositionControl() : vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; mission_item_triplet_s _mission_item_triplet = {0}; - accel_report _accel = {0}; + sensor_combined_s _sensor_combined = {0}; @@ -631,14 +638,14 @@ FixedwingPositionControl::vehicle_attitude_poll() } void -FixedwingPositionControl::vehicle_accel_poll() +FixedwingPositionControl::vehicle_sensor_combined_poll() { /* check if there is a new position */ - bool accel_updated; - orb_check(_accel_sub, &accel_updated); + bool sensors_updated; + orb_check(_sensor_combined_sub, &sensors_updated); - if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + if (sensors_updated) { + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); } } @@ -756,7 +763,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float baro_altitude = _global_pos.alt; /* filter speed and altitude for controller */ - math::Vector3 accel_body(_accel.x, _accel.y, _accel.z); + math::Vector3 accel_body(_sensor_combined.accelerometer_m_s2[0], _sensor_combined.accelerometer_m_s2[1], _sensor_combined.accelerometer_m_s2[2]); math::Vector3 accel_earth = _R_nb.transpose() * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); @@ -973,24 +980,30 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { /* Perform launch detection */ - bool do_fly_takeoff = false; - warnx("Launch detection running"); - if (launchDetector.launchDetectionEnabled()) { - launchDetector.update(_accel.x); - if (launchDetector.getLaunchDetected()) { - do_fly_takeoff = true; - warnx("Launch detected. Taking off!"); +// warnx("Launch detection running"); + if(!launch_detected) { //do not do further checks once a launch was detected + if (launchDetector.launchDetectionEnabled()) { +// warnx("Launch detection enabled"); + if(!launch_detection_message_sent) { + mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); + launch_detection_message_sent = true; + } + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + if (launchDetector.getLaunchDetected()) { + launch_detected = true; + warnx("Launch detected. Taking off!"); + } + } else { + /* no takeoff detection --> fly */ + launch_detected = true; } - } else { - /* no takeoff detection --> fly */ - do_fly_takeoff = true; } _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - if (do_fly_takeoff) { + if (launch_detected) { /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (altitude_error > 15.0f) { @@ -1037,6 +1050,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio land_onslope = false; } + /* reset takeoff/launch state */ + if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { + launch_detected = false; + launch_detection_message_sent = false; + } + if (was_circle_mode && !_l1_control.circle_mode()) { /* just kicked out of loiter, reset roll integrals */ _att_sp.roll_reset_integral = true; @@ -1151,7 +1170,7 @@ FixedwingPositionControl::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -1233,7 +1252,7 @@ FixedwingPositionControl::task_main() vehicle_attitude_poll(); vehicle_setpoint_poll(); - vehicle_accel_poll(); + vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); // vehicle_baro_poll(); |