Browse Source

Merge branch 'release_v1.0.0' into beta

sbg
Lorenz Meier 10 years ago
parent
commit
5de6d1b3c4
  1. 5
      ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
  2. 8
      ROMFS/px4fmu_common/init.d/10019_sk450_deadcat
  3. 3
      msg/airspeed.msg
  4. 12
      msg/mission_result.msg
  5. 33
      nuttx-configs/px4fmu-v2/nsh/defconfig
  6. 1
      src/drivers/drv_tone_alarm.h
  7. 126
      src/drivers/hil/hil.cpp
  8. 28
      src/drivers/ms5611/ms5611.cpp
  9. 2
      src/drivers/stm32/tone_alarm/tone_alarm.cpp
  10. 2
      src/lib/external_lgpl/tecs/tecs.cpp
  11. 48
      src/modules/commander/commander.cpp
  12. 33
      src/modules/commander/commander_helper.cpp
  13. 3
      src/modules/commander/commander_helper.h
  14. 2
      src/modules/commander/state_machine_helper.cpp
  15. 4
      src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
  16. 5
      src/modules/fw_att_control/fw_att_control_main.cpp
  17. 55
      src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
  18. 46
      src/modules/mavlink/mavlink_receiver.cpp
  19. 1
      src/modules/mavlink/mavlink_receiver.h
  20. 6
      src/modules/mc_att_control/mc_att_control_main.cpp
  21. 22
      src/modules/mc_pos_control/mc_pos_control_main.cpp
  22. 2
      src/modules/mc_pos_control/mc_pos_control_params.c
  23. 5
      src/modules/navigator/loiter.cpp
  24. 3
      src/modules/navigator/loiter.h
  25. 139
      src/modules/navigator/mission.cpp
  26. 2
      src/modules/navigator/mission.h
  27. 8
      src/modules/navigator/mission_block.cpp
  28. 2
      src/modules/navigator/mission_block.h
  29. 119
      src/modules/navigator/mission_feasibility_checker.cpp
  30. 9
      src/modules/navigator/mission_feasibility_checker.h
  31. 3
      src/modules/navigator/navigator.h
  32. 4
      src/modules/navigator/navigator_main.cpp
  33. 4
      src/modules/sensors/sensors.cpp
  34. 75
      src/modules/uORB/topics/mission_result.h
  35. 25
      src/modules/uavcan/uavcan_main.cpp
  36. 21
      src/modules/uavcan/uavcan_main.hpp

5
ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d

@ -9,16 +9,15 @@ sh /etc/init.d/rc.mc_defaults @@ -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

8
ROMFS/px4fmu_common/init.d/10019_sk450_deadcat

@ -10,13 +10,13 @@ sh /etc/init.d/rc.mc_defaults @@ -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

3
msg/airspeed.msg

@ -1,4 +1,5 @@ @@ -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

12
msg/mission_result.msg

@ -0,0 +1,12 @@ @@ -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

33
nuttx-configs/px4fmu-v2/nsh/defconfig

@ -367,7 +367,8 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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"

1
src/drivers/drv_tone_alarm.h

@ -152,6 +152,7 @@ enum { @@ -152,6 +152,7 @@ enum {
TONE_EKF_WARNING_TUNE,
TONE_BARO_WARNING_TUNE,
TONE_SINGLE_BEEP_TUNE,
TONE_HOME_SET,
TONE_NUMBER_OF_TUNES
};

126
src/drivers/hil/hil.cpp

@ -264,36 +264,42 @@ HIL::set_mode(Mode mode) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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;
}

28
src/drivers/ms5611/ms5611.cpp

@ -154,10 +154,12 @@ protected: @@ -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) @@ -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() @@ -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() @@ -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() @@ -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[]) @@ -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

2
src/drivers/stm32/tone_alarm/tone_alarm.cpp

@ -339,6 +339,7 @@ ToneAlarm::ToneAlarm() : @@ -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() : @@ -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()

2
src/lib/external_lgpl/tecs/tecs.cpp

@ -89,7 +89,7 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3 @@ -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;

48
src/modules/commander/commander.cpp

@ -180,8 +180,6 @@ static unsigned int leds_counter; @@ -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; @@ -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 @@ -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 & @@ -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[]) @@ -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[]) @@ -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, &parachute_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[]) @@ -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() @@ -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;

33
src/modules/commander/commander_helper.cpp

@ -172,6 +172,39 @@ void set_tune(int tune) @@ -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).
*/

3
src/modules/commander/commander_helper.h

@ -58,6 +58,9 @@ void buzzer_deinit(void); @@ -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);

2
src/modules/commander/state_machine_helper.cpp

@ -375,7 +375,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, @@ -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;

4
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@ -1062,7 +1062,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const @@ -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() @@ -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;
}

5
src/modules/fw_att_control/fw_att_control_main.cpp

@ -634,8 +634,9 @@ FixedwingAttitudeControl::task_main() @@ -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();

55
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@ -100,8 +100,8 @@ static int _control_task = -1; /**< task handle for sensor task */ @@ -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: @@ -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: @@ -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> &current_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> &current_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: @@ -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 @@ -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) @@ -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> &current_positi @@ -1085,7 +1094,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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> &current_positi @@ -1270,7 +1284,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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> &current_positi @@ -1399,7 +1413,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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> &current_positi @@ -1418,7 +1432,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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> &current_positi @@ -1492,7 +1506,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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> &current_positi @@ -1511,7 +1525,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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> &current_positi @@ -1544,6 +1558,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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() :

46
src/modules/mavlink/mavlink_receiver.cpp

@ -194,6 +194,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) @@ -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) { @@ -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)
{

1
src/modules/mavlink/mavlink_receiver.h

@ -126,6 +126,7 @@ private: @@ -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);

6
src/modules/mc_att_control/mc_att_control_main.cpp

@ -599,10 +599,10 @@ MulticopterAttitudeControl::vehicle_motor_limits_poll() @@ -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)

22
src/modules/mc_pos_control/mc_pos_control_main.cpp

@ -1034,6 +1034,21 @@ MulticopterPositionControl::task_main() @@ -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() @@ -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

2
src/modules/mc_pos_control/mc_pos_control_params.c

@ -50,7 +50,7 @@ @@ -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

5
src/modules/navigator/loiter.cpp

@ -55,7 +55,8 @@ @@ -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 @@ -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();

3
src/modules/navigator/loiter.h

@ -59,6 +59,9 @@ public: @@ -59,6 +59,9 @@ public:
virtual void on_activation();
virtual void on_active();
private:
control::BlockParamFloat _param_min_alt;
};
#endif

139
src/modules/navigator/mission.cpp

@ -78,7 +78,7 @@ Mission::Mission(Navigator *navigator, const char *name) : @@ -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() @@ -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() @@ -176,6 +194,7 @@ Mission::on_active()
&& _mission_type != MISSION_TYPE_NONE) {
heading_sp_update();
}
}
void
@ -197,6 +216,11 @@ Mission::update_onboard_mission() @@ -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() @@ -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) @@ -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() @@ -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() @@ -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() @@ -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;
}

2
src/modules/navigator/mission.h

@ -186,7 +186,7 @@ private: @@ -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 */

8
src/modules/navigator/mission_block.cpp

@ -228,7 +228,7 @@ MissionBlock::set_previous_pos_setpoint() @@ -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) @@ -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;

2
src/modules/navigator/mission_block.h

@ -91,7 +91,7 @@ protected: @@ -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;

119
src/modules/navigator/mission_feasibility_checker.cpp

@ -57,28 +57,45 @@ @@ -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 @@ -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 @@ -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, @@ -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, @@ -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 @@ -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);

9
src/modules/navigator/mission_feasibility_checker.h

@ -57,12 +57,14 @@ private: @@ -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: @@ -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);
};

3
src/modules/navigator/navigator.h

@ -163,6 +163,8 @@ public: @@ -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: @@ -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 */

4
src/modules/navigator/navigator_main.cpp

@ -125,6 +125,7 @@ Navigator::Navigator() : @@ -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[]) @@ -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() @@ -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

4
src/modules/sensors/sensors.cpp

@ -1288,6 +1288,10 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) @@ -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 */

75
src/modules/uORB/topics/mission_result.h

@ -1,75 +0,0 @@ @@ -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

25
src/modules/uavcan/uavcan_main.cpp

@ -53,14 +53,15 @@ @@ -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 @@ @@ -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() @@ -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) @@ -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) @@ -373,6 +383,7 @@ int UavcanNode::init(uavcan::NodeID node_id)
return ret;
}
#endif
/* Start the Node */
return _node.start();

21
src/modules/uavcan/uavcan_main.hpp

@ -34,6 +34,7 @@ @@ -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 @@ @@ -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: @@ -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: @@ -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;

Loading…
Cancel
Save