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

Replace mission_item_triplet with position_setpoint_triplet, WIP

This commit is contained in:
Anton Babushkin 2014-01-23 12:16:02 +01:00
parent 6c07a5c2cf
commit 6acb8fa66f
10 changed files with 304 additions and 344 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/controllib/uorb/blocks.cpp
View File

@ -54,8 +54,8 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {};
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
mission_item_s &missionCmd,
mission_item_s &lastMissionCmd)
position_setpoint_s &missionCmd,
position_setpoint_s &lastMissionCmd)
{
// heading to waypoint
@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
_missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20),
_missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20),
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz

8
src/modules/controllib/uorb/blocks.hpp
View File

@ -43,7 +43,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
@ -82,8 +82,8 @@ public:
virtual ~BlockWaypointGuidance();
void update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
mission_item_s &missionCmd,
mission_item_s &lastMissionCmd);
position_setpoint_s &missionCmd,
position_setpoint_s &lastMissionCmd);
float getPsiCmd() { return _psiCmd; }
};
@ -98,7 +98,7 @@ protected:
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
UOrbSubscription<vehicle_global_position_s> _pos;
UOrbSubscription<mission_item_triplet_s> _missionCmd;
UOrbSubscription<position_setpoint_triplet_s> _missionCmd;
UOrbSubscription<manual_control_setpoint_s> _manual;
UOrbSubscription<vehicle_status_s> _status;
UOrbSubscription<parameter_update_s> _param_update;

2
src/modules/fixedwing_backside/fixedwing.hpp
View File

@ -264,7 +264,7 @@ private:
BlockParamFloat _crMax;
struct pollfd _attPoll;
mission_item_triplet_s _lastMissionCmd;
position_setpoint_triplet_s _lastMissionCmd;
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
uint64_t _timeStamp;
public:

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

@ -68,7 +68,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
@ -126,7 +126,7 @@ private:
int _control_task; /**< task handle for sensor task */
int _global_pos_sub;
int _mission_item_triplet_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 */
@ -145,7 +145,7 @@ private:
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 mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
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 */
@ -332,10 +332,10 @@ private:
* Control position.
*/
bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
const struct mission_item_triplet_s &_mission_item_triplet);
const struct position_setpoint_triplet_s &_pos_sp_triplet);
float calculate_target_airspeed(float airspeed_demand);
void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet);
void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet);
/**
* Shim for calling task_main from task_create.
@ -367,7 +367,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* subscriptions */
_global_pos_sub(-1),
_mission_item_triplet_sub(-1),
_pos_sp_triplet_sub(-1),
_att_sub(-1),
_airspeed_sub(-1),
_control_mode_sub(-1),
@ -406,7 +406,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
airspeed_s _airspeed = {0};
vehicle_control_mode_s _control_mode = {0};
vehicle_global_position_s _global_pos = {0};
mission_item_triplet_s _mission_item_triplet = {0};
position_setpoint_triplet_s _pos_sp_triplet = {0};
sensor_combined_s _sensor_combined = {0};
@ -653,11 +653,11 @@ void
FixedwingPositionControl::vehicle_setpoint_poll()
{
/* check if there is a new setpoint */
bool mission_item_triplet_updated;
orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated);
bool pos_sp_triplet_updated;
orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
if (mission_item_triplet_updated) {
orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet);
if (pos_sp_triplet_updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
_setpoint_valid = true;
}
}
@ -700,7 +700,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
}
void
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet)
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) {
@ -713,12 +713,12 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
float distance = 0.0f;
float delta_altitude = 0.0f;
if (mission_item_triplet.previous_valid) {
distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon);
delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude;
if (pos_sp_triplet.previous.valid) {
distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt;
} else {
distance = get_distance_to_next_waypoint(current_position(0), current_position(1), mission_item_triplet.current.lat, mission_item_triplet.current.lon);
delta_altitude = mission_item_triplet.current.altitude - _global_pos.alt;
distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt;
}
float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
@ -751,11 +751,11 @@ void FixedwingPositionControl::navigation_capabilities_publish()
bool
FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
const struct mission_item_triplet_s &mission_item_triplet)
const struct position_setpoint_triplet_s &pos_sp_triplet)
{
bool setpoint = true;
calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet);
calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet);
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
@ -767,7 +767,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::Vector<3> accel_earth = _R_nb * accel_body;
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt;
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */
float throttle_max = 1.0f;
@ -785,58 +785,56 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_tecs.set_speed_weight(_parameters.speed_weight);
/* current waypoint (the one currently heading for) */
math::Vector<2> next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
/* current waypoint (the one currently heading for) */
math::Vector<2> curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
/* previous waypoint */
math::Vector<2> prev_wp;
if (mission_item_triplet.previous_valid) {
prev_wp(0) = mission_item_triplet.previous.lat;
prev_wp(1) = mission_item_triplet.previous.lon;
if (pos_sp_triplet.previous.valid) {
prev_wp(0) = pos_sp_triplet.previous.lat;
prev_wp(1) = pos_sp_triplet.previous.lon;
} else {
/*
* No valid previous waypoint, go for the current wp.
* This is automatically handled by the L1 library.
*/
prev_wp(0) = mission_item_triplet.current.lat;
prev_wp(1) = mission_item_triplet.current.lon;
prev_wp(0) = pos_sp_triplet.current.lat;
prev_wp(1) = pos_sp_triplet.current.lon;
}
if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
/* waypoint is a plain navigation waypoint */
_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();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint */
_l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius,
mission_item_triplet.current.loiter_direction, ground_speed);
_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
pos_sp_triplet.current.loiter_direction, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
@ -847,7 +845,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* heading hold, along the line connecting this and the last waypoint */
if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
if (mission_item_triplet.previous_valid) {
if (pos_sp_triplet.previous_valid) {
target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
} else {
target_bearing = _att.yaw;
@ -884,18 +882,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
// XXX this could make a great param
float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1)
float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
float airspeed_land = 1.3f * _parameters.airspeed_min;
float airspeed_approach = 1.3f * _parameters.airspeed_min;
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
if ( (_global_pos.alt < _pos_sp_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */
@ -914,12 +912,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude);
float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.altitude);
/* avoid climbout */
if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
{
flare_curve_alt = mission_item_triplet.current.altitude;
flare_curve_alt = pos_sp_triplet.current.altitude;
land_stayonground = true;
}
@ -977,7 +975,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
}
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
/* Perform launch detection */
// warnx("Launch detection running");
@ -1011,9 +1009,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (altitude_error > 15.0f) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
_airspeed.indicated_airspeed_m_s, eas2tas,
true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)),
true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
@ -1022,7 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@ -1037,14 +1035,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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), (mission_item_triplet.previous_valid) ? "valid" : "invalid");
// (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 land state */
if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) {
if (pos_sp_triplet.current.nav_cmd != NAV_CMD_LAND) {
land_noreturn_horizontal = false;
land_noreturn_vertical = false;
land_stayonground = false;
@ -1053,7 +1051,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
/* reset takeoff/launch state */
if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
if (pos_sp_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
launch_detected = false;
usePreTakeoffThrust = false;
}
@ -1176,7 +1174,7 @@ FixedwingPositionControl::task_main()
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@ -1271,7 +1269,7 @@ FixedwingPositionControl::task_main()
* Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint.
*/
if (control_position(current_position, ground_speed, _mission_item_triplet)) {
if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
@ -1285,7 +1283,7 @@ FixedwingPositionControl::task_main()
}
/* XXX check if radius makes sense here */
float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius);
float turn_distance = _l1_control.switch_distance(_pos_sp_triplet.current.acceptance_radius);
/* lazily publish navigation capabilities */
if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {

2
src/modules/mavlink/orb_listener.c
View File

@ -410,7 +410,7 @@ l_local_position(const struct listener *l)
void
l_global_position_setpoint(const struct listener *l)
{
struct mission_item_triplet_s triplet;
struct position_setpoint_triplet_s triplet;
orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet);
uint8_t coordinate_frame = MAV_FRAME_GLOBAL;

31
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -117,7 +117,7 @@ private:
int _manual_sub; /**< notification of manual control updates */
int _arming_sub; /**< arming status of outputs */
int _local_pos_sub; /**< vehicle local position */
int _mission_items_sub; /**< mission item triplet */
int _pos_sp_triplet_sub; /**< mission item triplet */
orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
@ -130,7 +130,7 @@ private:
struct actuator_armed_s _arming; /**< actuator arming status */
struct vehicle_local_position_s _local_pos; /**< vehicle local position */
struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */
struct mission_item_triplet_s _mission_items; /**< vehicle global position setpoint */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
struct {
@ -236,7 +236,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_manual_sub(-1),
_arming_sub(-1),
_local_pos_sub(-1),
_mission_items_sub(-1),
_pos_sp_triplet_sub(-1),
/* publications */
_local_pos_sp_pub(-1),
@ -250,7 +250,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
memset(&_arming, 0, sizeof(_arming));
memset(&_local_pos, 0, sizeof(_local_pos));
memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
memset(&_mission_items, 0, sizeof(_mission_items));
memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet));
memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
_params.pos_p.zero();
@ -405,10 +405,10 @@ MulticopterPositionControl::poll_subscriptions()
if (updated)
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
orb_check(_mission_items_sub, &updated);
orb_check(_pos_sp_triplet_sub, &updated);
if (updated)
orb_copy(ORB_ID(mission_item_triplet), _mission_items_sub, &_mission_items);
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
float
@ -450,7 +450,7 @@ MulticopterPositionControl::task_main()
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_mission_items_sub = orb_subscribe(ORB_ID(mission_item_triplet));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
parameters_update(true);
@ -626,16 +626,15 @@ MulticopterPositionControl::task_main()
} else {
/* AUTO */
if (_mission_items.current_valid) {
struct mission_item_s item = _mission_items.current;
if (_pos_sp_triplet.current.valid) {
struct position_setpoint_s current_sp = _pos_sp_triplet.current;
// TODO home altitude can be != ref_alt, check home_position topic
_pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt);
_pos_sp(2) = -(current_sp.alt - ref_alt);
map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1));
map_projection_project(current_sp.lat, current_sp.lon, &_pos_sp(0), &_pos_sp(1));
if (isfinite(_mission_items.current.yaw)) {
_att_sp.yaw_body = _mission_items.current.yaw;
if (isfinite(current_sp.yaw)) {
_att_sp.yaw_body = current_sp.yaw;
}
/* in case of interrupted mission don't go to waypoint but stay at current position */
@ -688,7 +687,7 @@ MulticopterPositionControl::task_main()
if (!_control_mode.flag_control_manual_enabled) {
/* use constant descend rate when landing, ignore altitude setpoint */
if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) {
if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) {
_vel_sp(2) = _params.land_speed;
}
@ -790,7 +789,7 @@ MulticopterPositionControl::task_main()
float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2));
float tilt_max = _params.tilt_max;
if (!_control_mode.flag_control_manual_enabled) {
if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) {
if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.land_tilt_max;
if (thr_min < 0.0f)

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

@ -64,7 +64,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
@ -152,7 +152,7 @@ private:
int _onboard_mission_sub; /**< notification of onboard mission updates */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
orb_advert_t _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 */
orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */
@ -160,8 +160,10 @@ private:
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 mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
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 */
perf_counter_t _loop_perf; /**< loop performance counter */
@ -323,35 +325,24 @@ private:
void set_rtl_item();
/**
* Helper function to get a loiter item
* Set position setpoint for mission item
*/
void get_loiter_item(mission_item_s *item);
void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item);
/**
* Helper function to get a takeoff item
*/
void get_takeoff_item(mission_item_s *item);
void get_takeoff_setpoint(position_setpoint_s *pos_sp);
/**
* Publish a new mission item triplet for position controller
*/
void publish_mission_item_triplet();
void publish_position_setpoint_triplet();
/**
* Publish vehicle_control_mode topic for controllers
*/
void publish_control_mode();
/**
* Compare two mission items if they are equivalent
* Two mission items can be considered equivalent for the purpose of the navigator even if some fields differ.
*
* @return true if equivalent, false otherwise
*/
bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b);
void add_home_pos_to_rtl(struct mission_item_s *new_mission_item);
};
namespace navigator
@ -385,7 +376,7 @@ Navigator::Navigator() :
_capabilities_sub(-1),
/* publications */
_triplet_pub(-1),
_pos_sp_triplet_pub(-1),
_mission_result_pub(-1),
_control_mode_pub(-1),
@ -402,6 +393,7 @@ Navigator::Navigator() :
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_set_nav_state_timestamp(0),
_mission_item_valid(false),
_need_takeoff(true),
_do_takeoff(false),
_geofence_violation_warning_sent(false)
@ -414,14 +406,9 @@ Navigator::Navigator() :
_parameter_handles.land_alt = param_find("NAV_LAND_ALT");
_parameter_handles.rtl_alt = param_find("NAV_RTL_ALT");
_mission_item_triplet.previous_valid = false;
_mission_item_triplet.current_valid = false;
_mission_item_triplet.next_valid = false;
memset(&_mission_item_triplet.previous, 0, sizeof(struct mission_item_s));
memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s));
memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s));
memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s));
memset(&_mission_result, 0, sizeof(struct mission_result_s));
memset(&_mission_item, 0, sizeof(struct mission_item_s));
nav_states_str[0] = "NONE";
nav_states_str[1] = "READY";
@ -482,7 +469,6 @@ Navigator::parameters_update()
void
Navigator::global_position_update()
{
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
}
@ -938,23 +924,25 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
void
Navigator::start_none()
{
_mission_item_triplet.previous_valid = false;
_mission_item_triplet.current_valid = false;
_mission_item_triplet.next_valid = false;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
_mission_item_valid = false;
_reset_loiter_pos = true;
_do_takeoff = false;
_rtl_state = RTL_STATE_NONE;
publish_mission_item_triplet();
publish_position_setpoint_triplet();
}
void
Navigator::start_ready()
{
_mission_item_triplet.previous_valid = false;
_mission_item_triplet.current_valid = false;
_mission_item_triplet.next_valid = false;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
_mission_item_valid = false;
_reset_loiter_pos = true;
_do_takeoff = false;
@ -964,7 +952,7 @@ Navigator::start_ready()
_rtl_state = RTL_STATE_NONE;
}
publish_mission_item_triplet();
publish_position_setpoint_triplet();
}
void
@ -973,38 +961,38 @@ Navigator::start_loiter()
_do_takeoff = false;
/* set loiter position if needed */
if (_reset_loiter_pos || !_mission_item_triplet.current_valid) {
if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) {
_reset_loiter_pos = false;
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw
_pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw
_mission_item_triplet.current.altitude_is_relative = false;
float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude;
/* use current altitude if above min altitude set by parameter */
if (_global_pos.alt < min_alt_amsl) {
_mission_item_triplet.current.altitude = min_alt_amsl;
_pos_sp_triplet.current.altitude = min_alt_amsl;
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
} else {
_mission_item_triplet.current.altitude = _global_pos.alt;
_pos_sp_triplet.current.altitude = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
}
_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
if (_rtl_state == RTL_STATE_LAND) {
/* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */
_rtl_state = RTL_STATE_DESCEND;
}
}
_mission_item_triplet.previous_valid = false;
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = false;
_mission_item_valid = false;
get_loiter_item(&_mission_item_triplet.current);
publish_mission_item_triplet();
publish_position_setpoint_triplet();
}
void
@ -1020,8 +1008,7 @@ void
Navigator::set_mission_item()
{
/* copy current mission to previous item */
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_reset_loiter_pos = true;
_do_takeoff = false;
@ -1030,36 +1017,34 @@ Navigator::set_mission_item()
bool onboard;
unsigned index;
ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index);
ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
if (ret == OK) {
_mission_item_triplet.current_valid = true;
add_home_pos_to_rtl(&_mission_item_triplet.current);
_mission_item_valid = true;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
_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) {
/* don't reset RTL state on RTL or LOITER items */
_rtl_state = RTL_STATE_NONE;
}
if (_vstatus.is_rotary_wing) {
if (_need_takeoff && (
_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
_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
)) {
/* do special TAKEOFF handling for VTOL */
_need_takeoff = false;
/* calculate desired takeoff altitude AMSL */
float takeoff_alt_amsl = _mission_item_triplet.current.altitude;
if (_mission_item_triplet.current.altitude_is_relative)
takeoff_alt_amsl += _home_pos.altitude;
float takeoff_alt_amsl = _pos_sp_triplet.current.alt;
if (_vstatus.condition_landed) {
/* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */
@ -1067,30 +1052,29 @@ Navigator::set_mission_item()
}
/* check if we really need takeoff */
if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item_triplet.current.acceptance_radius) {
if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - item.acceptance_radius) {
/* force TAKEOFF if landed or waypoint altitude is more than current */
_do_takeoff = true;
/* move current mission item to next */
memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.next_valid = true;
/* move current position setpoint to next */
memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
/* set current item to TAKEOFF */
get_takeoff_item(&_mission_item_triplet.current);
/* set current setpoint to takeoff */
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_mission_item_triplet.current.altitude = takeoff_alt_amsl;
_mission_item_triplet.current.altitude_is_relative = false;
_pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_pos_sp_triplet.current.alt = takeoff_alt_amsl;
_pos_sp_triplet.current.yaw = NAN;
_pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF;
}
} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
} else if (item.nav_cmd == NAV_CMD_LAND) {
/* will need takeoff after landing */
_need_takeoff = true;
}
}
if (_do_takeoff) {
mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude);
mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.altitude);
} else {
if (onboard) {
mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
@ -1100,23 +1084,24 @@ Navigator::set_mission_item()
}
} else {
/* since a mission is not advanced without WPs available, this is not supposed to happen */
_mission_item_triplet.current_valid = false;
_mission_item_valid = false;
_pos_sp_triplet.current.valid = false;
warnx("ERROR: current WP can't be set");
}
if (!_do_takeoff) {
ret = _mission.get_next_mission_item(&_mission_item_triplet.next);
mission_item_s item_next;
ret = _mission.get_next_mission_item(&item_next);
if (ret == OK) {
add_home_pos_to_rtl(&_mission_item_triplet.next);
_mission_item_triplet.next_valid = true;
position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next);
} else {
/* this will fail for the last WP */
_mission_item_triplet.next_valid = false;
_pos_sp_triplet.next.valid = false;
}
}
publish_mission_item_triplet();
publish_position_setpoint_triplet();
}
void
@ -1129,8 +1114,8 @@ Navigator::start_rtl()
} else {
_rtl_state = RTL_STATE_RETURN;
if (_reset_loiter_pos) {
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.altitude = _global_pos.alt;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt;
}
}
}
@ -1144,99 +1129,110 @@ Navigator::set_rtl_item()
{
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
float climb_alt = _home_pos.altitude + _parameters.rtl_alt;
if (_vstatus.condition_landed)
if (_vstatus.condition_landed) {
climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
}
_mission_item_valid = true;
_mission_item.lat = (double)_global_pos.lat / 1e7d;
_mission_item.lon = (double)_global_pos.lon / 1e7d;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = climb_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_mission_item_triplet.current.altitude = climb_alt;
_mission_item_triplet.current.yaw = NAN;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
_mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
_mission_item_triplet.current.time_inside = 0.0f;
_mission_item_triplet.current.pitch_min = 0.0f;
_mission_item_triplet.current.autocontinue = true;
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude);
break;
}
case RTL_STATE_RETURN: {
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
_mission_item_valid = true;
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
// don't change altitude
_mission_item.yaw = NAN; // TODO set heading to home
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
_mission_item_triplet.current.lat = _home_pos.lat;
_mission_item_triplet.current.lon = _home_pos.lon;
// don't change altitude setpoint
_mission_item_triplet.current.yaw = NAN;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
_mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
_mission_item_triplet.current.time_inside = 0.0f;
_mission_item_triplet.current.pitch_min = 0.0f;
_mission_item_triplet.current.autocontinue = true;
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
break;
}
case RTL_STATE_DESCEND: {
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
_mission_item_valid = true;
float descend_alt = _home_pos.altitude + _parameters.land_alt;
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_pos.altitude + _parameters.land_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.lat = _home_pos.lat;
_mission_item_triplet.current.lon = _home_pos.lon;
_mission_item_triplet.current.altitude = descend_alt;
_mission_item_triplet.current.yaw = NAN;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
_mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
_mission_item_triplet.current.time_inside = 0.0f;
_mission_item_triplet.current.pitch_min = 0.0f;
_mission_item_triplet.current.autocontinue = true;
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude);
break;
}
case RTL_STATE_LAND: {
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
_mission_item_valid = true;
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_pos.altitude;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.lat = _home_pos.lat;
_mission_item_triplet.current.lon = _home_pos.lon;
_mission_item_triplet.current.altitude = _home_pos.altitude;
_mission_item_triplet.current.yaw = NAN;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
_mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.nav_cmd = NAV_CMD_LAND;
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
_mission_item_triplet.current.time_inside = 0.0f;
_mission_item_triplet.current.pitch_min = 0.0f;
_mission_item_triplet.current.autocontinue = true;
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
break;
}
@ -1247,18 +1243,46 @@ Navigator::set_rtl_item()
}
}
publish_mission_item_triplet();
publish_position_setpoint_triplet();
}
void
Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
{
sp->valid = true;
if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
/* set home position for RTL item */
sp->lat = _home_pos.lat;
sp->lon = _home_pos.lon;
sp->alt = _home_pos.altitude + _parameters.rtl_alt;
} else {
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->alt + _home_pos.altitude : item->alt;
}
sp->yaw = item->yaw;
if (item->nav_cmd == NAV_CMD_TAKEOFF) {
sp->type = SETPOINT_TYPE_TAKEOFF;
} else if (item->nav_cmd == NAV_CMD_LAND) {
sp->type = SETPOINT_TYPE_LAND;
} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
sp->type = SETPOINT_TYPE_LOITER;
} else {
sp->type = SETPOINT_TYPE_NORMAL;
}
}
bool
Navigator::check_mission_item_reached()
{
/* only check if there is actually a mission item to check */
if (!_mission_item_triplet.current_valid) {
if (!_mission_item_valid) {
return false;
}
if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
if (_vstatus.is_rotary_wing) {
return _vstatus.condition_landed;
} else {
@ -1269,10 +1293,10 @@ Navigator::check_mission_item_reached()
}
/* XXX TODO count turns */
if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
_mission_item_triplet.current.loiter_radius > 0.01f) {
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
_mission_item.loiter_radius > 0.01f) {
return false;
}
@ -1282,8 +1306,8 @@ Navigator::check_mission_item_reached()
if (!_waypoint_position_reached) {
float acceptance_radius;
if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) {
acceptance_radius = _mission_item_triplet.current.acceptance_radius;
if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) {
acceptance_radius = _mission_item.acceptance_radius;
} else {
acceptance_radius = _parameters.acceptance_radius;
@ -1294,11 +1318,11 @@ Navigator::check_mission_item_reached()
float dist_z = -1.0f;
/* current relative or AMSL altitude depending on mission item altitude_is_relative flag */
float wp_alt_amsl = _mission_item_triplet.current.altitude;
if (_mission_item_triplet.current.altitude_is_relative)
_mission_item_triplet.current.altitude += _home_pos.altitude;
float wp_alt_amsl = _mission_item.altitude;
if (_mission_item.altitude_is_relative)
_mission_itemt.altitude += _home_pos.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl,
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
(double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt,
&dist_xy, &dist_z);
@ -1315,9 +1339,9 @@ Navigator::check_mission_item_reached()
}
if (!_waypoint_yaw_reached) {
if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) {
if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw);
float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
_waypoint_yaw_reached = true;
}
@ -1330,14 +1354,14 @@ Navigator::check_mission_item_reached()
if (_waypoint_position_reached && _waypoint_yaw_reached) {
if (_time_first_inside_orbit == 0) {
_time_first_inside_orbit = now;
if (_mission_item_triplet.current.time_inside > 0.01f) {
mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside);
if (_mission_item.time_inside > 0.01f) {
mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside);
}
}
/* check if the MAV was long enough inside the waypoint orbit */
if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6)
|| _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
_time_first_inside_orbit = 0;
_waypoint_yaw_reached = false;
_waypoint_position_reached = false;
@ -1389,67 +1413,16 @@ Navigator::on_mission_item_reached()
}
void
Navigator::get_loiter_item(struct mission_item_s *item)
{
//item->altitude_is_relative
//item->lat
//item->lon
//item->altitude
//item->yaw
item->loiter_radius = _parameters.loiter_radius;
item->loiter_direction = 1;
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
item->acceptance_radius = _parameters.acceptance_radius;
item->time_inside = 0.0f;
item->pitch_min = 0.0f;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
}
void
Navigator::get_takeoff_item(mission_item_s *item)
{
//item->altitude_is_relative
//item->lat
//item->lon
//item->altitude
item->yaw = NAN;
item->loiter_radius = _parameters.loiter_radius;
item->loiter_direction = 1;
item->nav_cmd = NAV_CMD_TAKEOFF;
item->acceptance_radius = _parameters.acceptance_radius;
item->time_inside = 0.0f;
item->pitch_min = 0.0;
item->autocontinue = true;
item->origin = ORIGIN_ONBOARD;
}
void
Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item)
{
if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
/* append the home position to RTL item */
new_mission_item->lat = _home_pos.lat;
new_mission_item->lon = _home_pos.lon;
new_mission_item->altitude = _home_pos.altitude + _parameters.rtl_alt;
new_mission_item->altitude_is_relative = false;
new_mission_item->loiter_radius = _parameters.loiter_radius;
new_mission_item->acceptance_radius = _parameters.acceptance_radius;
}
}
void
Navigator::publish_mission_item_triplet()
Navigator::publish_position_setpoint_triplet()
{
/* lazily publish the mission triplet only once available */
if (_triplet_pub > 0) {
if (_pos_sp_triplet_pub > 0) {
/* publish the mission triplet */
orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet);
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
} else {
/* advertise and publish */
_triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet);
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
}
}
@ -1534,24 +1507,6 @@ Navigator::publish_control_mode()
}
}
bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) {
if (a.altitude_is_relative == b.altitude_is_relative &&
fabs(a.lat - b.lat) < FLT_EPSILON &&
fabs(a.lon - b.lon) < FLT_EPSILON &&
fabsf(a.altitude - b.altitude) < FLT_EPSILON &&
fabsf(a.yaw - b.yaw) < FLT_EPSILON &&
fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON &&
a.loiter_direction == b.loiter_direction &&
a.nav_cmd == b.nav_cmd &&
fabsf(a.acceptance_radius - b.acceptance_radius) < FLT_EPSILON &&
fabsf(a.time_inside - b.time_inside) < FLT_EPSILON &&
a.autocontinue == b.autocontinue) {
return true;
} else {
return false;
}
}
void Navigator::add_fence_point(int argc, char *argv[])
{
_geofence.addPoint(argc, argv);

2
src/modules/sdlog2/sdlog2.c
View File

@ -750,7 +750,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
struct mission_item_triplet_s triplet;
struct position_setpoint_triplet_s triplet;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;

4
src/modules/uORB/objects_common.cpp
View File

@ -117,8 +117,8 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
#include "topics/vehicle_bodyframe_speed_setpoint.h"
ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s);
#include "topics/mission_item_triplet.h"
ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s);
#include "topics/position_setpoint_triplet.h"
ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
#include "topics/vehicle_global_velocity_setpoint.h"
ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);

38
src/modules/uORB/topics/mission_item_triplet.h → src/modules/uORB/topics/position_setpoint_triplet.h
View File

@ -46,31 +46,39 @@
#include <stdbool.h>
#include "../uORB.h"
#include "mission.h"
/**
* @addtogroup topics
* @{
*/
enum SETPOINT_TYPE
{
SETPOINT_TYPE_NORMAL = 0,
SETPOINT_TYPE_LOITER,
SETPOINT_TYPE_TAKEOFF,
SETPOINT_TYPE_LAND,
};
struct position_setpoint_s
{
bool valid; /**< true if point is valid */
double lat; /**< latitude, in deg */
double lon; /**< longitude, in deg */
float alt; /**< altitude AMSL, in m */
float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN to hold current yaw */
enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */
};
/**
* Global position setpoint triplet in WGS84 coordinates.
*
* This are the three next waypoints (or just the next two or one).
*/
struct mission_item_triplet_s
struct position_setpoint_triplet_s
{
bool previous_valid;
bool current_valid; /**< flag indicating previous mission item is valid */
bool next_valid; /**< flag indicating next mission item is valid */
struct mission_item_s previous;
struct mission_item_s current;
struct mission_item_s next;
int previous_index;
int current_index;
int next_index;
struct position_setpoint_s previous;
struct position_setpoint_s current;
struct position_setpoint_s next;
};
/**
@ -78,6 +86,6 @@ struct mission_item_triplet_s
*/
/* register this as object request broker structure */
ORB_DECLARE(mission_item_triplet);
ORB_DECLARE(position_setpoint_triplet);
#endif