mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-23 06:56:36 +08:00
Merge branch 'release_v1.0.0' into beta
This commit is contained in:
@@ -9,16 +9,15 @@ sh /etc/init.d/rc.mc_defaults
|
||||
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
# TODO tune roll/pitch separately
|
||||
param set MC_ROLL_P 7.0
|
||||
param set MC_ROLLRATE_P 0.13
|
||||
param set MC_ROLLRATE_I 0.05
|
||||
param set MC_ROLLRATE_D 0.004
|
||||
param set MC_PITCH_P 7.0
|
||||
param set MC_PITCHRATE_P 0.13
|
||||
param set MC_PITCHRATE_P 0.19
|
||||
param set MC_PITCHRATE_I 0.05
|
||||
param set MC_PITCHRATE_D 0.004
|
||||
param set MC_YAW_P 2.8
|
||||
param set MC_YAW_P 4.0
|
||||
param set MC_YAWRATE_P 0.2
|
||||
param set MC_YAWRATE_I 0.1
|
||||
param set MC_YAWRATE_D 0.0
|
||||
|
||||
@@ -10,13 +10,13 @@ sh /etc/init.d/rc.mc_defaults
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
param set MC_ROLL_P 6.0
|
||||
param set MC_ROLLRATE_P 0.04
|
||||
param set MC_ROLLRATE_I 0.1
|
||||
param set MC_ROLLRATE_P 0.08
|
||||
param set MC_ROLLRATE_I 0.03
|
||||
param set MC_ROLLRATE_D 0.0015
|
||||
|
||||
param set MC_PITCH_P 6.0
|
||||
param set MC_PITCHRATE_P 0.08
|
||||
param set MC_PITCHRATE_I 0.2
|
||||
param set MC_PITCHRATE_P 0.1
|
||||
param set MC_PITCHRATE_I 0.03
|
||||
param set MC_PITCHRATE_D 0.0015
|
||||
|
||||
param set MC_YAW_P 2.8
|
||||
|
||||
+2
-1
@@ -1,4 +1,5 @@
|
||||
uint64 timestamp # microseconds since system boot, needed to integrate
|
||||
float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown
|
||||
float32 true_airspeed_m_s # true airspeed in meters per second, -1 if unknown
|
||||
float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown
|
||||
float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown
|
||||
float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown
|
||||
|
||||
@@ -0,0 +1,12 @@
|
||||
uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified
|
||||
uint32 seq_reached # Sequence of the mission item which has been reached
|
||||
uint32 seq_current # Sequence of the current mission item
|
||||
bool valid # true if mission is valid
|
||||
bool warning # true if mission is valid, but has potentially problematic items leading to safety warnings
|
||||
bool reached # true if mission has been reached
|
||||
bool finished # true if mission has been completed
|
||||
bool stay_in_failsafe # true if the commander should not switch out of the failsafe mode
|
||||
bool flight_termination # true if the navigator demands a flight termination from the commander app
|
||||
bool item_do_jump_changed # true if the number of do jumps remaining has changed
|
||||
uint32 item_changed_index # indicate which item has changed
|
||||
uint32 item_do_jump_remaining # set to the number of do jumps remaining for that item
|
||||
@@ -367,7 +367,8 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
|
||||
CONFIG_DRAM_START=0x20000000
|
||||
CONFIG_DRAM_SIZE=262144
|
||||
CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
|
||||
CONFIG_ARCH_INTERRUPTSTACK=1500
|
||||
# The actual usage is 420 bytes
|
||||
CONFIG_ARCH_INTERRUPTSTACK=750
|
||||
|
||||
#
|
||||
# Boot options
|
||||
@@ -548,8 +549,8 @@ CONFIG_UART7_SERIAL_CONSOLE=y
|
||||
#
|
||||
# USART1 Configuration
|
||||
#
|
||||
CONFIG_USART1_RXBUFSIZE=512
|
||||
CONFIG_USART1_TXBUFSIZE=512
|
||||
CONFIG_USART1_RXBUFSIZE=600
|
||||
CONFIG_USART1_TXBUFSIZE=600
|
||||
CONFIG_USART1_BAUD=115200
|
||||
CONFIG_USART1_BITS=8
|
||||
CONFIG_USART1_PARITY=0
|
||||
@@ -561,7 +562,7 @@ CONFIG_USART1_2STOP=0
|
||||
# USART2 Configuration
|
||||
#
|
||||
CONFIG_USART2_RXBUFSIZE=600
|
||||
CONFIG_USART2_TXBUFSIZE=2200
|
||||
CONFIG_USART2_TXBUFSIZE=1860
|
||||
CONFIG_USART2_BAUD=57600
|
||||
CONFIG_USART2_BITS=8
|
||||
CONFIG_USART2_PARITY=0
|
||||
@@ -572,8 +573,8 @@ CONFIG_USART2_OFLOWCONTROL=y
|
||||
#
|
||||
# USART3 Configuration
|
||||
#
|
||||
CONFIG_USART3_RXBUFSIZE=512
|
||||
CONFIG_USART3_TXBUFSIZE=512
|
||||
CONFIG_USART3_RXBUFSIZE=400
|
||||
CONFIG_USART3_TXBUFSIZE=400
|
||||
CONFIG_USART3_BAUD=57600
|
||||
CONFIG_USART3_BITS=8
|
||||
CONFIG_USART3_PARITY=0
|
||||
@@ -584,8 +585,8 @@ CONFIG_USART3_OFLOWCONTROL=y
|
||||
#
|
||||
# UART4 Configuration
|
||||
#
|
||||
CONFIG_UART4_RXBUFSIZE=512
|
||||
CONFIG_UART4_TXBUFSIZE=512
|
||||
CONFIG_UART4_RXBUFSIZE=400
|
||||
CONFIG_UART4_TXBUFSIZE=400
|
||||
CONFIG_UART4_BAUD=57600
|
||||
CONFIG_UART4_BITS=8
|
||||
CONFIG_UART4_PARITY=0
|
||||
@@ -596,8 +597,8 @@ CONFIG_UART4_2STOP=0
|
||||
#
|
||||
# USART6 Configuration
|
||||
#
|
||||
CONFIG_USART6_RXBUFSIZE=512
|
||||
CONFIG_USART6_TXBUFSIZE=512
|
||||
CONFIG_USART6_RXBUFSIZE=400
|
||||
CONFIG_USART6_TXBUFSIZE=400
|
||||
CONFIG_USART6_BAUD=57600
|
||||
CONFIG_USART6_BITS=8
|
||||
CONFIG_USART6_PARITY=0
|
||||
@@ -608,8 +609,8 @@ CONFIG_USART6_2STOP=0
|
||||
#
|
||||
# UART7 Configuration
|
||||
#
|
||||
CONFIG_UART7_RXBUFSIZE=512
|
||||
CONFIG_UART7_TXBUFSIZE=512
|
||||
CONFIG_UART7_RXBUFSIZE=400
|
||||
CONFIG_UART7_TXBUFSIZE=400
|
||||
CONFIG_UART7_BAUD=57600
|
||||
CONFIG_UART7_BITS=8
|
||||
CONFIG_UART7_PARITY=0
|
||||
@@ -620,8 +621,8 @@ CONFIG_UART7_2STOP=0
|
||||
#
|
||||
# UART8 Configuration
|
||||
#
|
||||
CONFIG_UART8_RXBUFSIZE=512
|
||||
CONFIG_UART8_TXBUFSIZE=512
|
||||
CONFIG_UART8_RXBUFSIZE=400
|
||||
CONFIG_UART8_TXBUFSIZE=400
|
||||
CONFIG_UART8_BAUD=57600
|
||||
CONFIG_UART8_BITS=8
|
||||
CONFIG_UART8_PARITY=0
|
||||
@@ -663,8 +664,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512
|
||||
CONFIG_CDCACM_NWRREQS=4
|
||||
CONFIG_CDCACM_NRDREQS=4
|
||||
CONFIG_CDCACM_BULKIN_REQLEN=96
|
||||
CONFIG_CDCACM_RXBUFSIZE=1000
|
||||
CONFIG_CDCACM_TXBUFSIZE=8000
|
||||
CONFIG_CDCACM_RXBUFSIZE=600
|
||||
CONFIG_CDCACM_TXBUFSIZE=5000
|
||||
CONFIG_CDCACM_VENDORID=0x26ac
|
||||
CONFIG_CDCACM_PRODUCTID=0x0011
|
||||
CONFIG_CDCACM_VENDORSTR="3D Robotics"
|
||||
|
||||
@@ -152,6 +152,7 @@ enum {
|
||||
TONE_EKF_WARNING_TUNE,
|
||||
TONE_BARO_WARNING_TUNE,
|
||||
TONE_SINGLE_BEEP_TUNE,
|
||||
TONE_HOME_SET,
|
||||
TONE_NUMBER_OF_TUNES
|
||||
};
|
||||
|
||||
|
||||
+97
-29
@@ -264,36 +264,42 @@ HIL::set_mode(Mode mode)
|
||||
debug("MODE_2PWM");
|
||||
/* multi-port with flow control lines as PWM */
|
||||
_update_rate = 50; /* default output rate */
|
||||
_num_outputs = 2;
|
||||
break;
|
||||
|
||||
case MODE_4PWM:
|
||||
debug("MODE_4PWM");
|
||||
/* multi-port as 4 PWM outs */
|
||||
_update_rate = 50; /* default output rate */
|
||||
_num_outputs = 4;
|
||||
break;
|
||||
|
||||
case MODE_8PWM:
|
||||
debug("MODE_8PWM");
|
||||
/* multi-port as 8 PWM outs */
|
||||
_update_rate = 50; /* default output rate */
|
||||
break;
|
||||
debug("MODE_8PWM");
|
||||
/* multi-port as 8 PWM outs */
|
||||
_update_rate = 50; /* default output rate */
|
||||
_num_outputs = 8;
|
||||
break;
|
||||
|
||||
case MODE_12PWM:
|
||||
debug("MODE_12PWM");
|
||||
/* multi-port as 12 PWM outs */
|
||||
_update_rate = 50; /* default output rate */
|
||||
break;
|
||||
case MODE_12PWM:
|
||||
debug("MODE_12PWM");
|
||||
/* multi-port as 12 PWM outs */
|
||||
_update_rate = 50; /* default output rate */
|
||||
_num_outputs = 12;
|
||||
break;
|
||||
|
||||
case MODE_16PWM:
|
||||
debug("MODE_16PWM");
|
||||
/* multi-port as 16 PWM outs */
|
||||
_update_rate = 50; /* default output rate */
|
||||
break;
|
||||
case MODE_16PWM:
|
||||
debug("MODE_16PWM");
|
||||
/* multi-port as 16 PWM outs */
|
||||
_update_rate = 50; /* default output rate */
|
||||
_num_outputs = 16;
|
||||
break;
|
||||
|
||||
case MODE_NONE:
|
||||
debug("MODE_NONE");
|
||||
/* disable servo outputs and set a very low update rate */
|
||||
_update_rate = 10;
|
||||
_num_outputs = 0;
|
||||
break;
|
||||
|
||||
default:
|
||||
@@ -468,13 +474,6 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret;
|
||||
|
||||
debug("ioctl 0x%04x 0x%08x", cmd, arg);
|
||||
|
||||
// /* try it as a GPIO ioctl first */
|
||||
// ret = HIL::gpio_ioctl(filp, cmd, arg);
|
||||
// if (ret != -ENOTTY)
|
||||
// return ret;
|
||||
|
||||
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
|
||||
switch(_mode) {
|
||||
case MODE_2PWM:
|
||||
@@ -523,6 +522,62 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
// HIL always outputs at the alternate (usually faster) rate
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
|
||||
*(uint32_t *)arg = 400;
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_UPDATE_RATE:
|
||||
*(uint32_t *)arg = 400;
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
|
||||
*(uint32_t *)arg = 0;
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_FAILSAFE_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < _num_outputs; i++) {
|
||||
pwm->values[i] = 850;
|
||||
}
|
||||
|
||||
pwm->channel_count = _num_outputs;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_DISARMED_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < _num_outputs; i++) {
|
||||
pwm->values[i] = 900;
|
||||
}
|
||||
|
||||
pwm->channel_count = _num_outputs;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_MIN_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < _num_outputs; i++) {
|
||||
pwm->values[i] = 1000;
|
||||
}
|
||||
|
||||
pwm->channel_count = _num_outputs;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_MAX_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < _num_outputs; i++) {
|
||||
pwm->values[i] = 2000;
|
||||
}
|
||||
|
||||
pwm->channel_count = _num_outputs;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_SET(2):
|
||||
case PWM_SERVO_SET(3):
|
||||
if (_mode != MODE_4PWM) {
|
||||
@@ -543,18 +598,26 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET(2):
|
||||
case PWM_SERVO_GET(7):
|
||||
case PWM_SERVO_GET(6):
|
||||
case PWM_SERVO_GET(5):
|
||||
case PWM_SERVO_GET(4):
|
||||
if (_num_outputs < 8) {
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET(3):
|
||||
if (_mode != MODE_4PWM) {
|
||||
case PWM_SERVO_GET(2):
|
||||
if (_num_outputs < 4) {
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
}
|
||||
|
||||
/* FALLTHROUGH */
|
||||
case PWM_SERVO_GET(0):
|
||||
case PWM_SERVO_GET(1): {
|
||||
// channel = cmd - PWM_SERVO_SET(0);
|
||||
// *(servo_position_t *)arg = up_pwm_servo_get(channel);
|
||||
case PWM_SERVO_GET(1):
|
||||
case PWM_SERVO_GET(0): {
|
||||
*(servo_position_t *)arg = 1500;
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -566,11 +629,16 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_COUNT:
|
||||
case MIXERIOCGETOUTPUTCOUNT:
|
||||
if (_mode == MODE_4PWM) {
|
||||
*(unsigned *)arg = 4;
|
||||
if (_mode == MODE_8PWM) {
|
||||
*(unsigned *)arg = 8;
|
||||
|
||||
} else if (_mode == MODE_4PWM) {
|
||||
|
||||
*(unsigned *)arg = 4;
|
||||
} else {
|
||||
|
||||
*(unsigned *)arg = 2;
|
||||
}
|
||||
|
||||
|
||||
@@ -154,10 +154,12 @@ protected:
|
||||
/**
|
||||
* Initialize the automatic measurement state machine and start it.
|
||||
*
|
||||
* @param delay_ticks the number of queue ticks before executing the next cycle
|
||||
*
|
||||
* @note This function is called at open and error time. It might make sense
|
||||
* to make it more aggressive about resetting the bus in case of errors.
|
||||
*/
|
||||
void start_cycle();
|
||||
void start_cycle(unsigned delay_ticks = 1);
|
||||
|
||||
/**
|
||||
* Stop the automatic measurement state machine.
|
||||
@@ -515,7 +517,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
}
|
||||
|
||||
void
|
||||
MS5611::start_cycle()
|
||||
MS5611::start_cycle(unsigned delay_ticks)
|
||||
{
|
||||
|
||||
/* reset the report ring and state machine */
|
||||
@@ -524,7 +526,7 @@ MS5611::start_cycle()
|
||||
_reports->flush();
|
||||
|
||||
/* schedule a cycle to start things */
|
||||
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
|
||||
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, delay_ticks);
|
||||
}
|
||||
|
||||
void
|
||||
@@ -564,8 +566,11 @@ MS5611::cycle()
|
||||
}
|
||||
/* issue a reset command to the sensor */
|
||||
_interface->ioctl(IOCTL_RESET, dummy);
|
||||
/* reset the collection state machine and try again */
|
||||
start_cycle();
|
||||
/* reset the collection state machine and try again - we need
|
||||
* to wait 2.8 ms after issuing the sensor reset command
|
||||
* according to the MS5611 datasheet
|
||||
*/
|
||||
start_cycle(USEC2TICK(2800));
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -594,7 +599,6 @@ MS5611::cycle()
|
||||
/* measurement phase */
|
||||
ret = measure();
|
||||
if (ret != OK) {
|
||||
//log("measure error %d", ret);
|
||||
/* issue a reset command to the sensor */
|
||||
_interface->ioctl(IOCTL_RESET, dummy);
|
||||
/* reset the collection state machine and try again */
|
||||
@@ -1182,26 +1186,30 @@ ms5611_main(int argc, char *argv[])
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(verb, "start"))
|
||||
if (!strcmp(verb, "start")) {
|
||||
ms5611::start(busid);
|
||||
}
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(verb, "test"))
|
||||
if (!strcmp(verb, "test")) {
|
||||
ms5611::test(busid);
|
||||
}
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
if (!strcmp(verb, "reset"))
|
||||
if (!strcmp(verb, "reset")) {
|
||||
ms5611::reset(busid);
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(verb, "info"))
|
||||
if (!strcmp(verb, "info")) {
|
||||
ms5611::info();
|
||||
}
|
||||
|
||||
/*
|
||||
* Perform MSL pressure calibration given an altitude in metres
|
||||
|
||||
@@ -339,6 +339,7 @@ ToneAlarm::ToneAlarm() :
|
||||
_default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning
|
||||
_default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning
|
||||
_default_tunes[TONE_SINGLE_BEEP_TUNE] = "MFT100a8"; // single beep
|
||||
_default_tunes[TONE_HOME_SET] = "MFT100L4>G#6A#6B#4";
|
||||
|
||||
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
|
||||
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
|
||||
@@ -354,6 +355,7 @@ ToneAlarm::ToneAlarm() :
|
||||
_tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning
|
||||
_tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning
|
||||
_tune_names[TONE_SINGLE_BEEP_TUNE] = "beep"; // single beep
|
||||
_tune_names[TONE_HOME_SET] = "home_set";
|
||||
}
|
||||
|
||||
ToneAlarm::~ToneAlarm()
|
||||
|
||||
@@ -89,7 +89,7 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3
|
||||
// take 5 point moving average
|
||||
//_vel_dot = _vdot_filter.apply(temp);
|
||||
// XXX resolve this properly
|
||||
_vel_dot = 0.9f * _vel_dot + 0.1f * temp;
|
||||
_vel_dot = 0.95f * _vel_dot + 0.05f * temp;
|
||||
|
||||
} else {
|
||||
_vel_dot = 0.0f;
|
||||
|
||||
@@ -180,8 +180,6 @@ static unsigned int leds_counter;
|
||||
/* To remember when last notification was sent */
|
||||
static uint64_t last_print_mode_reject_time = 0;
|
||||
|
||||
static float takeoff_alt = 5.0f;
|
||||
static int parachute_enabled = 0;
|
||||
static float eph_threshold = 5.0f;
|
||||
static float epv_threshold = 10.0f;
|
||||
|
||||
@@ -192,6 +190,8 @@ static struct vehicle_control_mode_s control_mode;
|
||||
static struct offboard_control_mode_s offboard_control_mode;
|
||||
static struct home_position_s _home;
|
||||
|
||||
static unsigned _last_mission_instance = 0;
|
||||
|
||||
/**
|
||||
* The daemon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
@@ -450,7 +450,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
|
||||
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
|
||||
|
||||
// For HIL platforms, require that simulated sensors are connected
|
||||
if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
|
||||
if (arm && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL &&
|
||||
is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
|
||||
mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming");
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
@@ -840,7 +841,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
|
||||
|
||||
//Play tune first time we initialize HOME
|
||||
if (!status.condition_home_position_valid) {
|
||||
tune_positive(true);
|
||||
tune_home_set(true);
|
||||
}
|
||||
|
||||
/* mark home position as set */
|
||||
@@ -859,8 +860,6 @@ int commander_thread_main(int argc, char *argv[])
|
||||
param_t _param_sys_type = param_find("MAV_TYPE");
|
||||
param_t _param_system_id = param_find("MAV_SYS_ID");
|
||||
param_t _param_component_id = param_find("MAV_COMP_ID");
|
||||
param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
|
||||
param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
|
||||
param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN");
|
||||
param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
|
||||
param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T");
|
||||
@@ -1278,11 +1277,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
rc_calibration_check(mavlink_fd);
|
||||
}
|
||||
|
||||
/* navigation parameters */
|
||||
param_get(_param_takeoff_alt, &takeoff_alt);
|
||||
|
||||
/* Safety parameters */
|
||||
param_get(_param_enable_parachute, ¶chute_enabled);
|
||||
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
|
||||
param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
|
||||
param_get(_param_rc_loss_timeout, &rc_loss_timeout);
|
||||
@@ -1771,6 +1766,32 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
|
||||
|
||||
/* Only evaluate mission state if home is set,
|
||||
* this prevents false positives for the mission
|
||||
* rejection. Back off 2 seconds to not overlay
|
||||
* home tune.
|
||||
*/
|
||||
if (status.condition_home_position_valid &&
|
||||
(hrt_elapsed_time(&_home.timestamp) > 2000000) &&
|
||||
_last_mission_instance != mission_result.instance_count) {
|
||||
if (!mission_result.valid) {
|
||||
/* the mission is invalid */
|
||||
tune_mission_fail(true);
|
||||
warnx("mission fail");
|
||||
} else if (mission_result.warning) {
|
||||
/* the mission has a warning */
|
||||
tune_mission_fail(true);
|
||||
warnx("mission warning");
|
||||
} else {
|
||||
/* the mission is valid */
|
||||
tune_mission_ok(true);
|
||||
warnx("mission ok");
|
||||
}
|
||||
|
||||
/* prevent further feedback until the mission changes */
|
||||
_last_mission_instance = mission_result.instance_count;
|
||||
}
|
||||
|
||||
/* RC input check */
|
||||
if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 &&
|
||||
hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
|
||||
@@ -2504,12 +2525,15 @@ set_control_mode()
|
||||
control_mode.flag_control_termination_enabled = false;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
|
||||
/* override is not ok for the RTL and recovery mode */
|
||||
control_mode.flag_external_manual_override_ok = false;
|
||||
/* fallthrough */
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
|
||||
control_mode.flag_control_manual_enabled = false;
|
||||
control_mode.flag_control_auto_enabled = true;
|
||||
control_mode.flag_control_rates_enabled = true;
|
||||
|
||||
@@ -172,6 +172,39 @@ void set_tune(int tune)
|
||||
}
|
||||
}
|
||||
|
||||
void tune_home_set(bool use_buzzer)
|
||||
{
|
||||
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
|
||||
rgbled_set_color(RGBLED_COLOR_GREEN);
|
||||
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
|
||||
|
||||
if (use_buzzer) {
|
||||
set_tune(TONE_HOME_SET);
|
||||
}
|
||||
}
|
||||
|
||||
void tune_mission_ok(bool use_buzzer)
|
||||
{
|
||||
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
|
||||
rgbled_set_color(RGBLED_COLOR_GREEN);
|
||||
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
|
||||
|
||||
if (use_buzzer) {
|
||||
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
|
||||
}
|
||||
}
|
||||
|
||||
void tune_mission_fail(bool use_buzzer)
|
||||
{
|
||||
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
|
||||
rgbled_set_color(RGBLED_COLOR_GREEN);
|
||||
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
|
||||
|
||||
if (use_buzzer) {
|
||||
set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Blink green LED and play positive tune (if use_buzzer == true).
|
||||
*/
|
||||
|
||||
@@ -58,6 +58,9 @@ void buzzer_deinit(void);
|
||||
|
||||
void set_tune_override(int tune);
|
||||
void set_tune(int tune);
|
||||
void tune_home_set(bool use_buzzer);
|
||||
void tune_mission_ok(bool use_buzzer);
|
||||
void tune_mission_fail(bool use_buzzer);
|
||||
void tune_positive(bool use_buzzer);
|
||||
void tune_neutral(bool use_buzzer);
|
||||
void tune_negative(bool use_buzzer);
|
||||
|
||||
@@ -375,7 +375,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
|
||||
switch (new_state) {
|
||||
case vehicle_status_s::HIL_STATE_OFF:
|
||||
/* we're in HIL and unexpected things can happen if we disable HIL now */
|
||||
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
|
||||
mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
|
||||
ret = TRANSITION_DENIED;
|
||||
break;
|
||||
|
||||
|
||||
@@ -1062,7 +1062,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
|
||||
}
|
||||
|
||||
// Fuse Airspeed Measurements
|
||||
if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) {
|
||||
if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) {
|
||||
_ekf->fuseVtasData = true;
|
||||
_ekf->RecallStates(_ekf->statesAtVtasMeasTime,
|
||||
(IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
|
||||
@@ -1320,7 +1320,7 @@ void AttitudePositionEstimatorEKF::pollData()
|
||||
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
|
||||
perf_count(_perf_airspeed);
|
||||
|
||||
_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
|
||||
_ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -634,8 +634,9 @@ FixedwingAttitudeControl::task_main()
|
||||
|
||||
/* rate limit vehicle status updates to 5Hz */
|
||||
orb_set_interval(_vcontrol_mode_sub, 200);
|
||||
/* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */
|
||||
orb_set_interval(_att_sub, 17);
|
||||
/* do not limit the attitude updates in order to minimize latency.
|
||||
* actuator outputs are still limited by the individual drivers
|
||||
* properly to not saturate IO or physical limitations */
|
||||
|
||||
parameters_update();
|
||||
|
||||
|
||||
@@ -100,8 +100,8 @@ static int _control_task = -1; /**< task handle for sensor task */
|
||||
#define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode
|
||||
#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading
|
||||
|
||||
#define THROTTLE_THRESH 0.05f // max throttle from user which will not lead to motors spinning up in altitude controlled modes
|
||||
|
||||
static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
|
||||
static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode
|
||||
|
||||
/**
|
||||
* L1 control app start / stop handling function
|
||||
@@ -370,7 +370,7 @@ private:
|
||||
/**
|
||||
* Publish navigation capabilities
|
||||
*/
|
||||
void navigation_capabilities_publish();
|
||||
void navigation_capabilities_publish();
|
||||
|
||||
/**
|
||||
* Get a new waypoint based on heading and distance from current position
|
||||
@@ -386,27 +386,30 @@ private:
|
||||
/**
|
||||
* Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
|
||||
*/
|
||||
float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
|
||||
float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
|
||||
|
||||
/**
|
||||
* Control position.
|
||||
*/
|
||||
|
||||
/**
|
||||
* Do takeoff help when in altitude controlled modes
|
||||
* Do takeoff help when in altitude controlled modes
|
||||
*/
|
||||
void do_takeoff_help();
|
||||
void do_takeoff_help();
|
||||
|
||||
/**
|
||||
* Update desired altitude base on user pitch stick input
|
||||
* Update desired altitude base on user pitch stick input
|
||||
*
|
||||
* @param dt Time step
|
||||
* @return true if climbout mode was requested by user (climb with max rate and min airspeed)
|
||||
*/
|
||||
void update_desired_altitude(float dt);
|
||||
bool update_desired_altitude(float dt);
|
||||
|
||||
bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
|
||||
const struct position_setpoint_triplet_s &_pos_sp_triplet);
|
||||
|
||||
float calculate_target_airspeed(float airspeed_demand);
|
||||
void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet);
|
||||
float calculate_target_airspeed(float airspeed_demand);
|
||||
void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet);
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
@@ -421,12 +424,12 @@ private:
|
||||
/*
|
||||
* Reset takeoff state
|
||||
*/
|
||||
void reset_takeoff_state();
|
||||
void reset_takeoff_state();
|
||||
|
||||
/*
|
||||
* Reset landing state
|
||||
*/
|
||||
void reset_landing_state();
|
||||
void reset_landing_state();
|
||||
|
||||
/*
|
||||
* Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter)
|
||||
@@ -955,16 +958,20 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
|
||||
}
|
||||
}
|
||||
|
||||
void FixedwingPositionControl::update_desired_altitude(float dt)
|
||||
bool FixedwingPositionControl::update_desired_altitude(float dt)
|
||||
{
|
||||
const float deadBand = (60.0f/1000.0f);
|
||||
const float factor = 1.0f - deadBand;
|
||||
static bool was_in_deadband = false;
|
||||
bool climbout_mode = false;
|
||||
|
||||
// XXX the sign magic in this function needs to be fixed
|
||||
|
||||
if (_manual.x > deadBand) {
|
||||
float pitch = (_manual.x - deadBand) / factor;
|
||||
_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
|
||||
was_in_deadband = false;
|
||||
climbout_mode = (fabsf(_manual.x) > MANUAL_THROTTLE_CLIMBOUT_THRESH);
|
||||
} else if (_manual.x < - deadBand) {
|
||||
float pitch = (_manual.x + deadBand) / factor;
|
||||
_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
|
||||
@@ -976,6 +983,8 @@ void FixedwingPositionControl::update_desired_altitude(float dt)
|
||||
_hold_alt = _global_pos.alt;
|
||||
was_in_deadband = true;
|
||||
}
|
||||
|
||||
return climbout_mode;
|
||||
}
|
||||
|
||||
void FixedwingPositionControl::do_takeoff_help()
|
||||
@@ -1085,7 +1094,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
|
||||
}
|
||||
|
||||
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
|
||||
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
|
||||
_att_sp.thrust = 0.0f;
|
||||
_att_sp.roll_body = 0.0f;
|
||||
_att_sp.pitch_body = 0.0f;
|
||||
|
||||
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
|
||||
/* waypoint is a plain navigation waypoint */
|
||||
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
|
||||
_att_sp.roll_body = _l1_control.nav_roll();
|
||||
@@ -1270,7 +1284,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
/* Inform user that launchdetection is running */
|
||||
static hrt_abstime last_sent = 0;
|
||||
if(hrt_absolute_time() - last_sent > 4e6) {
|
||||
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
|
||||
mavlink_log_critical(_mavlink_fd, "Launchdetection running");
|
||||
last_sent = hrt_absolute_time();
|
||||
}
|
||||
|
||||
@@ -1399,7 +1413,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
_manual.z;
|
||||
|
||||
/* update desired altitude based on user pitch stick input */
|
||||
update_desired_altitude(dt);
|
||||
bool climbout_requested = update_desired_altitude(dt);
|
||||
|
||||
/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
|
||||
do_takeoff_help();
|
||||
@@ -1418,7 +1432,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
_parameters.throttle_min,
|
||||
throttle_max,
|
||||
_parameters.throttle_cruise,
|
||||
false,
|
||||
climbout_requested,
|
||||
math::radians(_parameters.pitch_limit_min),
|
||||
_global_pos.alt,
|
||||
ground_speed,
|
||||
@@ -1492,7 +1506,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
_manual.z;
|
||||
|
||||
/* update desired altitude based on user pitch stick input */
|
||||
update_desired_altitude(dt);
|
||||
bool climbout_requested = update_desired_altitude(dt);
|
||||
|
||||
/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
|
||||
do_takeoff_help();
|
||||
@@ -1511,7 +1525,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
_parameters.throttle_min,
|
||||
throttle_max,
|
||||
_parameters.throttle_cruise,
|
||||
false,
|
||||
climbout_requested,
|
||||
math::radians(_parameters.pitch_limit_min),
|
||||
_global_pos.alt,
|
||||
ground_speed,
|
||||
@@ -1544,6 +1558,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
/* making sure again that the correct thrust is used,
|
||||
* without depending on library calls for safety reasons */
|
||||
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
|
||||
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
|
||||
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
|
||||
_att_sp.thrust = 0.0f;
|
||||
} else {
|
||||
/* Copy thrust and pitch values from tecs */
|
||||
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
|
||||
|
||||
@@ -194,6 +194,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
||||
handle_message_manual_control(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
|
||||
handle_message_rc_channels_override(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
handle_message_heartbeat(msg);
|
||||
break;
|
||||
@@ -912,6 +916,48 @@ static int decode_switch_pos_n(uint16_t buttons, int sw) {
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_rc_channels_override_t man;
|
||||
mavlink_msg_rc_channels_override_decode(msg, &man);
|
||||
|
||||
// Check target
|
||||
if (man.target_system != 0 && man.target_system != _mavlink->get_system_id()) {
|
||||
return;
|
||||
}
|
||||
|
||||
struct rc_input_values rc = {};
|
||||
rc.timestamp_publication = hrt_absolute_time();
|
||||
rc.timestamp_last_signal = rc.timestamp_publication;
|
||||
|
||||
rc.channel_count = 8;
|
||||
rc.rc_failsafe = false;
|
||||
rc.rc_lost = false;
|
||||
rc.rc_lost_frame_count = 0;
|
||||
rc.rc_total_frame_count = 1;
|
||||
rc.rc_ppm_frame_length = 0;
|
||||
rc.input_source = RC_INPUT_SOURCE_MAVLINK;
|
||||
rc.rssi = RC_INPUT_RSSI_MAX;
|
||||
|
||||
/* channels */
|
||||
rc.values[0] = man.chan1_raw;
|
||||
rc.values[1] = man.chan2_raw;
|
||||
rc.values[2] = man.chan3_raw;
|
||||
rc.values[3] = man.chan4_raw;
|
||||
rc.values[4] = man.chan5_raw;
|
||||
rc.values[5] = man.chan6_raw;
|
||||
rc.values[6] = man.chan7_raw;
|
||||
rc.values[7] = man.chan8_raw;
|
||||
|
||||
if (_rc_pub <= 0) {
|
||||
_rc_pub = orb_advertise(ORB_ID(input_rc), &rc);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(input_rc), _rc_pub, &rc);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
|
||||
{
|
||||
|
||||
@@ -126,6 +126,7 @@ private:
|
||||
void handle_message_set_attitude_target(mavlink_message_t *msg);
|
||||
void handle_message_radio_status(mavlink_message_t *msg);
|
||||
void handle_message_manual_control(mavlink_message_t *msg);
|
||||
void handle_message_rc_channels_override(mavlink_message_t *msg);
|
||||
void handle_message_heartbeat(mavlink_message_t *msg);
|
||||
void handle_message_ping(mavlink_message_t *msg);
|
||||
void handle_message_request_data_stream(mavlink_message_t *msg);
|
||||
|
||||
@@ -599,10 +599,10 @@ MulticopterAttitudeControl::vehicle_motor_limits_poll()
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Attitude controller.
|
||||
* Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
|
||||
* Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes)
|
||||
* Input: 'vehicle_attitude_setpoint' topics (depending on mode)
|
||||
* Output: '_rates_sp' vector, '_thrust_sp'
|
||||
*/
|
||||
void
|
||||
MulticopterAttitudeControl::control_attitude(float dt)
|
||||
|
||||
@@ -1034,6 +1034,21 @@ MulticopterPositionControl::task_main()
|
||||
|
||||
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
|
||||
|
||||
/* make sure velocity setpoint is saturated in xy*/
|
||||
float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) +
|
||||
_vel_sp(1)*_vel_sp(1));
|
||||
if (vel_norm_xy > _params.vel_max(0)) {
|
||||
/* note assumes vel_max(0) == vel_max(1) */
|
||||
_vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy;
|
||||
_vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy;
|
||||
}
|
||||
|
||||
/* make sure velocity setpoint is saturated in z*/
|
||||
float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2));
|
||||
if (vel_norm_z > _params.vel_max(2)) {
|
||||
_vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z;
|
||||
}
|
||||
|
||||
if (!_control_mode.flag_control_altitude_enabled) {
|
||||
_reset_alt_sp = true;
|
||||
_vel_sp(2) = 0.0f;
|
||||
@@ -1381,14 +1396,15 @@ MulticopterPositionControl::task_main()
|
||||
}
|
||||
|
||||
//Control roll and pitch directly if we no aiding velocity controller is active
|
||||
if(!_control_mode.flag_control_velocity_enabled) {
|
||||
if (!_control_mode.flag_control_velocity_enabled) {
|
||||
_att_sp.roll_body = _manual.y * _params.man_roll_max;
|
||||
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
|
||||
}
|
||||
|
||||
//Control climb rate directly if no aiding altitude controller is active
|
||||
if(!_control_mode.flag_control_climb_rate_enabled) {
|
||||
_att_sp.thrust = math::min(_manual.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
|
||||
if (!_control_mode.flag_control_climb_rate_enabled) {
|
||||
_att_sp.thrust = math::min(_manual.z, _params.thr_max);
|
||||
_att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min);
|
||||
}
|
||||
|
||||
//Construct attitude setpoint rotation matrix
|
||||
|
||||
@@ -50,7 +50,7 @@
|
||||
* @max 1.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
|
||||
|
||||
/**
|
||||
* Maximum thrust
|
||||
|
||||
@@ -55,7 +55,8 @@
|
||||
#include "navigator.h"
|
||||
|
||||
Loiter::Loiter(Navigator *navigator, const char *name) :
|
||||
MissionBlock(navigator, name)
|
||||
MissionBlock(navigator, name),
|
||||
_param_min_alt(this, "MIS_TAKEOFF_ALT", false)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
@@ -74,7 +75,7 @@ void
|
||||
Loiter::on_activation()
|
||||
{
|
||||
/* set current mission item to loiter */
|
||||
set_loiter_item(&_mission_item);
|
||||
set_loiter_item(&_mission_item, _param_min_alt.get());
|
||||
|
||||
/* convert mission item to current setpoint */
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
@@ -59,6 +59,9 @@ public:
|
||||
virtual void on_activation();
|
||||
|
||||
virtual void on_active();
|
||||
|
||||
private:
|
||||
control::BlockParamFloat _param_min_alt;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -78,7 +78,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
|
||||
_takeoff(false),
|
||||
_mission_type(MISSION_TYPE_NONE),
|
||||
_inited(false),
|
||||
_dist_1wp_ok(false),
|
||||
_home_inited(false),
|
||||
_missionFeasiblityChecker(),
|
||||
_min_current_sp_distance_xy(FLT_MAX),
|
||||
_mission_item_previous_alt(NAN),
|
||||
@@ -110,6 +110,24 @@ Mission::on_inactive()
|
||||
update_offboard_mission();
|
||||
}
|
||||
|
||||
/* check if the home position became valid in the meantime */
|
||||
if ((_mission_type == MISSION_TYPE_NONE || _mission_type == MISSION_TYPE_OFFBOARD) &&
|
||||
!_home_inited && _navigator->home_position_valid()) {
|
||||
|
||||
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
|
||||
|
||||
_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
|
||||
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
|
||||
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
|
||||
_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
|
||||
|
||||
_navigator->increment_mission_instance_count();
|
||||
_navigator->set_mission_result_updated();
|
||||
|
||||
_home_inited = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* read mission topics on initialization */
|
||||
_inited = true;
|
||||
@@ -176,6 +194,7 @@ Mission::on_active()
|
||||
&& _mission_type != MISSION_TYPE_NONE) {
|
||||
heading_sp_update();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
@@ -197,6 +216,11 @@ Mission::update_onboard_mission()
|
||||
/* otherwise, just leave it */
|
||||
}
|
||||
|
||||
// XXX check validity here as well
|
||||
_navigator->get_mission_result()->valid = true;
|
||||
_navigator->increment_mission_instance_count();
|
||||
_navigator->set_mission_result_updated();
|
||||
|
||||
} else {
|
||||
_onboard_mission.count = 0;
|
||||
_onboard_mission.current_seq = 0;
|
||||
@@ -232,7 +256,13 @@ Mission::update_offboard_mission()
|
||||
|
||||
failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
|
||||
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
|
||||
_navigator->get_home_position()->alt, _navigator->home_position_valid());
|
||||
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
|
||||
_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
|
||||
|
||||
_navigator->get_mission_result()->valid = !failed;
|
||||
_navigator->increment_mission_instance_count();
|
||||
_navigator->set_mission_result_updated();
|
||||
|
||||
} else {
|
||||
warnx("offboard mission update failed");
|
||||
@@ -281,73 +311,6 @@ Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
Mission::check_dist_1wp()
|
||||
{
|
||||
if (_dist_1wp_ok) {
|
||||
/* always return true after at least one successful check */
|
||||
return true;
|
||||
}
|
||||
|
||||
/* check if first waypoint is not too far from home */
|
||||
if (_param_dist_1wp.get() > 0.0f) {
|
||||
if (_navigator->get_vstatus()->condition_home_position_valid) {
|
||||
struct mission_item_s mission_item;
|
||||
|
||||
/* find first waypoint (with lat/lon) item in datamanager */
|
||||
for (unsigned i = 0; i < _offboard_mission.count; i++) {
|
||||
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i,
|
||||
&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
|
||||
|
||||
/* check only items with valid lat/lon */
|
||||
if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
|
||||
mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
||||
mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
||||
mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
|
||||
mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
|
||||
mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
|
||||
|
||||
/* check distance from current position to item */
|
||||
float dist_to_1wp = get_distance_to_next_waypoint(
|
||||
mission_item.lat, mission_item.lon,
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
|
||||
|
||||
if (dist_to_1wp < _param_dist_1wp.get()) {
|
||||
_dist_1wp_ok = true;
|
||||
if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) {
|
||||
/* allow at 2/3 distance, but warn */
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
|
||||
}
|
||||
return true;
|
||||
|
||||
} else {
|
||||
/* item is too far from home */
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
/* error reading, mission is invalid */
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/* no waypoints found in mission, then we will not fly far away */
|
||||
_dist_1wp_ok = true;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "no home position");
|
||||
return false;
|
||||
}
|
||||
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Mission::set_mission_items()
|
||||
{
|
||||
@@ -367,24 +330,24 @@ Mission::set_mission_items()
|
||||
_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
|
||||
}
|
||||
|
||||
/* get home distance state */
|
||||
bool home_dist_ok = check_dist_1wp();
|
||||
/* the home dist check provides user feedback, so we initialize it to this */
|
||||
bool user_feedback_done = !home_dist_ok;
|
||||
bool user_feedback_done = false;
|
||||
|
||||
/* try setting onboard mission item */
|
||||
if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
|
||||
/* if mission type changed, notify */
|
||||
if (_mission_type != MISSION_TYPE_ONBOARD) {
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
|
||||
user_feedback_done = true;
|
||||
}
|
||||
_mission_type = MISSION_TYPE_ONBOARD;
|
||||
|
||||
/* try setting offboard mission item */
|
||||
} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
|
||||
} else if (read_mission_item(false, true, &_mission_item)) {
|
||||
/* if mission type changed, notify */
|
||||
if (_mission_type != MISSION_TYPE_OFFBOARD) {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
|
||||
user_feedback_done = true;
|
||||
}
|
||||
_mission_type = MISSION_TYPE_OFFBOARD;
|
||||
} else {
|
||||
@@ -392,21 +355,17 @@ Mission::set_mission_items()
|
||||
if (_mission_type != MISSION_TYPE_NONE) {
|
||||
/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
|
||||
user_feedback_done = true;
|
||||
|
||||
/* use last setpoint for loiter */
|
||||
_navigator->set_can_loiter_at_sp(true);
|
||||
|
||||
} else if (!user_feedback_done) {
|
||||
/* only tell users that we got no mission if there has not been any
|
||||
* better, more specific feedback yet
|
||||
* https://en.wikipedia.org/wiki/Loiter_(aeronautics)
|
||||
*/
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
|
||||
}
|
||||
|
||||
_mission_type = MISSION_TYPE_NONE;
|
||||
|
||||
/* set loiter mission item */
|
||||
set_loiter_item(&_mission_item);
|
||||
/* set loiter mission item and ensure that there is a minimum clearance from home */
|
||||
set_loiter_item(&_mission_item, _param_takeoff_alt.get());
|
||||
|
||||
/* update position setpoint triplet */
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
@@ -418,6 +377,24 @@ Mission::set_mission_items()
|
||||
|
||||
set_mission_finished();
|
||||
|
||||
if (!user_feedback_done) {
|
||||
/* only tell users that we got no mission if there has not been any
|
||||
* better, more specific feedback yet
|
||||
* https://en.wikipedia.org/wiki/Loiter_(aeronautics)
|
||||
*/
|
||||
|
||||
if (_navigator->get_vstatus()->condition_landed) {
|
||||
/* landed, refusing to take off without a mission */
|
||||
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff");
|
||||
} else {
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
|
||||
}
|
||||
|
||||
user_feedback_done = true;
|
||||
|
||||
}
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -186,7 +186,7 @@ private:
|
||||
} _mission_type;
|
||||
|
||||
bool _inited;
|
||||
bool _dist_1wp_ok;
|
||||
bool _home_inited;
|
||||
|
||||
MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */
|
||||
|
||||
|
||||
@@ -228,7 +228,7 @@ MissionBlock::set_previous_pos_setpoint()
|
||||
}
|
||||
|
||||
void
|
||||
MissionBlock::set_loiter_item(struct mission_item_s *item)
|
||||
MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
|
||||
{
|
||||
if (_navigator->get_vstatus()->condition_landed) {
|
||||
/* landed, don't takeoff, but switch to IDLE mode */
|
||||
@@ -246,10 +246,14 @@ MissionBlock::set_loiter_item(struct mission_item_s *item)
|
||||
item->altitude = pos_sp_triplet->current.alt;
|
||||
|
||||
} else {
|
||||
/* use current position */
|
||||
/* use current position and use return altitude as clearance */
|
||||
item->lat = _navigator->get_global_position()->lat;
|
||||
item->lon = _navigator->get_global_position()->lon;
|
||||
item->altitude = _navigator->get_global_position()->alt;
|
||||
|
||||
if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
|
||||
item->altitude = _navigator->get_home_position()->alt + min_clearance;
|
||||
}
|
||||
}
|
||||
|
||||
item->altitude_is_relative = false;
|
||||
|
||||
@@ -91,7 +91,7 @@ protected:
|
||||
/**
|
||||
* Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
|
||||
*/
|
||||
void set_loiter_item(struct mission_item_s *item);
|
||||
void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f);
|
||||
|
||||
mission_item_s _mission_item;
|
||||
bool _waypoint_position_reached;
|
||||
|
||||
@@ -57,28 +57,45 @@
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false)
|
||||
MissionFeasibilityChecker::MissionFeasibilityChecker() :
|
||||
_mavlink_fd(-1),
|
||||
_capabilities_sub(-1),
|
||||
_initDone(false),
|
||||
_dist_1wp_ok(false)
|
||||
{
|
||||
_nav_caps = {0};
|
||||
}
|
||||
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
|
||||
bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing,
|
||||
dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
|
||||
float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued)
|
||||
{
|
||||
bool failed = false;
|
||||
bool warned = false;
|
||||
/* Init if not done yet */
|
||||
init();
|
||||
|
||||
_mavlink_fd = mavlink_fd;
|
||||
|
||||
// check if all mission item commands are supported
|
||||
failed |= !checkMissionItemValidity(dm_current, nMissionItems);
|
||||
// first check if we have a valid position
|
||||
if (!home_valid /* can later use global / local pos for finer granularity */) {
|
||||
failed = true;
|
||||
warned = true;
|
||||
mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock.");
|
||||
} else {
|
||||
failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
|
||||
}
|
||||
|
||||
// check if all mission item commands are supported
|
||||
failed = failed || !checkMissionItemValidity(dm_current, nMissionItems);
|
||||
failed = failed || !checkGeofence(dm_current, nMissionItems, geofence);
|
||||
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
|
||||
|
||||
if (isRotarywing) {
|
||||
failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
|
||||
failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
|
||||
} else {
|
||||
failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
|
||||
failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
|
||||
}
|
||||
|
||||
if (!failed) {
|
||||
@@ -90,28 +107,20 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
|
||||
{
|
||||
|
||||
/* Perform checks and issue feedback to the user for all checks */
|
||||
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
|
||||
|
||||
/* Mission is only marked as feasible if all checks return true */
|
||||
return (resGeofence && resHomeAltitude);
|
||||
/* no custom rotary wing checks yet */
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
|
||||
{
|
||||
/* Update fixed wing navigation capabilites */
|
||||
updateNavigationCapabilities();
|
||||
// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
|
||||
|
||||
/* Perform checks and issue feedback to the user for all checks */
|
||||
bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
|
||||
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
|
||||
|
||||
/* Mission is only marked as feasible if all checks return true */
|
||||
return (resLanding && resGeofence && resHomeAltitude);
|
||||
return resLanding;
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
|
||||
@@ -137,7 +146,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error)
|
||||
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems,
|
||||
float home_alt, bool home_valid, bool &warning_issued, bool throw_error)
|
||||
{
|
||||
/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
|
||||
for (size_t i = 0; i < nMissionItems; i++) {
|
||||
@@ -145,17 +155,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
|
||||
const ssize_t len = sizeof(struct mission_item_s);
|
||||
|
||||
if (dm_read(dm_current, i, &missionitem, len) != len) {
|
||||
warning_issued = true;
|
||||
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
|
||||
if (throw_error) {
|
||||
return false;
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/* always reject relative alt without home set */
|
||||
if (missionitem.altitude_is_relative && !home_valid) {
|
||||
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
|
||||
warning_issued = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -163,6 +171,9 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
|
||||
float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
|
||||
|
||||
if (home_alt > wp_alt) {
|
||||
|
||||
warning_issued = true;
|
||||
|
||||
if (throw_error) {
|
||||
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i);
|
||||
return false;
|
||||
@@ -275,6 +286,68 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued)
|
||||
{
|
||||
if (_dist_1wp_ok) {
|
||||
/* always return true after at least one successful check */
|
||||
return true;
|
||||
}
|
||||
|
||||
/* check if first waypoint is not too far from home */
|
||||
if (dist_first_wp > 0.0f) {
|
||||
struct mission_item_s mission_item;
|
||||
|
||||
/* find first waypoint (with lat/lon) item in datamanager */
|
||||
for (unsigned i = 0; i < nMissionItems; i++) {
|
||||
if (dm_read(dm_current, i,
|
||||
&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
|
||||
|
||||
/* check only items with valid lat/lon */
|
||||
if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
|
||||
mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
||||
mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
||||
mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
|
||||
mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
|
||||
mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
|
||||
|
||||
/* check distance from current position to item */
|
||||
float dist_to_1wp = get_distance_to_next_waypoint(
|
||||
mission_item.lat, mission_item.lon, curr_lat, curr_lon);
|
||||
|
||||
if (dist_to_1wp < dist_first_wp) {
|
||||
_dist_1wp_ok = true;
|
||||
if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
|
||||
/* allow at 2/3 distance, but warn */
|
||||
mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
|
||||
warning_issued = true;
|
||||
}
|
||||
return true;
|
||||
|
||||
} else {
|
||||
/* item is too far from home */
|
||||
mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp);
|
||||
warning_issued = true;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
/* error reading, mission is invalid */
|
||||
mavlink_log_info(_mavlink_fd, "error reading offboard mission");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/* no waypoints found in mission, then we will not fly far away */
|
||||
_dist_1wp_ok = true;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
void MissionFeasibilityChecker::updateNavigationCapabilities()
|
||||
{
|
||||
(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
|
||||
|
||||
@@ -57,12 +57,14 @@ private:
|
||||
struct navigation_capabilities_s _nav_caps;
|
||||
|
||||
bool _initDone;
|
||||
bool _dist_1wp_ok;
|
||||
void init();
|
||||
|
||||
/* Checks for all airframes */
|
||||
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
|
||||
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false);
|
||||
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false);
|
||||
bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
|
||||
bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued);
|
||||
|
||||
/* Checks specific to fixedwing airframes */
|
||||
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
|
||||
@@ -79,8 +81,9 @@ public:
|
||||
/*
|
||||
* Returns true if mission is feasible and false otherwise
|
||||
*/
|
||||
bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
|
||||
float home_alt, bool home_valid);
|
||||
bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current,
|
||||
size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid,
|
||||
double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued);
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -163,6 +163,8 @@ public:
|
||||
float get_acceptance_radius(float mission_item_radius);
|
||||
int get_mavlink_fd() { return _mavlink_fd; }
|
||||
|
||||
void increment_mission_instance_count() { _mission_instance_count++; }
|
||||
|
||||
private:
|
||||
|
||||
bool _task_should_exit; /**< if true, sensor task should exit */
|
||||
@@ -205,6 +207,7 @@ private:
|
||||
bool _home_position_set;
|
||||
|
||||
bool _mission_item_valid; /**< flags if the current mission item is valid */
|
||||
int _mission_instance_count; /**< instance count for the current mission */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
|
||||
@@ -125,6 +125,7 @@ Navigator::Navigator() :
|
||||
_att_sp{},
|
||||
_home_position_set(false),
|
||||
_mission_item_valid(false),
|
||||
_mission_instance_count(0),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
|
||||
_geofence{},
|
||||
_geofence_violation_warning_sent(false),
|
||||
@@ -663,6 +664,8 @@ int navigator_main(int argc, char *argv[])
|
||||
void
|
||||
Navigator::publish_mission_result()
|
||||
{
|
||||
_mission_result.instance_count = _mission_instance_count;
|
||||
|
||||
/* lazily publish the mission result only once available */
|
||||
if (_mission_result_pub > 0) {
|
||||
/* publish mission result */
|
||||
@@ -679,6 +682,7 @@ Navigator::publish_mission_result()
|
||||
_mission_result.item_do_jump_changed = false;
|
||||
_mission_result.item_changed_index = 0;
|
||||
_mission_result.item_do_jump_remaining = 0;
|
||||
_mission_result.valid = true;
|
||||
}
|
||||
|
||||
void
|
||||
|
||||
@@ -1288,6 +1288,10 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
|
||||
_airspeed.true_airspeed_m_s = math::max(0.0f,
|
||||
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
|
||||
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
|
||||
_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
|
||||
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar * 1e2f,
|
||||
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
|
||||
|
||||
_airspeed.air_temperature_celsius = air_temperature_celsius;
|
||||
|
||||
/* announce the airspeed if needed, just publish else */
|
||||
|
||||
@@ -1,75 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* 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 mission_result.h
|
||||
* Mission results that navigator needs to pass on to commander and mavlink.
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Ban Siesta <bansiesta@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef TOPIC_MISSION_RESULT_H
|
||||
#define TOPIC_MISSION_RESULT_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
struct mission_result_s {
|
||||
unsigned seq_reached; /**< Sequence of the mission item which has been reached */
|
||||
unsigned seq_current; /**< Sequence of the current mission item */
|
||||
bool reached; /**< true if mission has been reached */
|
||||
bool finished; /**< true if mission has been completed */
|
||||
bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/
|
||||
bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */
|
||||
bool item_do_jump_changed; /**< true if the number of do jumps remaining has changed */
|
||||
unsigned item_changed_index; /**< indicate which item has changed */
|
||||
unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(mission_result);
|
||||
|
||||
#endif
|
||||
@@ -53,14 +53,15 @@
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include "uavcan_main.hpp"
|
||||
#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
|
||||
#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
|
||||
|
||||
#include <uavcan_posix/firmware_version_checker.hpp>
|
||||
|
||||
#if defined(USE_FW_NODE_SERVER)
|
||||
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
|
||||
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
|
||||
# include <uavcan_posix/firmware_version_checker.hpp>
|
||||
//todo:The Inclusion of file_server_backend is killing
|
||||
// #include <sys/types.h> and leaving OK undefined
|
||||
#define OK 0
|
||||
# define OK 0
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @file uavcan_main.cpp
|
||||
@@ -75,20 +76,26 @@
|
||||
* UavcanNode
|
||||
*/
|
||||
UavcanNode *UavcanNode::_instance;
|
||||
#if defined(USE_FW_NODE_SERVER)
|
||||
uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance;
|
||||
uavcan_posix::dynamic_node_id_server::FileEventTracer tracer;
|
||||
uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend;
|
||||
uavcan_posix::FirmwareVersionChecker fw_version_checker;
|
||||
|
||||
#endif
|
||||
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
|
||||
CDev("uavcan", UAVCAN_DEVICE_PATH),
|
||||
_node(can_driver, system_clock),
|
||||
_node_mutex(),
|
||||
#if !defined(USE_FW_NODE_SERVER)
|
||||
_esc_controller(_node)
|
||||
#else
|
||||
_esc_controller(_node),
|
||||
_fileserver_backend(_node),
|
||||
_node_info_retriever(_node),
|
||||
_fw_upgrade_trigger(_node, fw_version_checker),
|
||||
_fw_server(_node, _fileserver_backend)
|
||||
#endif
|
||||
|
||||
{
|
||||
_control_topics[0] = ORB_ID(actuator_controls_0);
|
||||
_control_topics[1] = ORB_ID(actuator_controls_1);
|
||||
@@ -154,7 +161,10 @@ UavcanNode::~UavcanNode()
|
||||
perf_free(_perfcnt_node_spin_elapsed);
|
||||
perf_free(_perfcnt_esc_mixer_output_elapsed);
|
||||
perf_free(_perfcnt_esc_mixer_total_elapsed);
|
||||
|
||||
#if defined(USE_FW_NODE_SERVER)
|
||||
delete(_server_instance);
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
@@ -305,7 +315,7 @@ int UavcanNode::init(uavcan::NodeID node_id)
|
||||
br = br->getSibling();
|
||||
}
|
||||
|
||||
|
||||
#if defined(USE_FW_NODE_SERVER)
|
||||
/* Initialize the fw version checker.
|
||||
* giving it it's path
|
||||
*/
|
||||
@@ -373,6 +383,7 @@ int UavcanNode::init(uavcan::NodeID node_id)
|
||||
return ret;
|
||||
}
|
||||
|
||||
#endif
|
||||
/* Start the Node */
|
||||
|
||||
return _node.start();
|
||||
|
||||
@@ -34,6 +34,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
#include <drivers/device/device.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
@@ -47,13 +48,13 @@
|
||||
#include "actuators/esc.hpp"
|
||||
#include "sensors/sensor_bridge.hpp"
|
||||
|
||||
|
||||
#include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
|
||||
#include <uavcan/protocol/node_info_retriever.hpp>
|
||||
#include <uavcan_posix/basic_file_server_backend.hpp>
|
||||
#include <uavcan/protocol/firmware_update_trigger.hpp>
|
||||
#include <uavcan/protocol/file_server.hpp>
|
||||
|
||||
#if defined(USE_FW_NODE_SERVER)
|
||||
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
|
||||
# include <uavcan/protocol/node_info_retriever.hpp>
|
||||
# include <uavcan_posix/basic_file_server_backend.hpp>
|
||||
# include <uavcan/protocol/firmware_update_trigger.hpp>
|
||||
# include <uavcan/protocol/file_server.hpp>
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @file uavcan_main.hpp
|
||||
@@ -147,7 +148,6 @@ private:
|
||||
unsigned _output_count = 0; ///< number of actuators currently available
|
||||
|
||||
static UavcanNode *_instance; ///< singleton pointer
|
||||
static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer
|
||||
|
||||
Node _node; ///< library instance
|
||||
pthread_mutex_t _node_mutex;
|
||||
@@ -155,11 +155,14 @@ private:
|
||||
UavcanEscController _esc_controller;
|
||||
|
||||
|
||||
#if defined(USE_FW_NODE_SERVER)
|
||||
static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer
|
||||
|
||||
uavcan_posix::BasicFileSeverBackend _fileserver_backend;
|
||||
uavcan::NodeInfoRetriever _node_info_retriever;
|
||||
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
|
||||
uavcan::BasicFileServer _fw_server;
|
||||
|
||||
#endif
|
||||
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
|
||||
|
||||
MixerGroup *_mixers = nullptr;
|
||||
|
||||
Reference in New Issue
Block a user