dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge remote-tracking branch 'upstream/master' into launchdetector 

Conflicts:
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
This commit is contained in:
Thomas Gubler 2014-03-06 21:53:51 +01:00
parent f2b46389ee 3207450037
commit b086bdf21e
46 changed files with 1211 additions and 1018 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -35,3 +35,4 @@ mavlink/include/mavlink/v0.9/
/Documentation/doxygen*objdb*tmp
.tags
.tags_sorted_by_file
.pydevproject

2
ROMFS/px4fmu_common/init.d/3031_phantom
View File

@ -5,4 +5,6 @@
# Simon Wilks <sjwilks@gmail.com>
#
sh /etc/init.d/rc.fw_defaults
set MIXER FMU_Q

35
ROMFS/px4fmu_common/init.d/4008_ardrone Normal file
View File

@ -0,0 +1,35 @@
#!nsh
#
# ARDrone
#
echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board"
# Just use the default multicopter settings.
sh /etc/init.d/rc.mc_defaults
#
# Load default params for this platform
#
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set MC_ROLL_P 5.0
param set MC_ROLLRATE_P 0.13
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.0
param set MC_PITCH_P 5.0
param set MC_PITCHRATE_P 0.13
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.0
param set MC_YAW_P 1.0
param set MC_YAW_D 0.1
param set MC_YAWRATE_P 0.15
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
param set MC_YAW_FF 0.15
fi
set OUTPUT_MODE ardrone
set USE_IO no
set MIXER skip

9
ROMFS/px4fmu_common/init.d/rc.autostart
View File

@ -106,6 +106,15 @@ then
sh /etc/init.d/4001_quad_x
fi
#
# ARDrone
#
if param compare SYS_AUTOSTART 4008 8
then
sh /etc/init.d/4008_ardrone
fi
if param compare SYS_AUTOSTART 4010 10
then
sh /etc/init.d/4010_dji_f330

9
ROMFS/px4fmu_common/init.d/rc.interface
View File

@ -3,7 +3,7 @@
# Script to configure control interface
#
if [ $MIXER != none ]
if [ $MIXER != none -a $MIXER != skip]
then
#
# Load mixer
@ -33,8 +33,11 @@ then
tone_alarm $TUNE_OUT_ERROR
fi
else
echo "[init] Mixer not defined"
tone_alarm $TUNE_OUT_ERROR
if [ $MIXER != skip ]
then
echo "[init] Mixer not defined"
tone_alarm $TUNE_OUT_ERROR
fi
fi
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]

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

@ -240,6 +240,11 @@ then
fi
fi
if [ $OUTPUT_MODE == ardrone ]
then
set FMU_MODE gpio_serial
fi
if [ $HIL == yes ]
then
set OUTPUT_MODE hil
@ -277,9 +282,9 @@ then
tone_alarm $TUNE_OUT_ERROR
fi
fi
if [ $OUTPUT_MODE == fmu ]
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE = ardrone ]
then
echo "[init] Use FMU PWM as primary output"
echo "[init] Use FMU as primary output"
if fmu mode_$FMU_MODE
then
echo "[init] FMU mode_$FMU_MODE started"
@ -294,7 +299,7 @@ then
then
set TTYS1_BUSY yes
fi
if [ $FMU_MODE == pwm_gpio ]
if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
then
set TTYS1_BUSY yes
fi
@ -351,7 +356,7 @@ then
fi
fi
else
if [ $OUTPUT_MODE != fmu ]
if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
then
if fmu mode_$FMU_MODE
then
@ -367,7 +372,7 @@ then
then
set TTYS1_BUSY yes
fi
if [ $FMU_MODE == pwm_gpio ]
if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
then
set TTYS1_BUSY yes
fi
@ -427,6 +432,14 @@ then
gps start
fi
#
# Start up ARDrone Motor interface
#
if [ $OUTPUT_MODE == ardrone ]
then
ardrone_interface start -d /dev/ttyS1
fi
#
# Fixed wing setup
#

BIN
ROMFS/px4fmu_common/logging/conv.zip
View File

Binary file not shown.

88
ROMFS/px4fmu_logging/init.d/rcS
View File

@ -1,88 +0,0 @@
#!nsh
#
# PX4FMU startup script for logging purposes
#
#
# Try to mount the microSD card.
#
echo "[init] looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
echo "[init] card mounted at /fs/microsd"
# Start playing the startup tune
tone_alarm start
else
echo "[init] no microSD card found"
# Play SOS
tone_alarm error
fi
uorb start
#
# Start sensor drivers here.
#
ms5611 start
adc start
# mag might be external
if hmc5883 start
then
echo "using HMC5883"
fi
if mpu6000 start
then
echo "using MPU6000"
fi
if l3gd20 start
then
echo "using L3GD20(H)"
fi
if lsm303d start
then
set BOARD fmuv2
else
set BOARD fmuv1
fi
# Start airspeed sensors
if meas_airspeed start
then
echo "using MEAS airspeed sensor"
else
if ets_airspeed start
then
echo "using ETS airspeed sensor (bus 3)"
else
if ets_airspeed start -b 1
then
echo "Using ETS airspeed sensor (bus 1)"
fi
fi
fi
#
# Start the sensor collection task.
# IMPORTANT: this also loads param offsets
# ALWAYS start this task before the
# preflight_check.
#
if sensors start
then
echo "SENSORS STARTED"
fi
sdlog2 start -r 250 -e -b 16
if sercon
then
echo "[init] USB interface connected"
# Try to get an USB console
nshterm /dev/ttyACM0 &
fi

BIN
ROMFS/px4fmu_logging/logging/conv.zip
View File

Binary file not shown.

1
Tools/fix_code_style.sh
View File

@ -16,4 +16,5 @@ astyle \
--ignore-exclude-errors-x \
--lineend=linux \
--exclude=EASTL \
--add-brackets \
$*

19
Tools/fix_code_style_ubuntu.sh
View File

@ -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 \

0
Tools/README.txt → Tools/sdlog2/README.txt
View File

0
Tools/logconv.m → Tools/sdlog2/logconv.m
View File

0
Tools/sdlog2_dump.py → Tools/sdlog2/sdlog2_dump.py
View File

6
nuttx-configs/px4fmu-v2/nsh/defconfig
View File

@ -306,7 +306,7 @@ CONFIG_UART5_RXDMA=y
# CONFIG_USART6_RS485 is not set
CONFIG_USART6_RXDMA=y
# CONFIG_UART7_RS485 is not set
# CONFIG_UART7_RXDMA is not set
CONFIG_UART7_RXDMA=y
# CONFIG_UART8_RS485 is not set
CONFIG_UART8_RXDMA=y
CONFIG_SERIAL_DISABLE_REORDERING=y
@ -539,8 +539,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2
# CONFIG_USART3_SERIAL_CONSOLE is not set
# CONFIG_UART4_SERIAL_CONSOLE is not set
# CONFIG_USART6_SERIAL_CONSOLE is not set
# CONFIG_UART7_SERIAL_CONSOLE is not set
CONFIG_UART8_SERIAL_CONSOLE=y
CONFIG_UART7_SERIAL_CONSOLE=y
# CONFIG_UART8_SERIAL_CONSOLE is not set
# CONFIG_NO_SERIAL_CONSOLE is not set
#

2
src/drivers/drv_pwm_output.h
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 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)

2
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -705,7 +705,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
foundMotorCount++;
if (Motor[i].MaxPWM == 250) {
if ((Motor[i].MaxPWM & 252) == 248) {
Motor[i].Version = BLCTRL_NEW;
} else {

2
src/drivers/px4fmu/fmu.cpp
View File

@ -1714,7 +1714,7 @@ fmu_main(int argc, char *argv[])
}
fprintf(stderr, "FMU: unrecognised command, try:\n");
fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)

57
src/drivers/px4io/px4io.cpp
View File

@ -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]);

1
src/lib/ecl/ecl.h
View File

@ -38,7 +38,6 @@
*/
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#define ecl_absolute_time hrt_absolute_time
#define ecl_elapsed_time hrt_elapsed_time

13
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
View File

@ -38,6 +38,8 @@
*
*/
#include <float.h>
#include "ecl_l1_pos_controller.h"
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 */
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 = (vector_A_to_airplane).normalized();
math::Vector<2> vector_A_to_airplane_unit;
/* 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 */

5
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -3,13 +3,10 @@
#include "tecs.h"
#include <ecl/ecl.h>
#include <systemlib/err.h>
#include <geo/geo.h>
using namespace math;
#ifndef CONSTANTS_ONE_G
#define CONSTANTS_ONE_G GRAVITY
#endif
/**
* @file tecs.cpp
*

27
src/lib/external_lgpl/tecs/tecs.h
View File

@ -28,16 +28,7 @@ class __EXPORT TECS
{
public:
TECS() :
_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),
_pitch_dem(0.0f),
_integ1_state(0.0f),
_integ2_state(0.0f),
_integ3_state(0.0f),
@ -45,8 +36,16 @@ public:
_integ5_state(0.0f),
_integ6_state(0.0f),
_integ7_state(0.0f),
_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),
_SKE_dem(0.0f),
_SPEdot_dem(0.0f),
@ -55,9 +54,9 @@ public:
_SKE_est(0.0f),
_SPEdot(0.0f),
_SKEdot(0.0f),
_vel_dot(0.0f),
_STEdotErrLast(0.0f) {
_airspeed_enabled(false),
_throttle_slewrate(0.0f)
{
}
bool airspeed_sensor_enabled() {

2
src/modules/commander/accelerometer_calibration.cpp
View File

@ -311,7 +311,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
(double)accel_ref[orient][2]);
data_collected[orient] = true;
tune_neutral();
tune_neutral(true);
}
close(sensor_combined_sub);

2
src/modules/commander/airspeed_calibration.cpp
View File

@ -142,7 +142,7 @@ int do_airspeed_calibration(int mavlink_fd)
}
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
tune_neutral();
tune_neutral(true);
close(diff_pres_sub);
return OK;

82
src/modules/commander/commander.cpp
View File

@ -610,7 +610,6 @@ int commander_thread_main(int argc, char *argv[])
/* not yet initialized */
commander_initialized = false;
bool battery_tune_played = false;
bool arm_tune_played = false;
/* set parameters */
@ -902,11 +901,13 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(safety), safety_sub, &safety);
// XXX this would be the right approach to do it, but do we *WANT* this?
// /* disarm if safety is now on and still armed */
// if (safety.safety_switch_available && !safety.safety_off) {
// (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
// }
/* disarm if safety is now on and still armed */
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
}
}
}
/* update global position estimate */
@ -961,7 +962,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
@ -1024,14 +1025,12 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
if (armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
@ -1105,7 +1104,7 @@ int commander_thread_main(int argc, char *argv[])
/* mark home position as set */
status.condition_home_position_valid = true;
tune_positive();
tune_positive(true);
}
}
@ -1200,8 +1199,9 @@ int commander_thread_main(int argc, char *argv[])
/* evaluate the main state machine according to mode switches */
res = set_main_state_rc(&status);
/* play tune on mode change only if armed, blink LED always */
if (res == TRANSITION_CHANGED) {
tune_positive();
tune_positive(armed.armed);
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
@ -1257,7 +1257,7 @@ int commander_thread_main(int argc, char *argv[])
/* flight termination in manual mode if assisted switch is on easy position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive();
tune_positive(armed.armed);
}
}
@ -1312,26 +1312,23 @@ int commander_thread_main(int argc, char *argv[])
/* play arming and battery warning tunes */
if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) {
/* play tune when armed */
if (tune_arm() == OK)
arm_tune_played = true;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
/* play tune on battery warning */
if (tune_low_bat() == OK)
battery_tune_played = true;
set_tune(TONE_ARMING_WARNING_TUNE);
arm_tune_played = true;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
/* play tune on battery critical */
if (tune_critical_bat() == OK)
battery_tune_played = true;
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
} else if (battery_tune_played) {
tune_stop();
battery_tune_played = false;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* play tune on battery warning or failsafe */
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
} else {
set_tune(TONE_STOP_TUNE);
}
/* reset arm_tune_played when disarmed */
if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) {
if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
arm_tune_played = false;
}
@ -1426,11 +1423,8 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
if (set_normal_color) {
/* set color */
if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
rgbled_set_color(RGBLED_COLOR_AMBER);
}
if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) {
rgbled_set_color(RGBLED_COLOR_AMBER);
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
} else {
@ -1697,15 +1691,9 @@ print_reject_mode(struct vehicle_status_s *status, const char *msg)
sprintf(s, "#audio: REJECT %s", msg);
mavlink_log_critical(mavlink_fd, s);
// only buzz if armed, because else we're driving people nuts indoors
// they really need to look at the leds as well.
if (status->arming_state == ARMING_STATE_ARMED) {
tune_negative();
} else {
// Always show the led indication
led_negative();
}
/* only buzz if armed, because else we're driving people nuts indoors
they really need to look at the leds as well. */
tune_negative(armed.armed);
}
}
@ -1719,7 +1707,7 @@ print_reject_arm(const char *msg)
char s[80];
sprintf(s, "#audio: %s", msg);
mavlink_log_critical(mavlink_fd, s);
tune_negative();
tune_negative(true);
}
}
@ -1727,27 +1715,27 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive();
tune_positive(true);
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
tune_negative();
tune_negative(true);
break;
case VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
tune_negative();
tune_negative(true);
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
tune_negative();
tune_negative(true);
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
tune_negative();
tune_negative(true);
break;
default:
@ -1887,9 +1875,9 @@ void *commander_low_prio_loop(void *arg)
}
if (calib_ret == OK)
tune_positive();
tune_positive(true);
else
tune_negative();
tune_negative(true);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);

91
src/modules/commander/commander_helper.cpp
View File

@ -45,6 +45,7 @@
#include <stdbool.h>
#include <fcntl.h>
#include <math.h>
#include <string.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@ -81,11 +82,22 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status)
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
}
static int buzzer;
static hrt_abstime blink_msg_end;
static int buzzer = -1;
static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message
static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence
static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end
static unsigned int tune_durations[TONE_NUMBER_OF_TUNES];
int buzzer_init()
{
tune_end = 0;
tune_current = 0;
memset(tune_durations, 0, sizeof(tune_durations));
tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000;
tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000;
tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000;
tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000;
buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
if (buzzer < 0) {
@ -101,58 +113,60 @@ void buzzer_deinit()
close(buzzer);
}
void tune_error()
{
ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
void set_tune(int tune) {
unsigned int new_tune_duration = tune_durations[tune];
/* don't interrupt currently playing non-repeating tune by repeating */
if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
/* allow interrupting current non-repeating tune by the same tune */
if (tune != tune_current || new_tune_duration != 0) {
ioctl(buzzer, TONE_SET_ALARM, tune);
}
tune_current = tune;
if (new_tune_duration != 0) {
tune_end = hrt_absolute_time() + new_tune_duration;
} else {
tune_end = 0;
}
}
}
void tune_positive()
/**
* Blink green LED and play positive tune (if use_buzzer == true).
*/
void tune_positive(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
if (use_buzzer) {
set_tune(TONE_NOTIFY_POSITIVE_TUNE);
}
}
void tune_neutral()
/**
* Blink white LED and play neutral tune (if use_buzzer == true).
*/
void tune_neutral(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_WHITE);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
}
}
void tune_negative()
{
led_negative();
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
}
void led_negative()
/**
* Blink red LED and play negative tune (if use_buzzer == true).
*/
void tune_negative(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_RED);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
}
int tune_arm()
{
return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE);
}
int tune_low_bat()
{
return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE);
}
int tune_critical_bat()
{
return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
}
void tune_stop()
{
ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
}
}
int blink_msg_state()
@ -161,6 +175,7 @@ int blink_msg_state()
return 0;
} else if (hrt_absolute_time() > blink_msg_end) {
blink_msg_end = 0;
return 2;
} else {
@ -168,8 +183,8 @@ int blink_msg_state()
}
}
static int leds;
static int rgbleds;
static int leds = -1;
static int rgbleds = -1;
int led_init()
{

14
src/modules/commander/commander_helper.h
View File

@ -54,16 +54,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status);
int buzzer_init(void);
void buzzer_deinit(void);
void tune_error(void);
void tune_positive(void);
void tune_neutral(void);
void tune_negative(void);
int tune_arm(void);
int tune_low_bat(void);
int tune_critical_bat(void);
void tune_stop(void);
void led_negative();
void set_tune(int tune);
void tune_positive(bool use_buzzer);
void tune_neutral(bool use_buzzer);
void tune_negative(bool use_buzzer);
int blink_msg_state();

134
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -130,23 +130,23 @@ private:
int _att_sub; /**< vehicle attitude subscription */
int _attitude_sub; /**< raw rc channels data 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 _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 _nav_capabilities_pub; /**< navigation capabilities publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
perf_counter_t _loop_perf; /**< loop performance counter */
@ -154,13 +154,13 @@ private:
/** manual control states */
float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */
float _loiter_hold_lat;
float _loiter_hold_lon;
double _loiter_hold_lat;
double _loiter_hold_lon;
float _loiter_hold_alt;
bool _loiter_hold;
float _launch_lat;
float _launch_lon;
double _launch_lat;
double _launch_lon;
float _launch_alt;
bool _launch_valid;
@ -194,7 +194,7 @@ private:
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
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;
TECS _tecs;
@ -235,7 +235,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;
@ -280,7 +279,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;
@ -374,6 +372,7 @@ FixedwingPositionControl *g_control;
FixedwingPositionControl::FixedwingPositionControl() :
_mavlink_fd(-1),
_task_should_exit(false),
_control_task(-1),
@ -392,39 +391,34 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
/* states */
_setpoint_valid(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_vertical(false),
land_stayonground(false),
land_motor_lim(false),
land_onslope(false),
flare_curve_alt_last(0.0f),
_mavlink_fd(-1),
launchDetector(),
launch_detected(false),
last_manual(false),
usePreTakeoffThrust(false),
last_manual(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;
_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
@ -444,7 +438,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");
@ -533,7 +526,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));
@ -600,8 +592,8 @@ FixedwingPositionControl::vehicle_control_mode_poll()
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
if (!was_armed && _control_mode.flag_armed) {
_launch_lat = _global_pos.lat / 1e7f;
_launch_lon = _global_pos.lon / 1e7f;
_launch_lat = _global_pos.lat;
_launch_lon = _global_pos.lon;
_launch_alt = _global_pos.alt;
_launch_valid = true;
}
@ -716,7 +708,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)
{
if (_global_pos_valid) {
if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
/* rotate ground speed vector with current attitude */
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@ -902,8 +894,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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);
@ -929,7 +923,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);
/* 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;
land_stayonground = true;
@ -948,38 +942,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);
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),
@ -1082,13 +1062,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
}
/* climb out control */
bool climb_out = false;
//XXX not used
/* user wants to climb out */
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
climb_out = true;
}
/* climb out control */
// bool climb_out = false;
//
// /* 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 */
@ -1307,7 +1289,7 @@ FixedwingPositionControl::task_main()
float turn_distance = _l1_control.switch_distance(100.0f);
/* 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 */
_nav_capabilities.turn_distance = turn_distance;

7
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -348,13 +348,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);
/**
*
*

61
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -53,11 +53,9 @@
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
@ -71,7 +69,6 @@
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
@ -84,8 +81,8 @@
*/
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
#define MIN_TAKEOFF_THROTTLE 0.3f
#define YAW_DEADZONE 0.05f
#define MIN_TAKEOFF_THRUST 0.2f
#define RATES_I_LIMIT 0.3f
class MulticopterAttitudeControl
@ -135,15 +132,13 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */
math::Matrix<3, 3> _R; /**< rotation matrix for current state */
math::Vector<3> _rates_prev; /**< angular rates on previous step */
math::Vector<3> _rates_sp; /**< angular rates setpoint */
math::Vector<3> _rates_int; /**< angular rates integral error */
float _thrust_sp; /**< thrust setpoint */
math::Vector<3> _att_control; /**< attitude control vector */
math::Matrix<3, 3> I; /**< identity matrix */
math::Matrix<3, 3> _I; /**< identity matrix */
bool _reset_yaw_sp; /**< reset yaw setpoint flag */
@ -262,13 +257,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_actuators_0_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc att control"))
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
{
memset(&_v_att, 0, sizeof(_v_att));
memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
memset(&_v_control_mode, 0, sizeof(_v_control_mode));
memset(&_actuators, 0, sizeof(_actuators));
memset(&_armed, 0, sizeof(_armed));
_params.att_p.zero();
@ -276,15 +273,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.rate_i.zero();
_params.rate_d.zero();
_R_sp.identity();
_R.identity();
_rates_prev.zero();
_rates_sp.zero();
_rates_int.zero();
_thrust_sp = 0.0f;
_att_control.zero();
I.identity();
_I.identity();
_params_handles.roll_p = param_find("MC_ROLL_P");
_params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
@ -535,16 +530,18 @@ MulticopterAttitudeControl::control_attitude(float dt)
_thrust_sp = _v_att_sp.thrust;
/* construct attitude setpoint rotation matrix */
math::Matrix<3, 3> R_sp;
if (_v_att_sp.R_valid) {
/* rotation matrix in _att_sp is valid, use it */
_R_sp.set(&_v_att_sp.R_body[0][0]);
R_sp.set(&_v_att_sp.R_body[0][0]);
} else {
/* rotation matrix in _att_sp is not valid, use euler angles instead */
_R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
/* copy rotation matrix back to setpoint struct */
memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body));
memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body));
_v_att_sp.R_valid = true;
}
@ -561,23 +558,24 @@ MulticopterAttitudeControl::control_attitude(float dt)
}
/* rotation matrix for current state */
_R.set(_v_att.R);
math::Matrix<3, 3> R;
R.set(_v_att.R);
/* all input data is ready, run controller itself */
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2));
math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2));
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
/* axis and sin(angle) of desired rotation */
math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z);
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
/* calculate angle error */
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;
/* calculate weight for yaw control */
float yaw_w = _R_sp(2, 2) * _R_sp(2, 2);
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
@ -600,15 +598,15 @@ MulticopterAttitudeControl::control_attitude(float dt)
e_R_cp(2, 1) = e_R_z_axis(0);
/* rotation matrix for roll/pitch only rotation */
R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {
/* zero roll/pitch rotation */
R_rp = _R;
R_rp = R;
}
/* R_rp and _R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0));
/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
@ -616,7 +614,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
math::Quaternion q;
q.from_dcm(_R.transposed() * _R_sp);
q.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q.imag();
e_R_d.normalize();
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
@ -658,7 +656,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
_rates_prev = rates;
/* update integral only if not saturated on low limit */
if (_thrust_sp > 0.2f) {
if (_thrust_sp > MIN_TAKEOFF_THRUST) {
for (int i = 0; i < 3; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
@ -695,9 +693,6 @@ MulticopterAttitudeControl::task_main()
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */
orb_set_interval(_v_att_sub, 5);
/* initialize parameters cache */
parameters_update();
@ -767,10 +762,12 @@ MulticopterAttitudeControl::task_main()
}
} else {
/* attitude controller disabled */
// TODO poll 'attitude_rates_setpoint' topic
_rates_sp.zero();
_thrust_sp = 0.0f;
/* attitude controller disabled, poll rates setpoint topic */
vehicle_rates_setpoint_poll();
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
_thrust_sp = _v_rates_sp.thrust;
}
if (_v_control_mode.flag_control_rates_enabled) {

5
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -51,7 +51,6 @@
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
@ -68,7 +67,6 @@
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
@ -732,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);
@ -794,7 +791,6 @@ MulticopterPositionControl::task_main()
}
thrust_int(2) = -i;
mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
}
} else {
@ -806,7 +802,6 @@ MulticopterPositionControl::task_main()
reset_int_xy = false;
thrust_int(0) = 0.0f;
thrust_int(1) = 0.0f;
mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral");
}
} else {

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

@ -154,17 +154,16 @@ private:
int _capabilities_sub; /**< notification of vehicle capabilities updates */
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 */
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 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_item_s _mission_item; /**< current mission item */
bool _mission_item_valid; /**< current mission item valid */
struct mission_item_s _mission_item; /**< current mission item */
perf_counter_t _loop_perf; /**< loop performance counter */
@ -178,21 +177,22 @@ private:
class Mission _mission;
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 _mission_item_valid; /**< current mission item valid */
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_yaw_reached;
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 _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (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) */
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;
char *nav_states_str[NAV_STATE_MAX];
const char *nav_states_str[NAV_STATE_MAX];
struct {
float min_altitude;
@ -321,6 +321,11 @@ private:
*/
bool onboard_mission_available(unsigned relative_index);
/**
* Reset all "reached" flags.
*/
void reset_reached();
/**
* Check if current mission item has been reached.
*/
@ -382,11 +387,11 @@ Navigator::Navigator() :
_global_pos_sub(-1),
_home_pos_sub(-1),
_vstatus_sub(-1),
_control_mode_sub(-1),
_params_sub(-1),
_offboard_mission_sub(-1),
_onboard_mission_sub(-1),
_capabilities_sub(-1),
_control_mode_sub(-1),
/* publications */
_pos_sp_triplet_pub(-1),
@ -395,22 +400,22 @@ Navigator::Navigator() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
/* states */
_rtl_state(RTL_STATE_NONE),
_geofence_violation_warning_sent(false),
_fence_valid(false),
_inside_fence(true),
_mission(),
_mission_item_valid(false),
_global_pos_valid(false),
_reset_loiter_pos(true),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_set_nav_state_timestamp(0),
_mission_item_valid(false),
_need_takeoff(true),
_do_takeoff(false),
_set_nav_state_timestamp(0),
_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.acceptance_radius = param_find("NAV_ACCEPT_RAD");
@ -695,7 +700,7 @@ Navigator::task_main()
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
/* switch to RTL if not already landed after RTL and home position set */
if (!(_rtl_state == RTL_STATE_DESCEND &&
(myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
(myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
_vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED);
}
@ -741,7 +746,7 @@ Navigator::task_main()
case NAV_STATE_RTL:
if (!(_rtl_state == RTL_STATE_DESCEND &&
(myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
(myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
_vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED);
}
@ -749,9 +754,7 @@ Navigator::task_main()
break;
case NAV_STATE_LAND:
if (myState != NAV_STATE_READY) {
dispatch(EVENT_LAND_REQUESTED);
}
dispatch(EVENT_LAND_REQUESTED);
break;
@ -853,11 +856,8 @@ 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 */
_time_first_inside_orbit = 0;
}
perf_end(_loop_perf);
@ -955,7 +955,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY},
@ -1009,6 +1009,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
void
Navigator::start_none()
{
reset_reached();
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
@ -1024,6 +1026,8 @@ Navigator::start_none()
void
Navigator::start_ready()
{
reset_reached();
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = false;
@ -1046,6 +1050,8 @@ Navigator::start_ready()
void
Navigator::start_loiter()
{
reset_reached();
_do_takeoff = false;
/* set loiter position if needed */
@ -1061,11 +1067,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");
}
}
@ -1091,6 +1097,8 @@ Navigator::start_mission()
void
Navigator::set_mission_item()
{
reset_reached();
/* copy current mission to previous item */
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
@ -1104,9 +1112,6 @@ Navigator::set_mission_item()
ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
if (ret == OK) {
/* reset time counter for new item */
_time_first_inside_orbit = 0;
_mission_item_valid = true;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
@ -1162,14 +1167,14 @@ Navigator::set_mission_item()
}
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 {
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);
}
}
@ -1224,6 +1229,8 @@ Navigator::start_rtl()
void
Navigator::start_land()
{
reset_reached();
/* this state can be requested by commander even if no global position available,
* in his case controller must perform landing without position control */
_do_takeoff = false;
@ -1255,6 +1262,8 @@ Navigator::start_land()
void
Navigator::start_land_home()
{
reset_reached();
/* land to home position, should be called when hovering above home, from RTL state */
_do_takeoff = false;
_reset_loiter_pos = true;
@ -1285,8 +1294,7 @@ Navigator::start_land_home()
void
Navigator::set_rtl_item()
{
/*reset time counter for new RTL item */
_time_first_inside_orbit = 0;
reset_reached();
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
@ -1318,7 +1326,7 @@ Navigator::set_rtl_item()
_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;
}
@ -1330,7 +1338,14 @@ Navigator::set_rtl_item()
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
// don't change altitude
_mission_item.yaw = NAN; // TODO set heading to home
if (_pos_sp_triplet.previous.valid) {
/* if previous setpoint is valid then use it to calculate heading to home */
_mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
} else {
/* else use current position */
_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
}
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@ -1344,7 +1359,7 @@ Navigator::set_rtl_item()
_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;
}
@ -1362,7 +1377,7 @@ Navigator::set_rtl_item()
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_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.autocontinue = _parameters.rtl_land_delay > -0.001f;
_mission_item.origin = ORIGIN_ONBOARD;
@ -1371,12 +1386,12 @@ Navigator::set_rtl_item()
_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;
}
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;
}
@ -1388,7 +1403,8 @@ Navigator::set_rtl_item()
void
Navigator::request_loiter_or_ready()
{
if (_vstatus.condition_landed) {
/* XXX workaround: no landing detector for fixedwing yet */
if (_vstatus.condition_landed && _vstatus.is_rotary_wing) {
dispatch(EVENT_READY_REQUESTED);
} else {
@ -1418,17 +1434,28 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_
sp->lon = _home_pos.lon;
sp->alt = _home_pos.alt + _parameters.rtl_alt;
if (_pos_sp_triplet.previous.valid) {
/* if previous setpoint is valid then use it to calculate heading to home */
sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon);
} else {
/* else use current position */
sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon);
}
sp->loiter_radius = _parameters.loiter_radius;
sp->loiter_direction = 1;
sp->pitch_min = 0.0f;
} else {
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude;
sp->yaw = item->yaw;
sp->loiter_radius = item->loiter_radius;
sp->loiter_direction = item->loiter_direction;
sp->pitch_min = item->pitch_min;
}
sp->yaw = item->yaw;
sp->loiter_radius = item->loiter_radius;
sp->loiter_direction = item->loiter_direction;
sp->pitch_min = item->pitch_min;
if (item->nav_cmd == NAV_CMD_TAKEOFF) {
sp->type = SETPOINT_TYPE_TAKEOFF;
@ -1505,7 +1532,7 @@ Navigator::check_mission_item_reached()
}
}
if (!_waypoint_yaw_reached) {
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
@ -1525,16 +1552,14 @@ 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", _mission_item.time_inside);
mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside);
}
}
/* check if the MAV was long enough inside the waypoint orbit */
if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
_time_first_inside_orbit = 0;
_waypoint_yaw_reached = false;
_waypoint_position_reached = false;
reset_reached();
return true;
}
}
@ -1543,6 +1568,15 @@ Navigator::check_mission_item_reached()
}
void
Navigator::reset_reached()
{
_time_first_inside_orbit = 0;
_waypoint_position_reached = false;
_waypoint_yaw_reached = false;
}
void
Navigator::on_mission_item_reached()
{
@ -1550,7 +1584,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 */

26
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -42,14 +42,11 @@
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <float.h>
#include <string.h>
#include <nuttx/config.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
@ -170,12 +167,13 @@ void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3],
FILE *f = fopen("/fs/microsd/inav.log", "a");
if (f) {
char *s = malloc(256);
snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]);
fputs(f, s);
snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
fputs(f, s);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]);
fwrite(s, 1, n, f);
n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
fwrite(s, 1, n, f);
free(s);
}
fsync(fileno(f));
fclose(f);
}
@ -711,6 +709,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
float x_est_prev[3], y_est_prev[3];
memcpy(x_est_prev, x_est, sizeof(x_est));
memcpy(y_est_prev, y_est, sizeof(y_est));
if (can_estimate_xy) {
/* inertial filter prediction for position */
inertial_filter_predict(dt, x_est);
@ -718,7 +721,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
thread_should_exit = true;
memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est));
}
/* inertial filter correction for position */
@ -742,7 +746,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
thread_should_exit = true;
memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est));
memset(corr_acc, 0, sizeof(corr_acc));
memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_flow, 0, sizeof(corr_flow));
}
}

34
src/modules/px4iofirmware/dsm.c
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
* 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;

2
src/modules/px4iofirmware/mixer.cpp
View File

@ -179,7 +179,7 @@ mixer_tick(void)
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
/* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
/* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
/* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
)
);

2
src/modules/px4iofirmware/registers.c
View File

@ -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:

3
src/modules/sdlog2/logbuffer.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2013, 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

3
src/modules/sdlog2/sdlog2.c
View File

@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -451,6 +449,7 @@ static void *logwriter_thread(void *arg)
n = available;
}
lseek(log_fd, 0, SEEK_CUR);
n = write(log_fd, read_ptr, n);
should_wait = (n == available) && !is_part;

2
src/modules/systemlib/perf_counter.h
View File

@ -39,6 +39,8 @@
#ifndef _SYSTEMLIB_PERF_COUNTER_H
#define _SYSTEMLIB_PERF_COUNTER_H value
#include <stdint.h>
/**
* Counter types.
*/

1
src/systemcmds/tests/module.mk
View File

@ -26,6 +26,7 @@ SRCS = test_adc.c \
test_mixer.cpp \
test_mathlib.cpp \
test_file.c \
test_file2.c \
tests_main.c \
test_param.c \
test_ppm_loopback.c \

196
src/systemcmds/tests/test_file2.c Normal file
View File

@ -0,0 +1,196 @@
/****************************************************************************
*
* Copyright (C) 2012 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file test_file2.c
*
* File write test.
*/
#include <sys/stat.h>
#include <dirent.h>
#include <stdio.h>
#include <stddef.h>
#include <unistd.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <string.h>
#include <stdlib.h>
#include <getopt.h>
#define FLAG_FSYNC 1
#define FLAG_LSEEK 2
/*
return a predictable value for any file offset to allow detection of corruption
*/
static uint8_t get_value(uint32_t ofs)
{
union {
uint32_t ofs;
uint8_t buf[4];
} u;
u.ofs = ofs;
return u.buf[ofs % 4];
}
static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags)
{
printf("Testing on %s with write_chunk=%u write_size=%u\n",
filename, (unsigned)write_chunk, (unsigned)write_size);
uint32_t ofs = 0;
int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC);
if (fd == -1) {
perror(filename);
exit(1);
}
// create a file of size write_size, in write_chunk blocks
uint8_t counter = 0;
while (ofs < write_size) {
uint8_t buffer[write_chunk];
for (uint16_t j=0; j<write_chunk; j++) {
buffer[j] = get_value(ofs);
ofs++;
}
if (write(fd, buffer, sizeof(buffer)) != sizeof(buffer)) {
printf("write failed at offset %u\n", ofs);
exit(1);
}
if (flags & FLAG_FSYNC) {
fsync(fd);
}
if (counter % 100 == 0) {
printf("write ofs=%u\r", ofs);
}
counter++;
}
close(fd);
printf("write ofs=%u\n", ofs);
// read and check
fd = open(filename, O_RDONLY);
if (fd == -1) {
perror(filename);
exit(1);
}
counter = 0;
ofs = 0;
while (ofs < write_size) {
uint8_t buffer[write_chunk];
if (counter % 100 == 0) {
printf("read ofs=%u\r", ofs);
}
counter++;
if (read(fd, buffer, sizeof(buffer)) != sizeof(buffer)) {
printf("read failed at offset %u\n", ofs);
exit(1);
}
for (uint16_t j=0; j<write_chunk; j++) {
if (buffer[j] != get_value(ofs)) {
printf("corruption at ofs=%u got %u\n", ofs, buffer[j]);
exit(1);
}
ofs++;
}
if (flags & FLAG_LSEEK) {
lseek(fd, 0, SEEK_CUR);
}
}
printf("read ofs=%u\n", ofs);
close(fd);
unlink(filename);
printf("All OK\n");
}
static void usage(void)
{
printf("test file2 [options] [filename]\n");
printf("\toptions:\n");
printf("\t-s SIZE set file size\n");
printf("\t-c CHUNK set IO chunk size\n");
printf("\t-F fsync on every write\n");
printf("\t-L lseek on every read\n");
}
int test_file2(int argc, char *argv[])
{
int opt;
uint16_t flags = 0;
const char *filename = "/fs/microsd/testfile2.dat";
uint32_t write_chunk = 64;
uint32_t write_size = 5*1024;
while ((opt = getopt(argc, argv, "c:s:FLh")) != EOF) {
switch (opt) {
case 'F':
flags |= FLAG_FSYNC;
break;
case 'L':
flags |= FLAG_LSEEK;
break;
case 's':
write_size = strtoul(optarg, NULL, 0);
break;
case 'c':
write_chunk = strtoul(optarg, NULL, 0);
break;
case 'h':
default:
usage();
exit(1);
}
}
argc -= optind;
argv += optind;
if (argc > 0) {
filename = argv[0];
}
/* check if microSD card is mounted */
struct stat buffer;
if (stat("/fs/microsd/", &buffer)) {
warnx("no microSD card mounted, aborting file test");
return 1;
}
test_corruption(filename, write_chunk, write_size, flags);
return 0;
}

38
src/systemcmds/tests/test_mount.c
View File

@ -141,8 +141,8 @@ test_mount(int argc, char *argv[])
/* announce mode switch */
if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) {
warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC");
fsync(stdout);
fsync(stderr);
fsync(fileno(stdout));
fsync(fileno(stderr));
usleep(20000);
}
@ -162,7 +162,7 @@ test_mount(int argc, char *argv[])
}
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);
write(cmd_fd, buf, strlen(buf) + 1);
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("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n");
fsync(stdout);
fsync(stderr);
fsync(fileno(stdout));
fsync(fileno(stderr));
usleep(50000);
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)));
/* 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 */
write_buf[i] = i+11;
}
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);
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
int wret = write(fd, write_buf + a, chunk_sizes[c]);
if (wret != chunk_sizes[c]) {
if (wret != (int)chunk_sizes[c]) {
warn("WRITE ERROR!");
if ((0x3 & (uintptr_t)(write_buf + a)))
@ -214,8 +212,8 @@ test_mount(int argc, char *argv[])
fsync(fd);
} else {
printf("#");
fsync(stdout);
fsync(stderr);
fsync(fileno(stdout));
fsync(fileno(stderr));
}
}
@ -224,12 +222,10 @@ test_mount(int argc, char *argv[])
}
printf(".");
fsync(stdout);
fsync(stderr);
fsync(fileno(stdout));
fsync(fileno(stderr));
usleep(200000);
end = hrt_absolute_time();
close(fd);
fd = open("/fs/microsd/testfile", O_RDONLY);
@ -237,7 +233,7 @@ test_mount(int argc, char *argv[])
for (unsigned i = 0; i < iterations; i++) {
int rret = read(fd, read_buf, chunk_sizes[c]);
if (rret != chunk_sizes[c]) {
if (rret != (int)chunk_sizes[c]) {
warnx("READ ERROR!");
return 1;
}
@ -245,7 +241,7 @@ test_mount(int argc, char *argv[])
/* compare value */
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]) {
warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a);
compare_ok = false;
@ -271,16 +267,16 @@ test_mount(int argc, char *argv[])
}
}
fsync(stdout);
fsync(stderr);
fsync(fileno(stdout));
fsync(fileno(stderr));
usleep(20000);
/* we always reboot for the next test if we get here */
warnx("Iteration done, rebooting..");
fsync(stdout);
fsync(stderr);
fsync(fileno(stdout));
fsync(fileno(stderr));
usleep(50000);
systemreset(false);

1
src/systemcmds/tests/tests.h
View File

@ -107,6 +107,7 @@ extern int test_jig_voltages(int argc, char *argv[]);
extern int test_param(int argc, char *argv[]);
extern int test_bson(int argc, char *argv[]);
extern int test_file(int argc, char *argv[]);
extern int test_file2(int argc, char *argv[]);
extern int test_mixer(int argc, char *argv[]);
extern int test_rc(int argc, char *argv[]);
extern int test_conv(int argc, char *argv[]);

1
src/systemcmds/tests/tests_main.c
View File

@ -104,6 +104,7 @@ const struct {
{"param", test_param, 0},
{"bson", test_bson, 0},
{"file", test_file, 0},
{"file2", test_file2, OPT_NOJIGTEST | OPT_NOALLTEST},
{"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST},
{"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST},
{"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST},

4
src/systemcmds/top/top.c
View File

@ -233,8 +233,8 @@ top_main(void)
system_load.tasks[i].tcb->pid,
CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
(system_load.tasks[i].total_runtime / 1000),
(int)(curr_loads[i] * 100),
(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 - (int)(curr_loads[i] * 100.0f)) * 1000),
stack_size - stack_free,
stack_size,
system_load.tasks[i].tcb->sched_priority,