forked from Archive/PX4-Autopilot
Merge branch 'beta' into ready_rtl_fix
This commit is contained in:
commit
4912a4af99
|
@ -35,3 +35,4 @@ mavlink/include/mavlink/v0.9/
|
||||||
/Documentation/doxygen*objdb*tmp
|
/Documentation/doxygen*objdb*tmp
|
||||||
.tags
|
.tags
|
||||||
.tags_sorted_by_file
|
.tags_sorted_by_file
|
||||||
|
.pydevproject
|
||||||
|
|
|
@ -16,4 +16,5 @@ astyle \
|
||||||
--ignore-exclude-errors-x \
|
--ignore-exclude-errors-x \
|
||||||
--lineend=linux \
|
--lineend=linux \
|
||||||
--exclude=EASTL \
|
--exclude=EASTL \
|
||||||
|
--add-brackets \
|
||||||
$*
|
$*
|
||||||
|
|
|
@ -1,19 +0,0 @@
|
||||||
#!/bin/sh
|
|
||||||
astyle \
|
|
||||||
--style=linux \
|
|
||||||
--indent=force-tab=8 \
|
|
||||||
--indent-cases \
|
|
||||||
--indent-preprocessor \
|
|
||||||
--break-blocks=all \
|
|
||||||
--pad-oper \
|
|
||||||
--pad-header \
|
|
||||||
--unpad-paren \
|
|
||||||
--keep-one-line-blocks \
|
|
||||||
--keep-one-line-statements \
|
|
||||||
--align-pointer=name \
|
|
||||||
--suffix=none \
|
|
||||||
--lineend=linux \
|
|
||||||
$*
|
|
||||||
#--ignore-exclude-errors-x \
|
|
||||||
#--exclude=EASTL \
|
|
||||||
#--align-reference=name \
|
|
|
@ -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)
|
||||||
|
|
|
@ -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]);
|
||||||
|
|
|
@ -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;
|
||||||
|
@ -431,7 +429,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 +517,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));
|
||||||
|
@ -889,8 +885,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_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 +914,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_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 +933,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_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),
|
||||||
|
|
|
@ -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);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
*
|
*
|
||||||
*
|
*
|
||||||
|
|
|
@ -730,7 +730,6 @@ MulticopterPositionControl::task_main()
|
||||||
} else {
|
} else {
|
||||||
/* run position & altitude controllers, calculate velocity setpoint */
|
/* run position & altitude controllers, calculate velocity setpoint */
|
||||||
math::Vector<3> pos_err;
|
math::Vector<3> pos_err;
|
||||||
float err_x, err_y;
|
|
||||||
get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]);
|
get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]);
|
||||||
pos_err(2) = -(_alt_sp - alt);
|
pos_err(2) = -(_alt_sp - alt);
|
||||||
|
|
||||||
|
|
|
@ -851,7 +851,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 */
|
||||||
|
@ -1059,11 +1059,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");
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -1160,14 +1160,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", _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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1316,7 +1316,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", climb_alt - _home_pos.alt);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1342,7 +1342,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", _mission_item.altitude - _home_pos.alt);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1369,12 +1369,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", _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;
|
||||||
}
|
}
|
||||||
|
@ -1523,7 +1523,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", _mission_item.time_inside);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1548,7 +1548,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 */
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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:
|
||||||
|
|
|
@ -39,6 +39,8 @@
|
||||||
#ifndef _SYSTEMLIB_PERF_COUNTER_H
|
#ifndef _SYSTEMLIB_PERF_COUNTER_H
|
||||||
#define _SYSTEMLIB_PERF_COUNTER_H value
|
#define _SYSTEMLIB_PERF_COUNTER_H value
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Counter types.
|
* Counter types.
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -141,8 +141,8 @@ test_mount(int argc, char *argv[])
|
||||||
/* announce mode switch */
|
/* announce mode switch */
|
||||||
if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) {
|
if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) {
|
||||||
warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC");
|
warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC");
|
||||||
fsync(stdout);
|
fsync(fileno(stdout));
|
||||||
fsync(stderr);
|
fsync(fileno(stderr));
|
||||||
usleep(20000);
|
usleep(20000);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -162,7 +162,7 @@ test_mount(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
char buf[64];
|
char buf[64];
|
||||||
int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort);
|
(void)sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort);
|
||||||
lseek(cmd_fd, 0, SEEK_SET);
|
lseek(cmd_fd, 0, SEEK_SET);
|
||||||
write(cmd_fd, buf, strlen(buf) + 1);
|
write(cmd_fd, buf, strlen(buf) + 1);
|
||||||
fsync(cmd_fd);
|
fsync(cmd_fd);
|
||||||
|
@ -174,8 +174,8 @@ test_mount(int argc, char *argv[])
|
||||||
|
|
||||||
printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC");
|
printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC");
|
||||||
printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n");
|
printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n");
|
||||||
fsync(stdout);
|
fsync(fileno(stdout));
|
||||||
fsync(stderr);
|
fsync(fileno(stderr));
|
||||||
usleep(50000);
|
usleep(50000);
|
||||||
|
|
||||||
for (unsigned a = 0; a < alignments; a++) {
|
for (unsigned a = 0; a < alignments; a++) {
|
||||||
|
@ -185,22 +185,20 @@ test_mount(int argc, char *argv[])
|
||||||
uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
|
uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
|
||||||
|
|
||||||
/* fill write buffer with known values */
|
/* fill write buffer with known values */
|
||||||
for (int i = 0; i < sizeof(write_buf); i++) {
|
for (unsigned i = 0; i < sizeof(write_buf); i++) {
|
||||||
/* this will wrap, but we just need a known value with spacing */
|
/* this will wrap, but we just need a known value with spacing */
|
||||||
write_buf[i] = i+11;
|
write_buf[i] = i+11;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
|
uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
|
||||||
hrt_abstime start, end;
|
|
||||||
|
|
||||||
int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
|
int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
|
||||||
|
|
||||||
start = hrt_absolute_time();
|
|
||||||
for (unsigned i = 0; i < iterations; i++) {
|
for (unsigned i = 0; i < iterations; i++) {
|
||||||
|
|
||||||
int wret = write(fd, write_buf + a, chunk_sizes[c]);
|
int wret = write(fd, write_buf + a, chunk_sizes[c]);
|
||||||
|
|
||||||
if (wret != chunk_sizes[c]) {
|
if (wret != (int)chunk_sizes[c]) {
|
||||||
warn("WRITE ERROR!");
|
warn("WRITE ERROR!");
|
||||||
|
|
||||||
if ((0x3 & (uintptr_t)(write_buf + a)))
|
if ((0x3 & (uintptr_t)(write_buf + a)))
|
||||||
|
@ -214,8 +212,8 @@ test_mount(int argc, char *argv[])
|
||||||
fsync(fd);
|
fsync(fd);
|
||||||
} else {
|
} else {
|
||||||
printf("#");
|
printf("#");
|
||||||
fsync(stdout);
|
fsync(fileno(stdout));
|
||||||
fsync(stderr);
|
fsync(fileno(stderr));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -224,12 +222,10 @@ test_mount(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
printf(".");
|
printf(".");
|
||||||
fsync(stdout);
|
fsync(fileno(stdout));
|
||||||
fsync(stderr);
|
fsync(fileno(stderr));
|
||||||
usleep(200000);
|
usleep(200000);
|
||||||
|
|
||||||
end = hrt_absolute_time();
|
|
||||||
|
|
||||||
close(fd);
|
close(fd);
|
||||||
fd = open("/fs/microsd/testfile", O_RDONLY);
|
fd = open("/fs/microsd/testfile", O_RDONLY);
|
||||||
|
|
||||||
|
@ -237,7 +233,7 @@ test_mount(int argc, char *argv[])
|
||||||
for (unsigned i = 0; i < iterations; i++) {
|
for (unsigned i = 0; i < iterations; i++) {
|
||||||
int rret = read(fd, read_buf, chunk_sizes[c]);
|
int rret = read(fd, read_buf, chunk_sizes[c]);
|
||||||
|
|
||||||
if (rret != chunk_sizes[c]) {
|
if (rret != (int)chunk_sizes[c]) {
|
||||||
warnx("READ ERROR!");
|
warnx("READ ERROR!");
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
@ -245,7 +241,7 @@ test_mount(int argc, char *argv[])
|
||||||
/* compare value */
|
/* compare value */
|
||||||
bool compare_ok = true;
|
bool compare_ok = true;
|
||||||
|
|
||||||
for (int j = 0; j < chunk_sizes[c]; j++) {
|
for (unsigned j = 0; j < chunk_sizes[c]; j++) {
|
||||||
if (read_buf[j] != write_buf[j + a]) {
|
if (read_buf[j] != write_buf[j + a]) {
|
||||||
warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a);
|
warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a);
|
||||||
compare_ok = false;
|
compare_ok = false;
|
||||||
|
@ -271,16 +267,16 @@ test_mount(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
fsync(stdout);
|
fsync(fileno(stdout));
|
||||||
fsync(stderr);
|
fsync(fileno(stderr));
|
||||||
usleep(20000);
|
usleep(20000);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/* we always reboot for the next test if we get here */
|
/* we always reboot for the next test if we get here */
|
||||||
warnx("Iteration done, rebooting..");
|
warnx("Iteration done, rebooting..");
|
||||||
fsync(stdout);
|
fsync(fileno(stdout));
|
||||||
fsync(stderr);
|
fsync(fileno(stderr));
|
||||||
usleep(50000);
|
usleep(50000);
|
||||||
systemreset(false);
|
systemreset(false);
|
||||||
|
|
||||||
|
|
|
@ -233,8 +233,8 @@ top_main(void)
|
||||||
system_load.tasks[i].tcb->pid,
|
system_load.tasks[i].tcb->pid,
|
||||||
CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
|
CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
|
||||||
(system_load.tasks[i].total_runtime / 1000),
|
(system_load.tasks[i].total_runtime / 1000),
|
||||||
(int)(curr_loads[i] * 100),
|
(int)(curr_loads[i] * 100.0f),
|
||||||
(int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
|
(int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000),
|
||||||
stack_size - stack_free,
|
stack_size - stack_free,
|
||||||
stack_size,
|
stack_size,
|
||||||
system_load.tasks[i].tcb->sched_priority,
|
system_load.tasks[i].tcb->sched_priority,
|
||||||
|
|
Loading…
Reference in New Issue