dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #700 from PX4/fix_mission_dive 

Fix mission dive
This commit is contained in:
Thomas Gubler 2014-03-03 15:28:49 +01:00
parent 49fe131019 909e138182
commit eb32c7198e
6 changed files with 95 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/lib/ecl/ecl.h
View File

@ -38,7 +38,6 @@
*/
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#define ecl_absolute_time hrt_absolute_time
#define ecl_elapsed_time hrt_elapsed_time

13
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
View File

@ -38,6 +38,8 @@
*
*/
#include <float.h>
#include "ecl_l1_pos_controller.h"
float ECL_L1_Pos_Controller::nav_roll()
@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con
/* calculate the vector from waypoint A to current position */
math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
/* store the normalized vector from waypoint A to current position */
math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
math::Vector<2> vector_A_to_airplane_unit;
/* prevent NaN when normalizing */
if (vector_A_to_airplane.length() > FLT_EPSILON) {
/* store the normalized vector from waypoint A to current position */
vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
} else {
vector_A_to_airplane_unit = vector_A_to_airplane;
}
/* calculate eta angle towards the loiter center */

5
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -3,13 +3,10 @@
#include "tecs.h"
#include <ecl/ecl.h>
#include <systemlib/err.h>
#include <geo/geo.h>
using namespace math;
#ifndef CONSTANTS_ONE_G
#define CONSTANTS_ONE_G GRAVITY
#endif
/**
* @file tecs.cpp
*

27
src/lib/external_lgpl/tecs/tecs.h
View File

@ -28,16 +28,7 @@ class __EXPORT TECS
{
public:
TECS() :
_airspeed_enabled(false),
_throttle_slewrate(0.0f),
_climbOutDem(false),
_hgt_dem_prev(0.0f),
_hgt_dem_adj_last(0.0f),
_hgt_dem_in_old(0.0f),
_TAS_dem_last(0.0f),
_TAS_dem_adj(0.0f),
_TAS_dem(0.0f),
_pitch_dem(0.0f),
_integ1_state(0.0f),
_integ2_state(0.0f),
_integ3_state(0.0f),
@ -45,8 +36,16 @@ public:
_integ5_state(0.0f),
_integ6_state(0.0f),
_integ7_state(0.0f),
_pitch_dem(0.0f),
_last_pitch_dem(0.0f),
_vel_dot(0.0f),
_TAS_dem(0.0f),
_TAS_dem_last(0.0f),
_hgt_dem_in_old(0.0f),
_hgt_dem_adj_last(0.0f),
_hgt_dem_prev(0.0f),
_TAS_dem_adj(0.0f),
_STEdotErrLast(0.0f),
_climbOutDem(false),
_SPE_dem(0.0f),
_SKE_dem(0.0f),
_SPEdot_dem(0.0f),
@ -55,9 +54,9 @@ public:
_SKE_est(0.0f),
_SPEdot(0.0f),
_SKEdot(0.0f),
_vel_dot(0.0f),
_STEdotErrLast(0.0f) {
_airspeed_enabled(false),
_throttle_slewrate(0.0f)
{
}
bool airspeed_sensor_enabled() {

94
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -130,23 +130,23 @@ private:
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 _control_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _sensor_combined_sub; /**< for 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 */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
perf_counter_t _loop_perf; /**< loop performance counter */
@ -154,13 +154,13 @@ private:
/** manual control states */
float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */
float _loiter_hold_lat;
float _loiter_hold_lon;
double _loiter_hold_lat;
double _loiter_hold_lon;
float _loiter_hold_alt;
bool _loiter_hold;
float _launch_lat;
float _launch_lon;
double _launch_lat;
double _launch_lon;
float _launch_alt;
bool _launch_valid;
@ -192,7 +192,7 @@ private:
uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures
float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
bool _global_pos_valid; ///< global position is valid
math::Matrix<3, 3> _R_nb; ///< current attitude
math::Matrix<3, 3> _R_nb; ///< current attitude
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
@ -360,6 +360,7 @@ FixedwingPositionControl *g_control;
FixedwingPositionControl::FixedwingPositionControl() :
_mavlink_fd(-1),
_task_should_exit(false),
_control_task(-1),
@ -378,38 +379,33 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
/* states */
_setpoint_valid(false),
_loiter_hold(false),
_airspeed_error(0.0f),
_airspeed_valid(false),
_groundspeed_undershoot(0.0f),
_global_pos_valid(false),
land_noreturn_horizontal(false),
land_noreturn_vertical(false),
land_stayonground(false),
land_motor_lim(false),
land_onslope(false),
flare_curve_alt_last(0.0f),
_mavlink_fd(-1),
launchDetector(),
launch_detected(false),
usePreTakeoffThrust(false)
usePreTakeoffThrust(false),
flare_curve_alt_last(0.0f),
launchDetector(),
_airspeed_error(0.0f),
_airspeed_valid(false),
_groundspeed_undershoot(0.0f),
_global_pos_valid(false),
_att(),
_att_sp(),
_nav_capabilities(),
_manual(),
_airspeed(),
_control_mode(),
_global_pos(),
_pos_sp_triplet(),
_sensor_combined()
{
/* safely initialize structs */
vehicle_attitude_s _att = {0};
vehicle_attitude_setpoint_s _att_sp = {0};
navigation_capabilities_s _nav_capabilities = {0};
manual_control_setpoint_s _manual = {0};
airspeed_s _airspeed = {0};
vehicle_control_mode_s _control_mode = {0};
vehicle_global_position_s _global_pos = {0};
position_setpoint_triplet_s _pos_sp_triplet = {0};
sensor_combined_s _sensor_combined = {0};
_nav_capabilities.turn_distance = 0.0f;
_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
@ -583,8 +579,8 @@ FixedwingPositionControl::vehicle_control_mode_poll()
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
if (!was_armed && _control_mode.flag_armed) {
_launch_lat = _global_pos.lat / 1e7f;
_launch_lon = _global_pos.lon / 1e7f;
_launch_lat = _global_pos.lat;
_launch_lon = _global_pos.lon;
_launch_alt = _global_pos.alt;
_launch_valid = true;
}
@ -699,7 +695,7 @@ void
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
{
if (_global_pos_valid) {
if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
/* rotate ground speed vector with current attitude */
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@ -1058,13 +1054,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
}
/* climb out control */
bool climb_out = false;
//XXX not used
/* user wants to climb out */
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
climb_out = true;
}
/* 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 seatbelt mode, set airspeed based on manual control */
@ -1271,7 +1269,7 @@ FixedwingPositionControl::task_main()
float turn_distance = _l1_control.switch_distance(100.0f);
/* lazily publish navigation capabilities */
if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
/* set new turn distance */
_nav_capabilities.turn_distance = turn_distance;

46
src/modules/navigator/navigator_main.cpp
View File

@ -154,17 +154,16 @@ private:
int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _control_mode_sub; /**< vehicle control mode subscription */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _mission_result_pub; /**< publish mission result topic */
struct vehicle_status_s _vstatus; /**< vehicle status */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct home_position_s _home_pos; /**< home position for RTL */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
struct mission_result_s _mission_result; /**< mission result for commander/mavlink */
struct mission_item_s _mission_item; /**< current mission item */
bool _mission_item_valid; /**< current mission item valid */
struct mission_item_s _mission_item; /**< current mission item */
perf_counter_t _loop_perf; /**< loop performance counter */
@ -178,21 +177,22 @@ private:
class Mission _mission;
bool _global_pos_valid; /**< track changes of global_position.global_valid flag */
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */
bool _mission_item_valid; /**< current mission item valid */
bool _global_pos_valid; /**< track changes of global_position.global_valid flag */
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
uint64_t _time_first_inside_orbit;
bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
MissionFeasibilityChecker missionFeasiblityChecker;
uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */
uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */
bool _pos_sp_triplet_updated;
char *nav_states_str[NAV_STATE_MAX];
const char *nav_states_str[NAV_STATE_MAX];
struct {
float min_altitude;
@ -387,11 +387,11 @@ Navigator::Navigator() :
_global_pos_sub(-1),
_home_pos_sub(-1),
_vstatus_sub(-1),
_control_mode_sub(-1),
_params_sub(-1),
_offboard_mission_sub(-1),
_onboard_mission_sub(-1),
_capabilities_sub(-1),
_control_mode_sub(-1),
/* publications */
_pos_sp_triplet_pub(-1),
@ -400,22 +400,22 @@ Navigator::Navigator() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
/* states */
_rtl_state(RTL_STATE_NONE),
_geofence_violation_warning_sent(false),
_fence_valid(false),
_inside_fence(true),
_mission(),
_mission_item_valid(false),
_global_pos_valid(false),
_reset_loiter_pos(true),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_set_nav_state_timestamp(0),
_mission_item_valid(false),
_need_takeoff(true),
_do_takeoff(false),
_set_nav_state_timestamp(0),
_pos_sp_triplet_updated(false),
_geofence_violation_warning_sent(false)
/* states */
_rtl_state(RTL_STATE_NONE)
{
_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
@ -1167,7 +1167,7 @@ Navigator::set_mission_item()
}
if (_do_takeoff) {
mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt);
mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt));
} else {
if (onboard) {
@ -1326,7 +1326,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", climb_alt - _home_pos.alt);
mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt));
break;
}
@ -1359,7 +1359,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt);
mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
break;
}
@ -1377,7 +1377,7 @@ Navigator::set_rtl_item()
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay;
_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f;
_mission_item.origin = ORIGIN_ONBOARD;
@ -1386,7 +1386,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt);
mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
break;
}
@ -1552,7 +1552,7 @@ Navigator::check_mission_item_reached()
_time_first_inside_orbit = now;
if (_mission_item.time_inside > 0.01f) {
mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", _mission_item.time_inside);
mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside);
}
}