fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 07:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'beta_mavlink' into beta_mavlink2

Browse Source
This commit is contained in:
Anton Babushkin
2014-02-24 23:49:36 +04:00
parent d8fdade6ab 926c4701c7
commit 9ffc70de40
14 changed files with 203 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/drv_pwm_output.h
+1 -1
View File
@@ -160,7 +160,7 @@ ORB_DECLARE(output_pwm);
#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ #define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ #define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
/** power up DSM receiver */ /** power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
src/drivers/px4io/px4io.cpp
+32 -25
View File
@@ -244,8 +244,7 @@ private:
volatile int _task; ///< worker task id volatile int _task; ///< worker task id
volatile bool _task_should_exit; ///< worker terminate flag volatile bool _task_should_exit; ///< worker terminate flag
int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread. int _mavlink_fd; ///< mavlink file descriptor.
int _thread_mavlink_fd; ///< mavlink file descriptor for thread.
perf_counter_t _perf_update; ///<local performance counter for status updates perf_counter_t _perf_update; ///<local performance counter for status updates
perf_counter_t _perf_write; ///<local performance counter for PWM control writes perf_counter_t _perf_write; ///<local performance counter for PWM control writes
@@ -474,7 +473,6 @@ PX4IO::PX4IO(device::Device *interface) :
_task(-1), _task(-1),
_task_should_exit(false), _task_should_exit(false),
_mavlink_fd(-1), _mavlink_fd(-1),
_thread_mavlink_fd(-1),
_perf_update(perf_alloc(PC_ELAPSED, "io update")), _perf_update(perf_alloc(PC_ELAPSED, "io update")),
_perf_write(perf_alloc(PC_ELAPSED, "io write")), _perf_write(perf_alloc(PC_ELAPSED, "io write")),
_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")), _perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
@@ -507,9 +505,6 @@ PX4IO::PX4IO(device::Device *interface) :
/* we need this potentially before it could be set in task_main */ /* we need this potentially before it could be set in task_main */
g_dev = this; g_dev = this;
/* open MAVLink text channel */
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
_debug_enabled = false; _debug_enabled = false;
_servorail_status.rssi_v = 0; _servorail_status.rssi_v = 0;
} }
@@ -785,7 +780,7 @@ PX4IO::task_main()
hrt_abstime poll_last = 0; hrt_abstime poll_last = 0;
hrt_abstime orb_check_last = 0; hrt_abstime orb_check_last = 0;
_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
/* /*
* Subscribe to the appropriate PWM output topic based on whether we are the * Subscribe to the appropriate PWM output topic based on whether we are the
@@ -880,6 +875,10 @@ PX4IO::task_main()
/* run at 5Hz */ /* run at 5Hz */
orb_check_last = now; orb_check_last = now;
/* try to claim the MAVLink log FD */
if (_mavlink_fd < 0)
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
/* check updates on uORB topics and handle it */ /* check updates on uORB topics and handle it */
bool updated = false; bool updated = false;
@@ -1275,16 +1274,14 @@ void
PX4IO::dsm_bind_ioctl(int dsmMode) PX4IO::dsm_bind_ioctl(int dsmMode)
{ {
if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
/* 0: dsm2, 1:dsmx */ mavlink_log_info(_mavlink_fd, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8"));
if ((dsmMode == 0) || (dsmMode == 1)) { int ret = ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8"));
ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); if (ret)
} else { mavlink_log_critical(_mavlink_fd, "binding failed.");
mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected");
}
} else { } else {
mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); mavlink_log_info(_mavlink_fd, "[IO] system armed, bind request rejected");
} }
} }
@@ -2115,14 +2112,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
break; break;
case DSM_BIND_START: case DSM_BIND_START:
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
usleep(500000); /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); if (arg == DSM2_BIND_PULSES ||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); arg == DSMX_BIND_PULSES ||
usleep(72000); arg == DSMX8_BIND_PULSES) {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
usleep(50000); usleep(500000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
usleep(72000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
usleep(50000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
ret = OK;
} else {
ret = -EINVAL;
}
break; break;
case DSM_BIND_POWER_UP: case DSM_BIND_POWER_UP:
@@ -2615,7 +2622,7 @@ bind(int argc, char *argv[])
#endif #endif
if (argc < 3) if (argc < 3)
errx(0, "needs argument, use dsm2 or dsmx"); errx(0, "needs argument, use dsm2, dsmx or dsmx8");
if (!strcmp(argv[2], "dsm2")) if (!strcmp(argv[2], "dsm2"))
pulses = DSM2_BIND_PULSES; pulses = DSM2_BIND_PULSES;
@@ -2624,7 +2631,7 @@ bind(int argc, char *argv[])
else if (!strcmp(argv[2], "dsmx8")) else if (!strcmp(argv[2], "dsmx8"))
pulses = DSMX8_BIND_PULSES; pulses = DSMX8_BIND_PULSES;
else else
errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]);
// Test for custom pulse parameter // Test for custom pulse parameter
if (argc > 3) if (argc > 3)
pulses = atoi(argv[3]); pulses = atoi(argv[3]);
src/lib/ecl/ecl.h
+1 -2
View File
@@ -38,7 +38,6 @@
*/ */
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <geo/geo.h>
#define ecl_absolute_time hrt_absolute_time #define ecl_absolute_time hrt_absolute_time
#define ecl_elapsed_time hrt_elapsed_time #define ecl_elapsed_time hrt_elapsed_time
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+11 -2
View File
@@ -38,6 +38,8 @@
* *
*/ */
#include <float.h>
#include "ecl_l1_pos_controller.h" #include "ecl_l1_pos_controller.h"
float ECL_L1_Pos_Controller::nav_roll() 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 */ /* 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); 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;
math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
/* 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 */ /* calculate eta angle towards the loiter center */
src/lib/external_lgpl/tecs/tecs.cpp
+1 -4
View File
@@ -3,13 +3,10 @@
#include "tecs.h" #include "tecs.h"
#include <ecl/ecl.h> #include <ecl/ecl.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <geo/geo.h>
using namespace math; using namespace math;
#ifndef CONSTANTS_ONE_G
#define CONSTANTS_ONE_G GRAVITY
#endif
/** /**
* @file tecs.cpp * @file tecs.cpp
* *
src/lib/external_lgpl/tecs/tecs.h
+13 -14
View File
@@ -28,16 +28,7 @@ class __EXPORT TECS
{ {
public: public:
TECS() : TECS() :
_pitch_dem(0.0f),
_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),
_integ1_state(0.0f), _integ1_state(0.0f),
_integ2_state(0.0f), _integ2_state(0.0f),
_integ3_state(0.0f), _integ3_state(0.0f),
@@ -45,8 +36,16 @@ public:
_integ5_state(0.0f), _integ5_state(0.0f),
_integ6_state(0.0f), _integ6_state(0.0f),
_integ7_state(0.0f), _integ7_state(0.0f),
_pitch_dem(0.0f),
_last_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), _SPE_dem(0.0f),
_SKE_dem(0.0f), _SKE_dem(0.0f),
_SPEdot_dem(0.0f), _SPEdot_dem(0.0f),
@@ -55,9 +54,9 @@ public:
_SKE_est(0.0f), _SKE_est(0.0f),
_SPEdot(0.0f), _SPEdot(0.0f),
_SKEdot(0.0f), _SKEdot(0.0f),
_vel_dot(0.0f), _airspeed_enabled(false),
_STEdotErrLast(0.0f) { _throttle_slewrate(0.0f)
{
} }
bool airspeed_sensor_enabled() { bool airspeed_sensor_enabled() {
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+58 -76
View File
@@ -130,23 +130,23 @@ private:
int _att_sub; /**< vehicle attitude subscription */ int _att_sub; /**< vehicle attitude subscription */
int _attitude_sub; /**< raw rc channels data subscription */ int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed 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 _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control 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 _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */ struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< vehicle status */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct position_setpoint_triplet_s _pos_sp_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 */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _loop_perf; /**< loop performance counter */
@@ -154,13 +154,13 @@ private:
/** manual control states */ /** manual control states */
float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */
float _loiter_hold_lat; double _loiter_hold_lat;
float _loiter_hold_lon; double _loiter_hold_lon;
float _loiter_hold_alt; float _loiter_hold_alt;
bool _loiter_hold; bool _loiter_hold;
float _launch_lat; double _launch_lat;
float _launch_lon; double _launch_lon;
float _launch_alt; float _launch_alt;
bool _launch_valid; bool _launch_valid;
@@ -192,7 +192,7 @@ private:
uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures 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 float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
bool _global_pos_valid; ///< global position is valid 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; ECL_L1_Pos_Controller _l1_control;
TECS _tecs; TECS _tecs;
@@ -233,7 +233,6 @@ private:
float speedrate_p; float speedrate_p;
float land_slope_angle; float land_slope_angle;
float land_slope_length;
float land_H1_virt; float land_H1_virt;
float land_flare_alt_relative; float land_flare_alt_relative;
float land_thrust_lim_alt_relative; float land_thrust_lim_alt_relative;
@@ -278,7 +277,6 @@ private:
param_t speedrate_p; param_t speedrate_p;
param_t land_slope_angle; param_t land_slope_angle;
param_t land_slope_length;
param_t land_H1_virt; param_t land_H1_virt;
param_t land_flare_alt_relative; param_t land_flare_alt_relative;
param_t land_thrust_lim_alt_relative; param_t land_thrust_lim_alt_relative;
@@ -362,6 +360,7 @@ FixedwingPositionControl *g_control;
FixedwingPositionControl::FixedwingPositionControl() : FixedwingPositionControl::FixedwingPositionControl() :
_mavlink_fd(-1),
_task_should_exit(false), _task_should_exit(false),
_control_task(-1), _control_task(-1),
@@ -380,38 +379,33 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* performance counters */ /* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
/* states */ /* states */
_setpoint_valid(false), _setpoint_valid(false),
_loiter_hold(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_horizontal(false),
land_noreturn_vertical(false), land_noreturn_vertical(false),
land_stayonground(false), land_stayonground(false),
land_motor_lim(false), land_motor_lim(false),
land_onslope(false), land_onslope(false),
flare_curve_alt_last(0.0f),
_mavlink_fd(-1),
launchDetector(),
launch_detected(false), 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; _nav_capabilities.turn_distance = 0.0f;
_parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_period = param_find("FW_L1_PERIOD");
@@ -431,7 +425,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
_parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); _parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
_parameter_handles.land_slope_length = param_find("FW_LND_SLLR");
_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
@@ -520,7 +513,6 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
@@ -587,8 +579,8 @@ FixedwingPositionControl::vehicle_control_mode_poll()
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
if (!was_armed && _control_mode.flag_armed) { if (!was_armed && _control_mode.flag_armed) {
_launch_lat = _global_pos.lat / 1e7f; _launch_lat = _global_pos.lat;
_launch_lon = _global_pos.lon / 1e7f; _launch_lon = _global_pos.lon;
_launch_alt = _global_pos.alt; _launch_alt = _global_pos.alt;
_launch_valid = true; _launch_valid = true;
} }
@@ -703,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) 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 */ /* rotate ground speed vector with current attitude */
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -889,8 +881,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_land = 1.3f * _parameters.airspeed_min;
float airspeed_approach = 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; /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
@@ -916,7 +910,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
/* avoid climbout */ /* avoid climbout */
if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground)
{ {
flare_curve_alt = pos_sp_triplet.current.alt; flare_curve_alt = pos_sp_triplet.current.alt;
land_stayonground = true; land_stayonground = true;
@@ -935,38 +929,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
//warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
flare_curve_alt_last = flare_curve_alt; flare_curve_alt_last = flare_curve_alt;
} else if (wp_distance < L_wp_distance) {
/* minimize speed to approach speed, stay on landing slope */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, flare_pitch_angle_rad,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
//warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length);
if (!land_onslope) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
land_onslope = true;
}
} else { } else {
/* intersect glide slope: /* intersect glide slope:
* if current position is higher or within 10m of slope follow the glide slope * minimize speed to approach speed
* if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope * if current position is higher or within 10m of slope follow the glide slope
* */ * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
* */
float altitude_desired = _global_pos.alt; float altitude_desired = _global_pos.alt;
if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { if (_global_pos.alt > landing_slope_alt_desired - 10.0f) {
/* stay on slope */ /* stay on slope */
altitude_desired = landing_slope_alt_desired; altitude_desired = landing_slope_alt_desired;
//warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); if (!land_onslope) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
land_onslope = true;
}
} else { } else {
/* continue horizontally */ /* continue horizontally */
altitude_desired = math::max(_global_pos.alt, L_altitude); altitude_desired = math::max(_global_pos.alt, L_altitude);
//warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance);
} }
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach),
@@ -1074,13 +1054,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_seatbelt_hold_heading = _att.yaw + _manual.yaw; _seatbelt_hold_heading = _att.yaw + _manual.yaw;
} }
/* climb out control */ //XXX not used
bool climb_out = false;
/* user wants to climb out */ /* climb out control */
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { // bool climb_out = false;
climb_out = true; //
} // /* 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 */ /* if in seatbelt mode, set airspeed based on manual control */
@@ -1287,7 +1269,7 @@ FixedwingPositionControl::task_main()
float turn_distance = _l1_control.switch_distance(100.0f); float turn_distance = _l1_control.switch_distance(100.0f);
/* lazily publish navigation capabilities */ /* 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 */ /* set new turn distance */
_nav_capabilities.turn_distance = turn_distance; _nav_capabilities.turn_distance = turn_distance;
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
-7
View File
@@ -348,13 +348,6 @@ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
*/ */
PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
/**
* Landing slope length
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
/** /**
* *
* *
src/modules/mavlink/mavlink_main.cpp
+21 -10
View File
@@ -169,8 +169,10 @@ Mavlink::Mavlink() :
/* performance counters */ /* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")),
_mavlink_hil_enabled(false) _mavlink_hil_enabled(false),
// _params_sub(-1) // _params_sub(-1)
mission_pub(-1)
{ {
wpm = &wpm_s; wpm = &wpm_s;
fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl;
@@ -985,7 +987,9 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
mavlink_missionlib_send_message(&msg); mavlink_missionlib_send_message(&msg);
if (_verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); } else if (seq == 0 && wpm->size == 0) {
/* don't broadcast if no WPs */
} else { } else {
mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
@@ -1139,7 +1143,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mission.current_index = wpc.seq; mission.current_index = wpc.seq;
publish_mission(); publish_mission();
mavlink_wpm_send_waypoint_current(wpc.seq); /* don't answer yet, wait for the navigator to respond, then publish the mission_result */
// mavlink_wpm_send_waypoint_current(wpc.seq);
} else { } else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
@@ -1534,7 +1539,7 @@ Mavlink::task_main(int argc, char *argv[])
argc -= 2; argc -= 2;
argv += 2; argv += 2;
while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { while ((ch = getopt(argc, argv, "b:d:eov")) != EOF) {
switch (ch) { switch (ch) {
case 'b': case 'b':
_baudrate = strtoul(optarg, NULL, 10); _baudrate = strtoul(optarg, NULL, 10);
@@ -1558,6 +1563,7 @@ Mavlink::task_main(int argc, char *argv[])
case 'v': case 'v':
_verbose = true; _verbose = true;
break;
default: default:
usage(); usage();
@@ -1701,8 +1707,7 @@ Mavlink::task_main(int argc, char *argv[])
thread_running = true; thread_running = true;
/* arm counter to go off immediately */ unsigned lowspeed_counter = 0;
unsigned lowspeed_counter = 10;
/* wakeup source(s) */ /* wakeup source(s) */
struct pollfd fds[1]; struct pollfd fds[1];
@@ -1732,7 +1737,7 @@ Mavlink::task_main(int argc, char *argv[])
} }
/* 1 Hz */ /* 1 Hz */
if (lowspeed_counter == 10) { if (lowspeed_counter % 10 == 0) {
mavlink_update_system(); mavlink_update_system();
/* translate the current system state to mavlink state and mode */ /* translate the current system state to mavlink state and mode */
@@ -1765,7 +1770,12 @@ Mavlink::task_main(int argc, char *argv[])
v_status.errors_count2, v_status.errors_count2,
v_status.errors_count3, v_status.errors_count3,
v_status.errors_count4); v_status.errors_count4);
lowspeed_counter = 0; }
/* 0.5 Hz */
if (lowspeed_counter % 20 == 0) {
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
} }
lowspeed_counter++; lowspeed_counter++;
@@ -1776,10 +1786,11 @@ Mavlink::task_main(int argc, char *argv[])
if (updated) { if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
if (_verbose) warnx("Got mission result"); if (_verbose) warnx("Got mission result: new current: %d", mission_result.index_current_mission);
if (mission_result.mission_reached) if (mission_result.mission_reached) {
mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached);
}
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
} }
src/modules/navigator/navigator_main.cpp
+31 -31
View File
@@ -153,17 +153,16 @@ private:
int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _control_mode_sub; /**< vehicle control mode subscription */ 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 */ orb_advert_t _mission_result_pub; /**< publish mission result topic */
struct vehicle_status_s _vstatus; /**< vehicle status */ 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 vehicle_global_position_s _global_pos; /**< global vehicle position */
struct home_position_s _home_pos; /**< home position for RTL */ 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_result_s _mission_result; /**< mission result for commander/mavlink */
struct mission_item_s _mission_item; /**< current mission item */ 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 */ perf_counter_t _loop_perf; /**< loop performance counter */
@@ -177,21 +176,22 @@ private:
class Mission _mission; class Mission _mission;
bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ bool _mission_item_valid; /**< current mission item valid */
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ 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_position_reached;
bool _waypoint_yaw_reached; bool _waypoint_yaw_reached;
uint64_t _time_first_inside_orbit; 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 _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 _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
MissionFeasibilityChecker missionFeasiblityChecker; 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; bool _pos_sp_triplet_updated;
char *nav_states_str[NAV_STATE_MAX]; const char *nav_states_str[NAV_STATE_MAX];
struct { struct {
float min_altitude; float min_altitude;
@@ -381,11 +381,11 @@ Navigator::Navigator() :
_global_pos_sub(-1), _global_pos_sub(-1),
_home_pos_sub(-1), _home_pos_sub(-1),
_vstatus_sub(-1), _vstatus_sub(-1),
_control_mode_sub(-1),
_params_sub(-1), _params_sub(-1),
_offboard_mission_sub(-1), _offboard_mission_sub(-1),
_onboard_mission_sub(-1), _onboard_mission_sub(-1),
_capabilities_sub(-1), _capabilities_sub(-1),
_control_mode_sub(-1),
/* publications */ /* publications */
_pos_sp_triplet_pub(-1), _pos_sp_triplet_pub(-1),
@@ -393,22 +393,22 @@ Navigator::Navigator() :
/* performance counters */ /* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
/* states */ _geofence_violation_warning_sent(false),
_rtl_state(RTL_STATE_NONE),
_fence_valid(false), _fence_valid(false),
_inside_fence(true), _inside_fence(true),
_mission(), _mission(),
_mission_item_valid(false),
_global_pos_valid(false), _global_pos_valid(false),
_reset_loiter_pos(true), _reset_loiter_pos(true),
_waypoint_position_reached(false), _waypoint_position_reached(false),
_waypoint_yaw_reached(false), _waypoint_yaw_reached(false),
_time_first_inside_orbit(0), _time_first_inside_orbit(0),
_set_nav_state_timestamp(0),
_mission_item_valid(false),
_need_takeoff(true), _need_takeoff(true),
_do_takeoff(false), _do_takeoff(false),
_set_nav_state_timestamp(0),
_pos_sp_triplet_updated(false), _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.min_altitude = param_find("NAV_MIN_ALT");
_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
@@ -853,7 +853,7 @@ Navigator::task_main()
/* notify user about state changes */ /* notify user about state changes */
if (myState != prevState) { if (myState != prevState) {
mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]);
prevState = myState; prevState = myState;
/* reset time counter on state changes */ /* reset time counter on state changes */
@@ -1061,11 +1061,11 @@ Navigator::start_loiter()
/* use current altitude if above min altitude set by parameter */ /* use current altitude if above min altitude set by parameter */
if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) {
_pos_sp_triplet.current.alt = min_alt_amsl; _pos_sp_triplet.current.alt = min_alt_amsl;
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
} else { } else {
_pos_sp_triplet.current.alt = _global_pos.alt; _pos_sp_triplet.current.alt = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
} }
} }
@@ -1105,7 +1105,7 @@ Navigator::set_mission_item()
if (ret == OK) { if (ret == OK) {
_mission.report_current_mission_item(); _mission.report_current_offboard_mission_item();
/* reset time counter for new item */ /* reset time counter for new item */
_time_first_inside_orbit = 0; _time_first_inside_orbit = 0;
@@ -1165,14 +1165,14 @@ Navigator::set_mission_item()
} }
if (_do_takeoff) { if (_do_takeoff) {
mavlink_log_info(_mavlink_fd, "[navigator] 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 { } else {
if (onboard) { if (onboard) {
mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index);
} else { } else {
mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index);
} }
} }
@@ -1321,7 +1321,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false; _pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] 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; break;
} }
@@ -1347,7 +1347,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false; _pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] 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; break;
} }
@@ -1365,7 +1365,7 @@ Navigator::set_rtl_item()
_mission_item.loiter_direction = 1; _mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _parameters.acceptance_radius; _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.pitch_min = 0.0f;
_mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f;
_mission_item.origin = ORIGIN_ONBOARD; _mission_item.origin = ORIGIN_ONBOARD;
@@ -1374,12 +1374,12 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false; _pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] 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; break;
} }
default: { default: {
mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state);
start_loiter(); start_loiter();
break; break;
} }
@@ -1528,7 +1528,7 @@ Navigator::check_mission_item_reached()
_time_first_inside_orbit = now; _time_first_inside_orbit = now;
if (_mission_item.time_inside > 0.01f) { if (_mission_item.time_inside > 0.01f) {
mavlink_log_info(_mavlink_fd, "[navigator] 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);
} }
} }
@@ -1556,7 +1556,7 @@ Navigator::on_mission_item_reached()
if (_do_takeoff) { if (_do_takeoff) {
/* takeoff completed */ /* takeoff completed */
_do_takeoff = false; _do_takeoff = false;
mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); mavlink_log_info(_mavlink_fd, "#audio: takeoff completed");
} else { } else {
/* advance by one mission item */ /* advance by one mission item */
src/modules/navigator/navigator_mission.cpp
+3 -5
View File
@@ -89,7 +89,7 @@ Mission::set_current_offboard_mission_index(int new_index)
_current_offboard_mission_index = 0; _current_offboard_mission_index = 0;
} }
} }
report_current_mission_item(); report_current_offboard_mission_item();
} }
void void
@@ -296,11 +296,9 @@ Mission::report_mission_item_reached()
} }
void void
Mission::report_current_mission_item() Mission::report_current_offboard_mission_item()
{ {
if (_current_mission_type == MISSION_TYPE_OFFBOARD) { _mission_result.index_current_mission = _current_offboard_mission_index;
_mission_result.index_current_mission = _current_offboard_mission_index;
}
} }
void void
src/modules/navigator/navigator_mission.h
+1 -1
View File
@@ -73,7 +73,7 @@ public:
void move_to_next(); void move_to_next();
void report_mission_item_reached(); void report_mission_item_reached();
void report_current_mission_item(); void report_current_offboard_mission_item();
void publish_mission_result(); void publish_mission_result();
private: private:
src/modules/px4iofirmware/dsm.c
+29 -5
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -162,6 +162,7 @@ dsm_guess_format(bool reset)
0xff, /* 8 channels (DX8) */ 0xff, /* 8 channels (DX8) */
0x1ff, /* 9 channels (DX9, etc.) */ 0x1ff, /* 9 channels (DX9, etc.) */
0x3ff, /* 10 channels (DX10) */ 0x3ff, /* 10 channels (DX10) */
0x1fff, /* 13 channels (DX10t) */
0x3fff /* 18 channels (DX10) */ 0x3fff /* 18 channels (DX10) */
}; };
unsigned votes10 = 0; unsigned votes10 = 0;
@@ -368,11 +369,25 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
if (channel >= *num_values) if (channel >= *num_values)
*num_values = channel + 1; *num_values = channel + 1;
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
if (dsm_channel_shift == 11) if (dsm_channel_shift == 10)
value /= 2; value *= 2;
value += 998; /*
* Spektrum scaling is special. There are these basic considerations
*
* * Midpoint is 1520 us
* * 100% travel channels are +- 400 us
*
* We obey the original Spektrum scaling (so a default setup will scale from
* 1100 - 1900 us), but we do not obey the weird 1520 us center point
* and instead (correctly) center the center around 1500 us. This is in order
* to get something useful without requiring the user to calibrate on a digital
* link for no reason.
*/
/* scaled integer for decent accuracy while staying efficient */
value = ((((int)value - 1024) * 1000) / 1700) + 1500;
/* /*
* Store the decoded channel into the R/C input buffer, taking into * Store the decoded channel into the R/C input buffer, taking into
@@ -400,6 +415,15 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
values[channel] = value; values[channel] = value;
} }
/*
* Spektrum likes to send junk in higher channel numbers to fill
* their packets. We don't know about a 13 channel model in their TX
* lines, so if we get a channel count of 13, we'll return 12 (the last
* data index that is stable).
*/
if (*num_values == 13)
*num_values = 12;
if (dsm_channel_shift == 11) { if (dsm_channel_shift == 11) {
/* Set the 11-bit data indicator */ /* Set the 11-bit data indicator */
*num_values |= 0x8000; *num_values |= 0x8000;
src/modules/px4iofirmware/registers.c
+1 -1
View File
@@ -566,7 +566,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break; break;
case PX4IO_P_SETUP_DSM: case PX4IO_P_SETUP_DSM:
dsm_bind(value & 0x0f, (value >> 4) & 7); dsm_bind(value & 0x0f, (value >> 4) & 0xF);
break; break;
default: default: