yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware

Browse Source
This commit is contained in:
Lorenz Meier
2014-06-01 11:50:29 +02:00
parent 0c35b7a8ee 6e34a7ac9c
commit 8f1c46be92
4 changed files with 83 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/rc.mc_defaults
View File

@@ -41,6 +41,7 @@ then
param set PE_POSNE_NOISE 0.5 param set PE_POSNE_NOISE 0.5
param set PE_POSD_NOISE 1.0 param set PE_POSD_NOISE 1.0
param set NAV_ACCEPT_RAD 2.0
fi fi
set PWM_RATE 400 set PWM_RATE 400

82
ROMFS/px4fmu_common/init.d/rcS
View File

@@ -65,12 +65,12 @@ then
# Start CDC/ACM serial driver # Start CDC/ACM serial driver
# #
sercon sercon
# #
# Start the ORB (first app to start) # Start the ORB (first app to start)
# #
uorb start uorb start
# #
# Load parameters # Load parameters
# #
@@ -79,7 +79,7 @@ then
then then
set PARAM_FILE /fs/mtd_params set PARAM_FILE /fs/mtd_params
fi fi
param select $PARAM_FILE param select $PARAM_FILE
if param load if param load
then then
@@ -87,7 +87,7 @@ then
else else
echo "[init] ERROR: Params loading failed: $PARAM_FILE" echo "[init] ERROR: Params loading failed: $PARAM_FILE"
fi fi
# #
# Start system state indicator # Start system state indicator
# #
@@ -105,7 +105,7 @@ then
if pca8574 start if pca8574 start
then then
fi fi
# #
# Set default values # Set default values
# #
@@ -126,7 +126,7 @@ then
set LOAD_DEFAULT_APPS yes set LOAD_DEFAULT_APPS yes
set GPS yes set GPS yes
set GPS_FAKE no set GPS_FAKE no
# #
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
# #
@@ -136,7 +136,7 @@ then
else else
set DO_AUTOCONFIG no set DO_AUTOCONFIG no
fi fi
# #
# Set USE_IO flag # Set USE_IO flag
# #
@@ -146,7 +146,7 @@ then
else else
set USE_IO no set USE_IO no
fi fi
# #
# Set parameters and env variables for selected AUTOSTART # Set parameters and env variables for selected AUTOSTART
# #
@@ -176,9 +176,9 @@ then
param set SYS_AUTOCONFIG 0 param set SYS_AUTOCONFIG 0
param save param save
fi fi
set IO_PRESENT no set IO_PRESENT no
if [ $USE_IO == yes ] if [ $USE_IO == yes ]
then then
# #
@@ -190,19 +190,19 @@ then
else else
set IO_FILE /etc/extras/px4io-v1_default.bin set IO_FILE /etc/extras/px4io-v1_default.bin
fi fi
if px4io checkcrc $IO_FILE if px4io checkcrc $IO_FILE
then then
echo "[init] PX4IO CRC OK" echo "[init] PX4IO CRC OK"
echo "PX4IO CRC OK" >> $LOG_FILE echo "PX4IO CRC OK" >> $LOG_FILE
set IO_PRESENT yes set IO_PRESENT yes
else else
echo "[init] Trying to update" echo "[init] Trying to update"
echo "PX4IO Trying to update" >> $LOG_FILE echo "PX4IO Trying to update" >> $LOG_FILE
tone_alarm MLL32CP8MB tone_alarm MLL32CP8MB
if px4io forceupdate 14662 $IO_FILE if px4io forceupdate 14662 $IO_FILE
then then
usleep 500000 usleep 500000
@@ -211,7 +211,7 @@ then
echo "[init] PX4IO CRC OK, update successful" echo "[init] PX4IO CRC OK, update successful"
echo "PX4IO CRC OK after updating" >> $LOG_FILE echo "PX4IO CRC OK after updating" >> $LOG_FILE
tone_alarm MLL8CDE tone_alarm MLL8CDE
set IO_PRESENT yes set IO_PRESENT yes
else else
echo "[init] ERROR: PX4IO update failed" echo "[init] ERROR: PX4IO update failed"
@@ -224,14 +224,14 @@ then
tone_alarm $TUNE_OUT_ERROR tone_alarm $TUNE_OUT_ERROR
fi fi
fi fi
if [ $IO_PRESENT == no ] if [ $IO_PRESENT == no ]
then then
echo "[init] ERROR: PX4IO not found" echo "[init] ERROR: PX4IO not found"
tone_alarm $TUNE_OUT_ERROR tone_alarm $TUNE_OUT_ERROR
fi fi
fi fi
# #
# Set default output if not set # Set default output if not set
# #
@@ -250,7 +250,7 @@ then
# Need IO for output but it not present, disable output # Need IO for output but it not present, disable output
set OUTPUT_MODE none set OUTPUT_MODE none
echo "[init] ERROR: PX4IO not found, disabling output" echo "[init] ERROR: PX4IO not found, disabling output"
# Avoid using ttyS0 for MAVLink on FMUv1 # Avoid using ttyS0 for MAVLink on FMUv1
if ver hwcmp PX4FMU_V1 if ver hwcmp PX4FMU_V1
then then
@@ -274,17 +274,17 @@ then
# Try to get an USB console # Try to get an USB console
nshterm /dev/ttyACM0 & nshterm /dev/ttyACM0 &
# #
# Start the Commander (needs to be this early for in-air-restarts) # Start the Commander (needs to be this early for in-air-restarts)
# #
commander start commander start
# #
# Start primary output # Start primary output
# #
set TTYS1_BUSY no 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 something is wrong with setup and we shouldn't try to enable output
if [ $OUTPUT_MODE != none ] if [ $OUTPUT_MODE != none ]
then then
@@ -300,7 +300,7 @@ then
tone_alarm $TUNE_OUT_ERROR tone_alarm $TUNE_OUT_ERROR
fi fi
fi fi
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
then then
echo "[init] Use FMU as primary output" echo "[init] Use FMU as primary output"
@@ -311,7 +311,7 @@ then
echo "[init] ERROR: FMU mode_$FMU_MODE start failed" echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
tone_alarm $TUNE_OUT_ERROR tone_alarm $TUNE_OUT_ERROR
fi fi
if ver hwcmp PX4FMU_V1 if ver hwcmp PX4FMU_V1
then then
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
@@ -324,7 +324,7 @@ then
fi fi
fi fi
fi fi
if [ $OUTPUT_MODE == mkblctrl ] if [ $OUTPUT_MODE == mkblctrl ]
then then
echo "[init] Use MKBLCTRL as primary output" echo "[init] Use MKBLCTRL as primary output"
@@ -337,7 +337,7 @@ then
then then
set MKBLCTRL_ARG "-mkmode +" set MKBLCTRL_ARG "-mkmode +"
fi fi
if mkblctrl $MKBLCTRL_ARG if mkblctrl $MKBLCTRL_ARG
then then
echo "[init] MKBLCTRL started" echo "[init] MKBLCTRL started"
@@ -345,9 +345,9 @@ then
echo "[init] ERROR: MKBLCTRL start failed" echo "[init] ERROR: MKBLCTRL start failed"
tone_alarm $TUNE_OUT_ERROR tone_alarm $TUNE_OUT_ERROR
fi fi
fi fi
if [ $OUTPUT_MODE == hil ] if [ $OUTPUT_MODE == hil ]
then then
echo "[init] Use HIL as primary output" echo "[init] Use HIL as primary output"
@@ -359,7 +359,7 @@ then
tone_alarm $TUNE_OUT_ERROR tone_alarm $TUNE_OUT_ERROR
fi fi
fi fi
# #
# Start IO or FMU for RC PPM input if needed # Start IO or FMU for RC PPM input if needed
# #
@@ -386,7 +386,7 @@ then
echo "[init] ERROR: FMU mode_$FMU_MODE start failed" echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
tone_alarm $TUNE_OUT_ERROR tone_alarm $TUNE_OUT_ERROR
fi fi
if ver hwcmp PX4FMU_V1 if ver hwcmp PX4FMU_V1
then then
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
@@ -401,7 +401,7 @@ then
fi fi
fi fi
fi fi
# #
# MAVLink # MAVLink
# #
@@ -422,7 +422,7 @@ then
fi fi
mavlink start $MAVLINK_FLAGS mavlink start $MAVLINK_FLAGS
# #
# Sensors, Logging, GPS # Sensors, Logging, GPS
# #
@@ -433,7 +433,7 @@ then
echo "[init] Start logging" echo "[init] Start logging"
sh /etc/init.d/rc.logging sh /etc/init.d/rc.logging
fi fi
if [ $GPS == yes ] if [ $GPS == yes ]
then then
echo "[init] Start GPS" echo "[init] Start GPS"
@@ -443,7 +443,7 @@ then
gps start -f gps start -f
else else
gps start gps start
fi fi
fi fi
# #
@@ -460,24 +460,24 @@ then
if [ $VEHICLE_TYPE == fw ] if [ $VEHICLE_TYPE == fw ]
then then
echo "[init] Vehicle type: FIXED WING" echo "[init] Vehicle type: FIXED WING"
if [ $MIXER == none ] if [ $MIXER == none ]
then then
# Set default mixer for fixed wing if not defined # Set default mixer for fixed wing if not defined
set MIXER FMU_AERT set MIXER FMU_AERT
fi fi
if [ $MAV_TYPE == none ] if [ $MAV_TYPE == none ]
then then
# Use MAV_TYPE = 1 (fixed wing) if not defined # Use MAV_TYPE = 1 (fixed wing) if not defined
set MAV_TYPE 1 set MAV_TYPE 1
fi fi
param set MAV_TYPE $MAV_TYPE param set MAV_TYPE $MAV_TYPE
# Load mixer and configure outputs # Load mixer and configure outputs
sh /etc/init.d/rc.interface sh /etc/init.d/rc.interface
# Start standard fixedwing apps # Start standard fixedwing apps
if [ $LOAD_DEFAULT_APPS == yes ] if [ $LOAD_DEFAULT_APPS == yes ]
then then
@@ -525,7 +525,7 @@ then
set MAV_TYPE 14 set MAV_TYPE 14
fi fi
fi fi
# Still no MAV_TYPE found # Still no MAV_TYPE found
if [ $MAV_TYPE == none ] if [ $MAV_TYPE == none ]
then then
@@ -533,10 +533,10 @@ then
else else
param set MAV_TYPE $MAV_TYPE param set MAV_TYPE $MAV_TYPE
fi fi
# Load mixer and configure outputs # Load mixer and configure outputs
sh /etc/init.d/rc.interface sh /etc/init.d/rc.interface
# Start standard multicopter apps # Start standard multicopter apps
if [ $LOAD_DEFAULT_APPS == yes ] if [ $LOAD_DEFAULT_APPS == yes ]
then then

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

@@ -105,7 +105,8 @@ static struct file_operations fops;
*/ */
extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); 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 * 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() if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
if (buf_free == 0) { /* Disable hardware flow control:
* if no successful write since a defined time
if (last_write_times[(unsigned)channel] != 0 && * and if the last try was not the last successful write
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { */
if (last_write_try_times[(unsigned)channel] != 0 &&
warnx("DISABLING HARDWARE FLOW CONTROL"); hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
instance->enable_flow_control(false); last_write_success_times[(unsigned)channel] !=
} last_write_try_times[(unsigned)channel])
{
} else { warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
/* apparently there is space left, although we might be
* partially overflooding the buffer already */
last_write_times[(unsigned)channel] = hrt_absolute_time();
} }
} }
/* If the wait until transmit flag is on, only transmit after we've received messages. /* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */ Otherwise, transmit all the time. */
if (instance->should_transmit()) { 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 */ /* check if there is space in the buffer, let it overflow else */
if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { 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); ssize_t ret = write(uart, ch, desired);
if (ret != desired) { if (ret != desired) {
warnx("TX FAIL"); warnx("TX FAIL");
} else {
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
} }
} }

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

@@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached()
/* XXX TODO count turns */ /* XXX TODO count turns */
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || 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.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
_mission_item.loiter_radius > 0.01f) { _mission_item.loiter_radius > 0.01f) {
@@ -1477,27 +1476,27 @@ Navigator::check_mission_item_reached()
acceptance_radius = _parameters.acceptance_radius; 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 (_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; _waypoint_position_reached = true;
} }
} else { } 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) { if (dist >= 0.0f && dist <= acceptance_radius) {
_waypoint_position_reached = true; _waypoint_position_reached = true;
} }
@@ -1567,7 +1566,14 @@ Navigator::on_mission_item_reached()
} }
if (_mission.current_mission_available()) { 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 { } else {
/* if no more mission items available then finish mission */ /* if no more mission items available then finish mission */