Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

Merge commit '95bdc1a9bd364ce95abe06b097579cc8a9162e33' into navigator_new_vector

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-30 08:55:26 +04:00
parent f084ddfeb0 95bdc1a9bd
commit a71b04775c
41 changed files with 1633 additions and 1978 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/31_io_phantom
+19 -14
View File
@@ -8,30 +8,35 @@ echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV"
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set FW_AIRSPD_MIN 11.4
param set FW_AIRSPD_TRIM 14
param set FW_AIRSPD_MAX 22
param set FW_L1_PERIOD 15
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_LIM_MAX 45
param set FW_P_LIM_MIN -45
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_R_IMAX 15
param set FW_R_P 80
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.8
param set FW_THR_LND_MAX 0
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_THR_MIN 0.5
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 17
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set FW_T_SINK_MIN 2.0
param set FW_Y_ROLLFF 1.0
param set RC_SCALE_ROLL 0.6
param set RC_SCALE_PITCH 0.6
param set TRIM_PITCH 0.1
param set SYS_AUTOCONFIG 0
param save
fi
ROMFS/px4fmu_common/init.d/33_io_wingwing
+91
View File
@@ -0,0 +1,91 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 9
param set FW_AIRSPD_MAX 14
param set FW_L1_PERIOD 10
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 20
param set FW_P_LIM_MAX 30
param set FW_P_LIM_MIN -20
param set FW_P_P 30
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 60
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 0.7
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 2.0
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ]
then
exit
fi
ROMFS/px4fmu_common/init.d/34_io_fx79
+91
View File
@@ -0,0 +1,91 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set FW_AIRSPD_MAX 20
param set FW_AIRSPD_TRIM 12
param set FW_AIRSPD_MIN 15
param set FW_L1_PERIOD 12
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 80
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.75
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 1.1
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_FX79.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix
else
echo "Using /etc/mixers/FMU_FX79.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ]
then
exit
fi
ROMFS/px4fmu_common/init.d/800_sdlogger
+60
View File
@@ -0,0 +1,60 @@
#!nsh
echo "[init] PX4FMU v1, v2 init to log only
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param save
fi
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
commander start
sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
sh /etc/init.d/rc.sensors
gps start
attitude_estimator_ekf start
position_estimator_inav start
if [ -d /fs/microsd ]
then
if [ $BOARD == fmuv1 ]
then
sdlog2 start -r 50 -e -b 16
else
sdlog2 start -r 200 -e -b 16
fi
fi
if [ $EXIT_ON_END == yes ]
then
exit
fi
ROMFS/px4fmu_common/init.d/rcS
+18
View File
@@ -331,6 +331,18 @@ then
set MODE custom
fi
if param compare SYS_AUTOSTART 33
then
sh /etc/init.d/33_io_wingwing
set MODE custom
fi
if param compare SYS_AUTOSTART 34
then
sh /etc/init.d/34_io_fx79
set MODE custom
fi
if param compare SYS_AUTOSTART 40
then
sh /etc/init.d/40_io_segway
@@ -355,6 +367,12 @@ then
set MODE custom
fi
if param compare SYS_AUTOSTART 800
then
sh /etc/init.d/800_sdlogger
set MODE custom
fi
# Start any custom extensions that might be missing
if [ -f /fs/microsd/etc/rc.local ]
then
ROMFS/px4fmu_common/mixers/FMU_FX79.mix
Executable
+72
View File
@@ -0,0 +1,72 @@
FX-79 Delta-wing mixer for PX4FMU
=================================
Designed for FX-79.
TODO (sjwilks): Add mixers for flaps.
This file defines mixers suitable for controlling a delta wing aircraft using
PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
assumed to be unused.
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
(roll), 1 (pitch) and 3 (thrust).
See the README for more information on the scaler format.
Elevon mixers
-------------
Three scalers total (output, roll, pitch).
On the assumption that the two elevon servos are physically reversed, the pitch
input is inverted between the two servos.
The scaling factor for roll inputs is adjusted to implement differential travel
for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 5000 8000 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 8000 5000 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
Output 2
--------
This mixer is empty.
Z:
Motor speed mixer
-----------------
Two scalers total (output, thrust).
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / flaps / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
src/drivers/px4io/px4io.cpp
+6 -10
View File
@@ -850,7 +850,7 @@ PX4IO::task_main()
/* we're not nice to the lower-priority control groups and only check them
when the primary group updated (which is now). */
io_set_control_groups();
(void)io_set_control_groups();
}
if (now >= poll_last + IO_POLL_INTERVAL) {
@@ -962,14 +962,14 @@ out:
int
PX4IO::io_set_control_groups()
{
bool attitude_ok = io_set_control_state(0);
int ret = io_set_control_state(0);
/* send auxiliary control groups */
(void)io_set_control_state(1);
(void)io_set_control_state(2);
(void)io_set_control_state(3);
return attitude_ok;
return ret;
}
int
@@ -1095,8 +1095,10 @@ PX4IO::io_set_rc_config()
* assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical
* controls.
*/
/* fill the mapping with an error condition triggering value */
for (unsigned i = 0; i < _max_rc_input; i++)
input_map[i] = -1;
input_map[i] = UINT8_MAX;
/*
* NOTE: The indices for mapped channels are 1-based
@@ -1128,12 +1130,6 @@ PX4IO::io_set_rc_config()
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan - 1] = 4;
ichan = 5;
for (unsigned i = 0; i < _max_rc_input; i++)
if (input_map[i] == -1)
input_map[i] = ichan++;
/*
* Iterate all possible RC inputs.
*/
src/drivers/stm32/drv_hrt.c
+44 -41
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 2013 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
@@ -168,7 +168,7 @@
# error HRT_TIMER_CLOCK must be greater than 1MHz
#endif
/*
/**
* Minimum/maximum deadlines.
*
* These are suitable for use with a 16-bit timer/counter clocked
@@ -276,7 +276,7 @@ static void hrt_call_invoke(void);
* Specific registers and bits used by PPM sub-functions
*/
#ifdef HRT_PPM_CHANNEL
/*
/*
* If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
*
* Note that we assume that M3 means STM32F1 (since we don't really care about the F2).
@@ -336,17 +336,18 @@ static void hrt_call_invoke(void);
/*
* PPM decoder tuning parameters
*/
# define PPM_MAX_PULSE_WIDTH 700 /* maximum width of a valid pulse */
# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */
# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */
# define PPM_MIN_START 2400 /* shortest valid start gap (only 2nd part of pulse) */
# define PPM_MIN_PULSE_WIDTH 200 /**< minimum width of a valid first pulse */
# define PPM_MAX_PULSE_WIDTH 600 /**< maximum width of a valid first pulse */
# define PPM_MIN_CHANNEL_VALUE 800 /**< shortest valid channel signal */
# define PPM_MAX_CHANNEL_VALUE 2200 /**< longest valid channel signal */
# define PPM_MIN_START 2300 /**< shortest valid start gap (only 2nd part of pulse) */
/* decoded PPM buffer */
#define PPM_MIN_CHANNELS 5
#define PPM_MAX_CHANNELS 20
/* Number of same-sized frames required to 'lock' */
#define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */
/** Number of same-sized frames required to 'lock' */
#define PPM_CHANNEL_LOCK 4 /**< should be less than the input timeout */
__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
__EXPORT uint16_t ppm_frame_length = 0;
@@ -363,12 +364,12 @@ unsigned ppm_pulse_next;
static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
/* PPM decoder state machine */
/** PPM decoder state machine */
struct {
uint16_t last_edge; /* last capture time */
uint16_t last_mark; /* last significant edge */
uint16_t frame_start; /* the frame width */
unsigned next_channel; /* next channel index */
uint16_t last_edge; /**< last capture time */
uint16_t last_mark; /**< last significant edge */
uint16_t frame_start; /**< the frame width */
unsigned next_channel; /**< next channel index */
enum {
UNSYNCH = 0,
ARM,
@@ -390,7 +391,7 @@ static void hrt_ppm_decode(uint32_t status);
# define CCER_PPM 0
#endif /* HRT_PPM_CHANNEL */
/*
/**
* Initialise the timer we are going to use.
*
* We expect that we'll own one of the reduced-function STM32 general
@@ -436,7 +437,7 @@ hrt_tim_init(void)
}
#ifdef HRT_PPM_CHANNEL
/*
/**
* Handle the PPM decoder state machine.
*/
static void
@@ -518,8 +519,8 @@ hrt_ppm_decode(uint32_t status)
case ARM:
/* we expect a pulse giving us the first mark */
if (width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too long */
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too short or too long */
/* record the mark timing, expect an inactive edge */
ppm.last_mark = ppm.last_edge;
@@ -533,8 +534,8 @@ hrt_ppm_decode(uint32_t status)
case INACTIVE:
/* we expect a short pulse */
if (width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too long */
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too short or too long */
/* this edge is not interesting, but now we are ready for the next mark */
ppm.phase = ACTIVE;
@@ -576,7 +577,7 @@ error:
}
#endif /* HRT_PPM_CHANNEL */
/*
/**
* Handle the compare interupt by calling the callout dispatcher
* and then re-scheduling the next deadline.
*/
@@ -605,6 +606,7 @@ hrt_tim_isr(int irq, void *context)
hrt_ppm_decode(status);
}
#endif
/* was this a timer tick? */
@@ -623,7 +625,7 @@ hrt_tim_isr(int irq, void *context)
return OK;
}
/*
/**
* Fetch a never-wrapping absolute time value in microseconds from
* some arbitrary epoch shortly after system start.
*/
@@ -670,7 +672,7 @@ hrt_absolute_time(void)
return abstime;
}
/*
/**
* Convert a timespec to absolute time
*/
hrt_abstime
@@ -684,7 +686,7 @@ ts_to_abstime(struct timespec *ts)
return result;
}
/*
/**
* Convert absolute time to a timespec.
*/
void
@@ -695,7 +697,7 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
ts->tv_nsec = abstime * 1000;
}
/*
/**
* Compare a time value with the current time.
*/
hrt_abstime
@@ -710,7 +712,7 @@ hrt_elapsed_time(const volatile hrt_abstime *then)
return delta;
}
/*
/**
* Store the absolute time in an interrupt-safe fashion
*/
hrt_abstime
@@ -725,7 +727,7 @@ hrt_store_absolute_time(volatile hrt_abstime *now)
return ts;
}
/*
/**
* Initalise the high-resolution timing module.
*/
void
@@ -740,7 +742,7 @@ hrt_init(void)
#endif
}
/*
/**
* Call callout(arg) after interval has elapsed.
*/
void
@@ -753,7 +755,7 @@ hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, v
arg);
}
/*
/**
* Call callout(arg) at calltime.
*/
void
@@ -762,7 +764,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v
hrt_call_internal(entry, calltime, 0, callout, arg);
}
/*
/**
* Call callout(arg) every period.
*/
void
@@ -781,13 +783,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
irqstate_t flags = irqsave();
/* if the entry is currently queued, remove it */
/* note that we are using a potentially uninitialised
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
/* note that we are using a potentially uninitialised
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
if (entry->deadline != 0)
sq_rem(&entry->link, &callout_queue);
@@ -801,7 +803,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
irqrestore(flags);
}
/*
/**
* If this returns true, the call has been invoked and removed from the callout list.
*
* Always returns false for repeating callouts.
@@ -812,7 +814,7 @@ hrt_called(struct hrt_call *entry)
return (entry->deadline == 0);
}
/*
/**
* Remove the entry from the callout list.
*/
void
@@ -895,17 +897,18 @@ hrt_call_invoke(void)
/* if the callout has a non-zero period, it has to be re-entered */
if (call->period != 0) {
// re-check call->deadline to allow for
// callouts to re-schedule themselves
// callouts to re-schedule themselves
// using hrt_call_delay()
if (call->deadline <= now) {
call->deadline = deadline + call->period;
}
hrt_call_enter(call);
}
}
}
/*
/**
* Reschedule the next timer interrupt.
*
* This routine must be called with interrupts disabled.
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+1 -1
View File
@@ -52,7 +52,6 @@ ECL_PitchController::ECL_PitchController() :
_tc(0.1f),
_k_p(0.0f),
_k_i(0.0f),
_k_d(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate_pos(0.0f),
@@ -91,6 +90,7 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl
}
/* calculate the offset in the rate resulting from rolling */
//xxx needs explanation and conversion to body angular rates or should be removed
float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
tanf(roll) * sinf(roll)) * _roll_ff;
if (inverted)
src/lib/ecl/attitude_fw/ecl_pitch_controller.h
-4
View File
@@ -77,9 +77,6 @@ public:
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_k_ff(float k_ff) {
_k_ff = k_ff;
@@ -119,7 +116,6 @@ private:
float _tc;
float _k_p;
float _k_i;
float _k_d;
float _k_ff;
float _integrator_max;
float _max_rate_pos;
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
-1
View File
@@ -52,7 +52,6 @@ ECL_RollController::ECL_RollController() :
_tc(0.1f),
_k_p(0.0f),
_k_i(0.0f),
_k_d(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate(0.0f),
src/lib/ecl/attitude_fw/ecl_roll_controller.h
-5
View File
@@ -80,10 +80,6 @@ public:
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
@@ -113,7 +109,6 @@ private:
float _tc;
float _k_p;
float _k_i;
float _k_d;
float _k_ff;
float _integrator_max;
float _max_rate;
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
-2
View File
@@ -50,11 +50,9 @@ ECL_YawController::ECL_YawController() :
_last_run(0),
_k_p(0.0f),
_k_i(0.0f),
_k_d(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate(0.0f),
_roll_ff(0.0f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
-9
View File
@@ -75,10 +75,6 @@ public:
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
@@ -91,10 +87,6 @@ public:
_max_rate = max_rate;
}
void set_k_roll_ff(float roll_ff) {
_roll_ff = roll_ff;
}
void set_coordinated_min_speed(float coordinated_min_speed) {
_coordinated_min_speed = coordinated_min_speed;
}
@@ -116,7 +108,6 @@ private:
uint64_t _last_run;
float _k_p;
float _k_i;
float _k_d;
float _k_ff;
float _integrator_max;
float _max_rate;
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+11 -9
View File
@@ -52,11 +52,13 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
/* gyro measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f);
/* accelerometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f);
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
/* accel measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
/* mag measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
/* offset estimation - UNUSED */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
@@ -75,10 +77,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->q3 = param_find("EKF_ATT_V3_Q3");
h->q4 = param_find("EKF_ATT_V3_Q4");
h->r0 = param_find("EKF_ATT_V3_R0");
h->r1 = param_find("EKF_ATT_V3_R1");
h->r2 = param_find("EKF_ATT_V3_R2");
h->r3 = param_find("EKF_ATT_V3_R3");
h->r0 = param_find("EKF_ATT_V4_R0");
h->r1 = param_find("EKF_ATT_V4_R1");
h->r2 = param_find("EKF_ATT_V4_R2");
h->r3 = param_find("EKF_ATT_V4_R3");
h->roll_off = param_find("ATT_ROLL_OFF3");
h->pitch_off = param_find("ATT_PITCH_OFF3");
src/modules/commander/commander.cpp
+42 -236
View File
@@ -69,7 +69,6 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
@@ -194,7 +193,7 @@ void usage(const char *reason);
/**
* React to commands that are sent e.g. from the mavlink module.
*/
void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
/**
* Mainloop of commander.
@@ -215,8 +214,6 @@ void print_reject_arm(const char *msg);
void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
*/
@@ -346,13 +343,12 @@ void print_status()
warnx("arming: %s", armed_str);
}
static orb_advert_t control_mode_pub;
static orb_advert_t status_pub;
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{
/* result of the command */
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
/* only handle high-priority commands here */
@@ -365,12 +361,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
/* if HIL got enabled, reset battery status state */
if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
/* reset the arming mode to disarmed */
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
if (arming_res != TRANSITION_DENIED) {
mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
@@ -386,12 +382,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_NOT_CHANGED;
if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) {
if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
} else {
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
}
if (arming_res == TRANSITION_CHANGED) {
@@ -401,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} else {
if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed);
arming_res = arming_state_transition(status, safety, new_arming_state, armed);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
@@ -462,29 +458,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break;
}
case VEHICLE_CMD_NAV_TAKEOFF: {
if (armed->armed) {
transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
if (nav_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
}
if (nav_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else {
/* reject TAKEOFF not armed */
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
break;
}
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
@@ -494,7 +467,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_DENIED;
} else {
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
}
if (arming_res == TRANSITION_CHANGED) {
@@ -513,7 +486,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode);
transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
@@ -524,29 +497,22 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
case VEHICLE_CMD_PREFLIGHT_STORAGE:
/* ignore commands that handled in low prio loop */
break;
default:
/* warn about unsupported commands */
answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
}
/* supported command handling stop */
if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
tune_positive();
} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* we do not care in the high prio loop about commands we don't know */
} else {
tune_negative();
if (result == VEHICLE_CMD_RESULT_DENIED) {
mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command);
} else if (result == VEHICLE_CMD_RESULT_FAILED) {
mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command);
} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
}
if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(*cmd, result);
}
/* send any requested ACKs */
@@ -564,9 +530,6 @@ static struct actuator_armed_s armed;
static struct safety_s safety;
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */
@@ -609,11 +572,9 @@ int commander_thread_main(int argc, char *argv[])
/* Initialize armed with all false */
memset(&armed, 0, sizeof(armed));
/* Initialize all flags to false */
memset(&control_mode, 0, sizeof(control_mode));
status.main_state = MAIN_STATE_MANUAL;
status.navigation_state = NAVIGATION_STATE_DIRECT;
status.set_nav_state = NAV_STATE_INIT;
status.set_nav_state_timestamp = 0;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
@@ -625,9 +586,6 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = true;
status.offboard_control_signal_lost = true;
/* allow manual override initially */
control_mode.flag_external_manual_override_ok = true;
/* set battery warning flag */
status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
status.condition_battery_voltage_valid = false;
@@ -635,9 +593,6 @@ int commander_thread_main(int argc, char *argv[])
// XXX for now just set sensors as initialized
status.condition_system_sensors_initialized = true;
// XXX just disable offboard control for now
control_mode.flag_control_offboard_enabled = false;
/* advertise to ORB */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
/* publish current state machine */
@@ -649,8 +604,6 @@ int commander_thread_main(int argc, char *argv[])
armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
/* home position */
orb_advert_t home_pub = -1;
struct home_position_s home;
@@ -803,11 +756,9 @@ int commander_thread_main(int argc, char *argv[])
status.system_type == VEHICLE_TYPE_QUADROTOR ||
status.system_type == VEHICLE_TYPE_HEXAROTOR ||
status.system_type == VEHICLE_TYPE_OCTOROTOR) {
control_mode.flag_external_manual_override_ok = false;
status.is_rotary_wing = true;
} else {
control_mode.flag_external_manual_override_ok = true;
status.is_rotary_wing = false;
}
@@ -859,7 +810,7 @@ int commander_thread_main(int argc, char *argv[])
// 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, &control_mode, ARMING_STATE_STANDBY, &armed);
// (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
// }
}
@@ -990,10 +941,10 @@ int commander_thread_main(int argc, char *argv[])
battery_tune_played = false;
if (armed.armed) {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
} else {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
}
status_changed = true;
@@ -1012,7 +963,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
// XXX check for sensors
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
} else {
// XXX: Add emergency stuff if sensors are lost
@@ -1102,15 +1053,13 @@ int commander_thread_main(int argc, char *argv[])
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
(status.condition_landed && (
status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
status.navigation_state == NAVIGATION_STATE_VECTOR
))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed);
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0;
} else {
@@ -1132,7 +1081,7 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
}
stick_on_counter = 0;
@@ -1185,12 +1134,12 @@ int commander_thread_main(int argc, char *argv[])
/* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON, &control_mode);
transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
if (flighttermination_res == TRANSITION_CHANGED) {
tune_positive();
}
} else {
flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode);
flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
}
@@ -1202,32 +1151,18 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
handle_command(&status, &safety, &control_mode, &cmd, &armed);
}
/* evaluate the navigation state machine */
transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
handle_command(&status, &safety, &cmd, &armed);
}
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = check_arming_state_changed();
bool main_state_changed = check_main_state_changed();
bool navigation_state_changed = check_navigation_state_changed();
bool flighttermination_state_changed = check_flighttermination_state_changed();
hrt_abstime t1 = hrt_absolute_time();
if (navigation_state_changed || arming_state_changed) {
control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic
}
if (arming_state_changed || main_state_changed || navigation_state_changed) {
mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
if (arming_state_changed || main_state_changed) {
mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state);
status_changed = true;
}
@@ -1235,8 +1170,6 @@ int commander_thread_main(int argc, char *argv[])
if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
status.timestamp = t1;
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
control_mode.timestamp = t1;
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
armed.timestamp = t1;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
@@ -1549,133 +1482,6 @@ print_reject_arm(const char *msg)
}
}
transition_result_t
check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
{
transition_result_t res = TRANSITION_DENIED;
if (status->main_state == MAIN_STATE_AUTO) {
if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
// TODO AUTO_LAND handling
if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't switch to other states until takeoff not completed */
// XXX: only respect the condition_landed when the local position is actually valid
if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
return TRANSITION_NOT_CHANGED;
}
}
if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
/* possibly on ground, switch to TAKEOFF if needed */
if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
return res;
}
}
/* switch to AUTO mode */
if (status->rc_signal_found_once && !status->rc_signal_lost) {
/* act depending on switches when manual control enabled */
if (status->return_switch == RETURN_SWITCH_RETURN) {
/* RTL */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
} else {
if (status->mission_switch == MISSION_SWITCH_MISSION) {
/* MISSION */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} else {
/* LOITER */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
}
} else {
/* switch to MISSION when no RC control and first time in some AUTO mode */
if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
res = TRANSITION_NOT_CHANGED;
} else {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
}
}
} else {
/* disarmed, always switch to AUTO_READY */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
}
} else {
/* manual control modes */
if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) {
/* switch to failsafe mode */
bool manual_control_old = control_mode->flag_control_manual_enabled;
if (!status->condition_landed && status->condition_local_position_valid) {
/* in air: try to hold position if possible */
res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
} else {
/* landed: don't try to hold position but land (if taking off) */
res = TRANSITION_DENIED;
}
if (res == TRANSITION_DENIED) {
res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
}
control_mode->flag_control_manual_enabled = false;
if (res == TRANSITION_NOT_CHANGED && manual_control_old) {
/* mark navigation state as changed to force immediate flag publishing */
set_navigation_state_changed();
res = TRANSITION_CHANGED;
}
if (res == TRANSITION_CHANGED) {
if (control_mode->flag_control_position_enabled) {
mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD");
} else {
if (status->condition_landed) {
mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)");
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD");
}
}
}
} else {
switch (status->main_state) {
case MAIN_STATE_MANUAL:
res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
break;
case MAIN_STATE_SEATBELT:
res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
break;
case MAIN_STATE_EASY:
res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
break;
default:
break;
}
}
}
return res;
}
void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
{
switch (result) {
@@ -1784,7 +1590,7 @@ void *commander_low_prio_loop(void *arg)
/* try to go to INIT/PREFLIGHT arming state */
// XXX disable interrupts in arming_state_transition
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) {
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@@ -1824,7 +1630,7 @@ void *commander_low_prio_loop(void *arg)
else
tune_negative();
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
break;
}
@@ -1876,7 +1682,7 @@ void *commander_low_prio_loop(void *arg)
}
default:
answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
/* don't answer on unsupported commands, it will be done in main loop */
break;
}
src/modules/commander/state_machine_helper.cpp
+7 -182
View File
@@ -46,7 +46,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@@ -64,12 +63,10 @@ static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool navigation_state_changed = true;
static bool flighttermination_state_changed = true;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
const struct vehicle_control_mode_s *control_mode,
arming_state_t new_arming_state, struct actuator_armed_s *armed)
{
/*
@@ -86,7 +83,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
} else {
/* enforce lockdown in HIL */
if (control_mode->flag_system_hil_enabled) {
if (status->hil_state == HIL_STATE_ON) {
armed->lockdown = true;
} else {
armed->lockdown = false;
@@ -109,7 +106,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* allow coming from INIT and disarming from ARMED */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED
|| control_mode->flag_system_hil_enabled) {
|| status->hil_state == HIL_STATE_ON) {
/* sensors need to be initialized for STANDBY state */
if (status->condition_system_sensors_initialized) {
@@ -126,7 +123,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* allow arming from STANDBY and IN-AIR-RESTORE */
if ((status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
&& (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */
&& (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = true;
@@ -289,169 +286,6 @@ check_main_state_changed()
}
}
transition_result_t
navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_navigation_state == status->navigation_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_navigation_state) {
case NAVIGATION_STATE_DIRECT:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_climb_rate_enabled = false;
control_mode->flag_control_manual_enabled = true;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_STABILIZE:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_climb_rate_enabled = false;
control_mode->flag_control_manual_enabled = true;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_ALTHOLD:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = true;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_VECTOR:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = true;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_AUTO_READY:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = false;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_climb_rate_enabled = false;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
break;
case NAVIGATION_STATE_AUTO_TAKEOFF:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
break;
case NAVIGATION_STATE_AUTO_LOITER:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
break;
case NAVIGATION_STATE_AUTO_RTL:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
break;
case NAVIGATION_STATE_AUTO_LAND:
/* deny transitions from landed state */
if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
}
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
status->navigation_state = new_navigation_state;
control_mode->auto_state = status->navigation_state;
navigation_state_changed = true;
}
}
return ret;
}
bool
check_navigation_state_changed()
{
if (navigation_state_changed) {
navigation_state_changed = false;
return true;
} else {
return false;
}
}
bool
check_flighttermination_state_changed()
{
@@ -464,16 +298,10 @@ check_flighttermination_state_changed()
}
}
void
set_navigation_state_changed()
{
navigation_state_changed = true;
}
/**
* Transition from one hil state to another
*/
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd)
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
bool valid_transition = false;
int ret = ERROR;
@@ -502,7 +330,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
|| current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
current_control_mode->flag_system_hil_enabled = true;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
}
@@ -521,9 +348,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
current_status->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
current_control_mode->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
// XXX also set lockdown here
ret = OK;
@@ -539,7 +363,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/**
* Transition from one flightermination state to another
*/
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode)
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state)
{
transition_result_t ret = TRANSITION_DENIED;
@@ -566,7 +390,8 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s *
if (ret == TRANSITION_CHANGED) {
flighttermination_state_changed = true;
control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
// TODO
//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
}
}
src/modules/commander/state_machine_helper.h
+3 -6
View File
@@ -48,7 +48,6 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_control_mode.h>
typedef enum {
TRANSITION_DENIED = -1,
@@ -58,7 +57,7 @@ typedef enum {
} transition_result_t;
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed);
arming_state_t new_arming_state, struct actuator_armed_s *armed);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
@@ -68,9 +67,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
bool check_main_state_changed();
transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode);
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state);
bool check_navigation_state_changed();
@@ -78,6 +75,6 @@ bool check_flighttermination_state_changed();
void set_navigation_state_changed();
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
#endif /* STATE_MACHINE_HELPER_H_ */
src/modules/fw_att_control/fw_att_control_main.cpp
-12
View File
@@ -310,7 +310,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.tconst = param_find("FW_ATT_TC");
_parameter_handles.p_p = param_find("FW_PR_P");
_parameter_handles.p_d = param_find("FW_PR_D");
_parameter_handles.p_i = param_find("FW_PR_I");
_parameter_handles.p_ff = param_find("FW_PR_FF");
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
@@ -319,7 +318,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
_parameter_handles.r_p = param_find("FW_RR_P");
_parameter_handles.r_d = param_find("FW_RR_D");
_parameter_handles.r_i = param_find("FW_RR_I");
_parameter_handles.r_ff = param_find("FW_RR_FF");
_parameter_handles.r_integrator_max = param_find("FW_RR_IMAX");
@@ -327,9 +325,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.y_p = param_find("FW_YR_P");
_parameter_handles.y_i = param_find("FW_YR_I");
_parameter_handles.y_d = param_find("FW_YR_D");
_parameter_handles.y_ff = param_find("FW_YR_FF");
_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
@@ -374,7 +370,6 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.tconst, &(_parameters.tconst));
param_get(_parameter_handles.p_p, &(_parameters.p_p));
param_get(_parameter_handles.p_d, &(_parameters.p_d));
param_get(_parameter_handles.p_i, &(_parameters.p_i));
param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
@@ -383,7 +378,6 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
param_get(_parameter_handles.r_p, &(_parameters.r_p));
param_get(_parameter_handles.r_d, &(_parameters.r_d));
param_get(_parameter_handles.r_i, &(_parameters.r_i));
param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
@@ -392,9 +386,7 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.y_p, &(_parameters.y_p));
param_get(_parameter_handles.y_i, &(_parameters.y_i));
param_get(_parameter_handles.y_d, &(_parameters.y_d));
param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward));
param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
@@ -407,7 +399,6 @@ FixedwingAttitudeControl::parameters_update()
_pitch_ctrl.set_time_constant(_parameters.tconst);
_pitch_ctrl.set_k_p(_parameters.p_p);
_pitch_ctrl.set_k_i(_parameters.p_i);
_pitch_ctrl.set_k_d(_parameters.p_d);
_pitch_ctrl.set_k_ff(_parameters.p_ff);
_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
@@ -418,7 +409,6 @@ FixedwingAttitudeControl::parameters_update()
_roll_ctrl.set_time_constant(_parameters.tconst);
_roll_ctrl.set_k_p(_parameters.r_p);
_roll_ctrl.set_k_i(_parameters.r_i);
_roll_ctrl.set_k_d(_parameters.r_d);
_roll_ctrl.set_k_ff(_parameters.r_ff);
_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
@@ -426,9 +416,7 @@ FixedwingAttitudeControl::parameters_update()
/* yaw control parameters */
_yaw_ctrl.set_k_p(_parameters.y_p);
_yaw_ctrl.set_k_i(_parameters.y_i);
_yaw_ctrl.set_k_d(_parameters.y_d);
_yaw_ctrl.set_k_ff(_parameters.y_ff);
_yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward);
_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
_yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
src/modules/fw_att_control/fw_att_control_params.c
+84 -50
View File
@@ -38,107 +38,141 @@ f * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Parameters defined by the fixed-wing attitude control task
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
//XXX resolve unclear naming of paramters: FW_P_P --> FW_PR_P
/*
* Controller parameters, accessible via MAVLink
*
*/
//xxx: update descriptions
// @DisplayName Attitude Time Constant
// @Description This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
// @Description This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
// @Range 0.4 to 1.0 seconds, in tens of seconds
PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
// @DisplayName Proportional gain.
// @Description This defines how much the elevator input will be commanded dependend on the current pitch error.
// @DisplayName Pitch rate proportional gain.
// @Description This defines how much the elevator input will be commanded depending on the current body angular rate error.
// @Range 10 to 200, 1 increments
PARAM_DEFINE_FLOAT(FW_PR_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
// @DisplayName Damping gain.
// @Description This gain damps the airframe pitch rate. In particular relevant for flying wings.
// @Range 0.0 to 10.0, 0.1 increments
PARAM_DEFINE_FLOAT(FW_PR_D, 0.0f); //xxx: remove
// @DisplayName Integrator gain.
// @DisplayName Pitch rate integrator gain.
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error.
// @Range 0 to 50.0
PARAM_DEFINE_FLOAT(FW_PR_I, 0.05f);
PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f);
// @DisplayName Maximum positive / up pitch rate.
// @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds, in 1 increments
PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
// @DisplayName Maximum negative / down pitch rate.
// @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds, in 1 increments
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
// @DisplayName Pitch Integrator Anti-Windup
// @Description This limits the range in degrees the integrator can wind up to.
// @Range 0.0 to 45.0
// @Increment 1.0
// @DisplayName Pitch rate integrator limit
// @Description The portion of the integrator part in the control surface deflection is limited to this value
// @Range 0.0 to 1
// @Increment 0.1
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
// @DisplayName Roll feedforward gain.
// @DisplayName Roll to Pitch feedforward gain.
// @Description This compensates during turns and ensures the nose stays level.
// @Range 0.5 2.0
// @Increment 0.05
// @User User
PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f);
PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...)
// @DisplayName Proportional Gain.
// @Description This gain controls the roll angle to roll actuator output.
// @DisplayName Roll rate proportional Gain.
// @Description This defines how much the aileron input will be commanded depending on the current body angular rate error.
// @Range 10.0 200.0
// @Increment 10.0
// @User User
PARAM_DEFINE_FLOAT(FW_RR_P, 0.5f);
PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
// @DisplayName Damping Gain
// @Description Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence.
// @Range 0.0 10.0
// @Increment 1.0
// @User User
PARAM_DEFINE_FLOAT(FW_RR_D, 0.0f); //xxx: remove
// @DisplayName Integrator Gain
// @Description This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors.
// @DisplayName Roll rate integrator Gain
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error.
// @Range 0.0 100.0
// @Increment 5.0
// @User User
PARAM_DEFINE_FLOAT(FW_RR_I, 0.05f);
PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f);
// @DisplayName Roll Integrator Anti-Windup
// @Description This limits the range in degrees the integrator can wind up to.
// @Range 0.0 to 45.0
// @Increment 1.0
// @Description The portion of the integrator part in the control surface deflection is limited to this value.
// @Range 0.0 to 1.0
// @Increment 0.1
PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
// @DisplayName Maximum Roll Rate
// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds
// @Increment 1.0
PARAM_DEFINE_FLOAT(FW_R_RMAX, 60);
PARAM_DEFINE_FLOAT(FW_R_RMAX, 0);
// @DisplayName Yaw rate proportional gain.
// @Description This defines how much the rudder input will be commanded depending on the current body angular rate error.
// @Range 10 to 200, 1 increments
PARAM_DEFINE_FLOAT(FW_YR_P, 0.05);
PARAM_DEFINE_FLOAT(FW_YR_P, 0.5);
PARAM_DEFINE_FLOAT(FW_YR_I, 0.05);
// @DisplayName Yaw rate integrator gain.
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error.
// @Range 0 to 50.0
PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f);
// @DisplayName Yaw rate integrator limit
// @Description The portion of the integrator part in the control surface deflection is limited to this value
// @Range 0.0 to 1
// @Increment 0.1
PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
PARAM_DEFINE_FLOAT(FW_YR_D, 0); //xxx: remove
PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0);
PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);
PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f);
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f);
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60);
PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1.0f);
PARAM_DEFINE_FLOAT(FW_RR_FF, 0.0f);
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.0f);
PARAM_DEFINE_FLOAT(FW_YR_FF, 0.0f);
// @DisplayName Maximum Yaw Rate
// @Description This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds
// @Increment 1.0
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0);
// @DisplayName Roll rate feed forward
// @Description Direct feed forward from rate setpoint to control surface output
// @Range 0 to 10
// @Increment 0.1
PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f);
// @DisplayName Pitch rate feed forward
// @Description Direct feed forward from rate setpoint to control surface output
// @Range 0 to 10
// @Increment 0.1
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
// @DisplayName Yaw rate feed forward
// @Description Direct feed forward from rate setpoint to control surface output
// @Range 0 to 10
// @Increment 0.1
PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
// @DisplayName Minimal speed for yaw coordination
// @Description For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable.
// @Range 0 to 90.0 degrees per seconds
// @Increment 1.0
PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */
// @DisplayName Minimum Airspeed
// @Description If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively
// @Range 0.0 to 30
PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
// @DisplayName Trim Airspeed
// @Description The TECS controller tries to fly at this airspeed
// @Range 0.0 to 30
PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
// @DisplayName Maximum Airspeed
// @Description If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
// @Range 0.0 to 30
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+14 -10
View File
@@ -49,6 +49,7 @@
* More details and acknowledgements in the referenced library headers.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@@ -227,6 +228,7 @@ private:
float land_H1_virt;
float land_flare_alt_relative;
float land_thrust_lim_horizontal_distance;
float land_heading_hold_horizontal_distance;
} _parameters; /**< local copies of interesting parameters */
@@ -271,6 +273,7 @@ private:
param_t land_H1_virt;
param_t land_flare_alt_relative;
param_t land_thrust_lim_horizontal_distance;
param_t land_heading_hold_horizontal_distance;
} _parameter_handles; /**< handles for interesting parameters */
@@ -420,6 +423,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_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_horizontal_distance = param_find("FW_LND_TLDIST");
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
@@ -508,6 +512,7 @@ FixedwingPositionControl::parameters_update()
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_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance));
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
_l1_control.set_l1_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period);
@@ -822,19 +827,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
const float heading_hold_distance = 15.0f;
if (wp_distance < heading_hold_distance || land_noreturn_horizontal) {
if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
/* heading hold, along the line connecting this and the last waypoint */
// if (mission_item_triplet.previous_valid) {
// target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), next_wp(0), next_wp(1));
// } else {
if (!land_noreturn_horizontal) //set target_bearing in first occurrence
target_bearing = _att.yaw;
//}
if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
if (mission_item_triplet.previous_valid) {
target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY());
} else {
target_bearing = _att.yaw;
}
mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
}
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+1
View File
@@ -120,3 +120,4 @@ PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
src/modules/mavlink/mavlink.c
+7 -5
View File
@@ -68,7 +68,6 @@
#include "waypoints.h"
#include "orb_topics.h"
#include "missionlib.h"
#include "mavlink_hil.h"
#include "util.h"
#include "waypoints.h"
@@ -221,6 +220,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
} else if (v_status.main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
// TODO use control_mode topic
/*
if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
@@ -234,6 +235,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
}
*/
}
*mavlink_custom_mode = custom_mode.data;
@@ -707,25 +709,25 @@ int mavlink_thread_main(int argc, char *argv[])
}
}
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
mavlink_waypoint_eventloop(hrt_absolute_time());
/* sleep quarter the time */
usleep(25000);
/* check if waypoint has been reached against the last positions */
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
mavlink_waypoint_eventloop(hrt_absolute_time());
/* sleep quarter the time */
usleep(25000);
/* send parameters at 20 Hz (if queued for sending) */
mavlink_pm_queued_send();
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
mavlink_waypoint_eventloop(hrt_absolute_time());
/* sleep quarter the time */
usleep(25000);
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
mavlink_waypoint_eventloop(hrt_absolute_time());
if (baudrate > 57600) {
mavlink_pm_queued_send();
src/modules/mavlink/mavlink_receiver.cpp
+1 -2
View File
@@ -79,7 +79,6 @@ __BEGIN_DECLS
#include "mavlink_bridge_header.h"
#include "waypoints.h"
#include "orb_topics.h"
#include "missionlib.h"
#include "mavlink_hil.h"
#include "mavlink_parameters.h"
#include "util.h"
@@ -846,7 +845,7 @@ receive_thread(void *arg)
handle_message(&msg);
/* handle packet with waypoint component */
mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
mavlink_wpm_message_handler(&msg);
/* handle packet with parameter component */
mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
src/modules/mavlink/missionlib.c
-399
View File
@@ -1,399 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* 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 missionlib.h
* MAVLink missionlib components
*/
// XXX trim includes
#include <nuttx/config.h>
#include <unistd.h>
#include <pthread.h>
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <fcntl.h>
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
#include <unistd.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <stdlib.h>
#include <poll.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <mavlink/mavlink_log.h>
#include "geo/geo.h"
#include "waypoints.h"
#include "orb_topics.h"
#include "missionlib.h"
#include "mavlink_hil.h"
#include "util.h"
#include "waypoints.h"
#include "mavlink_parameters.h"
static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
static uint64_t loiter_start_time;
#if 0
static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
struct vehicle_global_position_setpoint_s *sp);
#endif
int
mavlink_missionlib_send_message(mavlink_message_t *msg)
{
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
mavlink_send_uart_bytes(chan, missionlib_msg_buf, len);
return 0;
}
int
mavlink_missionlib_send_gcs_string(const char *string)
{
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
mavlink_statustext_t statustext;
int i = 0;
while (i < len - 1) {
statustext.text[i] = string[i];
if (string[i] == '\0')
break;
i++;
}
if (i > 1) {
/* Enforce null termination */
statustext.text[i] = '\0';
mavlink_message_t msg;
mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
return mavlink_missionlib_send_message(&msg);
} else {
return 1;
}
}
/**
* Get system time since boot in microseconds
*
* @return the system time since boot in microseconds
*/
uint64_t mavlink_missionlib_get_system_timestamp()
{
return hrt_absolute_time();
}
#if 0
/**
* Set special vehicle setpoint fields based on current mission item.
*
* @return true if the mission item could be interpreted
* successfully, it return false on failure.
*/
bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
struct vehicle_global_position_setpoint_s *sp)
{
switch (command) {
case MAV_CMD_NAV_LOITER_UNLIM:
sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
break;
case MAV_CMD_NAV_LOITER_TIME:
sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
loiter_start_time = hrt_absolute_time();
break;
// case MAV_CMD_NAV_LOITER_TURNS:
// sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT;
// break;
case MAV_CMD_NAV_WAYPOINT:
sp->nav_cmd = NAV_CMD_WAYPOINT;
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
break;
case MAV_CMD_NAV_LAND:
sp->nav_cmd = NAV_CMD_LAND;
break;
case MAV_CMD_NAV_TAKEOFF:
sp->nav_cmd = NAV_CMD_TAKEOFF;
break;
default:
/* abort */
return false;
}
sp->loiter_radius = param3;
sp->loiter_direction = (param3 >= 0) ? 1 : -1;
sp->param1 = param1;
sp->param2 = param2;
sp->param3 = param3;
sp->param4 = param4;
/* define the turn distance */
float orbit = 15.0f;
if (command == (int)MAV_CMD_NAV_WAYPOINT) {
orbit = param2;
} else if (command == (int)MAV_CMD_NAV_LOITER_TURNS ||
command == (int)MAV_CMD_NAV_LOITER_TIME ||
command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
orbit = param3;
} else {
// XXX set default orbit via param
// 15 initialized above
}
sp->turn_distance_xy = orbit;
sp->turn_distance_z = orbit;
}
/**
* This callback is executed each time a waypoint changes.
*
* It publishes the vehicle_global_position_setpoint_s or the
* vehicle_local_position_setpoint_s topic, depending on the type of waypoint
*/
void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
float param2, float param3, float param4, float param5_lat_x,
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
{
static orb_advert_t global_position_setpoint_pub = -1;
// static orb_advert_t global_position_set_triplet_pub = -1;
static orb_advert_t local_position_setpoint_pub = -1;
static unsigned last_waypoint_index = -1;
char buf[50] = {0};
// XXX include check if WP is supported, jump to next if not
/* Update controller setpoints */
if (frame == (int)MAV_FRAME_GLOBAL) {
/* global, absolute waypoint */
struct vehicle_global_position_setpoint_s sp;
sp.lat = param5_lat_x * 1e7f;
sp.lon = param6_lon_y * 1e7f;
sp.altitude = param7_alt_z;
sp.altitude_is_relative = false;
sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
set_special_fields(param1, param2, param3, param4, command, &sp);
/* Initialize setpoint publication if necessary */
if (global_position_setpoint_pub < 0) {
global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
} else {
orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
}
/* fill triplet: previous, current, next waypoint */
// struct vehicle_global_position_set_triplet_s triplet;
/* current waypoint is same as sp */
// memcpy(&(triplet.current), &sp, sizeof(sp));
/*
* Check if previous WP (in mission, not in execution order)
* is available and identify correct index
*/
int last_setpoint_index = -1;
bool last_setpoint_valid = false;
if (index > 0) {
last_setpoint_index = index - 1;
}
while (last_setpoint_index >= 0) {
if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL &&
(wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
last_setpoint_valid = true;
break;
}
last_setpoint_index--;
}
/*
* Check if next WP (in mission, not in execution order)
* is available and identify correct index
*/
int next_setpoint_index = -1;
bool next_setpoint_valid = false;
/* next waypoint */
if (wpm->size > 1) {
next_setpoint_index = index + 1;
}
while (next_setpoint_index < wpm->size) {
if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
next_setpoint_valid = true;
break;
}
next_setpoint_index++;
}
/* populate last and next */
// triplet.previous_valid = false;
// triplet.next_valid = false;
// if (last_setpoint_valid) {
// triplet.previous_valid = true;
// struct vehicle_global_position_setpoint_s sp;
// sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
// sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
// sp.altitude = wpm->waypoints[last_setpoint_index].z;
// sp.altitude_is_relative = false;
// sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
// set_special_fields(wpm->waypoints[last_setpoint_index].param1,
// wpm->waypoints[last_setpoint_index].param2,
// wpm->waypoints[last_setpoint_index].param3,
// wpm->waypoints[last_setpoint_index].param4,
// wpm->waypoints[last_setpoint_index].command, &sp);
// memcpy(&(triplet.previous), &sp, sizeof(sp));
// }
// if (next_setpoint_valid) {
// triplet.next_valid = true;
// struct vehicle_global_position_setpoint_s sp;
// sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
// sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
// sp.altitude = wpm->waypoints[next_setpoint_index].z;
// sp.altitude_is_relative = false;
// sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
// set_special_fields(wpm->waypoints[next_setpoint_index].param1,
// wpm->waypoints[next_setpoint_index].param2,
// wpm->waypoints[next_setpoint_index].param3,
// wpm->waypoints[next_setpoint_index].param4,
// wpm->waypoints[next_setpoint_index].command, &sp);
// memcpy(&(triplet.next), &sp, sizeof(sp));
// }
/* Initialize triplet publication if necessary */
// if (global_position_set_triplet_pub < 0) {
// global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
// } else {
// orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
// }
sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
} else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
/* global, relative alt (in relation to HOME) waypoint */
struct vehicle_global_position_setpoint_s sp;
sp.lat = param5_lat_x * 1e7f;
sp.lon = param6_lon_y * 1e7f;
sp.altitude = param7_alt_z;
sp.altitude_is_relative = true;
sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
set_special_fields(param1, param2, param3, param4, command, &sp);
/* Initialize publication if necessary */
if (global_position_setpoint_pub < 0) {
global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
} else {
orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
}
sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
} else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) {
/* local, absolute waypoint */
struct vehicle_local_position_setpoint_s sp;
sp.x = param5_lat_x;
sp.y = param6_lon_y;
sp.z = param7_alt_z;
sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
/* Initialize publication if necessary */
if (local_position_setpoint_pub < 0) {
local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp);
} else {
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp);
}
sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
} else {
warnx("non-navigation WP, ignoring");
mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring.");
return;
}
/* only set this for known waypoint types (non-navigation types would have returned earlier) */
last_waypoint_index = index;
mavlink_missionlib_send_gcs_string(buf);
printf("%s\n", buf);
//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
}
#endif
src/modules/mavlink/missionlib.h
-52
View File
@@ -1,52 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* 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 missionlib.h
* MAVLink mission helper library
*/
#pragma once
#include "mavlink_bridge_header.h"
//extern void mavlink_wpm_send_message(mavlink_message_t *msg);
//extern void mavlink_wpm_send_gcs_string(const char *string);
//extern uint64_t mavlink_wpm_get_system_timestamp(void);
extern int mavlink_missionlib_send_message(mavlink_message_t *msg);
extern int mavlink_missionlib_send_gcs_string(const char *string);
extern uint64_t mavlink_missionlib_get_system_timestamp(void);
extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
float param2, float param3, float param4, float param5_lat_x,
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
src/modules/mavlink/module.mk
-1
View File
@@ -37,7 +37,6 @@
MODULE_COMMAND = mavlink
SRCS += mavlink.c \
missionlib.c \
mavlink_parameters.c \
mavlink_receiver.cpp \
orb_listener.c \
src/modules/mavlink/orb_listener.c
-1
View File
@@ -60,7 +60,6 @@
#include "waypoints.h"
#include "orb_topics.h"
#include "missionlib.h"
#include "mavlink_hil.h"
#include "util.h"
src/modules/mavlink/orb_topics.h
+1
View File
@@ -45,6 +45,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/offboard_control_setpoint.h>
src/modules/mavlink/waypoints.c
+289 -740
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/waypoints.h
+9 -35
View File
@@ -46,19 +46,10 @@
or in the same folder as this source file */
#include <v1.0/mavlink_types.h>
// #ifndef MAVLINK_SEND_UART_BYTES
// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
// #endif
//extern mavlink_system_t mavlink_system;
#include "mavlink_bridge_header.h"
#include <stdbool.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/mission.h>
// FIXME XXX - TO BE MOVED TO XML
enum MAVLINK_WPM_STATES {
MAVLINK_WPM_STATE_IDLE = 0,
MAVLINK_WPM_STATE_SENDLIST,
@@ -79,44 +70,24 @@ enum MAVLINK_WPM_CODES {
};
/* WAYPOINT MANAGER - MISSION LIB */
#define MAVLINK_WPM_MAX_WP_COUNT 15
// #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates
#ifndef MAVLINK_WPM_TEXT_FEEDBACK
#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text
#endif
#define MAVLINK_WPM_MAX_WP_COUNT 255
#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds
#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint
#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
struct mavlink_wpm_storage {
mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints
// #ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
// mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints
// #endif
uint16_t size;
uint16_t max_size;
uint16_t rcv_size;
enum MAVLINK_WPM_STATES current_state;
int16_t current_wp_id; ///< Waypoint in current transmission
// int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards
uint16_t current_count;
uint8_t current_partner_sysid;
uint8_t current_partner_compid;
uint64_t timestamp_lastaction;
// uint64_t timestamp_last_send_setpoint;
// uint64_t timestamp_firstinside_orbit;
// uint64_t timestamp_lastoutside_orbit;
uint64_t timestamp_last_send_setpoint;
uint32_t timeout;
int current_dataman_id;
// uint32_t delay_setpoint;
// float accept_range_yaw;
// float accept_range_distance;
// bool yaw_reached;
// bool pos_reached;
// bool idle;
};
typedef struct mavlink_wpm_storage mavlink_wpm_storage;
@@ -126,13 +97,16 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio
void mavlink_wpm_init(mavlink_wpm_storage *state);
int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap);
void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos ,
struct vehicle_local_position_s *local_pos);
void mavlink_waypoint_eventloop(uint64_t now);
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
float param2, float param3, float param4, float param5_lat_x,
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
void mavlink_missionlib_send_message(mavlink_message_t *msg);
int mavlink_missionlib_send_gcs_string(const char *string);
#endif /* WAYPOINTS_H_ */
src/modules/mavlink_onboard/mavlink.c
+2 -1
View File
@@ -441,7 +441,8 @@ int mavlink_thread_main(int argc, char *argv[])
get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
// TODO fix navigation state, use control_mode topic
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, 0, mavlink_state);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
src/modules/multirotor_att_control/multirotor_att_control_main.c
+1 -1
View File
@@ -316,7 +316,7 @@ mc_thread_main(int argc, char *argv[])
}
} else {
if (!control_mode.flag_control_auto_enabled) {
if (!control_mode.flag_control_attitude_enabled) {
/* no control, try to stay on place */
if (!control_mode.flag_control_velocity_enabled) {
/* no velocity control, reset attitude setpoint */
src/modules/multirotor_pos_control/multirotor_pos_control.c
+553
View File
File diff suppressed because it is too large Load Diff
src/modules/navigator/navigator_main.cpp
+172 -118
View File
@@ -61,6 +61,7 @@
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/fence.h>
@@ -145,8 +146,10 @@ private:
orb_advert_t _triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _fence_pub; /**< publish fence topic */
orb_advert_t _mission_result_pub; /**< publish mission result topic */
orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */
struct vehicle_status_s _vstatus; /**< vehicle status */
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 mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
@@ -168,6 +171,8 @@ private:
MissionFeasibilityChecker missionFeasiblityChecker;
uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */
struct {
float min_altitude;
float loiter_radius;
@@ -192,21 +197,10 @@ private:
MAX_EVENT
};
enum State {
STATE_INIT,
STATE_NONE,
STATE_LOITER,
STATE_MISSION,
STATE_MISSION_LOITER,
STATE_RTL,
STATE_RTL_LOITER,
MAX_STATE
};
/**
* State machine transition table
*/
static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT];
static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT];
/**
* Update our local parameter cache.
@@ -300,6 +294,10 @@ private:
*/
void publish_mission_item_triplet();
/**
* Publish vehicle_control_mode topic for controllers
*/
void publish_control_mode();
/**
@@ -328,7 +326,7 @@ Navigator *g_navigator;
Navigator::Navigator() :
/* state machine transition table */
StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT),
StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
_task_should_exit(false),
_navigator_task(-1),
@@ -347,6 +345,7 @@ Navigator::Navigator() :
_triplet_pub(-1),
_fence_pub(-1),
_mission_result_pub(-1),
_control_mode_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -357,7 +356,8 @@ Navigator::Navigator() :
_mission(),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0)
_time_first_inside_orbit(0),
_set_nav_state_timestamp(0)
{
memset(&_fence, 0, sizeof(_fence));
@@ -375,7 +375,7 @@ Navigator::Navigator() :
memset(&_mission_result, 0, sizeof(struct mission_result_s));
/* Initialize state machine */
myState = STATE_INIT;
myState = NAV_STATE_INIT;
}
Navigator::~Navigator()
@@ -513,7 +513,6 @@ Navigator::task_main()
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
/* copy all topics first time */
vehicle_status_update();
@@ -524,9 +523,6 @@ Navigator::task_main()
offboard_mission_update(_vstatus.is_rotary_wing);
onboard_mission_update();
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
@@ -575,49 +571,41 @@ Navigator::task_main()
/* Evaluate state machine from commander and set the navigator mode accordingly */
if (_vstatus.main_state == MAIN_STATE_AUTO) {
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
/* commander requested new navigation mode, try to set it */
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
switch (_vstatus.navigation_state) {
case NAVIGATION_STATE_DIRECT:
case NAVIGATION_STATE_STABILIZE:
case NAVIGATION_STATE_ALTHOLD:
case NAVIGATION_STATE_VECTOR:
/* don't publish a mission item triplet */
dispatch(EVENT_NONE_REQUESTED);
switch (_vstatus.set_nav_state) {
case NAV_STATE_INIT:
case NAV_STATE_NONE:
/* nothing to do */
break;
case NAVIGATION_STATE_AUTO_READY:
case NAVIGATION_STATE_AUTO_TAKEOFF:
case NAVIGATION_STATE_AUTO_MISSION:
/* try mission if none is available, fallback to loiter instead */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
break;
case NAVIGATION_STATE_AUTO_LOITER:
case NAV_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED);
break;
case NAV_STATE_MISSION:
dispatch(EVENT_MISSION_REQUESTED);
break;
case NAVIGATION_STATE_AUTO_RTL:
case NAV_STATE_RTL:
dispatch(EVENT_RTL_REQUESTED);
break;
case NAVIGATION_STATE_AUTO_LAND:
/* TODO add this */
break;
default:
warnx("ERROR: Navigation state not supported");
warnx("ERROR: Requested navigation state not supported");
break;
}
} else {
/* try mission, if none is available fallback to loiter instead */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
break;
}
} else {
@@ -632,10 +620,8 @@ Navigator::task_main()
}
}
/* only update parameters if it changed */
if (fds[0].revents & POLLIN) {
parameters_update();
/* note that these new parameters won't be in effect until a mission triplet is published again */
}
@@ -670,9 +656,7 @@ Navigator::task_main()
if (mission_item_reached()) {
if (_vstatus.main_state == MAIN_STATE_AUTO &&
(_vstatus.navigation_state == NAVIGATION_STATE_AUTO_READY ||
_vstatus.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF ||
_vstatus.navigation_state == NAVIGATION_STATE_AUTO_MISSION)) {
(myState == NAV_STATE_MISSION)) {
/* advance by one mission item */
_mission.move_to_next();
@@ -688,6 +672,9 @@ Navigator::task_main()
}
}
}
publish_control_mode();
perf_end(_loop_perf);
}
@@ -740,25 +727,25 @@ Navigator::status()
}
switch (myState) {
case STATE_INIT:
case NAV_STATE_INIT:
warnx("State: Init");
break;
case STATE_NONE:
case NAV_STATE_NONE:
warnx("State: None");
break;
case STATE_LOITER:
case NAV_STATE_LOITER:
warnx("State: Loiter");
break;
case STATE_MISSION:
case NAV_STATE_MISSION:
warnx("State: Mission");
break;
case STATE_MISSION_LOITER:
case NAV_STATE_MISSION_LOITER:
warnx("State: Loiter after Mission");
break;
case STATE_RTL:
case NAV_STATE_RTL:
warnx("State: RTL");
break;
case STATE_RTL_LOITER:
case NAV_STATE_RTL_LOITER:
warnx("State: Loiter after RTL");
break;
default:
@@ -857,76 +844,76 @@ Navigator::fence_point(int argc, char *argv[])
StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = {
StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
{
/* STATE_INIT */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_INIT},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_INIT},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_INIT},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_INIT},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_INIT},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_INIT},
},
{
/* STATE_NONE */
/* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_NONE},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE},
/* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
},
{
/* STATE_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
},
{
/* STATE_MISSION */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), STATE_MISSION_LOITER},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), NAV_STATE_MISSION_LOITER},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION},
},
{
/* STATE_MISSION_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_MISSION_LOITER},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION_LOITER},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
},
{
/* STATE_RTL */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL},
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), STATE_RTL_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_RTL},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), NAV_STATE_RTL_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_RTL},
},
{
/* STATE_RTL_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_RTL_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
},
};
@@ -949,7 +936,7 @@ Navigator::start_loiter()
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_mission_item_triplet.current.yaw = 0.0f;
_mission_item_triplet.current.yaw = 0.0f; // TODO use current yaw sp here or set to undefined?
get_loiter_item(&_mission_item_triplet.current);
@@ -990,9 +977,8 @@ Navigator::start_mission()
mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
}
} else {
/* since a mission is not started without WPs available, this is not supposed to happen */
/* since a mission is started without knowledge if there are more mission items available, this can fail */
_mission_item_triplet.current_valid = false;
warnx("ERROR: current WP can't be set");
}
ret = _mission.get_next_mission_item(&_mission_item_triplet.next);
@@ -1239,6 +1225,76 @@ Navigator::publish_mission_item_triplet()
}
}
void
Navigator::publish_control_mode()
{
/* update vehicle_control_mode topic*/
_control_mode.main_state = _vstatus.main_state;
_control_mode.nav_state = static_cast<nav_state_t>(myState);
_control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR;
_control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing;
_control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON;
_control_mode.flag_control_offboard_enabled = false;
_control_mode.flag_control_flighttermination_enabled = false;
switch (_vstatus.main_state) {
case MAIN_STATE_MANUAL:
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing;
_control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing;
_control_mode.flag_control_altitude_enabled = false;
_control_mode.flag_control_climb_rate_enabled = false;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_SEATBELT:
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_EASY:
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
_control_mode.flag_control_position_enabled = true;
_control_mode.flag_control_velocity_enabled = true;
break;
case MAIN_STATE_AUTO:
_control_mode.flag_control_manual_enabled = false;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_position_enabled = true;
_control_mode.flag_control_velocity_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
break;
default:
break;
}
_control_mode.timestamp = hrt_absolute_time();
/* lazily publish the mission triplet only once available */
if (_control_mode_pub > 0) {
/* publish the mission triplet */
orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode);
} else {
/* advertise and publish */
_control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode);
}
}
bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) {
if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON &&
@@ -1251,11 +1307,9 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const
fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON &&
fabsf(a.radius - b.radius) < FLT_EPSILON &&
fabsf(a.time_inside - b.time_inside) < FLT_EPSILON &&
fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON &&
fabsf(a.index - b.index) < FLT_EPSILON) {
fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON) {
return true;
} else {
warnx("a.index %d, b.index %d", a.index, b.index);
return false;
}
}
src/modules/px4iofirmware/registers.c
+6 -2
View File
@@ -582,6 +582,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
/* this option is normally set last */
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint8_t count = 0;
bool disabled = false;
/* assert min..center..max ordering */
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
@@ -600,7 +601,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
} else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
count++;
}
@@ -608,7 +612,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (count) {
isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
} else {
} else if (!disabled) {
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
}
src/modules/sdlog2/sdlog2.c
+2 -1
View File
@@ -973,7 +973,8 @@ int sdlog2_thread_main(int argc, char *argv[])
// Don't orb_copy, it's already done few lines above
log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
// TODO use control_mode topic
//log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
log_msg.body.log_STAT.battery_current = buf_status.battery_current;
src/modules/sensors/sensor_params.c
+8 -8
View File
@@ -104,49 +104,49 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f);
PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
PARAM_DEFINE_FLOAT(RC5_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC9_MIN, 1000);
PARAM_DEFINE_FLOAT(RC9_TRIM, 1500);
src/modules/uORB/topics/mission.h
-1
View File
@@ -91,7 +91,6 @@ struct mission_item_s
float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
bool autocontinue; /**< true if next waypoint should follow after this one */
int index; /**< index matching the mavlink waypoint */
enum ORIGIN origin; /**< where the waypoint has been generated */
};
src/modules/uORB/topics/vehicle_control_mode.h
+16 -3
View File
@@ -48,6 +48,7 @@
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#include "vehicle_status.h"
/**
* @addtogroup topics @{
@@ -59,10 +60,25 @@
*
* Encodes the complete system state and is set by the commander app.
*/
typedef enum {
NAV_STATE_INIT = 0,
NAV_STATE_NONE,
NAV_STATE_LOITER,
NAV_STATE_MISSION,
NAV_STATE_MISSION_LOITER,
NAV_STATE_RTL,
NAV_STATE_RTL_LOITER,
NAV_STATE_MAX
} nav_state_t;
struct vehicle_control_mode_s
{
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
main_state_t main_state;
nav_state_t nav_state;
bool flag_armed;
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
@@ -79,9 +95,6 @@ struct vehicle_control_mode_s
bool flag_control_altitude_enabled; /**< true if altitude is controlled */
bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */
bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */
bool flag_control_auto_enabled; // TEMP
uint8_t auto_state; // TEMP navigation state for AUTO modes
};
/**
src/modules/uORB/topics/vehicle_status.h
+2 -16
View File
@@ -66,20 +66,6 @@ typedef enum {
MAIN_STATE_AUTO,
} main_state_t;
/* navigation state machine */
typedef enum {
NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization
NAVIGATION_STATE_STABILIZE, // attitude stabilization
NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization
NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization
NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff
NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode
NAVIGATION_STATE_AUTO_LOITER, // pause mission
NAVIGATION_STATE_AUTO_MISSION, // fly mission
NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND
NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector)
} navigation_state_t;
typedef enum {
ARMING_STATE_INIT = 0,
ARMING_STATE_STANDBY,
@@ -95,7 +81,6 @@ typedef enum {
HIL_STATE_ON
} hil_state_t;
typedef enum {
FLIGHTTERMINATION_STATE_OFF = 0,
FLIGHTTERMINATION_STATE_ON
@@ -182,7 +167,8 @@ struct vehicle_status_s
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
main_state_t main_state; /**< main state machine */
navigation_state_t navigation_state; /**< navigation state machine */
unsigned int set_nav_state; /**< set navigation state machine to specified value */
uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */
arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */