diff options
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.cpp | 59 | ||||
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.h | 11 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.cpp | 27 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.h | 8 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchMethod.h | 13 | ||||
-rw-r--r-- | src/lib/launchdetection/launchdetection_params.c | 11 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 106 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 54 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 2 |
9 files changed, 216 insertions, 75 deletions
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 9d479832f..65ae461db 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -49,9 +49,10 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : SuperBlock(parent, "CAT"), last_timestamp(hrt_absolute_time()), integrator(0.0f), - launchDetected(false), - threshold_accel(this, "A"), - threshold_time(this, "T") + state(LAUNCHDETECTION_RES_NONE), + thresholdAccel(this, "A"), + thresholdTime(this, "T"), + motorDelay(this, "MDEL") { } @@ -65,34 +66,56 @@ void CatapultLaunchMethod::update(float accel_x) float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f; last_timestamp = hrt_absolute_time(); - if (accel_x > threshold_accel.get()) { - 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_time.get()) { - launchDetected = true; + switch (state) { + case LAUNCHDETECTION_RES_NONE: + /* Detect a acceleration that is longer and stronger as the minimum given by the params */ + if (accel_x > thresholdAccel.get()) { + integrator += dt; + if (integrator > thresholdTime.get()) { + if (motorDelay.get() > 0.0f) { + state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; + warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full" + " throttle", (double)motorDelay.get()); + } else { + /* No motor delay set: go directly to enablemotors state */ + state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + warnx("Launch detected: state: enablemotors (delay not activated)"); + } + } + } else { + /* reset */ + reset(); } + break; + + case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL: + /* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is + * over to allow full throttle */ + motorDelayCounter += dt; + if (motorDelayCounter > motorDelay.get()) { + warnx("Launch detected: state enablemotors"); + state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } + break; + + default: + break; - } 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; } } -bool CatapultLaunchMethod::getLaunchDetected() +LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const { - return launchDetected; + return state; } void CatapultLaunchMethod::reset() { integrator = 0.0f; - launchDetected = false; + motorDelayCounter = 0.0f; + state = LAUNCHDETECTION_RES_NONE; } } diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index 23757f6f3..d918c3a76 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -57,16 +57,19 @@ public: ~CatapultLaunchMethod(); void update(float accel_x); - bool getLaunchDetected(); + LaunchDetectionResult getLaunchDetected() const; void reset(); private: hrt_abstime last_timestamp; float integrator; - bool launchDetected; + float motorDelayCounter; - control::BlockParamFloat threshold_accel; - control::BlockParamFloat threshold_time; + LaunchDetectionResult state; + + control::BlockParamFloat thresholdAccel; + control::BlockParamFloat thresholdTime; + control::BlockParamFloat motorDelay; }; diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 3bf47bbb0..2958c0a31 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -46,6 +46,7 @@ namespace launchdetection LaunchDetector::LaunchDetector() : SuperBlock(NULL, "LAUN"), + activeLaunchDetectionMethodIndex(-1), launchdetection_on(this, "ALL_ON"), throttlePreTakeoff(this, "THR_PRE") { @@ -65,7 +66,14 @@ LaunchDetector::~LaunchDetector() void LaunchDetector::reset() { /* Reset all detectors */ - launchMethods[0]->reset(); + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + launchMethods[i]->reset(); + } + + /* Reset active launchdetector */ + activeLaunchDetectionMethodIndex = -1; + + } void LaunchDetector::update(float accel_x) @@ -77,17 +85,24 @@ void LaunchDetector::update(float accel_x) } } -bool LaunchDetector::getLaunchDetected() +LaunchDetectionResult LaunchDetector::getLaunchDetected() { if (launchdetection_on.get() == 1) { - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { - if(launchMethods[i]->getLaunchDetected()) { - return true; + if (activeLaunchDetectionMethodIndex < 0) { + /* None of the active launchmethods has detected a launch, check all launchmethods */ + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { + warnx("selecting launchmethod %d", i); + activeLaunchDetectionMethodIndex = i; // from now on only check this method + return launchMethods[i]->getLaunchDetected(); + } } + } else { + return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected(); } } - return false; + return LAUNCHDETECTION_RES_NONE; } } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 1a214b66e..b48e724ba 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -59,7 +59,7 @@ public: void reset(); void update(float accel_x); - bool getLaunchDetected(); + LaunchDetectionResult getLaunchDetected(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } @@ -67,6 +67,12 @@ public: // virtual bool getLaunchDetected(); protected: private: + int activeLaunchDetectionMethodIndex; /**< holds a index to the launchMethod in the array launchMethods + which detected a Launch. If no launchMethod has detected a launch yet the + value is -1. Once one launchMetthod has detected a launch only this + method is checked for further adavancing in the state machine (e.g. when + to power up the motors) */ + LaunchMethod* launchMethods[1]; control::BlockParamInt launchdetection_on; control::BlockParamFloat throttlePreTakeoff; diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index e04467f6a..d2f091cea 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -44,11 +44,22 @@ namespace launchdetection { +enum LaunchDetectionResult { + LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */ + LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should + control the attitude. However any motors should not throttle + up and still be set to 'throttlePreTakeoff'. + For instance this is used to have a delay for the motor + when launching a fixed wing aircraft from a bungee */ + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, teh controller should control + attitude and also throttle up the motors. */ +}; + class LaunchMethod { public: virtual void update(float accel_x) = 0; - virtual bool getLaunchDetected() = 0; + virtual LaunchDetectionResult getLaunchDetected() const = 0; virtual void reset() = 0; protected: diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 8df8c696c..d35eb11f6 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -78,6 +78,17 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); /** + * Motor delay + * + * Delay between starting attitude control and powering up the thorttle (giving throttle control to the controller) + * Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate + * + * @unit seconds + * @min 0 + * @group Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f); +/** * Throttle setting while detecting launch. * * The throttle is set to this value while the system is waiting for the take-off. 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 5d62c90ab..ee08f3e98 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 @@ -102,6 +102,8 @@ static int _control_task = -1; /**< task handle for sensor task */ */ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); +using namespace launchdetection; + class FixedwingPositionControl { public: @@ -173,8 +175,7 @@ private: bool land_onslope; /* takeoff/launch states */ - bool launch_detected; - bool usePreTakeoffThrust; + LaunchDetectionResult launch_detection_state; bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) @@ -447,8 +448,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_stayonground(false), land_motor_lim(false), land_onslope(false), - launch_detected(false), - usePreTakeoffThrust(false), + launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), landingslope(), flare_curve_alt_rel_last(0.0f), @@ -1102,39 +1102,38 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ - if(!launch_detected) { //do not do further checks once a launch was detected - if (launchDetector.launchDetectionEnabled()) { - static hrt_abstime last_sent = 0; - if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); - last_sent = hrt_absolute_time(); - } - - /* Tell the attitude controller to stop integrating while we are waiting - * for the launch */ - _att_sp.roll_reset_integral = true; - _att_sp.pitch_reset_integral = true; - _att_sp.yaw_reset_integral = true; - - /* Detect launch */ - launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); - if (launchDetector.getLaunchDetected()) { - launch_detected = true; - mavlink_log_info(_mavlink_fd, "#audio: Takeoff"); - } - } else { - /* no takeoff detection --> fly */ - launch_detected = true; - warnx("launchdetection off"); + if (launchDetector.launchDetectionEnabled() && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + /* Inform user that launchdetection is running */ + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { + mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); + last_sent = hrt_absolute_time(); } + + /* Detect launch */ + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + + /* update our copy of the laucn detection state */ + launch_detection_state = launchDetector.getLaunchDetected(); + } else { + /* no takeoff detection --> fly */ + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + /* Set control values depending on the detection state */ + if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { + /* Launch has been detected, hence we have to control the plane. */ + + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - if (launch_detected) { - usePreTakeoffThrust = false; + /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use + * full throttle, otherwise we use the preTakeOff Throttle */ + float takeoff_throttle = launch_detection_state != + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? + launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { @@ -1145,7 +1144,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, + _parameters.throttle_min, takeoff_throttle, _parameters.throttle_cruise, true, math::max(math::radians(pos_sp_triplet.current.pitch_min), @@ -1155,7 +1154,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi TECS_MODE_TAKEOFF); /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); } else { tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, @@ -1164,17 +1164,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, - _parameters.throttle_max, + takeoff_throttle, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } - } else { - usePreTakeoffThrust = true; + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + + /* Set default roll and pitch setpoints during detection phase */ + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)); } + } /* reset landing state */ @@ -1211,13 +1220,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; - } - else if (usePreTakeoffThrust) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + /* making sure again that the correct thrust is used, + * without depending on library calls */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); + /* Copy thrust and pitch values from tecs */ + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max); + } + + /* During a takeoff waypoint while waiting for launch the pitch sp is set + * already (not by tecs) */ + if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + _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; @@ -1377,8 +1396,7 @@ FixedwingPositionControl::task_main() void FixedwingPositionControl::reset_takeoff_state() { - launch_detected = false; - usePreTakeoffThrust = false; + launch_detection_state = LAUNCHDETECTION_RES_NONE; launchDetector.reset(); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bed1bd789..1115304d4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -54,6 +54,7 @@ #include <drivers/drv_gyro.h> #include <drivers/drv_mag.h> #include <drivers/drv_baro.h> +#include <drivers/drv_range_finder.h> #include <time.h> #include <float.h> #include <unistd.h> @@ -102,6 +103,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _battery_pub(-1), _cmd_pub(-1), _flow_pub(-1), + _range_pub(-1), _offboard_control_sp_pub(-1), _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), @@ -200,6 +202,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_hil_state_quaternion(msg); break; + case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: + handle_message_hil_optical_flow(msg); + break; + default: break; } @@ -364,6 +370,52 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_hil_optical_flow_t flow; + mavlink_msg_hil_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + memset(&f, 0, sizeof(f)); + + f.timestamp = hrt_absolute_time(); + f.flow_timestamp = flow.time_usec; + f.flow_raw_x = flow.flow_x; + f.flow_raw_y = flow.flow_y; + f.flow_comp_x_m = flow.flow_comp_m_x; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + if (_flow_pub < 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + } + + /* Use distance value for range finder report */ + struct range_finder_report r; + memset(&r, 0, sizeof(f)); + + r.timestamp = hrt_absolute_time(); + r.error_count = 0; + r.type = RANGE_FINDER_TYPE_LASER; + r.distance = flow.ground_distance; + r.minimum_distance = 0.0f; + r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable + r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); + + if (_range_pub < 0) { + _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + } else { + orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); + } +} + +void MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) { mavlink_set_mode_t new_mode; @@ -444,7 +496,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) 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); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 91125736c..c4e12d8d8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,6 +112,7 @@ private: 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_hil_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); @@ -142,6 +143,7 @@ private: orb_advert_t _battery_pub; orb_advert_t _cmd_pub; orb_advert_t _flow_pub; + orb_advert_t _range_pub; orb_advert_t _offboard_control_sp_pub; orb_advert_t _local_pos_sp_pub; orb_advert_t _global_vel_sp_pub; |