diff options
author | Julian Oes <julian@oes.ch> | 2014-02-24 10:24:08 +0100 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-02-24 10:24:08 +0100 |
commit | 9bed1677bd5bfd6b0f07d78e5bdcfacec34564a2 (patch) | |
tree | f53334e32ab03f32cd0ed473e71ea1a221a997ac /src | |
parent | ae3e625ce8cec56527dc6809c0b48081773b47a3 (diff) | |
parent | 17ec859758ca165abaac0cef5889913985d72146 (diff) | |
download | px4-firmware-9bed1677bd5bfd6b0f07d78e5bdcfacec34564a2.tar.gz px4-firmware-9bed1677bd5bfd6b0f07d78e5bdcfacec34564a2.tar.bz2 px4-firmware-9bed1677bd5bfd6b0f07d78e5bdcfacec34564a2.zip |
Merge remote-tracking branch 'px4/beta' into fix_mission_dive
Conflicts:
src/modules/navigator/navigator_main.cpp
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/drv_pwm_output.h | 2 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 57 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 38 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 7 | ||||
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 1 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 24 | ||||
-rw-r--r-- | src/modules/px4iofirmware/dsm.c | 34 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 2 |
8 files changed, 86 insertions, 79 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index c3eea310f..7a93e513e 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -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 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 */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index efcf4d12b..7c7b3dcb7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -244,8 +244,7 @@ private: volatile int _task; ///< worker task id 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 _thread_mavlink_fd; ///< mavlink file descriptor for thread. + int _mavlink_fd; ///< mavlink file descriptor. perf_counter_t _perf_update; ///<local performance counter for status updates perf_counter_t _perf_write; ///<local performance counter for PWM control writes @@ -474,7 +473,6 @@ PX4IO::PX4IO(device::Device *interface) : _task(-1), _task_should_exit(false), _mavlink_fd(-1), - _thread_mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "io update")), _perf_write(perf_alloc(PC_ELAPSED, "io write")), _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 */ g_dev = this; - /* open MAVLink text channel */ - _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); - _debug_enabled = false; _servorail_status.rssi_v = 0; } @@ -785,7 +780,7 @@ PX4IO::task_main() hrt_abstime poll_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 @@ -880,6 +875,10 @@ PX4IO::task_main() /* run at 5Hz */ 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 */ bool updated = false; @@ -1275,16 +1274,14 @@ void PX4IO::dsm_bind_ioctl(int dsmMode) { if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { - /* 0: dsm2, 1:dsmx */ - if ((dsmMode == 0) || (dsmMode == 1)) { - 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)); - } else { - mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); - } + mavlink_log_info(_mavlink_fd, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8")); + int ret = ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); + + if (ret) + mavlink_log_critical(_mavlink_fd, "binding failed."); } 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; case DSM_BIND_START: - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); - usleep(500000); - 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); + + /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ + if (arg == DSM2_BIND_PULSES || + arg == DSMX_BIND_PULSES || + arg == DSMX8_BIND_PULSES) { + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + usleep(500000); + 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; case DSM_BIND_POWER_UP: @@ -2615,7 +2622,7 @@ bind(int argc, char *argv[]) #endif 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")) pulses = DSM2_BIND_PULSES; @@ -2624,7 +2631,7 @@ bind(int argc, char *argv[]) else if (!strcmp(argv[2], "dsmx8")) pulses = DSMX8_BIND_PULSES; 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 if (argc > 3) pulses = atoi(argv[3]); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 1feb539b9..ed57e0d34 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -233,7 +233,6 @@ private: float speedrate_p; float land_slope_angle; - float land_slope_length; float land_H1_virt; float land_flare_alt_relative; float land_thrust_lim_alt_relative; @@ -278,7 +277,6 @@ private: param_t speedrate_p; param_t land_slope_angle; - param_t land_slope_length; param_t land_H1_virt; param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; @@ -427,7 +425,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _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_length = param_find("FW_LND_SLLR"); _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); @@ -516,7 +513,6 @@ FixedwingPositionControl::parameters_update() 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_length, &(_parameters.land_slope_length)); 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_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); @@ -885,8 +881,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; + /* 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 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); @@ -931,38 +929,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); 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 { /* intersect glide 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 - * */ + * minimize speed to approach speed + * 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; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ 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 { /* continue horizontally */ 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), diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 62a340e90..0909135e1 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -349,13 +349,6 @@ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); /** - * Landing slope length - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); - -/** * * * @group L1 Control diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index b06655385..b4bac53d6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -730,7 +730,6 @@ MulticopterPositionControl::task_main() } else { /* run position & altitude controllers, calculate velocity setpoint */ 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]); pos_err(2) = -(_alt_sp - alt); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f59663ca0..7d122c26d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -853,7 +853,7 @@ Navigator::task_main() /* notify user about state changes */ 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; /* reset time counter on state changes */ @@ -1061,11 +1061,11 @@ Navigator::start_loiter() /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { _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 { _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"); } } @@ -1162,14 +1162,14 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); + mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); } else { if (onboard) { - 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 { - mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index); } } @@ -1318,7 +1318,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); + mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); break; } @@ -1344,7 +1344,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", (double)(_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; } @@ -1371,12 +1371,12 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", (double)(_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; } 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(); break; } @@ -1525,7 +1525,7 @@ Navigator::check_mission_item_reached() _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", (double)_mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside); } } @@ -1550,7 +1550,7 @@ Navigator::on_mission_item_reached() if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + mavlink_log_info(_mavlink_fd, "#audio: takeoff completed"); } else { /* advance by one mission item */ diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 60eda2319..d3f365822 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -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 * modification, are permitted provided that the following conditions @@ -162,6 +162,7 @@ dsm_guess_format(bool reset) 0xff, /* 8 channels (DX8) */ 0x1ff, /* 9 channels (DX9, etc.) */ 0x3ff, /* 10 channels (DX10) */ + 0x1fff, /* 13 channels (DX10t) */ 0x3fff /* 18 channels (DX10) */ }; unsigned votes10 = 0; @@ -368,11 +369,25 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) if (channel >= *num_values) *num_values = channel + 1; - /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - if (dsm_channel_shift == 11) - value /= 2; + /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ + if (dsm_channel_shift == 10) + 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 @@ -400,6 +415,15 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) 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) { /* Set the 11-bit data indicator */ *num_values |= 0x8000; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1335f52e1..97d25bbfa 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -566,7 +566,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_DSM: - dsm_bind(value & 0x0f, (value >> 4) & 7); + dsm_bind(value & 0x0f, (value >> 4) & 0xF); break; default: |