From d8aebc805ffc2b51a93d1c5276c84af7017f83b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 18:49:02 -0700 Subject: [PATCH 01/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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);