From 4ee647015a5fa4c2d8e324e75a9f2d749a1b6ca6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 May 2014 19:52:22 +0200 Subject: navigator: takeoff fix, always reach takeoff altitude, even if first waypoint is lower --- src/modules/navigator/navigator_main.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 87c893079..8eedcc90e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1477,27 +1477,27 @@ Navigator::check_mission_item_reached() acceptance_radius = _parameters.acceptance_radius; } - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; - - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); - if (_do_takeoff) { - if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { - /* require only altitude for takeoff */ + /* require only altitude for takeoff */ + if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) { _waypoint_position_reached = true; } } else { + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + /* calculate AMSL altitude for this waypoint */ + float wp_alt_amsl = _mission_item.altitude; + + if (_mission_item.altitude_is_relative) + wp_alt_amsl += _home_pos.alt; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + &dist_xy, &dist_z); + if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; } -- cgit v1.2.3 From 95a8414895d0f93bf92b8339ad15a1b3b4d1a7f2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 May 2014 19:55:24 +0200 Subject: rc.mc_defaults: set default acceptance radius (NAV_ACCEPT_RAD) to 2m --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index b6b0a5b4e..65f1e38c6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -41,6 +41,7 @@ then param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 + param set NAV_ACCEPT_RAD 2.0 fi set PWM_RATE 400 -- cgit v1.2.3 From fcb890553329b4092c7dca319f5f538865734b3a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 28 May 2014 08:33:04 +0200 Subject: navigator: autocontinue and RTL autolanding fixes --- src/modules/navigator/navigator_main.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 87c893079..310bdf9ea 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached() /* XXX TODO count turns */ if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && _mission_item.loiter_radius > 0.01f) { @@ -1567,7 +1566,14 @@ Navigator::on_mission_item_reached() } if (_mission.current_mission_available()) { - set_mission_item(); + if (_mission_item.autocontinue) { + /* continue mission */ + set_mission_item(); + + } else { + /* autocontinue disabled for this item */ + request_loiter_or_ready(); + } } else { /* if no more mission items available then finish mission */ -- cgit v1.2.3 From 138edca5445a66df3647c03529ad2a30a51fe083 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 31 May 2014 13:15:10 +0200 Subject: attempt to fix mavlink hardware flow control disable logic --- src/modules/mavlink/mavlink_main.cpp | 32 +++++++++++++++++--------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 28cdf65a3..540b88657 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -105,7 +105,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); -static uint64_t last_write_times[6] = {0}; +static uint64_t last_write_success_times[6] = {0}; +static uint64_t last_write_try_times[6] = {0}; /* * Internal function to send the bytes through the right serial port @@ -166,26 +167,25 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (instance->get_flow_control_enabled() && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - if (buf_free == 0) { - - if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { - - warnx("DISABLING HARDWARE FLOW CONTROL"); - instance->enable_flow_control(false); - } - - } else { - - /* apparently there is space left, although we might be - * partially overflooding the buffer already */ - last_write_times[(unsigned)channel] = hrt_absolute_time(); + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (last_write_try_times[(unsigned)channel] != 0 && + hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && + last_write_success_times[(unsigned)channel] != + last_write_try_times[(unsigned)channel]) + { + warnx("DISABLING HARDWARE FLOW CONTROL"); + instance->enable_flow_control(false); } + } /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (instance->should_transmit()) { + last_write_try_times[(unsigned)channel] = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { @@ -199,6 +199,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length ssize_t ret = write(uart, ch, desired); if (ret != desired) { warnx("TX FAIL"); + } else { + last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } } -- cgit v1.2.3 From 9ee42e8e5986dcdb2e5bc9ff5110ad7bed462f98 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 10:59:11 +0200 Subject: rcS: fix whitespace --- ROMFS/px4fmu_common/init.d/rcS | 82 +++++++++++++++++++++--------------------- 1 file changed, 41 insertions(+), 41 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 87ec4293e..6d06f897a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -65,12 +65,12 @@ then # Start CDC/ACM serial driver # sercon - + # # Start the ORB (first app to start) # uorb start - + # # Load parameters # @@ -79,7 +79,7 @@ then then set PARAM_FILE /fs/mtd_params fi - + param select $PARAM_FILE if param load then @@ -87,7 +87,7 @@ then else echo "[init] ERROR: Params loading failed: $PARAM_FILE" fi - + # # Start system state indicator # @@ -105,7 +105,7 @@ then if pca8574 start then fi - + # # Set default values # @@ -126,7 +126,7 @@ then set LOAD_DEFAULT_APPS yes set GPS yes set GPS_FAKE no - + # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts # @@ -136,7 +136,7 @@ then else set DO_AUTOCONFIG no fi - + # # Set USE_IO flag # @@ -146,7 +146,7 @@ then else set USE_IO no fi - + # # Set parameters and env variables for selected AUTOSTART # @@ -176,9 +176,9 @@ then param set SYS_AUTOCONFIG 0 param save fi - + set IO_PRESENT no - + if [ $USE_IO == yes ] then # @@ -190,19 +190,19 @@ then else set IO_FILE /etc/extras/px4io-v1_default.bin fi - + if px4io checkcrc $IO_FILE then echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $LOG_FILE - + set IO_PRESENT yes else echo "[init] Trying to update" echo "PX4IO Trying to update" >> $LOG_FILE - + tone_alarm MLL32CP8MB - + if px4io forceupdate 14662 $IO_FILE then usleep 500000 @@ -211,7 +211,7 @@ then echo "[init] PX4IO CRC OK, update successful" echo "PX4IO CRC OK after updating" >> $LOG_FILE tone_alarm MLL8CDE - + set IO_PRESENT yes else echo "[init] ERROR: PX4IO update failed" @@ -224,14 +224,14 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + if [ $IO_PRESENT == no ] then echo "[init] ERROR: PX4IO not found" tone_alarm $TUNE_OUT_ERROR fi fi - + # # Set default output if not set # @@ -250,7 +250,7 @@ then # Need IO for output but it not present, disable output set OUTPUT_MODE none echo "[init] ERROR: PX4IO not found, disabling output" - + # Avoid using ttyS0 for MAVLink on FMUv1 if ver hwcmp PX4FMU_V1 then @@ -274,17 +274,17 @@ then # Try to get an USB console nshterm /dev/ttyACM0 & - + # # Start the Commander (needs to be this early for in-air-restarts) # commander start - + # # Start primary output # set TTYS1_BUSY no - + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output if [ $OUTPUT_MODE != none ] then @@ -300,7 +300,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then echo "[init] Use FMU as primary output" @@ -311,7 +311,7 @@ then echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi - + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] @@ -324,7 +324,7 @@ then fi fi fi - + if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" @@ -337,7 +337,7 @@ then then set MKBLCTRL_ARG "-mkmode +" fi - + if mkblctrl $MKBLCTRL_ARG then echo "[init] MKBLCTRL started" @@ -345,9 +345,9 @@ then echo "[init] ERROR: MKBLCTRL start failed" tone_alarm $TUNE_OUT_ERROR fi - + fi - + if [ $OUTPUT_MODE == hil ] then echo "[init] Use HIL as primary output" @@ -359,7 +359,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + # # Start IO or FMU for RC PPM input if needed # @@ -386,7 +386,7 @@ then echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi - + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] @@ -401,7 +401,7 @@ then fi fi fi - + # # MAVLink # @@ -422,7 +422,7 @@ then fi mavlink start $MAVLINK_FLAGS - + # # Sensors, Logging, GPS # @@ -433,7 +433,7 @@ then echo "[init] Start logging" sh /etc/init.d/rc.logging fi - + if [ $GPS == yes ] then echo "[init] Start GPS" @@ -443,7 +443,7 @@ then gps start -f else gps start - fi + fi fi # @@ -460,24 +460,24 @@ then if [ $VEHICLE_TYPE == fw ] then echo "[init] Vehicle type: FIXED WING" - + if [ $MIXER == none ] then # Set default mixer for fixed wing if not defined set MIXER FMU_AERT fi - + if [ $MAV_TYPE == none ] then # Use MAV_TYPE = 1 (fixed wing) if not defined set MAV_TYPE 1 fi - + param set MAV_TYPE $MAV_TYPE - + # Load mixer and configure outputs sh /etc/init.d/rc.interface - + # Start standard fixedwing apps if [ $LOAD_DEFAULT_APPS == yes ] then @@ -525,7 +525,7 @@ then set MAV_TYPE 14 fi fi - + # Still no MAV_TYPE found if [ $MAV_TYPE == none ] then @@ -533,10 +533,10 @@ then else param set MAV_TYPE $MAV_TYPE fi - + # Load mixer and configure outputs sh /etc/init.d/rc.interface - + # Start standard multicopter apps if [ $LOAD_DEFAULT_APPS == yes ] then -- cgit v1.2.3