From d8aebc805ffc2b51a93d1c5276c84af7017f83b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 18:49:02 -0700 Subject: [PATCH 01/99] MAVLink app: Further improvements on simulated RC scaling --- src/modules/mavlink/mavlink_receiver.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8ebe0c696d..834c8398a1 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -948,7 +948,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) * which makes the corner positions unreachable. * scale yaw up and clip it to overcome this. */ - rc.values[2] = man.r / 1.2f + 1500; + rc.values[2] = man.r / 1.1f + 1500; if (rc.values[2] > 2000) { rc.values[2] = 2000; } else if (rc.values[2] < 1000) { @@ -956,7 +956,12 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) } /* throttle */ - rc.values[3] = man.z + 1000; + rc.values[3] = man.z / 0.9f + 1000; + if (rc.values[3] > 2000) { + rc.values[3] = 2000; + } else if (rc.values[3] < 1000) { + rc.values[3] = 1000; + } rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000; rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000; From 61904d3106faefbcc67bd96cb64d7b741b633bf6 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 29 May 2015 10:05:29 -0600 Subject: [PATCH 02/99] fix type mismatch --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index df4064a731..233fd3e007 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1009,7 +1009,7 @@ PX4IO::task_main() orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); // Check for a DSM pairing command - if (((int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) { + if (((uint32_t)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) { dsm_bind_ioctl((int)cmd.param2); } } From 164a1178b8c574f8bdbde963e1b8fb38a463602e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 30 May 2015 18:42:46 -0700 Subject: [PATCH 03/99] commander: Be more verbose about low battery warnings, do not trigger low battery warning sound in HIL --- src/modules/commander/commander.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7faf3149f7..07ec502001 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1582,6 +1582,8 @@ int commander_thread_main(int argc, char *argv[]) low_battery_voltage_actions_done = true; if (armed.armed) { mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); + } else { + mavlink_log_critical(mavlink_fd, "LOW BATTERY, TAKEOFF DISCOURAGED"); } status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW; status_changed = true; @@ -2054,11 +2056,13 @@ int commander_thread_main(int argc, char *argv[]) set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; - } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { + } else if (status.hil_state != vehicle_status_s::HIL_STATE_ON) && + status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { + } else if (status.hil_state != vehicle_status_s::HIL_STATE_ON) && + (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe)) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); From db3ac5f3ac7f30fdcb30d6b29d91312e4b36d618 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 30 May 2015 19:06:05 -0700 Subject: [PATCH 04/99] commander: Compile fix --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 07ec502001..67e5f7a58c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2056,12 +2056,12 @@ int commander_thread_main(int argc, char *argv[]) set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; - } else if (status.hil_state != vehicle_status_s::HIL_STATE_ON) && + } else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) && status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.hil_state != vehicle_status_s::HIL_STATE_ON) && + } else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) && (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe)) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); From f3b392513d34de1fedaea950cdbf480c47c232df Mon Sep 17 00:00:00 2001 From: Luis Rodrigues Date: Sun, 31 May 2015 21:09:21 +0200 Subject: [PATCH 05/99] fixed timming issued in I2C whoami communication --- src/drivers/trone/trone.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index deb89f2f65..82045b7d73 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -314,10 +314,16 @@ TRONE::probe() uint8_t who_am_i=0; const uint8_t cmd = TRONE_WHO_AM_I_REG; - if (transfer(&cmd, 1, &who_am_i, 1) == OK && who_am_i == TRONE_WHO_AM_I_REG_VAL) { - // it is responding correctly to a WHO_AM_I - return measure(); - } + + // set the I2C bus address + set_address(TRONE_BASEADDR); + + // can't use a single transfer as TROne need a bit of time for internal processing + if (transfer(&cmd, 1, nullptr, 0) == OK) { + if ( transfer(nullptr, 0, &who_am_i, 1) == OK && who_am_i == TRONE_WHO_AM_I_REG_VAL){ + return measure(); + } + } debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n", (unsigned)who_am_i, From 61a8e2e27089c630fafd94f7b399ea51813a6fd8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jun 2015 13:06:03 +0200 Subject: [PATCH 06/99] Caipi mixer improvements --- .../px4fmu_common/init.d/3100_tbs_caipirinha | 5 +- ROMFS/px4fmu_common/mixers/caipi.main.mix | 51 +++++++++++++++++++ 2 files changed, 55 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/mixers/caipi.main.mix diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 9e1c1c170a..d8cbb1fd9a 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -31,4 +31,7 @@ then param set FW_R_RMAX 0 fi -set MIXER Q +set MIXER caipi +# Provide ESC a constant 1000 us pulse +set PWM_OUT 4 +set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/mixers/caipi.main.mix b/ROMFS/px4fmu_common/mixers/caipi.main.mix new file mode 100644 index 0000000000..c68f7135f3 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/caipi.main.mix @@ -0,0 +1,51 @@ +Delta-wing mixer +=========================== + +Designed for TBS Caipirinha + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 300 -10000 10000 +S: 0 0 -10000 -10000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +M: 2 +O: 10000 10000 300 -10000 10000 +S: 0 0 -10000 -10000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 From 75082f90ec2d780a707a216953107ff70354253a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jun 2015 13:28:31 +0200 Subject: [PATCH 07/99] Final touchup on caipi mixer --- ROMFS/px4fmu_common/mixers/caipi.main.mix | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/caipi.main.mix b/ROMFS/px4fmu_common/mixers/caipi.main.mix index c68f7135f3..cfc49ff9f4 100644 --- a/ROMFS/px4fmu_common/mixers/caipi.main.mix +++ b/ROMFS/px4fmu_common/mixers/caipi.main.mix @@ -24,13 +24,13 @@ The scaling factor for roll inputs is adjusted to implement differential travel for the elevons. M: 2 -O: 10000 10000 300 -10000 10000 -S: 0 0 -10000 -10000 0 -10000 10000 +O: 10000 10000 -3000 -10000 10000 +S: 0 0 -9000 -9000 0 -10000 10000 S: 0 1 8000 8000 0 -10000 10000 M: 2 -O: 10000 10000 300 -10000 10000 -S: 0 0 -10000 -10000 0 -10000 10000 +O: 10000 10000 3000 -10000 10000 +S: 0 0 -9000 -9000 0 -10000 10000 S: 0 1 -8000 -8000 0 -10000 10000 Output 2 From 1d58190bf9f9ce4ba45420192ef9561cba0571ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jun 2015 13:31:12 +0200 Subject: [PATCH 08/99] Default altitude mode to first order hold (line between waypoints) and allow missions to be further away to still successfully start them --- src/modules/navigator/mission_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index b1bd4e540e..435229865c 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -81,7 +81,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * @max 1000 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900); /** * Altitude setpoint mode @@ -94,7 +94,7 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); * @max 1 * @group Mission */ -PARAM_DEFINE_INT32(MIS_ALTMODE, 0); +PARAM_DEFINE_INT32(MIS_ALTMODE, 1); /** * Multirotor only. Yaw setpoint mode. From 44cf402c6f35de64381ad67f5560f52daae51777 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jun 2015 13:46:39 +0200 Subject: [PATCH 09/99] FX79: Set better default values for airspeed and loiter radius --- ROMFS/px4fmu_common/init.d/3034_fx79 | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index d46147ede5..9f45c636b0 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -7,4 +7,12 @@ sh /etc/init.d/rc.fw_defaults +if [ $AUTOCNF == yes ] +then + param set NAV_LOITER_RAD 150 + param set FW_AIRSPD_MAX 30 + param set FW_AIRSPD_MIN 13 + param set FW_AIRSPD_TRIM 15 +fi + set MIXER FX79 From 9a16d9ebfaed012697d064ef00c1251e5e777e56 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jun 2015 18:44:05 +0200 Subject: [PATCH 10/99] IO Firmware: Code style fix, fix RSSI ADC lowpass --- src/modules/px4iofirmware/controls.c | 96 +++++++++++++++++++--------- 1 file changed, 66 insertions(+), 30 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index ac004f2127..f7ac061e2a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -63,6 +63,7 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; static uint16_t rc_value_override = 0; +static unsigned _rssi_adc_counts = 0; bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) { @@ -71,17 +72,22 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool uint8_t n_bytes = 0; uint8_t *bytes; *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + if (*dsm_updated) { r_raw_rc_count = temp_count & 0x7fff; - if (temp_count & 0x8000) + + if (temp_count & 0x8000) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - else + + } else { r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + } r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } + perf_end(c_gather_dsm); /* get data from FD and attempt to parse with DSM and ST24 libs */ @@ -94,7 +100,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool /* set updated flag if one complete packet was parsed */ st24_rssi = RC_INPUT_RSSI_MAX; *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, - &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); + &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*st24_updated) { @@ -121,7 +127,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool /* set updated flag if one complete packet was parsed */ sumd_rssi = RC_INPUT_RSSI_MAX; *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, - &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); + &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*sumd_updated) { @@ -170,7 +176,8 @@ controls_init(void) } void -controls_tick() { +controls_tick() +{ /* * Gather R/C control inputs from supported sources. @@ -184,19 +191,24 @@ controls_tick() { uint16_t rssi = 0; #ifdef ADC_RSSI + if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) { unsigned counts = adc_measure(ADC_RSSI); - if (counts != 0xffff) { - /* use 1:1 scaling on 3.3V ADC input */ - unsigned mV = counts * 3300 / 4096; - /* scale to 0..253 and lowpass */ - rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f); + if (counts != 0xffff) { + /* low pass*/ + _rssi_adc_counts = (_rssi_adc_counts * 0.99f) + (counts * 0.01f); + /* use 1:1 scaling on 3.3V, 12-Bit ADC input */ + unsigned mV = _rssi_adc_counts * 3300 / 4095; + /* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */ + rssi = (mV * RC_INPUT_RSSI_MAX / 3300); + if (rssi > RC_INPUT_RSSI_MAX) { rssi = RC_INPUT_RSSI_MAX; } } } + #endif /* zero RSSI if signal is lost */ @@ -207,21 +219,26 @@ controls_tick() { perf_begin(c_gather_dsm); bool dsm_updated, st24_updated, sumd_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); + if (dsm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; } + if (st24_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; } + if (sumd_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; } + perf_end(c_gather_dsm); perf_begin(c_gather_sbus); bool sbus_failsafe, sbus_frame_drop; - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; @@ -231,17 +248,19 @@ controls_tick() { if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; sbus_rssi = RC_INPUT_RSSI_MAX / 2; + } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } if (sbus_failsafe) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } - /* set RSSI to an emulated value if ADC RSSI is off */ + /* set RSSI to an emulated value if ADC RSSI is off */ if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { rssi = sbus_rssi; } @@ -257,17 +276,20 @@ controls_tick() { */ perf_begin(c_gather_ppm); bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); + if (ppm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } + perf_end(c_gather_ppm); /* limit number of channels to allowable data size */ - if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) + if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) { r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; + } /* store RSSI */ r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi; @@ -308,10 +330,13 @@ controls_tick() { /* * 1) Constrain to min/max values, as later processing depends on bounds. */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) { raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + } + + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) { raw = conf[PX4IO_P_RC_CONFIG_MAX]; + } /* * 2) Scale around the mid point differently for lower and upper range. @@ -330,10 +355,12 @@ controls_tick() { * DO NOT REMOVE OR ALTER STEP 1! */ if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)( + conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)( + conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); } else { /* in the configured dead zone, output zero */ @@ -347,6 +374,7 @@ controls_tick() { /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { /* invert channel if pitch - pulling the lever down means pitching up by convention */ @@ -360,6 +388,7 @@ controls_tick() { if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) || ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } @@ -389,6 +418,7 @@ controls_tick() { /* if we have enough channels (5) to control the vehicle, the mapping is ok */ if (assigned_channels > 4) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } @@ -408,9 +438,9 @@ controls_tick() { /* clear the input-kind flags here */ r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_RC_PPM | - PX4IO_P_STATUS_FLAGS_RC_DSM | - PX4IO_P_STATUS_FLAGS_RC_SBUS); + PX4IO_P_STATUS_FLAGS_RC_PPM | + PX4IO_P_STATUS_FLAGS_RC_DSM | + PX4IO_P_STATUS_FLAGS_RC_SBUS); } @@ -427,8 +457,8 @@ controls_tick() { if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_OVERRIDE | - PX4IO_P_STATUS_FLAGS_RC_OK); + PX4IO_P_STATUS_FLAGS_OVERRIDE | + PX4IO_P_STATUS_FLAGS_RC_OK); /* flag raw RC as lost */ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK); @@ -451,9 +481,9 @@ controls_tick() { * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; @@ -464,8 +494,9 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) { override = true; + } /* if the FMU is dead then enable override if we have a @@ -473,20 +504,23 @@ controls_tick() { */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) - override = true; + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + override = true; + } if (override) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ - if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) + if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) { mixer_tick(); + } } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } + } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } @@ -512,8 +546,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; - if (*num_values > PX4IO_RC_INPUT_CHANNELS) + + if (*num_values > PX4IO_RC_INPUT_CHANNELS) { *num_values = PX4IO_RC_INPUT_CHANNELS; + } for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) { values[i] = ppm_buffer[i]; From 75ad5875cb58dc31ecb68f2d83278e143a19512b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jun 2015 18:45:45 +0200 Subject: [PATCH 11/99] Low-pass analog RSSI stronger --- src/modules/px4iofirmware/controls.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index f7ac061e2a..6714305ac9 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -197,7 +197,7 @@ controls_tick() if (counts != 0xffff) { /* low pass*/ - _rssi_adc_counts = (_rssi_adc_counts * 0.99f) + (counts * 0.01f); + _rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f); /* use 1:1 scaling on 3.3V, 12-Bit ADC input */ unsigned mV = _rssi_adc_counts * 3300 / 4095; /* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */ From 0c0500f8da603c81c3703428efa77f1bed12b134 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jun 2015 19:08:28 +0200 Subject: [PATCH 12/99] IO Firmware: Do not build ADC code if there is no ADC input for RSSI --- src/modules/px4iofirmware/controls.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 6714305ac9..1a35cf3e43 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -63,7 +63,10 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; static uint16_t rc_value_override = 0; + +#ifdef ADC_RSSI static unsigned _rssi_adc_counts = 0; +#endif bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) { From b0ed795ba826b64fed0a9572be11ea07554eb591 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 01:12:10 +0200 Subject: [PATCH 13/99] Caipirinha: Improve default parameters based on Wing-Wing params --- .../px4fmu_common/init.d/3100_tbs_caipirinha | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index d8cbb1fd9a..5a7d73c060 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -9,26 +9,26 @@ sh /etc/init.d/rc.fw_defaults if [ $AUTOCNF == yes ] then - - # TODO: these are the X5 default parameters, update them to the caipi - - param set FW_AIRSPD_MIN 15 - param set FW_AIRSPD_TRIM 20 - param set FW_AIRSPD_MAX 40 + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_MIN 12 + param set FW_AIRSPD_TRIM 16 param set FW_ATT_TC 0.3 param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.3 - param set FW_PR_I 0 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.03 - param set FW_P_ROLLFF 0 - param set FW_RR_FF 0.3 - param set FW_RR_I 0 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_PR_I 0.01 + param set FW_PR_IMAX 0.4 + param set FW_PR_P 0.08 + param set FW_RR_FF 0.6 + param set FW_RR_I 0.01 param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.03 - param set FW_R_LIM 60 - param set FW_R_RMAX 0 + param set FW_RR_P 0.04 fi set MIXER caipi From 363a4821656d5fd8afef2887150f6ee705074af5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 10:14:58 +0200 Subject: [PATCH 14/99] Move navigation_capabilities uORB topic to generated message set --- msg/navigation_capabilities.msg | 5 ++ .../uORB/topics/navigation_capabilities.h | 70 ------------------- 2 files changed, 5 insertions(+), 70 deletions(-) create mode 100644 msg/navigation_capabilities.msg delete mode 100644 src/modules/uORB/topics/navigation_capabilities.h diff --git a/msg/navigation_capabilities.msg b/msg/navigation_capabilities.msg new file mode 100644 index 0000000000..235b5df03b --- /dev/null +++ b/msg/navigation_capabilities.msg @@ -0,0 +1,5 @@ +uint64 timestamp # timestamp this topic was emitted +float32 turn_distance # the optimal distance to a waypoint to switch to the next +float32 landing_horizontal_slope_displacement +float32 landing_slope_angle_rad +float32 landing_flare_length diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h deleted file mode 100644 index 7a5ae98914..0000000000 --- a/src/modules/uORB/topics/navigation_capabilities.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * 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 navigation_capabilities.h - * - * Definition of navigation capabilities uORB topic. - */ - -#ifndef TOPIC_NAVIGATION_CAPABILITIES_H_ -#define TOPIC_NAVIGATION_CAPABILITIES_H_ - -#include "../uORB.h" -#include - -/** - * @addtogroup topics - * @{ - */ - -/** - * Airspeed - */ -struct navigation_capabilities_s { - float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ - - /* Landing parameters: see fw_pos_control_l1/landingslope.h */ - float landing_horizontal_slope_displacement; - float landing_slope_angle_rad; - float landing_flare_length; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(navigation_capabilities); - -#endif From bd3fbd5fa240478849fc18ac56548e1dc5dd3794 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 10:15:37 +0200 Subject: [PATCH 15/99] Fixed wing HIL: More suitable tuning gains by default --- .../init.d/1000_rc_fw_easystar.hil | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 40b9ed8df5..1e3dc6c5f6 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -6,5 +6,30 @@ sh /etc/init.d/rc.fw_defaults +if [ $AUTOCNF == yes ] +then + param set BAT_N_CELLS 3 + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_MIN 12 + param set FW_AIRSPD_TRIM 14 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_PR_I 0.1 + param set FW_PR_IMAX 0.4 + param set FW_PR_P 0.2 + param set FW_RR_FF 0.6 + param set FW_RR_I 0.1 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.3 +fi + set HIL yes set MIXER AERT From 68276ff3455032d3b45381b90903c0ed1e80a835 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 5 Jun 2015 06:43:10 -1000 Subject: [PATCH 16/99] Back Port from Master - Changes to build on latest uavcan master with FW upload and Node ID --- makefiles/toolchain_gnu-arm-eabi.mk | 8 +- src/lib/uavcan | 2 +- src/modules/uavcan/module.mk | 9 +- src/modules/uavcan/sensors/baro.cpp | 100 ++++++++++++------- src/modules/uavcan/sensors/baro.hpp | 24 +++-- src/modules/uavcan/uavcan_main.cpp | 146 +++++++++++++++++++++++++--- src/modules/uavcan/uavcan_main.hpp | 44 ++++++++- 7 files changed, 270 insertions(+), 63 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 3b9fefb3ef..44c63dc20b 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -130,6 +130,12 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) ARCHCFLAGS = -std=gnu99 ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ +# +# Provide defaults, but allow for module override +WFRAME_LARGER_THAN ?= 1024 + + + # Generic warnings # ARCHWARNINGS = -Wall \ @@ -138,7 +144,7 @@ ARCHWARNINGS = -Wall \ -Wdouble-promotion \ -Wshadow \ -Wfloat-equal \ - -Wframe-larger-than=1024 \ + -Wframe-larger-than=$(WFRAME_LARGER_THAN) \ -Wpointer-arith \ -Wlogical-op \ -Wmissing-declarations \ diff --git a/src/lib/uavcan b/src/lib/uavcan index 7719f7692a..1f1679c75d 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 7719f7692aba67f01b6321773bb7be13f23d2f68 +Subproject commit 1f1679c75d989420350e69339d48b53203c5e874 diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index e3fd31a9a1..1d6c205586 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -39,6 +39,8 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -O3 +MODULE_STACKSIZE = 3200 +WFRAME_LARGER_THAN = 1400 # Main SRCS += uavcan_main.cpp \ @@ -65,7 +67,6 @@ INCLUDE_DIRS += $(LIBUAVCAN_INC) # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS - # # libuavcan drivers for STM32 # @@ -75,6 +76,12 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_STM32_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 +# +# libuavcan drivers for posix +# +include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/posix/include.mk +INCLUDE_DIRS += $(LIBUAVCAN_POSIX_INC) + # # Invoke DSDL compiler # diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 576e758df5..8198d1d994 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -41,30 +41,44 @@ const char *const UavcanBarometerBridge::NAME = "baro"; -UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : -UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), -_sub_air_data(node), -_reports(nullptr) +UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : + UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), + _sub_air_pressure_data(node), + _sub_air_temperature_data(node), + _reports(nullptr) { + last_temperature = 0.0f; } int UavcanBarometerBridge::init() { int res = device::CDev::init(); + if (res < 0) { return res; } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(baro_report)); - if (_reports == nullptr) - return -1; - res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); + if (_reports == nullptr) { + return -1; + } + + res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); + if (res < 0) { log("failed to start uavcan sub: %d", res); return res; } + + res = _sub_air_temperature_data.start(AirTemperatureCbBinder(this, &UavcanBarometerBridge::air_temperature_sub_cb)); + + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + return 0; } @@ -75,8 +89,9 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } while (count--) { if (_reports->get(baro_buf)) { @@ -93,47 +108,62 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case BAROIOCSMSLPRESSURE: { - if ((arg < 80000) || (arg > 120000)) { - return -EINVAL; - } else { - log("new msl pressure %u", _msl_pressure); - _msl_pressure = arg; + if ((arg < 80000) || (arg > 120000)) { + return -EINVAL; + + } else { + log("new msl pressure %u", _msl_pressure); + _msl_pressure = arg; + return OK; + } + } + + case BAROIOCGMSLPRESSURE: { + return _msl_pressure; + } + + case SENSORIOCSPOLLRATE: { + // not supported yet, pretend that everything is ok return OK; } - } - case BAROIOCGMSLPRESSURE: { - return _msl_pressure; - } - case SENSORIOCSPOLLRATE: { - // not supported yet, pretend that everything is ok - return OK; - } + case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } + default: { - return CDev::ioctl(filp, cmd, arg); - } + return CDev::ioctl(filp, cmd, arg); + } } } -void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure &msg) +void UavcanBarometerBridge::air_temperature_sub_cb(const + uavcan::ReceivedDataStructure &msg) +{ + last_temperature = msg.static_temperature; +} + +void UavcanBarometerBridge::air_pressure_sub_cb(const + uavcan::ReceivedDataStructure &msg) { baro_report report; report.timestamp = msg.getMonotonicTimestamp().toUSec(); - report.temperature = msg.static_temperature; + report.temperature = last_temperature; report.pressure = msg.static_pressure / 100.0F; // Convert to millibar report.error_count = 0; diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 6a39eebfb6..98dc5cf42f 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -40,7 +40,8 @@ #include "sensor_bridge.hpp" #include -#include +#include +#include class RingBuffer; @@ -56,17 +57,26 @@ public: int init() override; private: - ssize_t read(struct file *filp, char *buffer, size_t buflen); + ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; - void air_data_sub_cb(const uavcan::ReceivedDataStructure &msg); + void air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg); + void air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg); typedef uavcan::MethodBinder < UavcanBarometerBridge *, void (UavcanBarometerBridge::*) - (const uavcan::ReceivedDataStructure &) > - AirDataCbBinder; + (const uavcan::ReceivedDataStructure &) > + AirPressureCbBinder; - uavcan::Subscriber _sub_air_data; + typedef uavcan::MethodBinder < UavcanBarometerBridge *, + void (UavcanBarometerBridge::*) + (const uavcan::ReceivedDataStructure &) > + AirTemperatureCbBinder; + + uavcan::Subscriber _sub_air_pressure_data; + uavcan::Subscriber _sub_air_temperature_data; unsigned _msl_pressure = 101325; - RingBuffer *_reports; + RingBuffer *_reports; + float last_temperature; + }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ca92d09ecb..c18d6e5d1c 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -53,25 +53,42 @@ #include #include "uavcan_main.hpp" +#include +#include + +#include + +//todo:The Inclusion of file_server_backend is killing +// #include and leaving OK undefined +#define OK 0 /** * @file uavcan_main.cpp * - * Implements basic functinality of UAVCAN node. + * Implements basic functionality of UAVCAN node. * * @author Pavel Kirienko + * David Sidrane */ /* * UavcanNode */ UavcanNode *UavcanNode::_instance; +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; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), _node_mutex(), - _esc_controller(_node) + _esc_controller(_node), + _fileserver_backend(_node), + _node_info_retriever(_node), + _fw_upgrade_trigger(_node, fw_version_checker), + _fw_server(_node, _fileserver_backend) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -79,6 +96,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _control_topics[3] = ORB_ID(actuator_controls_3); const int res = pthread_mutex_init(&_node_mutex, nullptr); + if (res < 0) { std::abort(); } @@ -124,6 +142,7 @@ UavcanNode::~UavcanNode() // Removing the sensor bridges auto br = _sensor_bridges.getHead(); + while (br != nullptr) { auto next = br->getSibling(); delete br; @@ -135,6 +154,8 @@ UavcanNode::~UavcanNode() perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); + delete(_server_instance); + } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -196,7 +217,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, - static_cast(run_trampoline), nullptr); + static_cast(run_trampoline), nullptr); if (_instance->_task < 0) { warnx("start failed: %d", errno); @@ -228,8 +249,10 @@ void UavcanNode::fill_node_info() if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) { hwver.major = 1; + } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) { hwver.major = 2; + } else { ; // All other values of HW_ARCH resolve to zero } @@ -241,6 +264,7 @@ void UavcanNode::fill_node_info() _node.setHardwareVersion(hwver); } + int UavcanNode::init(uavcan::NodeID node_id) { int ret = -1; @@ -260,6 +284,7 @@ int UavcanNode::init(uavcan::NodeID node_id) // Actuators ret = _esc_controller.init(); + if (ret < 0) { return ret; } @@ -267,26 +292,101 @@ int UavcanNode::init(uavcan::NodeID node_id) // Sensor bridges IUavcanSensorBridge::make_all(_node, _sensor_bridges); auto br = _sensor_bridges.getHead(); + while (br != nullptr) { ret = br->init(); + if (ret < 0) { warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret); return ret; } + warnx("sensor bridge '%s' init ok", br->get_name()); br = br->getSibling(); } + + /* Initialize the fw version checker. + * giving it it's path + */ + + ret = fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH); + + if (ret < 0) { + return ret; + } + + + /* Start fw file server back */ + + ret = _fw_server.start(); + + if (ret < 0) { + return ret; + } + + /* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */ + + ret = storage_backend.init(UAVCAN_NODE_DB_PATH); + + if (ret < 0) { + return ret; + } + + /* Initialize trace in the UAVCAN_NODE_DB_PATH directory */ + + ret = tracer.init(UAVCAN_LOG_FILE); + + if (ret < 0) { + return ret; + } + + /* Create dynamic node id server for the Firmware updates directory */ + + _server_instance = new uavcan::dynamic_node_id_server::CentralizedServer(_node, storage_backend, tracer); + + if (_server_instance == 0) { + return -ENOMEM; + } + + /* Initialize the dynamic node id server */ + ret = _server_instance->init(_node.getNodeStatusProvider().getHardwareVersion().unique_id); + + if (ret < 0) { + return ret; + } + + /* Start node info retriever to fetch node info from new nodes */ + + ret = _node_info_retriever.start(); + + if (ret < 0) { + return ret; + } + + + /* Start the fw version checker */ + + ret = _fw_upgrade_trigger.start(_node_info_retriever, fw_version_checker.getFirmwarePath()); + + if (ret < 0) { + return ret; + } + + /* Start the Node */ + return _node.start(); } void UavcanNode::node_spin_once() { perf_begin(_perfcnt_node_spin_elapsed); - const int spin_res = _node.spin(uavcan::MonotonicTime()); + const int spin_res = _node.spinOnce(); + if (spin_res < 0) { warnx("node spin error %i", spin_res); } + perf_end(_perfcnt_node_spin_elapsed); } @@ -297,9 +397,11 @@ void UavcanNode::node_spin_once() int UavcanNode::add_poll_fd(int fd) { int ret = _poll_fds_num; + if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) { errx(1, "uavcan: too many poll fds, exiting"); } + _poll_fds[_poll_fds_num] = ::pollfd(); _poll_fds[_poll_fds_num].fd = fd; _poll_fds[_poll_fds_num].events = POLLIN; @@ -312,8 +414,6 @@ int UavcanNode::run() { (void)pthread_mutex_lock(&_node_mutex); - const unsigned PollTimeoutMs = 50; - // XXX figure out the output count _output_count = 2; @@ -324,8 +424,8 @@ int UavcanNode::run() memset(&_outputs, 0, sizeof(_outputs)); const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); - if (busevent_fd < 0) - { + + if (busevent_fd < 0) { warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); _task_should_exit = true; } @@ -354,6 +454,7 @@ int UavcanNode::run() } while (!_task_should_exit) { + // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); @@ -379,9 +480,11 @@ int UavcanNode::run() if (poll_ret < 0) { log("poll error %d", errno); continue; + } else { // get controls for required topics bool controls_updated = false; + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[_poll_ids[i]].revents & POLLIN) { @@ -401,8 +504,9 @@ int UavcanNode::run() if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { _actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; } + memcpy(&_outputs.output[0], &_actuator_direct.values[0], - _actuator_direct.nvalues*sizeof(float)); + _actuator_direct.nvalues * sizeof(float)); _outputs.noutputs = _actuator_direct.nvalues; new_output = true; } @@ -410,11 +514,14 @@ int UavcanNode::run() // can we mix? if (_test_in_progress) { memset(&_outputs, 0, sizeof(_outputs)); + if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { - _outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; - _outputs.noutputs = _test_motor.motor_number+1; + _outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f; + _outputs.noutputs = _test_motor.motor_number + 1; } + new_output = true; + } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, @@ -451,6 +558,7 @@ int UavcanNode::run() _outputs.output[i] = 1.0f; } } + // Output to the bus _outputs.timestamp = hrt_absolute_time(); perf_begin(_perfcnt_esc_mixer_output_elapsed); @@ -509,6 +617,7 @@ UavcanNode::teardown() _control_subs[i] = -1; } } + return (_armed_sub >= 0) ? ::close(_armed_sub) : 0; } @@ -526,12 +635,14 @@ UavcanNode::subscribe() // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + // the first fd used by CAN for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); } + if (unsub_groups & (1 << i)) { warnx("unsubscribe from actuator_controls_%d", i); ::close(_control_subs[i]); @@ -583,8 +694,9 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); - if (_mixers == nullptr) + if (_mixers == nullptr) { _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); + } if (_mixers == nullptr) { _groups_required = 0; @@ -600,6 +712,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) _mixers = nullptr; _groups_required = 0; ret = -EINVAL; + } else { _mixers->groups_required(_groups_required); @@ -640,9 +753,10 @@ UavcanNode::print_info() if (_outputs.noutputs != 0) { printf("ESC output: "); - for (uint8_t i=0; i<_outputs.noutputs; i++) { - printf("%d ", (int)(_outputs.output[i]*1000)); + for (uint8_t i = 0; i < _outputs.noutputs; i++) { + printf("%d ", (int)(_outputs.output[i] * 1000)); } + printf("\n"); // ESC status @@ -653,7 +767,8 @@ UavcanNode::print_info() printf("ESC Status:\n"); printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n"); - for (uint8_t i=0; i<_outputs.noutputs; i++) { + + for (uint8_t i = 0; i < _outputs.noutputs; i++) { printf("%d\t", esc.esc[i].esc_address); printf("%3.2f\t", (double)esc.esc[i].esc_voltage); printf("%3.2f\t", (double)esc.esc[i].esc_current); @@ -669,6 +784,7 @@ UavcanNode::print_info() // Sensor bridges auto br = _sensor_bridges.getHead(); + while (br != nullptr) { printf("Sensor '%s':\n", br->get_name()); br->print_status(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 96b3ec1a60..30d0a363b7 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -47,6 +47,14 @@ #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" + +#include +#include +#include +#include +#include + + /** * @file uavcan_main.hpp * @@ -57,18 +65,40 @@ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 #define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" +#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db" +#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw" +#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" // we add two to allow for actuator_direct and busevent #define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) - /** * A UAVCAN node. */ class UavcanNode : public device::CDev { + static constexpr unsigned MaxBitRatePerSec = 1000000; + static constexpr unsigned bitPerFrame = 148; + static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame; + static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1); + + static constexpr unsigned PollTimeoutMs = 10; + static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why - static constexpr unsigned RxQueueLenPerIface = 64; - static constexpr unsigned StackSize = 3000; + + /* + * This memory is reserved for uavcan to use for queuing CAN frames. + * At 1Mbit there is approximately one CAN frame every 145 uS. + * The number of buffers sets how long you can go without calling + * node_spin_xxxx. Since our task is the only one running and the + * driver will light the fd when there is a CAN frame we can nun with + * a minimum number of buffers to conserver memory. Each buffer is + * 32 bytes. So 5 buffers costs 160 bytes and gives us a poll rate + * of ~1 mS + * 1000000/200 + */ + + static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At + static constexpr unsigned StackSize = 3400; public: typedef uavcan::Node Node; @@ -117,11 +147,19 @@ 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; UavcanEscController _esc_controller; + + uavcan_posix::BasicFileSeverBackend _fileserver_backend; + uavcan::NodeInfoRetriever _node_info_retriever; + uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; + uavcan::BasicFileServer _fw_server; + List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; From a0f2075d5ab3322192fde9729b038e835453af6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 22:44:00 +0200 Subject: [PATCH 17/99] navigator: Decide feasibility of mission based on current position, not home --- src/modules/navigator/mission.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a68f4de012..e36e3fa386 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -297,10 +297,10 @@ Mission::check_dist_1wp() mission_item.nav_cmd == NAV_CMD_TAKEOFF || mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { - /* check distance from home to item */ + /* check distance from current position to item */ float dist_to_1wp = get_distance_to_next_waypoint( mission_item.lat, mission_item.lon, - _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); if (dist_to_1wp < _param_dist_1wp.get()) { _dist_1wp_ok = true; From 2f80ddd11a4c1028d3f795bc655f71ca1f3df054 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 5 Jun 2015 21:45:34 +0200 Subject: [PATCH 18/99] Correct the yaw gains to flyable values. --- ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 8ab65be7b9..bd99e0e554 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -18,10 +18,11 @@ then param set MC_PITCHRATE_P 0.13 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 0.5 + param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.5 param set BAT_N_CELLS 4 fi From 8e935e6fa6917f316b7d9add7e7513c1c814a9a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 May 2015 12:58:44 +0200 Subject: [PATCH 19/99] Add new stabilize mode --- msg/vehicle_status.msg | 6 ++- src/modules/commander/commander.cpp | 50 ++++++++++++++++--- src/modules/commander/px4_custom_mode.h | 42 ++++++++++++++-- .../commander/state_machine_helper.cpp | 6 +++ 4 files changed, 92 insertions(+), 12 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 07484425b3..68e4aa9dc6 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -7,7 +7,8 @@ uint8 MAIN_STATE_AUTO_LOITER = 4 uint8 MAIN_STATE_AUTO_RTL = 5 uint8 MAIN_STATE_ACRO = 6 uint8 MAIN_STATE_OFFBOARD = 7 -uint8 MAIN_STATE_MAX = 8 +uint8 MAIN_STATE_STAB = 8 +uint8 MAIN_STATE_MAX = 9 # If you change the order, add or remove arming_state_t states make sure to update the arrays # in state_machine_helper.cpp as well. @@ -39,7 +40,8 @@ uint8 NAVIGATION_STATE_LAND = 11 # Land mode uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 -uint8 NAVIGATION_STATE_MAX = 15 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_MAX = 16 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 67e5f7a58c..6e7e6c030a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -37,9 +37,9 @@ * * @author Petri Tanskanen * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin */ #include @@ -88,7 +88,7 @@ #include #include #include - #include +#include #include #include @@ -497,6 +497,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s /* ACRO */ main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) { + /* STABILIZED */ + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD); @@ -514,6 +518,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + /* STABILIZED */ + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB); + } else { /* MANUAL */ main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); } @@ -838,6 +845,7 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO"; + main_states_str[vehicle_status_s::MAIN_STATE_STAB] = "STAB"; main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; @@ -851,6 +859,7 @@ int commander_thread_main(int argc, char *argv[]) const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; @@ -1737,7 +1746,10 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && - (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) && + (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || + status.main_state == vehicle_status_s::MAIN_STATE_ACRO || + status.main_state == vehicle_status_s::MAIN_STATE_STAB || + status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1770,7 +1782,8 @@ int commander_thread_main(int argc, char *argv[]) * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ - if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) { + if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) && + (status.main_state != vehicle_status_s::MAIN_STATE_STAB)) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { @@ -1939,6 +1952,7 @@ int commander_thread_main(int argc, char *argv[]) * and both failed we want to terminate the flight */ if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && + status.main_state !=vehicle_status_s::MAIN_STATE_STAB && status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && ((status.data_link_lost && status.gps_failure) || @@ -1963,6 +1977,7 @@ int commander_thread_main(int argc, char *argv[]) * if both failed we want to terminate the flight */ if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || + status.main_state !=vehicle_status_s::MAIN_STATE_STAB || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status.gps_failure) || @@ -2290,6 +2305,17 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + + /* manual mode is stabilized already for multirotors, so switch to acro + * for any non-manual mode + */ + if (status.is_rotary_wing) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); + } else { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); + } + + } else if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_MIDDLE) { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); } else { @@ -2401,6 +2427,18 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case vehicle_status_s::NAVIGATION_STATE_STAB: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index eaf309288f..baf651f08c 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -1,8 +1,41 @@ -/* - * px4_custom_mode.h +/**************************************************************************** * - * Created on: 09.08.2013 - * Author: ton + * Copyright (c) 2013-2015 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 px4_custom_mode.h + * PX4 custom flight modes + * + * @author Anton Babushkin */ #ifndef PX4_CUSTOM_MODE_H_ @@ -17,6 +50,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_MAIN_MODE_ACRO, PX4_CUSTOM_MAIN_MODE_OFFBOARD, + PX4_CUSTOM_MAIN_MODE_STABILIZED }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a2086e9577..4d90091a1d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -299,6 +299,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta switch (new_main_state) { case vehicle_status_s::MAIN_STATE_MANUAL: case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_STAB: ret = TRANSITION_CHANGED; break; @@ -488,6 +489,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en switch (status->main_state) { case vehicle_status_s::MAIN_STATE_ACRO: case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_STAB: case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ @@ -514,6 +516,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; + case vehicle_status_s::MAIN_STATE_STAB: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; + break; + case vehicle_status_s::MAIN_STATE_ALTCTL: status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; break; From 5197be67a7974afa2763b6af618e6a4f09e2c34c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 May 2015 13:08:09 +0200 Subject: [PATCH 20/99] FW control: Add skeleton for distinct altitude control and position control flight modes Conflicts: src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp --- .../fw_att_control/fw_att_control_main.cpp | 26 ++++ .../fw_pos_control_l1_main.cpp | 143 +++++++++++++++++- 2 files changed, 167 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 8b0d452a3e..31778e9c81 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -854,6 +854,32 @@ FixedwingAttitudeControl::task_main() _yaw_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { + + /* the pilot does not want to change direction, + * take straight attitude setpoint from position controller + */ + if (fabsf(_manual.y) < 0.01f) { + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + } else { + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + } + + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + throttle_sp = _att_sp.thrust; + + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } + + } else if (_vcontrol_mode.flag_control_altitude_enabled) { /* * Velocity should be controlled and manual is enabled. */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a50fc53bf7..ec944c8b1d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -165,7 +165,11 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - float _hold_alt; /**< hold altitude for velocity mode */ + float _hold_alt; /**< hold altitude for altitude mode */ + float _hdg_hold_yaw; /**< hold heading for velocity mode */ + bool _hdg_hold_enabled; /**< heading hold enabled */ + struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ + struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** ¤t_positi /* reset hold altitude */ _hold_alt = _global_pos.alt; + /* reset hold yaw */ + _hdg_hold_yaw = _att.yaw; /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -1251,7 +1294,103 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (_control_mode.flag_control_velocity_enabled && _control_mode.flag_control_altitude_enabled) { - /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ + /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed, + heading is set to a distant waypoint */ + + const float deadBand = (60.0f/1000.0f); + const float factor = 1.0f - deadBand; + if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { + /* Need to init because last loop iteration was in a different mode */ + _hold_alt = _global_pos.alt; + _hdg_hold_yaw = _att.yaw; + } + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ + if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { + /* reset integrators */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } + } + _control_mode_current = FW_POSCTRL_MODE_POSITION; + + /* Get demanded airspeed */ + float altctrl_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; + + /* Get demanded vertical velocity from pitch control */ + static bool was_in_deadband = false; + if (_manual.x > deadBand) { + float pitch = (_manual.x - deadBand) / factor; + _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; + was_in_deadband = false; + } else if (_manual.x < - deadBand) { + float pitch = (_manual.x + deadBand) / factor; + _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; + was_in_deadband = false; + } else if (!was_in_deadband) { + /* store altitude at which manual.x was inside deadBand + * The aircraft should immediately try to fly at this altitude + * as this is what the pilot expects when he moves the stick to the center */ + _hold_alt = _global_pos.alt; + was_in_deadband = true; + } + tecs_update_pitch_throttle(_hold_alt, + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed, + TECS_MODE_NORMAL); + + /* heading control */ + + if (fabsf(_manual.y) < 0.01f) { + /* heading / roll is zero, lock onto current heading */ + + // XXX calculate a waypoint in some distance + // and lock on to it + + /* just switched back from non heading-hold to heading hold */ + if (!_hdg_hold_enabled) { + _hdg_hold_enabled = true; + _hdg_hold_yaw = _att.yaw; + + get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); + } + + /* we have a valid heading hold position, are we too close? */ + if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < 1000) { + get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + } + + math::Vector<2> prev_wp; + prev_wp(0) = (float)_hdg_hold_prev_wp.lat; + prev_wp(1) = (float)_hdg_hold_prev_wp.lon; + + math::Vector<2> curr_wp; + curr_wp(0) = (float)_hdg_hold_curr_wp.lat; + curr_wp(1) = (float)_hdg_hold_curr_wp.lon; + + /* populate l1 control setpoint */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + } else { + _hdg_hold_enabled = false; + } + + } else if (_control_mode.flag_control_altitude_enabled) { + /* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; From 0e11f1632c2dcd2f6417ba52cafa96d497fe3030 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 May 2015 21:07:53 +0200 Subject: [PATCH 21/99] MAVLink app: send out right mode flags for new stabilized mode --- src/modules/mavlink/mavlink_messages.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1753aaec4e..a333fb8529 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -130,6 +130,12 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; + case vehicle_status_s::NAVIGATION_STATE_STAB: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED; + break; + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; From 8b6593495c6e433b6e4c2bb6f600a76f85b3f3d2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 23 May 2015 10:20:38 +0200 Subject: [PATCH 22/99] reset waypoints when switching to fw pos_ctrl mode --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ec944c8b1d..d5360df6f0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1303,6 +1303,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; _hdg_hold_yaw = _att.yaw; + _hdg_hold_enabled = false; // this makes sure the waypoints are reset below } /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { From 629002738b67c95fb1d39f2e79eea2b7999182ae Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 23 May 2015 12:29:08 +0200 Subject: [PATCH 23/99] define new mode for altitude --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d5360df6f0..10d1441f64 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -209,6 +209,7 @@ private: enum FW_POSCTRL_MODE { FW_POSCTRL_MODE_AUTO, FW_POSCTRL_MODE_POSITION, + FW_POSCTRL_MODE_ALTITUDE, FW_POSCTRL_MODE_OTHER } _control_mode_current; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode. @@ -1395,7 +1396,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; - if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { + if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) { /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; } @@ -1407,7 +1408,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); } } - _control_mode_current = FW_POSCTRL_MODE_POSITION; + _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; /* Get demanded airspeed */ float altctrl_airspeed = _parameters.airspeed_min + From 032484bd315ee5f958f3eeb9f62ada7aec6e6772 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 23 May 2015 21:23:39 +0200 Subject: [PATCH 24/99] added takeoff protection in altitude controlled modes, code duplication cleanup --- .../fw_pos_control_l1_main.cpp | 114 ++++++++++++------ 1 file changed, 76 insertions(+), 38 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 10d1441f64..5c7a9a2a86 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -166,6 +166,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ float _hold_alt; /**< hold altitude for altitude mode */ + float _ground_alt; /**< ground altitude at which plane was launched */ float _hdg_hold_yaw; /**< hold heading for velocity mode */ bool _hdg_hold_enabled; /**< heading hold enabled */ struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ @@ -180,6 +181,9 @@ private: bool land_onslope; bool land_useterrain; + bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ + hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ + /* takeoff/launch states */ LaunchDetectionResult launch_detection_state; @@ -379,6 +383,17 @@ private: /** * Control position. */ + + /** + * Do takeoff help when in altitude controlled modes + */ + void do_takeoff_help(); + + /** + * Update desired altitude base on user pitch stick input + */ + void 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); @@ -470,6 +485,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), _hold_alt(0.0f), + _ground_alt(0.0f), _hdg_hold_yaw(0.0f), _hdg_hold_enabled(false), _hdg_hold_prev_wp{}, @@ -482,6 +498,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), land_useterrain(false), + _was_in_air(false), + _time_went_in_air(0), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), landingslope(), @@ -923,6 +941,41 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint } } +void 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; + + if (_manual.x > deadBand) { + float pitch = (_manual.x - deadBand) / factor; + _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; + was_in_deadband = false; + } else if (_manual.x < - deadBand) { + float pitch = (_manual.x + deadBand) / factor; + _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; + was_in_deadband = false; + } else if (!was_in_deadband) { + /* store altitude at which manual.x was inside deadBand + * The aircraft should immediately try to fly at this altitude + * as this is what the pilot expects when he moves the stick to the center */ + _hold_alt = _global_pos.alt; + was_in_deadband = true; + } +} + +void FixedwingPositionControl::do_takeoff_help() +{ + const hrt_abstime delta_takeoff = 10000000; + const float throttle_threshold = 0.3f; + const float delta_alt_takeoff = 30.0f; + + /* demand 30 m above ground if user switched into this mode during takeoff */ + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { + _hold_alt = _ground_alt + delta_alt_takeoff; + } +} + bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) @@ -954,6 +1007,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* no throttle limit as default */ float throttle_max = 1.0f; + /* save time when airplane is in air */ + if (!_was_in_air && !_vehicle_status.condition_landed) { + _was_in_air = true; + _time_went_in_air = hrt_absolute_time(); + _ground_alt = _global_pos.alt; + } + /* reset flag when airplane landed */ + if (_vehicle_status.condition_landed) { + _was_in_air = false; + } + if (_control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { /* AUTONOMOUS FLIGHT */ @@ -1298,8 +1362,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed, heading is set to a distant waypoint */ - const float deadBand = (60.0f/1000.0f); - const float factor = 1.0f - deadBand; if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; @@ -1321,23 +1383,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; - /* Get demanded vertical velocity from pitch control */ - static bool was_in_deadband = false; - if (_manual.x > deadBand) { - float pitch = (_manual.x - deadBand) / factor; - _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; - was_in_deadband = false; - } else if (_manual.x < - deadBand) { - float pitch = (_manual.x + deadBand) / factor; - _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; - was_in_deadband = false; - } else if (!was_in_deadband) { - /* store altitude at which manual.x was inside deadBand - * The aircraft should immediately try to fly at this altitude - * as this is what the pilot expects when he moves the stick to the center */ - _hold_alt = _global_pos.alt; - was_in_deadband = true; - } + /* update desired altitude based on user pitch stick input */ + update_desired_altitude(dt); + + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ + do_takeoff_help(); + tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, eas2tas, @@ -1394,8 +1445,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (_control_mode.flag_control_altitude_enabled) { /* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ - const float deadBand = (60.0f/1000.0f); - const float factor = 1.0f - deadBand; if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) { /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; @@ -1415,23 +1464,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; - /* Get demanded vertical velocity from pitch control */ - static bool was_in_deadband = false; - if (_manual.x > deadBand) { - float pitch = (_manual.x - deadBand) / factor; - _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; - was_in_deadband = false; - } else if (_manual.x < - deadBand) { - float pitch = (_manual.x + deadBand) / factor; - _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; - was_in_deadband = false; - } else if (!was_in_deadband) { - /* store altitude at which manual.x was inside deadBand - * The aircraft should immediately try to fly at this altitude - * as this is what the pilot expects when he moves the stick to the center */ - _hold_alt = _global_pos.alt; - was_in_deadband = true; - } + /* update desired altitude based on user pitch stick input */ + update_desired_altitude(dt); + + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ + do_takeoff_help(); + tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, eas2tas, From 126ad2247ec232b522cdaf71e86b8bee85e2d720 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 23:00:58 -0700 Subject: [PATCH 25/99] PX4IO: Code style fixes --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 233fd3e007..e76349642f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1240,7 +1240,7 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; - if (have_armed == OK) { + if (have_armed == OK) { _in_esc_calibration_mode = armed.in_esc_calibration_mode; if (armed.armed || _in_esc_calibration_mode) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; From 4594945d5737cad4f5f5db243b3e2b8980d50c13 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 23:01:19 -0700 Subject: [PATCH 26/99] commander: Forbid override in stabilized mode --- src/modules/commander/commander.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6e7e6c030a..e3386c29f6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2437,6 +2437,8 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; control_mode.flag_control_termination_enabled = false; + /* override is not ok in stabilized mode */ + control_mode.flag_external_manual_override_ok = false; break; case vehicle_status_s::NAVIGATION_STATE_ALTCTL: From ad9b875aea25aa88e83c49e92f0669406e39a817 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 23:04:32 -0700 Subject: [PATCH 27/99] Pos control: Update symbol name --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5c7a9a2a86..17ef22b0b7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1401,7 +1401,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed, - TECS_MODE_NORMAL); + tecs_status_s::TECS_MODE_NORMAL); /* heading control */ From c99489ee9d2b912ef11292678ba7ed8f1e131f95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 23:14:06 -0700 Subject: [PATCH 28/99] FW pos control: Implement hardcore throttle limiting in manual interaction mode --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 17ef22b0b7..18698f798a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1389,13 +1389,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ do_takeoff_help(); + /* throttle limiting */ + throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < 0.05f) { + throttle_max = 0.0f; + } + tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, - _parameters.throttle_max, + throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), @@ -1470,13 +1476,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ do_takeoff_help(); + /* throttle limiting */ + throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < 0.05f) { + throttle_max = 0.0f; + } + tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, - _parameters.throttle_max, + throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), From 21fde4b3f6a32fa8f467bfdc80d9eead15fb8cf9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 29 May 2015 10:30:16 -0700 Subject: [PATCH 29/99] fw att control: Code style fixes --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 31778e9c81..d685631125 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1100,8 +1100,8 @@ FixedwingAttitudeControl::task_main() /* Only publish if any of the proper modes are enabled */ if(_vcontrol_mode.flag_control_rates_enabled || - _vcontrol_mode.flag_control_attitude_enabled || - _vcontrol_mode.flag_control_manual_enabled) + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub > 0) { From 46d969772cc672dee7ca9d0b238ed90b4abb6c51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 29 May 2015 11:53:34 -0700 Subject: [PATCH 30/99] Fixed wing heading hold: Engage only after wings got close to level already --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index d685631125..b7194461e6 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -858,7 +858,7 @@ FixedwingAttitudeControl::task_main() /* the pilot does not want to change direction, * take straight attitude setpoint from position controller */ - if (fabsf(_manual.y) < 0.01f) { + if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; } else { roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 18698f798a..4e4a3356f5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1411,12 +1411,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* heading control */ - if (fabsf(_manual.y) < 0.01f) { + if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { /* heading / roll is zero, lock onto current heading */ - // XXX calculate a waypoint in some distance - // and lock on to it - /* just switched back from non heading-hold to heading hold */ if (!_hdg_hold_enabled) { _hdg_hold_enabled = true; From f6dc9c972760d388f740d41edb3bc060af7b6e66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 29 May 2015 11:54:38 -0700 Subject: [PATCH 31/99] multicopter manual attitude control: Leave some margin for yaw control --- src/modules/mc_att_control/mc_att_control_main.cpp | 3 ++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 97d8625b02..4136107414 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -97,6 +97,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f +#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f class MulticopterAttitudeControl { @@ -831,7 +832,7 @@ MulticopterAttitudeControl::task_main() if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); - _thrust_sp = _manual_control_sp.z; + _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index e5ea4d1047..7a3a5a679b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -83,6 +83,7 @@ #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f #define MIN_DIST 0.01f +#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f /** * Multicopter position control app start / stop handling function @@ -1387,7 +1388,7 @@ MulticopterPositionControl::task_main() //Control climb rate directly if no aiding altitude controller is active if(!_control_mode.flag_control_climb_rate_enabled) { - _att_sp.thrust = _manual.z; + _att_sp.thrust = math::min(_manual.z, MANUAL_THROTTLE_MAX_MULTICOPTER); } //Construct attitude setpoint rotation matrix From 7f07e4306ae66e37cd3ce1499c6e4f9a893bbe5c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 30 May 2015 19:07:14 -0700 Subject: [PATCH 32/99] commander: Fix non-existing middle switch position for acro switch --- src/modules/commander/commander.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e3386c29f6..7bf67775d1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2315,9 +2315,6 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); } - } else if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_MIDDLE) { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); - } else { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); } From e5e4db1923f865e189c4909e0efa918ec3fb06c8 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 2 Jun 2015 14:25:27 +0200 Subject: [PATCH 33/99] POSCTRL: - make sure we always intialize the waypoints on the desired path and that the previous waypoint is behind the plane - replace magic numbers with defines --- .../fw_pos_control_l1_main.cpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 4e4a3356f5..f1f09473eb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -95,6 +95,10 @@ static int _control_task = -1; /**< task handle for sensor task */ +#define HDG_HOLD_DIST_NEXT 3000.0f +#define HDG_HOLD_REACHED_DIST 1000.0f +#define HDG_HOLD_SET_BACK_DIST 100.0f + /** * L1 control app start / stop handling function @@ -911,14 +915,17 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa waypoint_prev.lat = _global_pos.lat; waypoint_prev.lon = _global_pos.lon; } else { - // use waypoint which is still in front of us - waypoint_prev.lat = waypoint_next.lat; - waypoint_prev.lon = waypoint_next.lon; + /* + for previous waypoint use the one still in front of us but shift it such that it is + located on the desired flight path but HDG_HOLD_SET_BACK_DIST behind us + */ + waypoint_prev.lat = waypoint_next.lat - cos(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_prev.lon = waypoint_next.lon - sin(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6; } waypoint_next.valid = true; - waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)distance / 1e6; - waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)distance / 1e6; + waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6; waypoint_next.alt = _hold_alt; } @@ -1424,8 +1431,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* we have a valid heading hold position, are we too close? */ if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, - _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < 1000) { - get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { + get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); } math::Vector<2> prev_wp; From 1f3c5d00e43b1aebe765290e138094b9f59d4577 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 4 Jun 2015 08:28:43 -0600 Subject: [PATCH 34/99] fix posctl heading hold --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f1f09473eb..93244b0bb4 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -912,8 +912,8 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa if (flag_init) { // on init set first waypoint to momentary position - waypoint_prev.lat = _global_pos.lat; - waypoint_prev.lon = _global_pos.lon; + waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; } else { /* for previous waypoint use the one still in front of us but shift it such that it is From ef6e07fc9bf6f4cdf51f74c930b57d8becad185d Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 5 Jun 2015 16:01:52 +0200 Subject: [PATCH 35/99] fixed wing position control: - only lock on yaw if yawrate is below threshold - remove more magic numbers --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 93244b0bb4..1b4d7a2e15 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -94,10 +94,13 @@ #include static int _control_task = -1; /**< task handle for sensor task */ +#define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode +#define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode +#define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane +#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 HDG_HOLD_DIST_NEXT 3000.0f -#define HDG_HOLD_REACHED_DIST 1000.0f -#define HDG_HOLD_SET_BACK_DIST 100.0f +#define THROTTLE_THRESH 0.05f // max throttle from user which will not lead to motors spinning up in altitude controlled modes /** @@ -1398,7 +1401,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* throttle limiting */ throttle_max = _parameters.throttle_max; - if (fabsf(_manual.z) < 0.05f) { + if (fabsf(_manual.z) < THROTTLE_THRESH) { throttle_max = 0.0f; } @@ -1418,7 +1421,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* heading control */ - if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { + if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH && fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH) { /* heading / roll is zero, lock onto current heading */ /* just switched back from non heading-hold to heading hold */ @@ -1482,7 +1485,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* throttle limiting */ throttle_max = _parameters.throttle_max; - if (fabsf(_manual.z) < 0.05f) { + if (fabsf(_manual.z) < THROTTLE_THRESH) { throttle_max = 0.0f; } From e9634fbe47e41b12bb8751d52f43d648c0f6b704 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 5 Jun 2015 21:18:25 -0600 Subject: [PATCH 36/99] suppress preflight check failure for HIL autostart_ids --- src/modules/commander/commander.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 67e5f7a58c..acf41e741d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1134,11 +1134,19 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); - if (!status.condition_system_sensors_initialized) { - set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune - } - else { + param_get(_param_autostart_id, &autostart_id); + if (autostart_id > 1999) { + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); + if (!status.condition_system_sensors_initialized) { + set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune + } + else { + set_tune_override(TONE_STARTUP_TUNE); //normal boot tune + } + } else { + // HIL configuration selected: real sensors will be disabled + warnx("autostart_id: %d", autostart_id); + status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } @@ -1311,7 +1319,7 @@ int commander_thread_main(int argc, char *argv[]) /* provide RC and sensor status feedback to the user */ (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -2781,7 +2789,7 @@ void *commander_low_prio_loop(void *arg) } status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); From 9588e6fc6875c934aeb3e7b6de8af3a609e0d5ed Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 5 Jun 2015 21:23:35 -0600 Subject: [PATCH 37/99] whitespace --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index acf41e741d..c9af11920d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1319,7 +1319,7 @@ int commander_thread_main(int argc, char *argv[]) /* provide RC and sensor status feedback to the user */ (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -2789,7 +2789,7 @@ void *commander_low_prio_loop(void *arg) } status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); From 6ed43cb3a4d36c15345332652850670215914cd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 09:45:07 +0200 Subject: [PATCH 38/99] commander: Allow to disarm via switch in HIL --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c2816fd93f..ca4a30196b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1400,7 +1400,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ - if (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { + if (safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : vehicle_status_s::ARMING_STATE_STANDBY_ERROR); @@ -1411,7 +1411,7 @@ int commander_thread_main(int argc, char *argv[]) } } - //Notify the user if the status of the safety switch changes + /* notify the user if the status of the safety switch changes */ if (safety.safety_switch_available && previous_safety_off != safety.safety_off) { if (safety.safety_off) { From 7d2536f2dd4295726b1aae4e1f26b2dddc68c487 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 09:45:50 +0200 Subject: [PATCH 39/99] ROMFS: Allocate only 12K buffer for sdlog, now that we do only log at 100 Hz --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 6bfbfea396..5d374745a1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -11,7 +11,7 @@ then then fi else - if sdlog2 start -r 100 -a -b 22 -t + if sdlog2 start -r 100 -a -b 12 -t then fi fi From 9af8ba49ac6a8c43fb9a782c3bb0a2d91ea75e10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 11:35:09 +0200 Subject: [PATCH 40/99] HIL fix: Also publish filtered airspeed --- src/modules/mavlink/mavlink_receiver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 834c8398a1..7ede93ec2d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1292,6 +1292,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.baro_timestamp = timestamp; hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa + hil_sensors.differential_pressure_filtered_pa = hil_sensors.differential_pressure_pa; hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ From 3e0c14dea28d6b5fec3eaa05ab9681ef82545159 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 11:35:29 +0200 Subject: [PATCH 41/99] MAVLink FTP: Do not list hidden directories by default --- src/modules/mavlink/mavlink_ftp.cpp | 5 +++-- src/modules/mavlink/mavlink_ftp.h | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 42cabb4ba6..54cb1b186a 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -299,7 +299,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req) /// @brief Responds to a List command MavlinkFTP::ErrorCode -MavlinkFTP::_workList(PayloadHeader* payload) +MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden) { char dirPath[kMaxDataLength]; strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); @@ -375,7 +375,8 @@ MavlinkFTP::_workList(PayloadHeader* payload) } break; case DTYPE_DIRECTORY: - if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + if ((!list_hidden && (strncmp(entry.d_name, ".", 1) == 0)) || + strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { // Don't bother sending these back direntType = kDirentSkip; } else { diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 33b8996f71..998c6b3cc6 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -130,7 +130,7 @@ private: void _reply(mavlink_file_transfer_protocol_t* ftp_req); int _copy_file(const char *src_path, const char *dst_path, size_t length); - ErrorCode _workList(PayloadHeader *payload); + ErrorCode _workList(PayloadHeader *payload, bool list_hidden = false); ErrorCode _workOpen(PayloadHeader *payload, int oflag); ErrorCode _workRead(PayloadHeader *payload); ErrorCode _workBurst(PayloadHeader* payload, uint8_t target_system_id); From b7986e6fdd103064128d0933f7cb32ab4252159b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 12:43:45 +0200 Subject: [PATCH 42/99] land detector: Improve performance for fixed wing setups --- .../land_detector/FixedwingLandDetector.cpp | 14 ++++++---- src/modules/land_detector/LandDetector.h | 2 +- .../land_detector/land_detector_params.c | 27 ++++++++++++++++--- 3 files changed, 34 insertions(+), 9 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index e26954d1a6..6e8393617e 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -84,11 +84,15 @@ bool FixedwingLandDetector::update() const uint64_t now = hrt_absolute_time(); bool landDetected = false; - // TODO: reset filtered values on arming? - _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { + _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + } + + if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + } // crude land detector for fixedwing if (_velocity_xy_filtered < _params.maxVelocity diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index b2984003b9..68f96288cd 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -87,7 +87,7 @@ protected: virtual void initialize() = 0; /** - * @brief Convinience function for polling uORB subscriptions + * @brief Convenience function for polling uORB subscriptions * @return true if there was new data and it was successfully copied **/ bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 9cf57b5fc9..b670dcc035 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -45,6 +45,8 @@ * * Maximum vertical velocity allowed to trigger a land (m/s up and down) * + * @unit m/s + * * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); @@ -54,6 +56,8 @@ PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); * * Maximum horizontal velocity allowed to trigger a land (m/s) * + * @unit m/s + * * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); @@ -63,6 +67,8 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); * * Maximum allowed around each axis to trigger a land (degrees per second) * + * @unit deg/s + * * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); @@ -72,6 +78,9 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); * * Maximum actuator output on throttle before triggering a land * + * @min 0.1 + * @max 0.5 + * * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); @@ -81,24 +90,36 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); * * Maximum horizontal velocity allowed to trigger a land (m/s) * + * @min 0.5 + * @max 10 + * @unit m/s + * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.40f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 4.0f); /** * Fixedwing max climb rate * * Maximum vertical velocity allowed to trigger a land (m/s up and down) * + * @min 5 + * @max 20 + * @unit m/s + * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); /** * Airspeed max * * Maximum airspeed allowed to trigger a land (m/s) * + * @min 4 + * @max 20 + * @unit m/s + * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 8.00f); From ab61ebca2afa49936802c86abeb9547f8685f336 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 10:53:23 +0200 Subject: [PATCH 43/99] Revert "commander: Allow to disarm via switch in HIL" This reverts commit 6ed43cb3a4d36c15345332652850670215914cd2. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ca4a30196b..c2816fd93f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1400,7 +1400,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ - if (safety.safety_switch_available && !safety.safety_off && armed.armed) { + if (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : vehicle_status_s::ARMING_STATE_STANDBY_ERROR); @@ -1411,7 +1411,7 @@ int commander_thread_main(int argc, char *argv[]) } } - /* notify the user if the status of the safety switch changes */ + //Notify the user if the status of the safety switch changes if (safety.safety_switch_available && previous_safety_off != safety.safety_off) { if (safety.safety_off) { From 05f935cd77355ffde9e91fe340ffa6b773df09c7 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 6 Jun 2015 07:56:48 -0600 Subject: [PATCH 44/99] inhibit more sensor checks --- src/modules/commander/commander.cpp | 13 ++++++++++--- src/modules/commander/state_machine_helper.cpp | 3 ++- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c2816fd93f..d48cccd3ed 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1154,7 +1154,7 @@ int commander_thread_main(int argc, char *argv[]) } } else { // HIL configuration selected: real sensors will be disabled - warnx("autostart_id: %d", autostart_id); + warnx("HIL configuration; autostart_id: %d, onboard IMU sensors disabled", autostart_id); status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } @@ -1327,8 +1327,15 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + if (autostart_id > 1999) { + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + } else { + /* HIL configuration: check only RC input */ + warnx("new telemetry link connected: checking RC status"); + (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false); + } } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4d90091a1d..92f73ca21f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -125,7 +125,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s int prearm_ret = OK; /* only perform the check if we have to */ - if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED + && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = prearm_check(status, mavlink_fd); } From 02efa5a24cfc9bf4a51c922526856c9a205cb177 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 22:13:35 +0200 Subject: [PATCH 45/99] commander: Better text feedback --- src/modules/commander/commander.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d48cccd3ed..2b6933c27a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1695,7 +1695,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.gps_failure && !gpsIsNoisy) { status.gps_failure = false; status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); + mavlink_log_critical(mavlink_fd, "gps fix regained"); } } else if (!status.gps_failure) { @@ -1729,12 +1729,12 @@ int commander_thread_main(int argc, char *argv[]) if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination"); flight_termination_printed = true; } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + mavlink_log_critical(mavlink_fd, "Flight termination active"); } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination @@ -1744,7 +1744,7 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_info(mavlink_fd, "Detected RC signal first time"); + mavlink_log_info(mavlink_fd, "Detected radio control"); status_changed = true; } else { From d40f94bf26a89c349f5d7633434d3721b4720170 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 7 Jun 2015 22:27:31 +0200 Subject: [PATCH 46/99] fixed wing posctrl: - lock desired yaw once yaw speed is small --- .../fw_pos_control_l1_main.cpp | 62 +++++++++++-------- 1 file changed, 37 insertions(+), 25 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 1b4d7a2e15..733cdc51d3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -176,6 +176,7 @@ private: float _ground_alt; /**< ground altitude at which plane was launched */ float _hdg_hold_yaw; /**< hold heading for velocity mode */ bool _hdg_hold_enabled; /**< heading hold enabled */ + bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** ¤t_positi _hold_alt = _global_pos.alt; _hdg_hold_yaw = _att.yaw; _hdg_hold_enabled = false; // this makes sure the waypoints are reset below + _yaw_lock_engaged = false; } /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { @@ -1421,38 +1424,47 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* heading control */ - if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH && fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH) { + if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ + if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { + // little yaw movement, lock to current heading + _yaw_lock_engaged = true; - /* just switched back from non heading-hold to heading hold */ - if (!_hdg_hold_enabled) { - _hdg_hold_enabled = true; - _hdg_hold_yaw = _att.yaw; - - get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); } - /* we have a valid heading hold position, are we too close? */ - if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, - _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { - get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + if (_yaw_lock_engaged) { + + /* just switched back from non heading-hold to heading hold */ + if (!_hdg_hold_enabled) { + _hdg_hold_enabled = true; + _hdg_hold_yaw = _att.yaw; + + get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); + } + + /* we have a valid heading hold position, are we too close? */ + if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { + get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + } + + math::Vector<2> prev_wp; + prev_wp(0) = (float)_hdg_hold_prev_wp.lat; + prev_wp(1) = (float)_hdg_hold_prev_wp.lon; + + math::Vector<2> curr_wp; + curr_wp(0) = (float)_hdg_hold_curr_wp.lat; + curr_wp(1) = (float)_hdg_hold_curr_wp.lon; + + /* populate l1 control setpoint */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); } - - math::Vector<2> prev_wp; - prev_wp(0) = (float)_hdg_hold_prev_wp.lat; - prev_wp(1) = (float)_hdg_hold_prev_wp.lon; - - math::Vector<2> curr_wp; - curr_wp(0) = (float)_hdg_hold_curr_wp.lat; - curr_wp(1) = (float)_hdg_hold_curr_wp.lon; - - /* populate l1 control setpoint */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); - - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); } else { _hdg_hold_enabled = false; + _yaw_lock_engaged = false; } } else if (_control_mode.flag_control_altitude_enabled) { From aec4f359acd7320ff3193b46e41e0c6b2843adcf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 22:06:26 +0200 Subject: [PATCH 47/99] Caipi config: Fix maintainer --- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 5a7d73c060..d984035675 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -2,7 +2,7 @@ # # TBS Caipirinha # -# Thomas Gubler +# Lorenz Meier # sh /etc/init.d/rc.fw_defaults @@ -29,6 +29,7 @@ then param set FW_RR_I 0.01 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 + param set SYS_COMPANION 157600 fi set MIXER caipi From 6309aa612be30e4b98dbec7d4dde5068bce17839 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 22:06:48 +0200 Subject: [PATCH 48/99] MAVLink app: Introduce OSD mode --- src/modules/mavlink/mavlink_main.cpp | 13 ++++++++++++- src/modules/mavlink/mavlink_main.h | 3 ++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 348185e693..5cba6c644e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1295,6 +1295,8 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MAVLINK_MODE_ONBOARD; } else if (strcmp(optarg, "onboard") == 0) { _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(optarg, "osd") == 0) { + _mode = MAVLINK_MODE_OSD; } break; @@ -1451,7 +1453,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 50.0f); configure_stream("HIGHRES_IMU", 50.0f); - configure_stream("VFR_HUD", 5.0f); configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); @@ -1468,6 +1469,16 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); break; + case MAVLINK_MODE_OSD: + configure_stream("SYS_STATUS", 5.0f); + configure_stream("ATTITUDE", 30.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 10.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("BATTERY_STATUS", 1.0f); + configure_stream("SYSTEM_TIME", 1.0f); + break; + default: break; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 71749e2923..cf95c4d40c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -128,7 +128,8 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_ONBOARD + MAVLINK_MODE_ONBOARD, + MAVLINK_MODE_OSD }; void set_mode(enum MAVLINK_MODE); From 0083d6e732eb61bbe6075eb70475c25d32362a32 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 22:07:03 +0200 Subject: [PATCH 49/99] systemlib: Update system param names --- src/modules/systemlib/system_params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 17ce65d13c..275d6dca45 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -88,9 +88,9 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); * Companion computer interface * * Configures the baud rate of the companion computer interface. -* Set to zero to disable, set to 921600 to enable. -* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for -* other baud rates. +* Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!) +* 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1. +* 157600: enables OSD mode at 57600 baud, 8N1. * * @min 0 * @max 921600 From f22fdc5b0b65d17bf7a030a2726f02415cdbc1ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 22:07:23 +0200 Subject: [PATCH 50/99] ROMFS: Support for new autostart IDs --- ROMFS/px4fmu_common/init.d/rcS | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2dfe3d8e30..13b35313e7 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -484,6 +484,10 @@ then then mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 fi + if param compare SYS_COMPANION 157600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000 + fi fi # UAVCAN From c798b1165aa9919409c7ba5d7c339c5c754dd990 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 11:25:25 +0200 Subject: [PATCH 51/99] MAVLink app: Complete OSD config --- src/modules/mavlink/mavlink_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5cba6c644e..872406775b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1471,12 +1471,14 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_OSD: configure_stream("SYS_STATUS", 5.0f); - configure_stream("ATTITUDE", 30.0f); + configure_stream("ATTITUDE", 25.0f); + configure_stream("VFR_HUD", 5.0f); configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 10.0f); configure_stream("ATTITUDE_TARGET", 10.0f); configure_stream("BATTERY_STATUS", 1.0f); configure_stream("SYSTEM_TIME", 1.0f); + configure_stream("RC_CHANNELS_RAW", 5.0f); break; default: From cb2ddbe57ba937625a7de0cf039b22f3ef9437ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 14:28:53 +0200 Subject: [PATCH 52/99] Caipi Mixer: Fix directions --- ROMFS/px4fmu_common/mixers/caipi.main.mix | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/caipi.main.mix b/ROMFS/px4fmu_common/mixers/caipi.main.mix index cfc49ff9f4..e9d730449a 100644 --- a/ROMFS/px4fmu_common/mixers/caipi.main.mix +++ b/ROMFS/px4fmu_common/mixers/caipi.main.mix @@ -25,13 +25,13 @@ for the elevons. M: 2 O: 10000 10000 -3000 -10000 10000 -S: 0 0 -9000 -9000 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 +S: 0 0 8000 8000 0 -10000 10000 +S: 0 1 9000 9000 0 -10000 10000 M: 2 O: 10000 10000 3000 -10000 10000 -S: 0 0 -9000 -9000 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 +S: 0 0 8000 8000 0 -10000 10000 +S: 0 1 -9000 -9000 0 -10000 10000 Output 2 -------- From 2903e350a74348653fa370dec3747c4dba02bd40 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 14:45:48 +0200 Subject: [PATCH 53/99] Caipi: Fix mixer and reverse params --- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 2 ++ ROMFS/px4fmu_common/mixers/caipi.main.mix | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index d984035675..1fd96d6d3d 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -30,6 +30,8 @@ then param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 param set SYS_COMPANION 157600 + param set PWM_MAIN_REV0 1 + param set PWM_MAIN_REV1 1 fi set MIXER caipi diff --git a/ROMFS/px4fmu_common/mixers/caipi.main.mix b/ROMFS/px4fmu_common/mixers/caipi.main.mix index e9d730449a..af053deab4 100644 --- a/ROMFS/px4fmu_common/mixers/caipi.main.mix +++ b/ROMFS/px4fmu_common/mixers/caipi.main.mix @@ -24,12 +24,12 @@ The scaling factor for roll inputs is adjusted to implement differential travel for the elevons. M: 2 -O: 10000 10000 -3000 -10000 10000 +O: 10000 10000 3000 -10000 10000 S: 0 0 8000 8000 0 -10000 10000 S: 0 1 9000 9000 0 -10000 10000 M: 2 -O: 10000 10000 3000 -10000 10000 +O: 10000 10000 -3000 -10000 10000 S: 0 0 8000 8000 0 -10000 10000 S: 0 1 -9000 -9000 0 -10000 10000 From 9bbb315144f6b27cb266763c1760f7095c671b7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 15:19:41 +0200 Subject: [PATCH 54/99] commander: Print home position --- src/modules/commander/commander.cpp | 28 ++++++++++++---------------- 1 file changed, 12 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2b6933c27a..7b92942b33 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -184,6 +184,7 @@ static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; static struct offboard_control_mode_s offboard_control_mode; +static struct home_position_s _home; /** * The daemon app only briefly exists to start @@ -376,6 +377,8 @@ void print_status() warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion"); warnx("usb powered: %s", (status.usb_connected) ? "yes" : "no"); warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", _home.lat, _home.lon, (double)_home.alt); + warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z); /* read all relevant states */ int state_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -957,8 +960,7 @@ int commander_thread_main(int argc, char *argv[]) /* home position */ orb_advert_t home_pub = -1; - struct home_position_s home; - memset(&home, 0, sizeof(home)); + memset(&_home, 0, sizeof(_home)); /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ orb_advert_t mission_pub = -1; @@ -1955,7 +1957,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &home_pub)) { status_changed = true; } } @@ -2017,12 +2019,12 @@ int commander_thread_main(int argc, char *argv[]) //First time home position update if (!status.condition_home_position_valid) { - commander_set_home_position(home_pub, home, local_position, global_position); + commander_set_home_position(home_pub, _home, local_position, global_position); } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { - commander_set_home_position(home_pub, home, local_position, global_position); + commander_set_home_position(home_pub, _home, local_position, global_position); } /* print new state */ @@ -2039,14 +2041,6 @@ int commander_thread_main(int argc, char *argv[]) mission_result.finished, mission_result.stay_in_failsafe); - // TODO handle mode changes by commands - if (main_state_changed) { - status_changed = true; - warnx("main state: %s", main_states_str[status.main_state]); - mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); - main_state_changed = false; - } - if (status.failsafe != failsafe_old) { status_changed = true; @@ -2060,10 +2054,12 @@ int commander_thread_main(int argc, char *argv[]) failsafe_old = status.failsafe; } - if (nav_state_changed) { + // TODO handle mode changes by commands + if (main_state_changed || nav_state_changed) { status_changed = true; - warnx("nav state: %s", nav_states_str[status.nav_state]); - mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]); + warnx("main state: %s nav state: %s", main_states_str[status.main_state], nav_states_str[status.nav_state]); + mavlink_log_info(mavlink_fd, "Flight mode: %s", nav_states_str[status.nav_state]); + main_state_changed = false; } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ From ac215fe2cba70e32af884f23d789ce6b8f6075c3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 8 Jun 2015 15:47:40 +0200 Subject: [PATCH 55/99] allow to give away some thrust for yaw control --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 4d9d60c2d8..aa0a8e7418 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -312,6 +312,9 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) } } else if(out > 1.0f) { + // allow to reduce thrust to get some yaw response + float thrust_reduction = fminf(0.15f, out - 1.0f); + thrust -= thrust_reduction; yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale; if(status_reg != NULL) { From c7be59038c0aafeb25c32159bf850bd69fc47a79 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 10:16:24 +0200 Subject: [PATCH 56/99] L1 pos control: Publish timestamp when setting nav capabilities --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 1b4d7a2e15..b882cc08c4 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -900,6 +900,8 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c void FixedwingPositionControl::navigation_capabilities_publish() { + _nav_capabilities.timestamp = hrt_absolute_time(); + if (_nav_capabilities_pub > 0) { orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); } else { From 0f3438eb17a9222cbe43e640269fa5fe739b49f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 10:16:51 +0200 Subject: [PATCH 57/99] Navigator: Obey minimum turn radius the controller is capabable of. --- src/modules/navigator/navigator.h | 2 +- src/modules/navigator/navigator_main.cpp | 14 ++++++++++++++ 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d2acce7899..83f3487a00 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -144,7 +144,7 @@ public: Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } - float get_acceptance_radius() { return _param_acceptance_radius.get(); } + float get_acceptance_radius(); int get_mavlink_fd() { return _mavlink_fd; } private: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 295172ebb2..4bd7c08be0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -571,6 +571,20 @@ Navigator::publish_position_setpoint_triplet() } } +float +Navigator::get_acceptance_radius() +{ + float radius = _param_acceptance_radius.get(); + + if (hrt_elapsed_time(&_nav_caps.timestamp) < 1000000) { + if (_nav_caps.turn_distance > radius) { + radius = _nav_caps.turn_distance; + } + } + + return radius; +} + void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); From b2a694f71dcc246edd32c7b8befa0b3dd6f4879d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 10:34:17 +0200 Subject: [PATCH 58/99] navigator: Use the controller radius also as lower bound for mission items --- src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/navigator/navigator.h | 15 +++++++++++++++ src/modules/navigator/navigator_main.cpp | 8 +++++++- 3 files changed, 24 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index dce7d45171..42c74428ad 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -123,12 +123,12 @@ MissionBlock::is_mission_item_reached() * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ - if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) { _waypoint_position_reached = true; } } else { /* for normal mission items used their acceptance radius */ - if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { _waypoint_position_reached = true; } } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 83f3487a00..95e3f9f964 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -144,7 +144,22 @@ public: Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } + + /** + * Get the acceptance radius + * + * @return the distance at which the next waypoint should be used + */ float get_acceptance_radius(); + + /** + * Get the acceptance radius given the mission item preset radius + * + * @param mission_item_radius the radius to use in case the controller-derived radius is smaller + * + * @return the distance at which the next waypoint should be used + */ + float get_acceptance_radius(float mission_item_radius); int get_mavlink_fd() { return _mavlink_fd; } private: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4bd7c08be0..e05d079a0a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -574,7 +574,13 @@ Navigator::publish_position_setpoint_triplet() float Navigator::get_acceptance_radius() { - float radius = _param_acceptance_radius.get(); + return get_acceptance_radius(_param_acceptance_radius.get()); +} + +float +Navigator::get_acceptance_radius(float mission_item_radius) +{ + float radius = mission_item_radius; if (hrt_elapsed_time(&_nav_caps.timestamp) < 1000000) { if (_nav_caps.turn_distance > radius) { From 1fb743412810727d18f2815f541be1f75c11bfeb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 19:20:42 +0200 Subject: [PATCH 59/99] Navigation capabilites: Ensure regular publication of updated topic --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 ++- src/modules/navigator/navigator_main.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index b882cc08c4..350706f263 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1676,7 +1676,8 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) { + if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON + && turn_distance > 0)) { /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e05d079a0a..97baa1235f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -582,7 +582,7 @@ Navigator::get_acceptance_radius(float mission_item_radius) { float radius = mission_item_radius; - if (hrt_elapsed_time(&_nav_caps.timestamp) < 1000000) { + if (hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) { if (_nav_caps.turn_distance > radius) { radius = _nav_caps.turn_distance; } From 1ecbf674aaffdbc6cfa7e3f30959283c9e8dba3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 7 Jun 2015 11:51:06 +0200 Subject: [PATCH 60/99] navigator: Finish rework of switch distance to account for vehicle dynamics --- src/modules/navigator/mission.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index e36e3fa386..98b01c7578 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -586,7 +586,7 @@ Mission::altitude_sp_foh_update() } /* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */ - if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) { + if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < 0.0f) { return; } @@ -608,7 +608,7 @@ Mission::altitude_sp_foh_update() /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ - if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) { + if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { pos_sp_triplet->current.alt = _mission_item.altitude; } else { /* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp @@ -617,7 +617,7 @@ Mission::altitude_sp_foh_update() * radius around the current waypoint **/ float delta_alt = (_mission_item.altitude - _mission_item_previous_alt); - float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius); + float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius)); float a = _mission_item_previous_alt - grad * _distance_current_previous; pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; From f02ffa5a907b301cdaf21f5f5b4de7035f52888c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 7 Jun 2015 11:51:44 +0200 Subject: [PATCH 61/99] Att / Pos EKF: Fix handling of altitude initialization for local frame --- .../AttitudePositionEstimatorEKF.h | 3 +- .../ekf_att_pos_estimator_main.cpp | 42 +++++++++++-------- 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 4d5e56a961..6a9e3aadbc 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -174,7 +174,7 @@ private: struct map_projection_reference_s _pos_ref; - float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ + float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ hrt_abstime _last_debug_print = 0; @@ -193,7 +193,6 @@ private: bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received bool _baro_init; - float _baroAltRef; bool _gps_initialized; hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index f3fe048e78..46f278b7d4 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -153,7 +153,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _sensor_combined {}, _pos_ref{}, - _baro_ref_offset(0.0f), + _filter_ref_offset(0.0f), _baro_gps_offset(0.0f), /* performance counters */ @@ -173,7 +173,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _gpsIsGood(false), _previousGPSTimestamp(0), _baro_init(false), - _baroAltRef(0.0f), _gps_initialized(false), _filter_start_time(0), _last_sensor_timestamp(0), @@ -404,6 +403,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Count the reset condition perf_count(_perf_reset); + _filter_ref_offset = _ekf->states[9]; } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); @@ -585,6 +585,7 @@ void AttitudePositionEstimatorEKF::task_main() _baro_init = false; _gps_initialized = false; + _last_sensor_timestamp = hrt_absolute_time(); _last_run = _last_sensor_timestamp; @@ -643,12 +644,13 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->posNE[1] = 0.0f; _local_pos.ref_alt = 0.0f; - _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _baro_alt_filt = _baro.altitude; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + _filter_ref_offset = -_baro.altitude; + } else { if (!_gps_initialized && _gpsIsGood) { @@ -711,7 +713,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame + _filter_ref_offset = _ekf->states[9]; // this should become zero in the local frame // init filtered gps and baro altitudes _gps_alt_filt = gps_alt; @@ -750,7 +752,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, - (double)_baro_ref_offset); + (double)_filter_ref_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); #endif @@ -811,7 +813,8 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef; + _local_pos.z = _ekf->states[9] - _filter_ref_offset; + //_local_pos.z_stable = _ekf->states[9]; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; @@ -859,7 +862,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = (-_local_pos.z) - _baro_gps_offset; + _global_pos.alt = (-_local_pos.z) - _filter_ref_offset - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -1070,8 +1073,9 @@ void AttitudePositionEstimatorEKF::print_status() // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); - printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); - printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset, + printf("alt RAW: baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)_ekf->gpsHgt); + printf("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)\n", (double)(_local_pos.z), (double)_global_pos.alt); + printf("filter ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_filter_ref_offset, (double)_baro_gps_offset); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); @@ -1400,9 +1404,9 @@ void AttitudePositionEstimatorEKF::pollData() } baro_last = _baro.timestamp; - if(!_baro_init) { + if (!_baro_init) { _baro_init = true; - _baroAltRef = _baro.altitude; + _baro_alt_filt = _baro.altitude; } _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); @@ -1494,30 +1498,34 @@ int AttitudePositionEstimatorEKF::trip_nan() float nan_val = 0.0f / 0.0f; - warnx("system not armed, tripping state vector with NaN values"); + warnx("system not armed, tripping state vector with NaN"); _ekf->states[5] = nan_val; usleep(100000); - warnx("tripping covariance #1 with NaN values"); + warnx("tripping covariance #1 with NaN"); _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #2 with NaN values"); + warnx("tripping covariance #2 with NaN"); _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #3 with NaN values"); + warnx("tripping covariance #3 with NaN"); _ekf->P[3][3] = nan_val; // covariance matrix usleep(100000); - warnx("tripping Kalman gains with NaN values"); + warnx("tripping Kalman gains with NaN"); _ekf->Kfusion[0] = nan_val; // Kalman gains usleep(100000); - warnx("tripping stored states[0] with NaN values"); + warnx("tripping stored states[0] with NaN"); _ekf->storedStates[0][0] = nan_val; usleep(100000); + warnx("tripping states[9] with NaN"); + _ekf->states[9] = nan_val; + usleep(100000); + warnx("\nDONE - FILTER STATE:"); print_status(); } From e1ecac078d6f999722a1210ee983e2e77c8b1590 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 11:23:18 +0200 Subject: [PATCH 62/99] EKF: Harden GPS offset filter value for HIL --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 46f278b7d4..d408ea2ddb 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1321,7 +1321,11 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD)); // update LPF - _gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + float filter_step += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + + if (isfinite(filter_step)) { + _gps_alt_filt += filter_step; + } } //check if we had a GPS outage for a long time From fe09e53b5bb7fe9f7d9734b23f3fa96145139d44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 14:12:12 +0200 Subject: [PATCH 63/99] EKF reset handling: Ensure altitude reinitializes correctly --- .../AttitudePositionEstimatorEKF.h | 6 ++ .../ekf_att_pos_estimator_main.cpp | 63 ++++++++++++++----- 2 files changed, 52 insertions(+), 17 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 6a9e3aadbc..0dd2b72d92 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -332,6 +332,12 @@ private: **/ void initializeGPS(); + /** + * Initialize the reference position for the local coordinate frame + */ + void initReferencePosition(hrt_abstime timestamp, + double lat, double lon, float gps_alt, float baro_alt); + /** * @brief * Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index d408ea2ddb..3ca47ebd49 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -401,9 +401,20 @@ int AttitudePositionEstimatorEKF::check_filter_state() // If error flag is set, we got a filter reset if (check && ekf_report.error) { + print_status(); + // Count the reset condition perf_count(_perf_reset); - _filter_ref_offset = _ekf->states[9]; + // GPS is in scaled integers, convert + double lat = _gps.lat / 1.0e7; + double lon = _gps.lon / 1.0e7; + float gps_alt = _gps.alt / 1e3f; + + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + warnx("FILTER RESET"); + initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); @@ -649,7 +660,7 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - _filter_ref_offset = -_baro.altitude; + warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset); } else { @@ -704,6 +715,32 @@ void AttitudePositionEstimatorEKF::task_main() _exit(0); } +void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, + double lat, double lon, float gps_alt, float baro_alt) +{ + // Reference altitude + if (isfinite(_ekf->states[9])) { + _filter_ref_offset = _ekf->states[9]; + } else if (isfinite(-_ekf->hgtMea)) { + _filter_ref_offset = -_ekf->hgtMea; + } else { + _filter_ref_offset = -_baro.altitude; + } + + // init filtered gps and baro altitudes + _gps_alt_filt = gps_alt; + _baro_alt_filt = baro_alt; + + // Initialize projection + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; + _local_pos.ref_alt = gps_alt; + _local_pos.ref_timestamp = timestamp; + + map_projection_init(&_pos_ref, lat, lon); + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); +} + void AttitudePositionEstimatorEKF::initializeGPS() { // GPS is in scaled integers, convert @@ -713,11 +750,6 @@ void AttitudePositionEstimatorEKF::initializeGPS() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _filter_ref_offset = _ekf->states[9]; // this should become zero in the local frame - - // init filtered gps and baro altitudes - _gps_alt_filt = gps_alt; - _baro_alt_filt = _baro.altitude; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = _ekf->baroHgt; @@ -739,14 +771,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); - // Initialize projection - _local_pos.ref_lat = lat; - _local_pos.ref_lon = lon; - _local_pos.ref_alt = gps_alt; - _local_pos.ref_timestamp = _gps.timestamp_position; - - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); #if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, @@ -1321,7 +1346,7 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD)); // update LPF - float filter_step += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); if (isfinite(filter_step)) { _gps_alt_filt += filter_step; @@ -1416,7 +1441,11 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; - _baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + + if (isfinite(filter_step)) { + _baro_alt_filt += filter_step; + } perf_count(_perf_baro); } From 3eebd8eb30a0956b9c7ce1d8fc6ddfb81141d4e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 14:28:19 +0200 Subject: [PATCH 64/99] EKF: Prevent bad data from being published --- .../ekf_att_pos_estimator_main.cpp | 25 ++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3ca47ebd49..451e14d202 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -401,8 +401,6 @@ int AttitudePositionEstimatorEKF::check_filter_state() // If error flag is set, we got a filter reset if (check && ekf_report.error) { - print_status(); - // Count the reset condition perf_count(_perf_reset); // GPS is in scaled integers, convert @@ -413,7 +411,6 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - warnx("FILTER RESET"); initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { @@ -854,6 +851,17 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.z_global = false; _local_pos.yaw = _att.yaw; + if (!isfinite(_local_pos.x) || + !isfinite(_local_pos.y) || + !isfinite(_local_pos.z) || + !isfinite(_local_pos.vx) || + !isfinite(_local_pos.vy) || + !isfinite(_local_pos.vz)) + { + // bad data, abort publication + return; + } + /* lazily publish the local position only once available */ if (_local_pos_pub > 0) { /* publish the attitude setpoint */ @@ -902,6 +910,17 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; + if (!isfinite(_global_pos.lat) || + !isfinite(_global_pos.lon) || + !isfinite(_global_pos.alt) || + !isfinite(_global_pos.vel_n) || + !isfinite(_global_pos.vel_e) || + !isfinite(_global_pos.vel_d)) + { + // bad data, abort publication + return; + } + /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { /* publish the global position */ From 0aa47236bfee600cf40fa3936bfec7ffb4ff104b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 16:48:42 +0200 Subject: [PATCH 65/99] commander: Only print reject mode message every 10 seconds. Set home position also if armed via command. Warn that arming via shell does not set home position. --- src/modules/commander/commander.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7b92942b33..d524aab787 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -143,7 +143,9 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 -#define PRINT_MODE_REJECT_INTERVAL 2000000 +#define PRINT_MODE_REJECT_INTERVAL 10000000 + +#define INAIR_RESTART_HOLDOFF_INTERVAL 2000000 enum MAV_MODE_FLAG { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ @@ -347,6 +349,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "arm")) { int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); arm_disarm(true, mavlink_fd_local, "command line"); + warnx("note: not updating home position on commandline arming!"); close(mavlink_fd_local); exit(0); } @@ -478,6 +481,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // Transition the arming state arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + if (arming_ret == TRANSITION_CHANGED && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + commander_set_home_position(home_pub, _home, local_position, global_position); + } + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { @@ -2023,7 +2031,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { + else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { commander_set_home_position(home_pub, _home, local_position, global_position); } From 900c81e67cea119e7e145a2ad2f680643853e475 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 17:18:25 +0200 Subject: [PATCH 66/99] commander: Compile fix for home init on arming via CMD --- src/modules/commander/commander.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d524aab787..64fd972ba4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -171,6 +171,7 @@ static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */ +static hrt_abstime commander_boot_timestamp = 0; static unsigned int leds_counter; /* To remember when last notification was sent */ @@ -210,7 +211,7 @@ void usage(const char *reason); */ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - orb_advert_t *home_pub); + struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub); /** * Mainloop of commander. @@ -453,7 +454,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, - struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) + struct home_position_s *home, struct vehicle_global_position_s *global_pos, + struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) @@ -482,8 +484,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - if (arming_ret == TRANSITION_CHANGED && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { - commander_set_home_position(home_pub, _home, local_position, global_position); + if (arming_ret == TRANSITION_CHANGED && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + commander_set_home_position(*home_pub, *home, *local_pos, *global_pos); } if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { @@ -1169,7 +1171,7 @@ int commander_thread_main(int argc, char *argv[]) set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } - const hrt_abstime commander_boot_timestamp = hrt_absolute_time(); + commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; @@ -1965,7 +1967,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) { status_changed = true; } } From da6a07421ba2cb652adbb1d01ab58f5a7c345bc0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jun 2015 09:15:11 +0200 Subject: [PATCH 67/99] EKF: Add hysteresis to mag failover --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 451e14d202..74d6dbadf0 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -71,6 +71,7 @@ static uint64_t IMUusec = 0; static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure +static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) /** * estimator app start / stop handling function @@ -1488,7 +1489,7 @@ void AttitudePositionEstimatorEKF::pollData() /* fail over to the 2nd mag if we know the first is down */ if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us && - _sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount && + _sensor_combined.magnetometer_errcount <= (_sensor_combined.magnetometer1_errcount + MAG_SWITCH_HYSTERESIS) && mag0.length() > 0.1f) { _ekf->magData.x = mag0.x; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset @@ -1516,7 +1517,7 @@ void AttitudePositionEstimatorEKF::pollData() } if (last_mag_main != _mag_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main); + mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main); } } From b46b1228088f558a81f435013924b168aeec4317 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jun 2015 14:16:05 +0200 Subject: [PATCH 68/99] PX4IO firmware: Support trim parameters for RPY --- src/modules/px4iofirmware/mixer.cpp | 21 +++++++++++++++++++++ src/modules/px4iofirmware/protocol.h | 3 +++ src/modules/px4iofirmware/px4io.h | 4 ++++ src/modules/px4iofirmware/registers.c | 3 +++ 4 files changed, 31 insertions(+) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1bb25104fa..596064f15f 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -311,6 +311,13 @@ mixer_callback(uintptr_t handle, case MIX_FMU: if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); + if (control_group == 0 && control_index == 0) { + control += REG_TO_FLOAT(r_setup_trim_roll); + } else if (control_group == 0 && control_index == 1) { + control += REG_TO_FLOAT(r_setup_trim_pitch); + } else if (control_group == 0 && control_index == 2) { + control += REG_TO_FLOAT(r_setup_trim_yaw); + } break; } return -1; @@ -318,6 +325,13 @@ mixer_callback(uintptr_t handle, case MIX_OVERRIDE: if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + if (control_group == 0 && control_index == 0) { + control += REG_TO_FLOAT(r_setup_trim_roll); + } else if (control_group == 0 && control_index == 1) { + control += REG_TO_FLOAT(r_setup_trim_pitch); + } else if (control_group == 0 && control_index == 2) { + control += REG_TO_FLOAT(r_setup_trim_yaw); + } break; } return -1; @@ -326,6 +340,13 @@ mixer_callback(uintptr_t handle, /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + if (control_group == 0 && control_index == 0) { + control += REG_TO_FLOAT(r_setup_trim_roll); + } else if (control_group == 0 && control_index == 1) { + control += REG_TO_FLOAT(r_setup_trim_pitch); + } else if (control_group == 0 && control_index == 2) { + control += REG_TO_FLOAT(r_setup_trim_yaw); + } break; } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index a649920320..64ba0f93c4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -234,6 +234,9 @@ enum { /* DSM bind states */ #define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ #define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ +#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */ /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index b38d374b9a..f0c7bde5dd 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -113,6 +113,10 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_setup_pwm_reverse r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] +#define r_setup_trim_roll r_page_setup[PX4IO_P_SETUP_TRIM_ROLL] +#define r_setup_trim_pitch r_page_setup[PX4IO_P_SETUP_TRIM_PITCH] +#define r_setup_trim_yaw r_page_setup[PX4IO_P_SETUP_TRIM_YAW] + #define r_control_values (&r_page_controls[0]) /* diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 977bc59fff..3bcf3968a7 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -174,6 +174,9 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, [PX4IO_P_SETUP_PWM_REVERSE] = 0, + [PX4IO_P_SETUP_TRIM_ROLL] = 0, + [PX4IO_P_SETUP_TRIM_PITCH] = 0, + [PX4IO_P_SETUP_TRIM_YAW] = 0 }; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 From 90362a9889887d5879a1d01d6c3ec6671dcbf570 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jun 2015 14:16:37 +0200 Subject: [PATCH 69/99] FW attitude controller: Fix usage of trim parameters to apply only to the final outputs --- .../fw_att_control/fw_att_control_main.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index b7194461e6..c44f29a404 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -861,8 +861,8 @@ FixedwingAttitudeControl::task_main() if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; } else { - roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) - + _parameters.rollsp_offset_rad; + roll_sp = (_manual.y * _parameters.man_roll_max) + + _parameters.rollsp_offset_rad; } pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; @@ -883,8 +883,7 @@ FixedwingAttitudeControl::task_main() /* * Velocity should be controlled and manual is enabled. */ - roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) - + _parameters.rollsp_offset_rad; + roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; @@ -911,10 +910,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) - + _parameters.rollsp_offset_rad; - pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) - + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad; /* allow manual control of rudder deflection */ yaw_manual = _manual.r; throttle_sp = _manual.z; @@ -1081,9 +1078,9 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ - _actuators.control[0] = _manual.y; - _actuators.control[1] = -_manual.x; - _actuators.control[2] = _manual.r; + _actuators.control[0] = _manual.y + _parameters.trim_roll; + _actuators.control[1] = -_manual.x + _parameters.trim_pitch; + _actuators.control[2] = _manual.r + _parameters.trim_yaw; _actuators.control[3] = _manual.z; _actuators.control[4] = _manual.flaps; } From 284da7d34457b00431652c1732c149bbd861d7e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jun 2015 14:19:00 +0200 Subject: [PATCH 70/99] PX4IO driver: Support trim values --- src/drivers/px4io/px4io.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e76349642f..5fa4d29554 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1114,6 +1114,27 @@ PX4IO::task_main() } (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask); + + float trim_val; + param_t trim_parm; + + trim_parm = param_find("TRIM_ROLL"); + if (trim_parm != PARAM_INVALID) { + param_get(trim_parm, &trim_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(trim_val)); + } + + trim_parm = param_find("TRIM_PITCH"); + if (trim_parm != PARAM_INVALID) { + param_get(trim_parm, &trim_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(trim_val)); + } + + trim_parm = param_find("TRIM_YAW"); + if (trim_parm != PARAM_INVALID) { + param_get(trim_parm, &trim_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(trim_val)); + } } } From 8259102bf1ffe7a8dd264424e5f3c6604c40a1cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jun 2015 23:06:01 +0200 Subject: [PATCH 71/99] PX4 IO driver: Fix TRIM upload --- src/drivers/px4io/px4io.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5fa4d29554..be3c2e1375 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2148,7 +2148,14 @@ PX4IO::print_status(bool extended_status) for (unsigned i = 0; i < _max_actuators; i++) { printf("%s", (pwm_invert_mask & (1 << i)) ? "x" : "_"); } - printf("]\n"); + printf("]"); + + float trim_roll = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL)); + float trim_pitch = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH)); + float trim_yaw = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW)); + + printf(" trims: r: %8.4f p: %8.4f y: %8.4f\n", + (double)trim_roll, (double)trim_pitch, (double)trim_yaw); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); From 9bb91ea031bb3e8a6023260268a7a041582823a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jun 2015 23:06:20 +0200 Subject: [PATCH 72/99] PX4 IO firmware: Do not reject trim --- src/modules/px4iofirmware/registers.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 3bcf3968a7..6f1341c347 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -630,6 +630,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] = value; break; + case PX4IO_P_SETUP_TRIM_ROLL: + case PX4IO_P_SETUP_TRIM_PITCH: + case PX4IO_P_SETUP_TRIM_YAW: + r_page_setup[offset] = value; + break; + default: return -1; } From 8a3d3f61e6d8c47822511621bd2425a0a2535ceb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jun 2015 23:08:49 +0200 Subject: [PATCH 73/99] commander: Better error handling for RC trim --- src/modules/commander/rc_calibration.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 0776894fb7..6ddd734931 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -56,7 +56,7 @@ static const int ERROR = -1; int do_trim_calibration(int mavlink_fd) { int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); - usleep(200000); + usleep(400000); struct manual_control_setpoint_s sp; bool changed; orb_check(sub_man, &changed); @@ -70,18 +70,18 @@ int do_trim_calibration(int mavlink_fd) /* set parameters */ float p = sp.y; - param_set(param_find("TRIM_ROLL"), &p); + int p1r = param_set(param_find("TRIM_ROLL"), &p); p = sp.x; - param_set(param_find("TRIM_PITCH"), &p); + int p2r = param_set(param_find("TRIM_PITCH"), &p); p = sp.r; - param_set(param_find("TRIM_YAW"), &p); + int p3r = param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ /* auto-save */ int save_ret = param_save_default(); - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL"); + if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) { + mavlink_log_critical(mavlink_fd, "TRIM: PARAM SET FAIL"); close(sub_man); return ERROR; } From 45cd05b57a7e3aa1bb288b402ee5437c35d688d3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 10 Jun 2015 17:06:42 +0200 Subject: [PATCH 74/99] invert pitch trim parameter --- src/modules/commander/rc_calibration.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 6ddd734931..08261c9890 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -71,7 +71,11 @@ int do_trim_calibration(int mavlink_fd) /* set parameters */ float p = sp.y; int p1r = param_set(param_find("TRIM_ROLL"), &p); - p = sp.x; + /* + we explicitly swap sign here because the trim is added to the actuator controls + which are moving in an inverse sense to manual pitch inputs + */ + p = -sp.x; int p2r = param_set(param_find("TRIM_PITCH"), &p); p = sp.r; int p3r = param_set(param_find("TRIM_YAW"), &p); From e224441ac14b4ed384d77995b7dbdbc047cee6a5 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 6 Jun 2015 10:34:01 -0600 Subject: [PATCH 75/99] special treatment and warning message for HIL platform arming --- src/modules/commander/commander.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 64fd972ba4..c6f5eaa7b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -437,6 +437,12 @@ 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 (autostart_id < 2000 && 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; + } + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, From 71da3976abf593c7d515b8aa78b49e7f8553a51e Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 6 Jun 2015 15:03:03 -0600 Subject: [PATCH 76/99] add HIL autostart ID range macros and remove warnx --- src/modules/commander/commander.cpp | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c6f5eaa7b5..8091a73ece 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -147,6 +147,9 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define INAIR_RESTART_HOLDOFF_INTERVAL 2000000 +#define HIL_ID_MIN 1000 +#define HIL_ID_MAX 1999 + enum MAV_MODE_FLAG { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ @@ -438,7 +441,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 (autostart_id < 2000 && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX + && 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; } @@ -1162,7 +1166,11 @@ int commander_thread_main(int argc, char *argv[]) // Run preflight check param_get(_param_autostart_id, &autostart_id); - if (autostart_id > 1999) { + if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) { + // HIL configuration selected: real sensors will be disabled + status.condition_system_sensors_initialized = false; + set_tune_override(TONE_STARTUP_TUNE); //normal boot tune + } else { status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune @@ -1170,11 +1178,6 @@ int commander_thread_main(int argc, char *argv[]) else { set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } - } else { - // HIL configuration selected: real sensors will be disabled - warnx("HIL configuration; autostart_id: %d, onboard IMU sensors disabled", autostart_id); - status.condition_system_sensors_initialized = false; - set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } commander_boot_timestamp = hrt_absolute_time(); @@ -1345,14 +1348,14 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - if (autostart_id > 1999) { - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); - } else { + if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) { /* HIL configuration: check only RC input */ - warnx("new telemetry link connected: checking RC status"); (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false); + } else { + /* check sensors also */ + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); } } From 7e48c66c221fc281b37dfb0db188440f89823ce6 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 10 Jun 2015 11:05:56 -0600 Subject: [PATCH 77/99] add is_hil_setup() --- src/modules/commander/commander.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8091a73ece..becf585452 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -256,6 +256,15 @@ void *commander_low_prio_loop(void *arg); void answer_command(struct vehicle_command_s &cmd, unsigned result); +/** + * check whether autostart ID is in the reserved range for HIL setups + */ +bool is_hil_setup(int id); + +bool is_hil_setup(int id) { + return (id >= HIL_ID_MIN) && (id <= HIL_ID_MAX); +} + int commander_main(int argc, char *argv[]) { @@ -441,8 +450,7 @@ 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 (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX - && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + if (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; } @@ -1166,7 +1174,7 @@ int commander_thread_main(int argc, char *argv[]) // Run preflight check param_get(_param_autostart_id, &autostart_id); - if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) { + if (is_hil_setup(autostart_id)) { // HIL configuration selected: real sensors will be disabled status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune @@ -1348,7 +1356,7 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) { + if (is_hil_setup(autostart_id)) { /* HIL configuration: check only RC input */ (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false); From e8a9c200561fea02f24a97883e6c617d07b251ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 12:30:05 +0200 Subject: [PATCH 78/99] EKF: Ensure we start with zero local altitude and zero GPS offset. Since the filter is not publishing any data at this point this is not relevant in operation, but might be important later if we publish a separate altitude estimate topic --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 74d6dbadf0..50410eac7d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -631,7 +631,10 @@ void AttitudePositionEstimatorEKF::task_main() // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds // maintain heavily filtered values for both baro and gps altitude // Assume the filtered output should be identical for both sensors - _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; + + if (_gpsIsGood) { + _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; + } // if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { // _last_debug_print = hrt_absolute_time(); // perf_print_counter(_perf_baro); @@ -658,6 +661,8 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + _filter_ref_offset = -_baro.altitude; + warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset); } else { From 086123fe8419f9382d0c0e2d80e1fe7a27a17ae6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 12:40:39 +0200 Subject: [PATCH 79/99] Fix RC failsafe handling when landed --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 92f73ca21f..7a379612da 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -494,7 +494,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ - if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { + if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !status->condition_landed) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { From 25c23dbf4cb58d4e8c94a749801a1129ef7cdf34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 13:19:36 +0200 Subject: [PATCH 80/99] back out payload mixer from FX79 default config --- ROMFS/px4fmu_common/mixers/FX79.main.mix | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FX79.main.mix b/ROMFS/px4fmu_common/mixers/FX79.main.mix index b8879af9ee..3968a2b08d 100755 --- a/ROMFS/px4fmu_common/mixers/FX79.main.mix +++ b/ROMFS/px4fmu_common/mixers/FX79.main.mix @@ -51,19 +51,3 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 - -Inputs to the mixer come from channel group 2 (payload), channels 0 -(bay servo 1), 1 (bay servo 2) and 3 (drop release). ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 0 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 1 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 2 -10000 -10000 0 -10000 10000 From f0f3ffaec1b012fd95d220a9379ae07ed30053e9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 13:31:58 +0200 Subject: [PATCH 81/99] IO firmware: Do not apply trim values a second time --- src/modules/px4iofirmware/mixer.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 596064f15f..9fadec57c0 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -311,13 +311,6 @@ mixer_callback(uintptr_t handle, case MIX_FMU: if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); - if (control_group == 0 && control_index == 0) { - control += REG_TO_FLOAT(r_setup_trim_roll); - } else if (control_group == 0 && control_index == 1) { - control += REG_TO_FLOAT(r_setup_trim_pitch); - } else if (control_group == 0 && control_index == 2) { - control += REG_TO_FLOAT(r_setup_trim_yaw); - } break; } return -1; From 3cc2b7ed1285489b4cda8360d720738e0d146e0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 14:51:34 +0200 Subject: [PATCH 82/99] EKF: Add small gyro failover hysteresis --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 50410eac7d..b9897ffcfc 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -72,6 +72,8 @@ static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in secon static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) +static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) +static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) /** * estimator app start / stop handling function @@ -1224,7 +1226,7 @@ void AttitudePositionEstimatorEKF::pollData() if (isfinite(_sensor_combined.gyro_rad_s[0]) && isfinite(_sensor_combined.gyro_rad_s[1]) && isfinite(_sensor_combined.gyro_rad_s[2]) && - (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { + (_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) { _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; @@ -1263,7 +1265,7 @@ void AttitudePositionEstimatorEKF::pollData() int last_accel_main = _accel_main; /* fail over to the 2nd accel if we know the first is down */ - if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) { + if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) { _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; From 000434be15c9ac8a93fa0e675f790464d8b5d23e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 14:51:49 +0200 Subject: [PATCH 83/99] IO mixer: Limit outputs to proper range --- src/modules/px4iofirmware/mixer.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 9fadec57c0..5e6a3a585a 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -353,6 +353,13 @@ mixer_callback(uintptr_t handle, return -1; } + /* limit output */ + if (control > 1.0f) { + control = 1.0f; + } else if (control < -1.0f) { + control = -1.0f; + } + return 0; } From 9155e8a7fe2a7611c6b1ed136b5691475546a65c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 16:35:10 +0200 Subject: [PATCH 84/99] FX79: Increase travel --- ROMFS/px4fmu_common/mixers/FX79.main.mix | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FX79.main.mix b/ROMFS/px4fmu_common/mixers/FX79.main.mix index 3968a2b08d..3223eff133 100755 --- a/ROMFS/px4fmu_common/mixers/FX79.main.mix +++ b/ROMFS/px4fmu_common/mixers/FX79.main.mix @@ -27,13 +27,13 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 7500 7500 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 +S: 0 0 8500 8500 0 -10000 10000 +S: 0 1 9500 9500 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 7500 7500 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 +S: 0 0 8500 8500 0 -10000 10000 +S: 0 1 -9500 -9500 0 -10000 10000 Output 2 -------- From f2b81ce69a88683289ead9de33ce83efc3831a1f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 12:22:47 +0200 Subject: [PATCH 85/99] commander: Only update home position if not armed already --- src/modules/commander/commander.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index becf585452..7c15992f40 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -499,10 +499,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state - arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED; + + arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command"); /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - if (arming_ret == TRANSITION_CHANGED && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + if (cmd_arm && (arming_ret == TRANSITION_CHANGED) && + (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) { + commander_set_home_position(*home_pub, *home, *local_pos, *global_pos); } @@ -2041,11 +2045,11 @@ int commander_thread_main(int argc, char *argv[]) } } - //Get current timestamp + /* Get current timestamp */ const hrt_abstime now = hrt_absolute_time(); - //First time home position update - if (!status.condition_home_position_valid) { + /* First time home position update - but only if disarmed */ + if (!status.condition_home_position_valid && !armed.armed) { commander_set_home_position(home_pub, _home, local_position, global_position); } From bc48634101dff97b7e4cd2d5e3008613a1c0df6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 12:24:26 +0200 Subject: [PATCH 86/99] Navigator: Reject missions with relative altitude if no home was set before arming --- src/modules/navigator/mission.cpp | 2 +- .../navigator/mission_feasibility_checker.cpp | 23 ++++++++++++------- .../navigator/mission_feasibility_checker.h | 8 +++---- src/modules/navigator/navigator.h | 1 + 4 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 98b01c7578..4eaeca1d8e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -232,7 +232,7 @@ Mission::update_offboard_mission() failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt); + _navigator->get_home_position()->alt, _navigator->home_position_valid()); } else { warnx("offboard mission update failed"); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index d9297f25bf..19baa79fcd 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -63,7 +63,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { bool failed = false; /* Init if not done yet */ @@ -80,25 +80,25 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); else - failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); return !failed; } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +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); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); /* Mission is only marked as feasible if all checks return true */ return (resGeofence && resHomeAltitude); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +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(); @@ -107,7 +107,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre /* 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); + 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); @@ -136,7 +136,7 @@ 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 throw_error) +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, 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++) { @@ -152,6 +152,13 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } + if (missionitem.altitude_is_relative && !home_valid) { + if (throw_error) { + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); + return false; + } + } + /* calculate the global waypoint altitude */ float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 94b6b29226..964bd6097d 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -61,16 +61,16 @@ private: /* 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 throw_error = false); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false); bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); public: MissionFeasibilityChecker(); @@ -80,7 +80,7 @@ public: * Returns true if mission is feasible and false otherwise */ bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt); + float home_alt, bool home_valid); }; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 95e3f9f964..782a297fbb 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -134,6 +134,7 @@ public: struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } + bool home_position_valid() { return _home_position_set; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } struct geofence_result_s* get_geofence_result() { return &_geofence_result; } From a66b1b9d04c6c200879576e2f4d9609e229606c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 17:02:11 +0200 Subject: [PATCH 87/99] Improve feedback when auto mode is rejected due to a non suitable mission --- src/modules/navigator/mission.cpp | 4 +-- .../navigator/mission_feasibility_checker.cpp | 25 +++++++++---------- .../navigator/mission_feasibility_checker.h | 2 +- src/modules/navigator/navigator_main.cpp | 5 +++- 4 files changed, 19 insertions(+), 17 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 4eaeca1d8e..13fcd5daca 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -230,7 +230,7 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + 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()); @@ -374,7 +374,7 @@ Mission::set_mission_items() } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); + mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; } else { diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 19baa79fcd..9d1dc7c7e6 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -63,26 +63,27 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(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) { bool failed = false; /* Init if not done yet */ init(); - /* Open mavlink fd */ - if (_mavlink_fd < 0) { - /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - } + _mavlink_fd = mavlink_fd; // check if all mission item commands are supported failed |= !checkMissionItemValidity(dm_current, nMissionItems); - if (isRotarywing) + if (isRotarywing) { failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); - else + } else { failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); + } + + if (!failed) { + mavlink_log_info(_mavlink_fd, "Mission checked and ready."); + } return !failed; } @@ -152,11 +153,10 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } + /* always reject relative alt without home set */ if (missionitem.altitude_is_relative && !home_valid) { - if (throw_error) { - mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); - return false; - } + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); + return false; } /* calculate the global waypoint altitude */ @@ -204,7 +204,6 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s return false; } } - mavlink_log_info(_mavlink_fd, "Mission ready."); return true; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 964bd6097d..9c9511be3d 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -79,7 +79,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, + bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 97baa1235f..8cfce50879 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -209,7 +209,10 @@ Navigator::home_position_update() if (updated) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); - _home_position_set = true; + + if (_home_pos.timestamp > 0) { + _home_position_set = true; + } } } From e94bc7960c027d8e806e026b3c25d79c378a6fe7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 10 Jun 2015 21:20:57 +0300 Subject: [PATCH 88/99] libuavcan submodule updated --- .gitmodules | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index 6a4f815d42..b05de95995 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,9 +4,9 @@ [submodule "NuttX"] path = NuttX url = git://github.com/PX4/NuttX.git -[submodule "uavcan"] +[submodule "libuavcan"] path = src/lib/uavcan - url = git://github.com/pavel-kirienko/uavcan.git + url = git://github.com/UAVCAN/libuavcan.git [submodule "Tools/genmsg"] path = Tools/genmsg url = https://github.com/ros/genmsg.git From 891829d3a770bb98d7fd40ae1b0f7b39a188bbda Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 19:25:17 +0200 Subject: [PATCH 89/99] Land detector: Protect fixed wing logic from false-positives due to bad input data --- src/modules/land_detector/FixedwingLandDetector.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 6e8393617e..5f7ded9cb2 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -85,9 +85,16 @@ bool FixedwingLandDetector::update() bool landDetected = false; if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { - _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + if (isfinite(val)) { + _velocity_xy_filtered = val; + } + val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + + if (isfinite(val)) { + _velocity_z_filtered = val; + } } if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { From 7540aa6b87bc8d958f0818ac2ff8a43e7942f2ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 21:05:38 +0200 Subject: [PATCH 90/99] Navigator: Make logic using previous and current altitudes consistent --- src/modules/navigator/mission.cpp | 21 ++++++++++++++------- src/modules/navigator/mission.h | 2 ++ 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 13fcd5daca..1d47167a04 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -271,6 +271,16 @@ Mission::advance_mission() } } +int +Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item) +{ + if (_mission_item.altitude_is_relative) { + return _mission_item.altitude + _navigator->get_home_position()->alt; + } else { + return _mission_item.altitude; + } +} + bool Mission::check_dist_1wp() { @@ -354,7 +364,7 @@ Mission::set_mission_items() /* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */ if (pos_sp_triplet->previous.valid) { - _mission_item_previous_alt = _mission_item.altitude; + _mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item); } /* get home distance state */ @@ -440,10 +450,7 @@ Mission::set_mission_items() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next); /* calculate takeoff altitude */ - float takeoff_alt = _mission_item.altitude; - if (_mission_item.altitude_is_relative) { - takeoff_alt += _navigator->get_home_position()->alt; - } + float takeoff_alt = get_absolute_altitude_for_item(_mission_item); /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_navigator->get_vstatus()->condition_landed) { @@ -609,14 +616,14 @@ Mission::altitude_sp_foh_update() /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { - pos_sp_triplet->current.alt = _mission_item.altitude; + pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item); } else { /* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp * of the mission item as it is used to check if the mission item is reached * The setpoint is set linearly and such that the system reaches the current altitude at the acceptance * radius around the current waypoint **/ - float delta_alt = (_mission_item.altitude - _mission_item_previous_alt); + float delta_alt = (get_absolute_altitude_for_item(_mission_item) - _mission_item_previous_alt); float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius)); float a = _mission_item_previous_alt - grad * _distance_current_previous; pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index e9f78e8fdc..bc9a2c6c82 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -132,6 +132,8 @@ private: */ void altitude_sp_foh_reset(); + int get_absolute_altitude_for_item(struct mission_item_s &mission_item); + /** * Read current or next mission item from the dataman and watch out for DO_JUMPS * @return true if successful From 315683124d10cc34673d9993f2340ae0f0378b20 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 12 Jun 2015 08:47:46 +0200 Subject: [PATCH 91/99] fix posctl th param meta --- src/modules/sensors/sensor_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 1bebea2060..7b3932638b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1285,7 +1285,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * * @min -1 * @max 1 - * + * @group Radio Switches * */ PARAM_DEFINE_FLOAT(RC_POSCTL_TH, 0.5f); From 863a6e88119384f4aa5776f5c9e264e06697632b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 09:27:15 +0200 Subject: [PATCH 92/99] HIL driver: Fix boot order race --- src/drivers/hil/hil.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 961ec47246..a7c2e83e34 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -331,10 +331,9 @@ HIL::task_main() /* advertise the mixed control outputs */ actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); - /* advertise the mixed control outputs */ - int dummy; - _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), - &outputs, &dummy, ORB_PRIO_LOW); + /* advertise the mixed control outputs, insist on the first group output */ + _t_outputs = orb_advertise(ORB_ID(actuator_outputs), + &outputs); pollfd fds[2]; fds[0].fd = _t_actuators; From 57279856788d3e879ee76ae827791e25be3d8f3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 09:31:10 +0200 Subject: [PATCH 93/99] Comment out unused pca8574 driver in boot --- ROMFS/px4fmu_common/init.d/rcS | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 13b35313e7..8a93737ef8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -101,9 +101,10 @@ then fi fi - if pca8574 start - then - fi + # Currently unused, but might be useful down the road + #if pca8574 start + #then + #fi # # Set default values From 267fb408b1f5cfc27235b70f5dc8457e135622b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 11:08:30 +0200 Subject: [PATCH 94/99] Update MC thrust limit default param value and add explanation --- src/modules/mc_pos_control/mc_pos_control_params.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index fc73b31f59..ade43ffb91 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -55,13 +55,16 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f); /** * Maximum thrust * - * Limit max allowed thrust. + * Limit max allowed thrust. Setting a value of one can put + * the system into actuator saturation as no spread between + * the motors is possible any more. A value of 0.8 - 0.9 + * is recommended. * * @min 0.0 * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f); /** * Proportional gain for vertical position error From 7e44a234118eb8b940f2323f10a6023c1ce5f4a5 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 12 Jun 2015 13:50:02 +0200 Subject: [PATCH 95/99] fix: take current trim values into account when doing trim calibration --- src/modules/commander/rc_calibration.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 08261c9890..c9a8343b66 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -68,16 +68,26 @@ int do_trim_calibration(int mavlink_fd) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - /* set parameters */ - float p = sp.y; + /* load trim values which are active */ + float roll_trim_active; + param_get(param_find("TRIM_ROLL"), &roll_trim_active); + float pitch_trim_active; + param_get(param_find("TRIM_PITCH"), &pitch_trim_active); + float yaw_trim_active; + param_get(param_find("TRIM_YAW"), &yaw_trim_active); + + /* set parameters: the new trim values are the combination of active trim values + and the values coming from the remote control of the user + */ + float p = sp.y + roll_trim_active; int p1r = param_set(param_find("TRIM_ROLL"), &p); /* we explicitly swap sign here because the trim is added to the actuator controls which are moving in an inverse sense to manual pitch inputs */ - p = -sp.x; + p = -sp.x + pitch_trim_active; int p2r = param_set(param_find("TRIM_PITCH"), &p); - p = sp.r; + p = sp.r + yaw_trim_active; int p3r = param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ From 540ffa7861c0583ed6b13bfa1fdfab04193fd74b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 14:02:47 +0200 Subject: [PATCH 96/99] Let user know we are loitering now --- src/modules/navigator/mission.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 1d47167a04..fe876ee8b1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -390,7 +390,8 @@ Mission::set_mission_items() } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished"); + /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); @@ -398,8 +399,9 @@ Mission::set_mission_items() } 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"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering"); } _mission_type = MISSION_TYPE_NONE; From 428611119fe7b4cd27695e9435edea4ff0582f23 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 12 Jun 2015 06:49:15 -1000 Subject: [PATCH 97/99] Merged GIT version changes --- Makefile | 51 +++++++++++++++++++++++++---- makefiles/setup.mk | 4 ++- src/modules/systemlib/git_version.h | 2 ++ src/modules/systemlib/module.mk | 4 +-- src/systemcmds/ver/ver.c | 3 ++ 5 files changed, 55 insertions(+), 9 deletions(-) diff --git a/Makefile b/Makefile index 47fe326b92..aedf595aef 100644 --- a/Makefile +++ b/Makefile @@ -51,10 +51,6 @@ endif GIT_DESC_SHORT := $(shell echo $(GIT_DESC) | cut -c1-16) -$(shell echo "#include " > $(BUILD_DIR)git_version.c) -$(shell echo "const char* px4_git_version = \"$(GIT_DESC)\";" >> $(BUILD_DIR)git_version.c) -$(shell echo "const uint64_t px4_git_version_binary = 0x$(GIT_DESC_SHORT);" >> $(BUILD_DIR)git_version.c) - # # Canned firmware configurations that we (know how to) build. # @@ -129,7 +125,7 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 .PHONY: $(FIRMWARES) $(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ -$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: generateuorbtopicheaders checksubmodules +$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: checkgitversion generateuorbtopicheaders checksubmodules @$(ECHO) %%%% @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% @@ -216,11 +212,53 @@ menuconfig: @$(ECHO) "" endif -$(NUTTX_SRC): checksubmodules +$(NUTTX_SRC): checkgitversion checksubmodules $(UAVCAN_DIR): $(Q) (./Tools/check_submodules.sh) + +ifeq ($(PX4_TARGET_OS),nuttx) +# TODO +# Move the above nuttx specific rules into $(PX4_BASE)makefiles/firmware_nuttx.mk +endif +ifeq ($(PX4_TARGET_OS),posix) +include $(PX4_BASE)makefiles/firmware_posix.mk +endif +ifeq ($(PX4_TARGET_OS),posix-arm) +include $(PX4_BASE)makefiles/firmware_posix.mk +endif +ifeq ($(PX4_TARGET_OS),qurt) +include $(PX4_BASE)makefiles/firmware_qurt.mk +endif + +# +# Versioning +# + +GIT_VER_FILE = $(PX4_VERSIONING_DIR).build_git_ver +GIT_HEADER_FILE = $(PX4_VERSIONING_DIR)build_git_version.h + +$(GIT_VER_FILE) : + $(Q) if [ ! -f $(GIT_VER_FILE) ]; then \ + $(MKDIR) -p $(PX4_VERSIONING_DIR); \ + $(ECHO) "" > $(GIT_VER_FILE); \ + fi + +.PHONY: checkgitversion +checkgitversion: $(GIT_VER_FILE) + $(Q) if [ "$(GIT_DESC)" != "$(shell cat $(GIT_VER_FILE))" ]; then \ + $(ECHO) "/* Auto Magically Generated file */" > $(GIT_HEADER_FILE); \ + $(ECHO) "/* Do not edit! */" >> $(GIT_HEADER_FILE); \ + $(ECHO) "#define PX4_GIT_VERSION_STR \"$(GIT_DESC)\"" >> $(GIT_HEADER_FILE); \ + $(ECHO) "#define PX4_GIT_VERSION_BINARY 0x$(GIT_DESC_SHORT)" >> $(GIT_HEADER_FILE); \ + $(ECHO) $(GIT_DESC) > $(GIT_VER_FILE); \ + fi + +# +# Submodule Checks +# + .PHONY: checksubmodules checksubmodules: $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) @@ -280,6 +318,7 @@ check_format: clean: @echo > /dev/null $(Q) $(RMDIR) $(BUILD_DIR)*.build + $(Q) $(RMDIR) $(PX4_VERSIONING_DIR) $(Q) $(REMOVE) $(IMAGE_DIR)*.px4 .PHONY: distclean diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 351b9f1b71..ead85e7fc9 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -55,6 +55,7 @@ export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/ export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/ export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/ +export PX4_VERSIONING_DIR = $(BUILD_DIR)versioning/ # # Default include paths @@ -63,7 +64,8 @@ export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ $(PX4_MODULE_SRC)/modules/ \ $(PX4_INCLUDE_DIR) \ $(PX4_LIB_DIR) \ - $(PX4_PLATFORMS_DIR) + $(PX4_PLATFORMS_DIR) \ + $(PX4_VERSIONING_DIR) # # Tools diff --git a/src/modules/systemlib/git_version.h b/src/modules/systemlib/git_version.h index 9b8f9e860e..5f8d411088 100644 --- a/src/modules/systemlib/git_version.h +++ b/src/modules/systemlib/git_version.h @@ -41,6 +41,8 @@ #include +#include "build_git_version.h" + __BEGIN_DECLS __EXPORT extern const char* px4_git_version; diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index ff409fa473..81e7d87536 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -55,8 +55,8 @@ SRCS = err.c \ pwm_limit/pwm_limit.c \ circuit_breaker.cpp \ circuit_breaker_params.c \ - mcu_version.c \ - $(BUILD_DIR)git_version.c + mcu_version.c + circuit_breaker_params.c MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index a248dfb79c..caac93024a 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -57,6 +57,9 @@ static const char sz_ver_all_str[] = "all"; static const char mcu_ver_str[] = "mcu"; static const char mcu_uid_str[] = "uid"; +const char* px4_git_version = PX4_GIT_VERSION_STR; +const uint64_t px4_git_version_binary = PX4_GIT_VERSION_BINARY; + static void usage(const char *reason) { if (reason != NULL) { From 9585bb4a3c35f9b02574a61c5ece2e6952f15aa2 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 12 Jun 2015 07:54:51 -1000 Subject: [PATCH 98/99] Missing slash --- src/modules/systemlib/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 81e7d87536..f80d8009ae 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -55,7 +55,7 @@ SRCS = err.c \ pwm_limit/pwm_limit.c \ circuit_breaker.cpp \ circuit_breaker_params.c \ - mcu_version.c + mcu_version.c \ circuit_breaker_params.c MAXOPTIMIZATION = -Os From a0176474c7740a4a04c2eb91eba57d8ce64027a1 Mon Sep 17 00:00:00 2001 From: Elikos default Date: Thu, 11 Jun 2015 20:28:47 -0400 Subject: [PATCH 99/99] fix NaN yaw breaking attitude setpoints when going back into posctl from offboard --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- src/platforms/ros/nodes/mavlink/mavlink.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7ede93ec2d..1a6494ad93 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -621,7 +621,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } /* set the yaw sp value */ - if (!offboard_control_mode.ignore_attitude) { + if (!offboard_control_mode.ignore_attitude && !isnan(set_position_target_local_ned.yaw)) { pos_sp_triplet.current.yaw_valid = true; pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; @@ -630,7 +630,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } /* set the yawrate sp value */ - if (!offboard_control_mode.ignore_bodyrate) { + if (!offboard_control_mode.ignore_bodyrate && !isnan(set_position_target_local_ned.yaw)) { pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 2758979a29..c92dd0843d 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -273,7 +273,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * } /* set the yaw sp value */ - if (!offboard_control_mode.ignore_attitude) { + if (!offboard_control_mode.ignore_attitude && !isnan(set_position_target_local_ned.yaw)) { pos_sp_triplet.current.yaw_valid = true; pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; @@ -282,7 +282,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * } /* set the yawrate sp value */ - if (!offboard_control_mode.ignore_bodyrate) { + if (!offboard_control_mode.ignore_bodyrate && !isnan(set_position_target_local_ned.yaw)) { pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;