From 40d47fb069cb60303102f3bfba21f6dc5e0aa5a9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 11:45:20 +0200 Subject: fw pos control: use new lauchdetector states --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 73 +++++++++++++--------- 1 file changed, 43 insertions(+), 30 deletions(-) (limited to 'src/modules') 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 350ce6dec..6dd458516 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: @@ -171,7 +173,7 @@ private: bool land_onslope; /* takeoff/launch states */ - bool launch_detected; + LaunchDetectionResult launch_detection_state; bool usePreTakeoffThrust; bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) @@ -438,7 +440,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_stayonground(false), land_motor_lim(false), land_onslope(false), - launch_detected(false), + launch_detection_state(LAUNCHDETECTION_RES_NONE), usePreTakeoffThrust(false), last_manual(false), landingslope(), @@ -1076,39 +1078,51 @@ 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(); - } + 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(); + /* Depending on launch detection state set control flags */ + if(launch_detection_state == LAUNCHDETECTION_RES_NONE) { /* 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"); + usePreTakeoffThrust = true; + } else if (launch_detection_state == LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL) { + usePreTakeoffThrust = true; + } else { + usePreTakeoffThrust = false; } + } else { + /* no takeoff detection --> fly */ + usePreTakeoffThrust = false; + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; } - _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_detection_state != LAUNCHDETECTION_RES_NONE) { + /* Launch has been detected, hence we have to control the plane. The usePreTakeoffThrust + * flag determines how much thrust we apply */ - if (launch_detected) { - usePreTakeoffThrust = false; + _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(); + + float takeoff_throttle = usePreTakeoffThrust ? 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) { @@ -1119,7 +1133,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), @@ -1138,17 +1152,15 @@ 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; } + } /* reset landing state */ @@ -1182,6 +1194,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } + /* making sure again that the correct thrust is used, without depending on library calls */ if (usePreTakeoffThrust) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } @@ -1342,7 +1355,7 @@ FixedwingPositionControl::task_main() void FixedwingPositionControl::reset_takeoff_state() { - launch_detected = false; + launch_detection_state = LAUNCHDETECTION_RES_NONE; usePreTakeoffThrust = false; launchDetector.reset(); } -- cgit v1.2.3 From 9f5004be2d282a0bb8f2618545d0c6174df2e29a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 12:13:30 +0200 Subject: fw pos control: set default roll, pitch while waiting for launch --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) (limited to 'src/modules') 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 6dd458516..9cbe5483b 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 @@ -1143,7 +1143,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, @@ -1159,6 +1160,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _global_pos.alt, ground_speed); } + } else { + /* 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)); } } -- cgit v1.2.3 From cfbbe60291028c60a5dd0a8f2b8bad265a7ee839 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 12:41:48 +0200 Subject: fw pos control: launchdetection logic cleanup --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 44 +++++++++------------- 1 file changed, 18 insertions(+), 26 deletions(-) (limited to 'src/modules') 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 9cbe5483b..da81b0dba 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 @@ -174,7 +174,6 @@ private: /* takeoff/launch states */ LaunchDetectionResult launch_detection_state; - bool usePreTakeoffThrust; bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) @@ -441,7 +440,6 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), launch_detection_state(LAUNCHDETECTION_RES_NONE), - usePreTakeoffThrust(false), last_manual(false), landingslope(), flare_curve_alt_rel_last(0.0f), @@ -1092,37 +1090,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update our copy of the laucn detection state */ launch_detection_state = launchDetector.getLaunchDetected(); - - /* Depending on launch detection state set control flags */ - if(launch_detection_state == LAUNCHDETECTION_RES_NONE) { - /* 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; - - usePreTakeoffThrust = true; - } else if (launch_detection_state == LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL) { - usePreTakeoffThrust = true; - } else { - usePreTakeoffThrust = false; - } } else { /* no takeoff detection --> fly */ - usePreTakeoffThrust = false; - launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } + /* 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. The usePreTakeoffThrust - * flag determines how much thrust we apply */ + /* 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(); - float takeoff_throttle = usePreTakeoffThrust ? launchDetector.getThrottlePreTakeoff() : - _parameters.throttle_max; + /* 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) { @@ -1161,6 +1146,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ground_speed); } } else { + /* 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), @@ -1200,8 +1191,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - /* making sure again that the correct thrust is used, without depending on library calls */ - if (usePreTakeoffThrust) { + /* Copy thrust and pitch values from tecs + * making sure again that the correct thrust is used, without depending on library calls */ + if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { @@ -1362,7 +1355,6 @@ FixedwingPositionControl::task_main() void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; - usePreTakeoffThrust = false; launchDetector.reset(); } -- cgit v1.2.3 From c591444c1e2e4124d28a03653c0ccdbcd305c1c2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 12:48:14 +0200 Subject: fw pos control: set pitch sp correctly while waiting for launch --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) (limited to 'src/modules') 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 da81b0dba..b3a78ad82 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 @@ -1192,7 +1192,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* Copy thrust and pitch values from tecs - * making sure again that the correct thrust is used, without depending on library calls */ + * making sure again that the correct thrust is used, + * without depending on library calls */ if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); @@ -1200,7 +1201,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi else { _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(); + /* 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(); + } if (_control_mode.flag_control_position_enabled) { last_manual = false; -- cgit v1.2.3 From 752b89b99811e27082be8147c7ff8426f0199478 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Sep 2014 23:24:54 +0200 Subject: parse hil_optical_flow message publish to flow and range finder topic --- src/modules/mavlink/mavlink_receiver.cpp | 54 +++++++++++++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 2 ++ 2 files changed, 55 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bed1bd789..45ea0e168 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -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; } @@ -363,6 +369,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); + } + + 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) { @@ -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; -- cgit v1.2.3 From 49f1637d7f6486295c5fc7f541fe06ed934688cf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Sep 2014 23:32:24 +0200 Subject: comment and whitespace --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 45ea0e168..1115304d4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -396,6 +396,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) 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)); @@ -409,7 +410,6 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) 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); } -- cgit v1.2.3