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

FW add mavlink NAV_CONTROLLER_OUTPUT

This commit is contained in:
Daniel Agar 2016-04-22 21:45:34 -04:00 committed by Lorenz Meier
parent 5fb1de906c
commit eae726e345
9 changed files with 170 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
msg/navigation_capabilities.msg → msg/fw_pos_ctrl_status.msg
View File

@ -1,5 +1,14 @@
uint64 timestamp # timestamp this topic was emitted
float32 nav_roll
float32 nav_pitch
float32 nav_bearing
float32 target_bearing
float32 wp_dist
float32 xtrack_error
float32 turn_distance # the optimal distance to a waypoint to switch to the next
float32 landing_horizontal_slope_displacement
float32 landing_slope_angle_rad
float32 landing_flare_length

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

@ -86,9 +86,9 @@
#include <systemlib/systemlib.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_combined.h>
@ -170,13 +170,13 @@ private:
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
orb_advert_t _fw_pos_ctrl_status_pub; /**< navigation capabilities publication */
orb_id_t _attitude_setpoint_id;
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct fw_pos_ctrl_status_s _fw_pos_ctrl_status; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< control mode */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
@ -408,7 +408,7 @@ private:
/**
* Publish navigation capabilities
*/
void navigation_capabilities_publish();
void fw_pos_ctrl_status_publish();
/**
* Get a new waypoint based on heading and distance from current position
@ -530,7 +530,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* publications */
_attitude_sp_pub(nullptr),
_tecs_status_pub(nullptr),
_nav_capabilities_pub(nullptr),
_fw_pos_ctrl_status_pub(nullptr),
/* publication ID */
_attitude_setpoint_id(0),
@ -538,7 +538,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* states */
_ctrl_state(),
_att_sp(),
_nav_capabilities(),
_fw_pos_ctrl_status(),
_manual(),
_control_mode(),
_vehicle_status(),
@ -593,7 +593,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_l1_control(),
_control_mode_current(FW_POSCTRL_MODE_OTHER)
{
_nav_capabilities = {};
_fw_pos_ctrl_status = {};
_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
@ -769,10 +769,10 @@ FixedwingPositionControl::parameters_update()
_parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
/* Update and publish the navigation capabilities */
_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
_nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement();
_nav_capabilities.landing_flare_length = landingslope.flare_length();
navigation_capabilities_publish();
_fw_pos_ctrl_status.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
_fw_pos_ctrl_status.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement();
_fw_pos_ctrl_status.landing_flare_length = landingslope.flare_length();
fw_pos_ctrl_status_publish();
/* Update Launch Detector Parameters */
launchDetector.updateParams();
@ -1016,15 +1016,15 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
}
}
void FixedwingPositionControl::navigation_capabilities_publish()
void FixedwingPositionControl::fw_pos_ctrl_status_publish()
{
_nav_capabilities.timestamp = hrt_absolute_time();
_fw_pos_ctrl_status.timestamp = hrt_absolute_time();
if (_nav_capabilities_pub != nullptr) {
orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
if (_fw_pos_ctrl_status_pub != nullptr) {
orb_publish(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_pub, &_fw_pos_ctrl_status);
} else {
_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
_fw_pos_ctrl_status_pub = orb_advertise(ORB_ID(fw_pos_ctrl_status), &_fw_pos_ctrl_status);
}
}
@ -1337,7 +1337,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float alt_sp;
if (_nav_capabilities.abort_landing == true) {
if (_fw_pos_ctrl_status.abort_landing == true) {
// if we entered loiter due to an aborted landing, demand
// altitude setpoint well above landing waypoint
alt_sp = pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff;
@ -1349,7 +1349,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (in_takeoff_situation() ||
((_global_pos.alt < pos_sp_triplet.current.alt + _parameters.climbout_diff)
&& _nav_capabilities.abort_landing == true)) {
&& _fw_pos_ctrl_status.abort_landing == true)) {
/* 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));
@ -1465,7 +1465,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
// still no valid terrain, abort landing
terrain_alt = pos_sp_triplet.current.alt;
_nav_capabilities.abort_landing = true;
_fw_pos_ctrl_status.abort_landing = true;
}
} else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000)
@ -1478,7 +1478,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
// terrain alt was not valid for long time, abort landing
terrain_alt = _t_alt_prev_valid;
_nav_capabilities.abort_landing = true;
_fw_pos_ctrl_status.abort_landing = true;
}
} else {
@ -1954,7 +1954,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_time_last_t_alt = 0;
// reset lading abort state
_nav_capabilities.abort_landing = false;
_fw_pos_ctrl_status.abort_landing = false;
/* no flight mode applies, do not publish an attitude setpoint */
setpoint = false;
@ -2169,15 +2169,24 @@ FixedwingPositionControl::task_main()
float turn_distance = _l1_control.switch_distance(100.0f);
/* lazily publish navigation capabilities */
if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000)
|| (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON
if ((hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) > 1000000)
|| (fabsf(turn_distance - _fw_pos_ctrl_status.turn_distance) > FLT_EPSILON
&& turn_distance > 0)) {
/* set new turn distance */
_nav_capabilities.turn_distance = turn_distance;
_fw_pos_ctrl_status.turn_distance = turn_distance;
navigation_capabilities_publish();
_fw_pos_ctrl_status.nav_roll = _l1_control.nav_roll();
_fw_pos_ctrl_status.nav_pitch = get_tecs_pitch();
_fw_pos_ctrl_status.nav_bearing = _l1_control.nav_bearing();
_fw_pos_ctrl_status.target_bearing = _l1_control.target_bearing();
_fw_pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();
math::Vector<2> curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon);
_fw_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
fw_pos_ctrl_status_publish();
}
}
@ -2242,9 +2251,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
_was_in_transition = true;
_asp_after_transition = _ctrl_state.airspeed;
// after transition we ramp up desired airspeed from the speed we had coming out of the transition
} else if (_was_in_transition) {
// after transition we ramp up desired airspeed from the speed we had coming out of the transition
_asp_after_transition += dt * 2; // increase 2m/s
if (_asp_after_transition < v_sp && _ctrl_state.airspeed < v_sp) {

3
src/modules/mavlink/mavlink_main.cpp
View File

@ -1840,6 +1840,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("NAMED_VALUE_FLOAT", 1.0f);
configure_stream("ESTIMATOR_STATUS", 0.5f);
configure_stream("ADSB_VEHICLE", 2.0f);
configure_stream("NAV_CONTROLLER_OUTPUT", 2.0f);
break;
case MAVLINK_MODE_ONBOARD:
@ -1870,6 +1871,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("VISION_POSITION_NED", 10.0f);
configure_stream("ESTIMATOR_STATUS", 1.0f);
configure_stream("ADSB_VEHICLE", 10.0f);
configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
break;
case MAVLINK_MODE_OSD:
@ -1919,6 +1921,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("VISION_POSITION_NED", 10.0f);
configure_stream("ESTIMATOR_STATUS", 5.0f);
configure_stream("ADSB_VEHICLE", 20.0f);
configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
default:
break;
}

121
src/modules/mavlink/mavlink_messages.cpp
View File

@ -46,38 +46,39 @@
#include <commander/px4_custom_mode.h>
#include <lib/geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/err.h>
@ -2576,6 +2577,72 @@ protected:
}
};
class MavlinkStreamNavControllerOutput : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamNavControllerOutput::get_name_static();
}
static const char *get_name_static()
{
return "NAV_CONTROLLER_OUTPUT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamNavControllerOutput(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_fw_pos_ctrl_status_sub;
MavlinkOrbSubscription *_tecs_status_sub;
/* do not allow top copying this class */
MavlinkStreamNavControllerOutput(MavlinkStreamNavControllerOutput &);
MavlinkStreamNavControllerOutput& operator = (const MavlinkStreamNavControllerOutput &);
protected:
explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink),
_fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))),
_tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status)))
{}
void send(const hrt_abstime t)
{
struct fw_pos_ctrl_status_s _fw_pos_ctrl_status = {};
struct tecs_status_s _tecs_status = {};
const bool updated_fw_pos_ctrl_status = _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status);
const bool updated_tecs = _tecs_status_sub->update(&_tecs_status);
if (updated_fw_pos_ctrl_status || updated_tecs) {
mavlink_nav_controller_output_t msg = {};
msg.nav_roll = math::degrees(_fw_pos_ctrl_status.nav_roll);
msg.nav_pitch = math::degrees(_fw_pos_ctrl_status.nav_pitch);
msg.nav_bearing = (int16_t)math::degrees(_fw_pos_ctrl_status.nav_bearing);
msg.target_bearing = (int16_t)math::degrees(_fw_pos_ctrl_status.target_bearing);
msg.wp_dist = (uint16_t)_fw_pos_ctrl_status.wp_dist;
msg.xtrack_error = _fw_pos_ctrl_status.xtrack_error;
msg.alt_error = _tecs_status.altitude_filtered - _tecs_status.altitudeSp;
msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeedSp;
_mavlink->send_message(MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, &msg);
}
}
};
class MavlinkStreamCameraCapture : public MavlinkStream
{

15
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -55,11 +55,11 @@
MissionFeasibilityChecker::MissionFeasibilityChecker() :
_mavlink_log_pub(nullptr),
_capabilities_sub(-1),
_fw_pos_ctrl_status_sub(-1),
_initDone(false),
_dist_1wp_ok(false)
{
_nav_caps = {0};
_fw_pos_ctrl_status = {};
}
@ -275,7 +275,6 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
/* Go through all mission items and search for a landing waypoint
* if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
@ -293,15 +292,15 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
}
float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon);
float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
// warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
// wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
// warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f",
// _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length);
if (wp_distance > _nav_caps.landing_flare_length) {
if (wp_distance > _fw_pos_ctrl_status.landing_flare_length) {
/* Last wp is before flare region */
if (delta_altitude < 0) {
@ -424,14 +423,14 @@ MissionFeasibilityChecker::isPositionCommand(unsigned cmd){
void MissionFeasibilityChecker::updateNavigationCapabilities()
{
(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
(void)orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
}
void MissionFeasibilityChecker::init()
{
if (!_initDone) {
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
_initDone = true;
}

6
src/modules/navigator/mission_feasibility_checker.h
View File

@ -44,7 +44,7 @@
#include <unistd.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <dataman/dataman.h>
#include "geofence.h"
@ -54,8 +54,8 @@ class MissionFeasibilityChecker
private:
orb_advert_t *_mavlink_log_pub;
int _capabilities_sub;
struct navigation_capabilities_s _nav_caps;
int _fw_pos_ctrl_status_sub;
struct fw_pos_ctrl_status_s _fw_pos_ctrl_status;
bool _initDone;
bool _dist_1wp_ok;

8
src/modules/navigator/navigator.h
View File

@ -207,7 +207,7 @@ private:
int _home_pos_sub; /**< home position subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _land_detected_sub; /**< vehicle land detected subscription */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _fw_pos_ctrl_status_sub; /**< notification of vehicle capabilities updates */
int _control_mode_sub; /**< vehicle control mode subscription */
int _onboard_mission_sub; /**< onboard mission subscription */
int _offboard_mission_sub; /**< offboard mission subscription */
@ -229,7 +229,7 @@ private:
sensor_combined_s _sensor_combined; /**< sensor values */
home_position_s _home_pos; /**< home position for RTL */
mission_item_s _mission_item; /**< current mission item */
navigation_capabilities_s _nav_caps; /**< navigation capabilities */
fw_pos_ctrl_status_s _fw_pos_ctrl_status; /**< fixed wing navigation capabilities */
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
position_setpoint_triplet_s _reposition_triplet; /**< triplet for non-mission direct position command */
position_setpoint_triplet_s _takeoff_triplet; /**< triplet for non-mission direct takeoff command */
@ -301,9 +301,9 @@ private:
void home_position_update(bool force=false);
/**
* Retreive navigation capabilities
* Retrieve fixed wing navigation capabilities
*/
void navigation_capabilities_update();
void fw_pos_ctrl_status_update();
/**
* Retrieve vehicle status

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

@ -70,7 +70,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/fence.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/vehicle_command.h>
#include <drivers/drv_baro.h>
@ -108,7 +108,7 @@ Navigator::Navigator() :
_home_pos_sub(-1),
_vstatus_sub(-1),
_land_detected_sub(-1),
_capabilities_sub(-1),
_fw_pos_ctrl_status_sub(-1),
_control_mode_sub(-1),
_onboard_mission_sub(-1),
_offboard_mission_sub(-1),
@ -126,7 +126,7 @@ Navigator::Navigator() :
_sensor_combined{},
_home_pos{},
_mission_item{},
_nav_caps{},
_fw_pos_ctrl_status{},
_pos_sp_triplet{},
_reposition_triplet{},
_takeoff_triplet{},
@ -231,9 +231,9 @@ Navigator::home_position_update(bool force)
}
void
Navigator::navigation_capabilities_update()
Navigator::fw_pos_ctrl_status_update()
{
orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
}
void
@ -298,7 +298,7 @@ Navigator::task_main()
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@ -316,7 +316,7 @@ Navigator::task_main()
gps_position_update();
sensor_combined_update();
home_position_update(true);
navigation_capabilities_update();
fw_pos_ctrl_status_update();
params_update();
/* wakeup source(s) */
@ -395,9 +395,9 @@ Navigator::task_main()
}
/* navigation capabilities updated */
orb_check(_capabilities_sub, &updated);
orb_check(_fw_pos_ctrl_status_sub, &updated);
if (updated) {
navigation_capabilities_update();
fw_pos_ctrl_status_update();
}
/* home position updated */
@ -524,7 +524,7 @@ Navigator::task_main()
_can_loiter_at_sp = false;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
if (_nav_caps.abort_landing) {
if (_fw_pos_ctrl_status.abort_landing) {
// pos controller aborted landing, requests loiter
// above landing waypoint
_navigation_mode = &_loiter;
@ -732,9 +732,9 @@ Navigator::get_acceptance_radius(float mission_item_radius)
// when in fixed wing mode
// this might need locking against a commanded transition
// so that a stale _vstatus doesn't trigger an accepted mission item.
if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode && hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) {
if (_nav_caps.turn_distance > radius) {
radius = _nav_caps.turn_distance;
if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode) {
if ((hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) < 5000000) && (_fw_pos_ctrl_status.turn_distance > radius)) {
radius = _fw_pos_ctrl_status.turn_distance;
}
}

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

@ -229,8 +229,8 @@ ORB_DEFINE(test_motor, struct test_motor_s);
#include "topics/debug_key_value.h"
ORB_DEFINE(debug_key_value, struct debug_key_value_s);
#include "topics/navigation_capabilities.h"
ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s);
#include "topics/fw_pos_ctrl_status.h"
ORB_DEFINE(fw_pos_ctrl_status, struct fw_pos_ctrl_status_s);
#include "topics/esc_status.h"
ORB_DEFINE(esc_status, struct esc_status_s);