diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-29 21:21:16 +0100 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-29 21:27:44 +0100 |
commit | cf6d89301b774ae019d1d2c089a30a8ed0d7308f (patch) | |
tree | 7e56739c95dedf53cfb0bae88e6ba65c88b661c8 /src | |
parent | 01c9092213449c761759bcda11ef9613226be713 (diff) | |
parent | 6f559b279e3d03dbf28eff436b41f3b022c5fa82 (diff) | |
download | px4-firmware-cf6d89301b774ae019d1d2c089a30a8ed0d7308f.tar.gz px4-firmware-cf6d89301b774ae019d1d2c089a30a8ed0d7308f.tar.bz2 px4-firmware-cf6d89301b774ae019d1d2c089a30a8ed0d7308f.zip |
Merge branch 'beta' into offboard2
Diffstat (limited to 'src')
55 files changed, 1929 insertions, 1531 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5e45cc936..f73a3ef01 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -91,7 +91,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 8152ea4d4..ef9bb5cad 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -114,7 +114,7 @@ /* XXX these should be UART pins */ #define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) #define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) -#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) +#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) /* * High-resolution timer diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index ccd01edf5..9f8c0eeb2 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -124,8 +124,6 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_ADC_VSERVO); stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ - - stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); stm32_configgpio(GPIO_SBUS_OUTPUT); /* sbus output enable is active low - disable it by default */ diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 51f916f37..88da94b1e 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -189,6 +189,10 @@ ORB_DECLARE(output_pwm); /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) +/** set the number of servos in (unsigned)arg - allows change of + * split between servos and GPIO */ +#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 63b2d2d29..cfcf91e3f 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -225,16 +225,16 @@ void frsky_send_frame2(int uart) float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; int sec = 0; - if (global_pos.valid) { + if (global_pos.global_valid) { time_t time_gps = global_pos.time_gps_usec / 1000000; struct tm *tm_gps = gmtime(&time_gps); course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; - lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f); + lat = frsky_format_gps(abs(global_pos.lat)); lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; - lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f); + lon = frsky_format_gps(abs(global_pos.lon)); lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; - speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) + speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) * 25.0f / 46.0f; alt = global_pos.alt; sec = tm_gps->tm_sec; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d3b99ae66..9b9c11af2 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -849,42 +849,24 @@ HMC5883::collect() /* scale values for output */ - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { - /* to align the sensor axes with the board, x and y need to be flipped */ - new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; - } else { -#endif - /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, - * therefore switch x and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; -#ifdef PX4_I2C_BUS_ONBOARD - } + // convert onboard so it matches offboard for the + // scaling below + report.y = -report.y; + report.x = -report.x; + } #endif + /* the standard external mag by 3DR has x pointing to the + * right, y pointing backwards, and z down, therefore switch x + * and y and invert y */ + new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + if (_mag_topic != -1) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); @@ -910,6 +892,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) struct mag_report report; ssize_t sz; int ret = 1; + uint8_t good_count = 0; // XXX do something smarter here int fd = (int)enable; @@ -932,31 +915,16 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) 1.0f, }; - float avg_excited[3] = {0.0f, 0.0f, 0.0f}; - unsigned i; + float sum_excited[3] = {0.0f, 0.0f, 0.0f}; - warnx("starting mag scale calibration"); + /* expected axis scaling. The datasheet says that 766 will + * be places in the X and Y axes and 713 in the Z + * axis. Experiments show that in fact 766 is placed in X, + * and 713 in Y and Z. This is relative to a base of 660 + * LSM/Ga, giving 1.16 and 1.08 */ + float expected_cal[3] = { 1.16f, 1.08f, 1.08f }; - /* do a simple demand read */ - sz = read(filp, (char *)&report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("immediate read failed"); - ret = 1; - goto out; - } - - warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - warnx("time: %lld", report.timestamp); - warnx("sampling 500 samples for scaling offset"); - - /* set the queue depth to 10 */ - /* don't do this for now, it can lead to a crash in start() respectively work_queue() */ -// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { -// warn("failed to set queue depth"); -// ret = 1; -// goto out; -// } + warnx("starting mag scale calibration"); /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { @@ -965,8 +933,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* Set to 2.5 Gauss */ - if (OK != ioctl(filp, MAGIOCSRANGE, 2)) { + /* Set to 2.5 Gauss. We ask for 3 to get the right part of + * the chained if statement above. */ + if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { warnx("failed to set 2.5 Ga range"); ret = 1; goto out; @@ -990,8 +959,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* read the sensor 10x and report each value */ - for (i = 0; i < 500; i++) { + // discard 10 samples to let the sensor settle + for (uint8_t i = 0; i < 10; i++) { struct pollfd fds; /* wait for data to be ready */ @@ -1009,32 +978,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) if (sz != sizeof(report)) { warn("periodic read failed"); + ret = -EIO; goto out; + } + } - } else { - avg_excited[0] += report.x; - avg_excited[1] += report.y; - avg_excited[2] += report.z; + /* read the sensor up to 50x, stopping when we have 10 good values */ + for (uint8_t i = 0; i < 50 && good_count < 10; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = ::poll(&fds, 1, 2000); + + if (ret != 1) { + warn("timed out waiting for sensor data"); + goto out; + } + + /* now go get it */ + sz = ::read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("periodic read failed"); + ret = -EIO; + goto out; + } + float cal[3] = {fabsf(expected_cal[0] / report.x), + fabsf(expected_cal[1] / report.y), + fabsf(expected_cal[2] / report.z)}; + + if (cal[0] > 0.7f && cal[0] < 1.35f && + cal[1] > 0.7f && cal[1] < 1.35f && + cal[2] > 0.7f && cal[2] < 1.35f) { + good_count++; + sum_excited[0] += cal[0]; + sum_excited[1] += cal[1]; + sum_excited[2] += cal[2]; } //warnx("periodic read %u", i); //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + //warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]); } - avg_excited[0] /= i; - avg_excited[1] /= i; - avg_excited[2] /= i; + if (good_count < 5) { + warn("failed calibration"); + ret = -EIO; + goto out; + } - warnx("done. Performed %u reads", i); - warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); +#if 0 + warnx("measurement avg: %.6f %.6f %.6f", + (double)sum_excited[0]/good_count, + (double)sum_excited[1]/good_count, + (double)sum_excited[2]/good_count); +#endif float scaling[3]; - /* calculate axis scaling */ - scaling[0] = fabsf(1.16f / avg_excited[0]); - /* second axis inverted */ - scaling[1] = fabsf(1.16f / -avg_excited[1]); - scaling[2] = fabsf(1.08f / avg_excited[2]); + scaling[0] = sum_excited[0] / good_count; + scaling[1] = sum_excited[1] / good_count; + scaling[2] = sum_excited[2] / good_count; warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); @@ -1165,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable) conf_reg &= ~0x03; } + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg); + ret = write_reg(ADDR_CONF_A, conf_reg); if (OK != ret) @@ -1173,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable) uint8_t conf_reg_ret; read_reg(ADDR_CONF_A, conf_reg_ret); + //print_info(); + return !(conf_reg == conf_reg_ret); } @@ -1211,6 +1221,10 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); + printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); + printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", + (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, + (double)1.0/_range_scale, (double)_range_ga); _reports->print_info("report queue"); } diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index a95c4576b..9251cff7b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -77,7 +77,6 @@ #include <systemlib/err.h> #include <systemlib/param/param.h> #include <systemlib/perf_counter.h> -#include <mathlib/mathlib.h> #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> @@ -178,24 +177,17 @@ MEASAirspeed::collect() return ret; } - //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); - uint16_t temp = (val[3] & 0xE0) << 8 | val[2]; - - // XXX leaving this in until new calculation method has been cross-checked - //diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); - //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; - dp_raw = 0x3FFF & dp_raw; //mask the used bits + /* mask the used bits */ + dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; - // XXX we may want to smooth out the readings to remove noise. - - // Calculate differential pressure. As its centered around 8000 - // and can go positive or negative, enforce absolute value -// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + /* calculate differential pressure. As its centered around 8000 + * and can go positive or negative, enforce absolute value + */ const float P_min = -1.0f; const float P_max = 1.0f; float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; @@ -204,7 +196,7 @@ MEASAirspeed::collect() struct differential_pressure_s report; - // Track maximum differential pressure measured (so we can work out top speed). + /* track maximum differential pressure measured (so we can work out top speed). */ if (diff_press_pa > _max_differential_pressure_pa) { _max_differential_pressure_pa = diff_press_pa; } @@ -392,7 +384,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %d pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4b1b0ed0b..c067d363b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1006,6 +1006,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_COUNT: { + /* change the number of outputs that are enabled for + * PWM. This is used to change the split between GPIO + * and PWM under control of the flight config + * parameters. Note that this does not allow for + * changing a set of pins to be used for serial on + * FMUv1 + */ + switch (arg) { + case 0: + set_mode(MODE_NONE); + break; + + case 2: + set_mode(MODE_2PWM); + break; + + case 4: + set_mode(MODE_4PWM); + break; + +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + case 6: + set_mode(MODE_6PWM); + break; +#endif + + default: + ret = -EINVAL; + break; + } + break; + } + case MIXERIOCRESET: if (_mixers != nullptr) { delete _mixers; @@ -1443,7 +1477,6 @@ void sensor_reset(int ms) { int fd; - int ret; fd = open(PX4FMU_DEVICE_PATH, O_RDWR); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c8f77a611..df847a64d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -451,7 +451,7 @@ private: namespace { -PX4IO *g_dev; +PX4IO *g_dev = nullptr; } @@ -505,7 +505,7 @@ PX4IO::PX4IO(device::Device *interface) : /* open MAVLink text channel */ _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); - _debug_enabled = true; + _debug_enabled = false; _servorail_status.rssi_v = 0; } @@ -580,6 +580,12 @@ PX4IO::init() /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol == _io_reg_get_error) { + log("failed to communicate with IO"); + mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort."); + return -1; + } + if (protocol != PX4IO_PROTOCOL_VERSION) { log("protocol/firmware mismatch"); mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); @@ -774,8 +780,6 @@ PX4IO::task_main() hrt_abstime poll_last = 0; hrt_abstime orb_check_last = 0; - log("starting"); - _thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); /* @@ -809,8 +813,6 @@ PX4IO::task_main() fds[0].fd = _t_actuator_controls_0; fds[0].events = POLLIN; - log("ready"); - /* lock against the ioctl handler */ lock(); @@ -1454,8 +1456,10 @@ PX4IO::io_publish_raw_rc() /* set RSSI */ - // XXX the correct scaling needs to be validated here - rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; + if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) { + // XXX the correct scaling needs to be validated here + rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; + } /* lazily advertise on first publication */ if (_to_input_rc == 0) { @@ -1671,7 +1675,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) total_len++; } - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } /* print mixer chunk */ if (debuglevel > 5 || ret) { @@ -1695,7 +1710,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) msg->text[0] = '\n'; msg->text[1] = '\0'; - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } if (ret) return ret; @@ -1808,7 +1834,7 @@ PX4IO::print_status() printf("\n"); - if (raw_inputs > 0) { + if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); @@ -2365,8 +2391,10 @@ start(int argc, char *argv[]) /* create the driver - it will set g_dev */ (void)new PX4IO(interface); - if (g_dev == nullptr) + if (g_dev == nullptr) { + delete interface; errx(1, "driver alloc failed"); + } if (OK != g_dev->init()) { delete g_dev; @@ -2642,17 +2670,17 @@ monitor(void) read(0, &c, 1); if (cancels-- == 0) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ exit(0); } } if (g_dev != nullptr) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ (void)g_dev->print_status(); (void)g_dev->print_debug(); - printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n"); + printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n"); } else { errx(1, "driver not loaded, exiting"); @@ -2695,6 +2723,7 @@ px4io_main(int argc, char *argv[]) printf("[px4io] loaded, detaching first\n"); /* stop the driver */ delete g_dev; + g_dev = nullptr; } PX4IO_Uploader *up; @@ -2767,18 +2796,30 @@ px4io_main(int argc, char *argv[]) } if (g_dev == nullptr) { warnx("px4io is not started, still attempting upgrade"); - } else { - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); + + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) { + delete interface; + errx(1, "driver alloc failed"); } + } - // tear down the px4io instance - delete g_dev; + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); } + // tear down the px4io instance + delete g_dev; + g_dev = nullptr; + // upload the specified firmware const char *fn[2]; fn[0] = argv[3]; @@ -2836,6 +2877,7 @@ px4io_main(int argc, char *argv[]) /* stop the driver */ delete g_dev; + g_dev = nullptr; exit(0); } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 41f93a8ee..dd8abbac5 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -51,6 +51,7 @@ #include <poll.h> #include <termios.h> #include <sys/stat.h> +#include <nuttx/arch.h> #include <crc32.h> @@ -120,8 +121,15 @@ PX4IO_Uploader::upload(const char *filenames[]) cfsetspeed(&t, 115200); tcsetattr(_io_fd, TCSANOW, &t); - /* look for the bootloader */ - ret = sync(); + /* look for the bootloader for 150 ms */ + for (int i = 0; i < 15; i++) { + ret = sync(); + if (ret == OK) { + break; + } else { + usleep(10000); + } + } if (ret != OK) { /* this is immediately fatal */ @@ -226,6 +234,11 @@ PX4IO_Uploader::upload(const char *filenames[]) close(_fw_fd); close(_io_fd); _io_fd = -1; + + // sleep for enough time for the IO chip to boot. This makes + // forceupdate more reliably startup IO again after update + up_udelay(100*1000); + return ret; } diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 22387a3e2..55f63eef9 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -91,7 +91,7 @@ private: void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 1000); + int get_sync(unsigned timeout = 40); int sync(); int get_info(int param, uint32_t &val); int erase(); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index b286e0007..067d77364 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -181,11 +181,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v * Calculate heading error of current position to desired position */ - /* - * PX4 uses 1e7 scaled integers to represent global coordinates for max resolution, - * so they need to be scaled by 1e7 and converted to IEEE double precision floating point. - */ - float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d); + float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon); /* calculate heading error */ float yaw_err = att->yaw - bearing; diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 08fe2b696..446cd5bee 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -72,7 +72,6 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ - const double r_earth = 6371000; double lat1 = phi_1; double lon1 = lambda_0; @@ -81,7 +80,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are double lon2 = lambda_0 + 0.5 / 180 * M_PI; double sin_lat_2 = sin(lat2); double cos_lat_2 = cos(lat2); - double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; + double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH; /* 2) calculate distance rho on plane */ double k_bar = 0; @@ -188,8 +187,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); - const double radius_earth = 6371000.0d; - return radius_earth * c; + return CONSTANTS_RADIUS_OF_EARTH * c; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) @@ -210,7 +208,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub return theta; } -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -221,11 +219,11 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - *vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); - *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon); + *v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + *v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); } -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -236,8 +234,17 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - *vy = CONSTANTS_RADIUS_OF_EARTH * d_lon; - *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad); + *v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat; + *v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad); +} + +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + + *lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; + *lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; } // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 5f4bce698..94afb4df0 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -115,9 +115,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e); -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e); + +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res); __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index aca3fe7b6..668bac5d9 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -314,14 +314,13 @@ void KalmanNav::updatePublications() // global position publication _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; - _pos.valid = true; - _pos.lat = getLatDegE7(); - _pos.lon = getLonDegE7(); + _pos.global_valid = true; + _pos.lat = lat * M_RAD_TO_DEG; + _pos.lon = lon * M_RAD_TO_DEG; _pos.alt = float(alt); - _pos.relative_alt = float(alt); // TODO, make relative - _pos.vx = vN; - _pos.vy = vE; - _pos.vz = vD; + _pos.vel_n = vN; + _pos.vel_e = vE; + _pos.vel_d = vD; _pos.yaw = psi; // local position publication diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 6a1bec153..620185fb7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -410,13 +410,13 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + } else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; - vel(0) = global_pos.vx; - vel(1) = global_pos.vy; - vel(2) = global_pos.vz; + vel(0) = global_pos.vel_n; + vel(1) = global_pos.vel_e; + vel(2) = global_pos.vel_d; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bec6884e2..e1cca8bfb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -200,9 +200,11 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status); + +void set_control_mode(); void print_reject_mode(const char *msg); @@ -414,51 +416,45 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set main state */ transition_result_t main_res = TRANSITION_DENIED; - if (status->rc_signal_lost) { - /* allow mode switching by command only if no RC signal */ - if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { - /* use autopilot-specific mode */ - if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { - /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); - - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { - /* OFFBOARD */ - main_res = main_state_transition(status, MAIN_STATE_OFFBOARD); - } + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); - } else { - /* use base mode */ - if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); - - } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { - if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); - - } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { - /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } - } + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { + /* OFFBOARD */ + main_res = main_state_transition(status, MAIN_STATE_OFFBOARD); } } else { - mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd"); + /* use base mode */ + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } + } } if (main_res == TRANSITION_CHANGED) @@ -527,7 +523,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); + transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -566,17 +562,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } static struct vehicle_status_s status; - -/* armed topic */ +static struct vehicle_control_mode_s control_mode; static struct actuator_armed_s armed; - static struct safety_s safety; int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ commander_initialized = false; - bool home_position_set = false; bool battery_tune_played = false; bool arm_tune_played = false; @@ -590,6 +583,28 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); + char *main_states_str[MAIN_STATE_MAX]; + main_states_str[0] = "MANUAL"; + main_states_str[1] = "SEATBELT"; + main_states_str[2] = "EASY"; + main_states_str[3] = "AUTO"; + main_states_str[4] = "OFFBOARD"; + + char *arming_states_str[ARMING_STATE_MAX]; + arming_states_str[0] = "INIT"; + arming_states_str[1] = "STANDBY"; + arming_states_str[2] = "ARMED"; + arming_states_str[3] = "ARMED_ERROR"; + arming_states_str[4] = "STANDBY_ERROR"; + arming_states_str[5] = "REBOOT"; + arming_states_str[6] = "IN_AIR_RESTORE"; + + char *failsafe_states_str[FAILSAFE_STATE_MAX]; + failsafe_states_str[0] = "NORMAL"; + failsafe_states_str[1] = "RTL"; + failsafe_states_str[2] = "LAND"; + failsafe_states_str[3] = "TERMINATION"; + /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -604,21 +619,15 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - /* Main state machine */ - /* make sure we are in preflight state */ + /* vehicle status topic */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value - - /* armed topic */ - orb_advert_t armed_pub; - /* Initialize armed with all false */ - memset(&armed, 0, sizeof(armed)); - status.main_state = MAIN_STATE_MANUAL; status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; + status.failsafe_state = FAILSAFE_STATE_NORMAL; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -635,14 +644,20 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), &status); - /* publish current state machine */ - - /* publish initial state */ status.counter++; status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); + + /* publish initial state */ + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + + /* armed topic */ + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* vehicle control mode topic */ + memset(&control_mode, 0, sizeof(control_mode)); + orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); @@ -703,10 +718,15 @@ int commander_thread_main(int argc, char *argv[]) struct manual_control_setpoint_s sp_man; memset(&sp_man, 0, sizeof(sp_man)); + /* Subscribe to offboard control data */ + int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s offboard_sp; + memset(&offboard_sp, 0, sizeof(offboard_sp)); + /* Subscribe to offboard control data */ - int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s offboard_sp; - memset(&offboard_sp, 0, sizeof(offboard_sp)); + int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s sp_offboard; + memset(&sp_offboard, 0, sizeof(sp_offboard)); /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -820,12 +840,16 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - orb_check(offboard_sp_sub, &updated); - if (updated) { orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp); } + orb_check(sp_offboard_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + } + orb_check(sensor_sub, &updated); if (updated) { @@ -862,7 +886,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed); /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -1018,19 +1042,18 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (!home_position_set && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.valid) { - /* copy position data to uORB home message, store it locally as well */ - + && global_position.global_valid) { - home.lat = (double)global_position.lat / 1e7d; - home.lon = (double)global_position.lon / 1e7d; - home.altitude = (float)global_position.alt; + /* copy position data to uORB home message, store it locally as well */ + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { @@ -1041,13 +1064,13 @@ int commander_thread_main(int argc, char *argv[]) } /* mark home position as set */ - home_position_set = true; + status.condition_home_position_valid = true; tune_positive(); } } /* start RC input check */ - if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1122,24 +1145,27 @@ int commander_thread_main(int argc, char *argv[]) } } else if (res == TRANSITION_DENIED) { - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + } + + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } - /* fill current_status according to mode switches */ + /* fill status according to mode switches */ check_mode_switches(&sp_man, &status); - /* evaluate the main state machine */ - res = check_main_state_machine(&status); + /* evaluate the main state machine according to mode switches */ + res = set_main_state_rc(&status); if (res == TRANSITION_CHANGED) { - //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); tune_positive(); } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } } else { @@ -1149,26 +1175,66 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && armed.armed && status.main_state != MAIN_STATE_OFFBOARD) { - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + /* switch to OFFBOARD mode if offboard signal available */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); + if (res == TRANSITION_DENIED) { + /* can't switch to OFFBOARD, do normal failsafe if needed */ + if (armed.armed) { + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); - } else if (status.main_state != MAIN_STATE_SEATBELT) { - res = main_state_transition(&status, MAIN_STATE_SEATBELT); + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } + + } else if (status.main_state == MAIN_STATE_OFFBOARD) { + /* check if OFFBOARD mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD); + + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } + + } else { + /* failsafe for manual modes */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + + if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } + } + + } else { + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* reset failsafe when disarmed */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } } } } /* check offboard signal */ - if (hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) { + if (offboard_sp.timestamp != 0 && hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) { if (!status.offboard_control_signal_found_once) { status.offboard_control_signal_found_once = true; mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time"); @@ -1189,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); if (res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); + mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); } } @@ -1211,19 +1277,14 @@ int commander_thread_main(int argc, char *argv[]) } } - /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ - if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); - - if (flighttermination_res == TRANSITION_CHANGED) { + // TODO remove this hack + /* flight termination in manual mode if assisted switch is on easy position */ + if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } - - } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } - /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -1239,19 +1300,35 @@ int commander_thread_main(int argc, char *argv[]) /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); - bool flighttermination_state_changed = check_flighttermination_state_changed(); + bool failsafe_state_changed = check_failsafe_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); + /* print new state */ + if (arming_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); + } + + if (main_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + } + + if (failsafe_state_changed) { status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + set_control_mode(); + control_mode.timestamp = t1; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1317,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[]) led_deinit(); buzzer_deinit(); close(sp_man_sub); - close(offboard_sp_sub); + close(sp_offboard_sub); close(local_position_sub); close(global_position_sub); close(gps_sub); @@ -1424,133 +1501,235 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) { /* main mode switch */ if (!isfinite(sp_man->mode_switch)) { warnx("mode sw not finite"); - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_AUTO; + status->mode_switch = MODE_SWITCH_AUTO; } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else { - current_status->mode_switch = MODE_SWITCH_ASSISTED; + status->mode_switch = MODE_SWITCH_ASSISTED; } /* return switch */ if (!isfinite(sp_man->return_switch)) { - current_status->return_switch = RETURN_SWITCH_NONE; + status->return_switch = RETURN_SWITCH_NONE; } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - current_status->return_switch = RETURN_SWITCH_RETURN; + status->return_switch = RETURN_SWITCH_RETURN; } else { - current_status->return_switch = RETURN_SWITCH_NORMAL; + status->return_switch = RETURN_SWITCH_NORMAL; } /* assisted switch */ if (!isfinite(sp_man->assisted_switch)) { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - current_status->assisted_switch = ASSISTED_SWITCH_EASY; + status->assisted_switch = ASSISTED_SWITCH_EASY; } else { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } /* mission switch */ if (!isfinite(sp_man->mission_switch)) { - current_status->mission_switch = MISSION_SWITCH_NONE; + status->mission_switch = MISSION_SWITCH_NONE; } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - current_status->mission_switch = MISSION_SWITCH_LOITER; + status->mission_switch = MISSION_SWITCH_LOITER; } else { - current_status->mission_switch = MISSION_SWITCH_MISSION; + status->mission_switch = MISSION_SWITCH_MISSION; } - /* offboard switch */ + /* offboard switch */ if (!isfinite(sp_man->offboard_switch)) { - current_status->offboard_switch = OFFBOARD_SWITCH_NONE; + status->offboard_switch = OFFBOARD_SWITCH_NONE; } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { - current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; } else { - current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; + status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; } } transition_result_t -check_main_state_machine(struct vehicle_status_s *current_status) +set_main_state_rc(struct vehicle_status_s *status) { - /* evaluate the main state machine */ + /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; - if (current_status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { - /* offboard switch overrides main switch */ - res = main_state_transition(current_status, MAIN_STATE_OFFBOARD); + /* offboard switch overrides main switch */ + if (status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { + res = main_state_transition(status, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { + print_reject_mode("OFFBOARD"); - } else { - switch (current_status->mode_switch) { - case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + } else { + return res; + } + } - case MODE_SWITCH_ASSISTED: - if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY); + /* offboard switched off or denied, check mode switch */ + switch (status->mode_switch) { + case MODE_SWITCH_MANUAL: + res = main_state_transition(status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + case MODE_SWITCH_ASSISTED: + if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(status, MAIN_STATE_EASY); - // else fallback to SEATBELT - print_reject_mode("EASY"); - } + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + // else fallback to SEATBELT + print_reject_mode("EASY"); + } - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this mode + res = main_state_transition(status, MAIN_STATE_SEATBELT); - if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages - print_reject_mode("SEATBELT"); + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this mode - // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + print_reject_mode("SEATBELT"); - case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO); + // else fallback to MANUAL + res = main_state_transition(status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + case MODE_SWITCH_AUTO: + res = main_state_transition(status, MAIN_STATE_AUTO); - // else fallback to SEATBELT (EASY likely will not work too) - print_reject_mode("AUTO"); - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + // else fallback to SEATBELT (EASY likely will not work too) + print_reject_mode("AUTO"); + res = main_state_transition(status, MAIN_STATE_SEATBELT); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to MANUAL + res = main_state_transition(status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; + + default: + break; + } - // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here + return res; +} + +void +set_control_mode() +{ + /* set vehicle_control_mode according to main state and failsafe state */ + control_mode.flag_armed = armed.armed; + control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + + control_mode.flag_control_termination_enabled = false; + + /* set this flag when navigator should act */ + bool navigator_enabled = false; + + switch (status.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (status.main_state) { + case MAIN_STATE_MANUAL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = status.is_rotary_wing; + control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; break; + case MAIN_STATE_EASY: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + default: break; } + + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_LAND: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + /* disable all controllers on termination */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_termination_enabled = true; + break; + + default: + break; } - return res; + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + } } void diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4dd63b5e1..23d3be55d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -63,7 +63,7 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool flighttermination_state_changed = true; +static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, @@ -220,61 +220,66 @@ check_arming_state_changed() } transition_result_t -main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) +main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_main_state == current_state->main_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; - } else { + case MAIN_STATE_SEATBELT: - switch (new_main_state) { - case MAIN_STATE_MANUAL: + /* need at minimum altitude estimate */ + if (!status->is_rotary_wing || + (status->condition_local_altitude_valid || + status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; - break; - - case MAIN_STATE_SEATBELT: - - /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } + } - break; + break; - case MAIN_STATE_EASY: + case MAIN_STATE_EASY: - /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } + /* need at minimum local position estimate */ + if (status->condition_local_position_valid || + status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } - break; + break; - case MAIN_STATE_AUTO: + case MAIN_STATE_AUTO: - /* need global position estimate */ - if (current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } + /* need global position estimate */ + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } - break; + break; - case MAIN_STATE_OFFBOARD: + case MAIN_STATE_OFFBOARD: + /* need global position estimate */ + if (!status->offboard_control_signal_lost) { ret = TRANSITION_CHANGED; - - break; } - if (ret == TRANSITION_CHANGED) { - current_state->main_state = new_main_state; + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + if (status->main_state != new_main_state) { + status->main_state = new_main_state; main_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; } } @@ -294,10 +299,10 @@ check_main_state_changed() } bool -check_flighttermination_state_changed() +check_failsafe_state_changed() { - if (flighttermination_state_changed) { - flighttermination_state_changed = false; + if (failsafe_state_changed) { + failsafe_state_changed = false; return true; } else { @@ -368,28 +373,49 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /** -* Transition from one flightermination state to another +* Transition from one failsafe state to another */ -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->flighttermination_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { + /* transitions from TERMINATION to other states not allowed */ + if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { + ret = TRANSITION_NOT_CHANGED; + } } else { - - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: + switch (new_failsafe_state) { + case FAILSAFE_STATE_NORMAL: + /* always allowed (except from TERMINATION state) */ ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; - warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); break; - case FLIGHTTERMINATION_STATE_OFF: + case FAILSAFE_STATE_RTL: + /* global position and home position required for RTL */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->set_nav_state = NAV_STATE_RTL; + status->set_nav_state_timestamp = hrt_absolute_time(); + ret = TRANSITION_CHANGED; + } + + break; + + case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ + if (status->condition_local_altitude_valid || status->condition_global_position_valid) { + status->set_nav_state = NAV_STATE_LAND; + status->set_nav_state_timestamp = hrt_absolute_time(); + ret = TRANSITION_CHANGED; + } + + break; + + case FAILSAFE_STATE_TERMINATION: + /* always allowed */ ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; break; default: @@ -397,9 +423,13 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s * } if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + if (status->failsafe_state != new_failsafe_state) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; + } } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e569fb4f3..f04879ff9 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state); +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); bool check_navigation_state_changed(); -bool check_flighttermination_state_changed(); +bool check_failsafe_state_changed(); void set_navigation_state_changed(); diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index e213ac17f..e8fecef0d 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -54,8 +54,8 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {}; void BlockWaypointGuidance::update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - mission_item_s &missionCmd, - mission_item_s &lastMissionCmd) + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd) { // heading to waypoint @@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20), + _missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20), _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 8cc0d77d4..7c80c4b2b 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -43,7 +43,7 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> @@ -82,8 +82,8 @@ public: virtual ~BlockWaypointGuidance(); void update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - mission_item_s &missionCmd, - mission_item_s &lastMissionCmd); + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd); float getPsiCmd() { return _psiCmd; } }; @@ -98,7 +98,7 @@ protected: UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; UOrbSubscription<vehicle_global_position_s> _pos; - UOrbSubscription<mission_item_triplet_s> _missionCmd; + UOrbSubscription<position_setpoint_triplet_s> _missionCmd; UOrbSubscription<manual_control_setpoint_s> _manual; UOrbSubscription<vehicle_status_s> _status; UOrbSubscription<parameter_update_s> _param_update; diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 108e9896d..f7c0b6148 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -174,9 +174,9 @@ void BlockMultiModeBacksideAutopilot::update() // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); @@ -236,9 +236,9 @@ void BlockMultiModeBacksideAutopilot::update() // for the purpose of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index b4dbc36b0..e1c85c261 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -264,7 +264,7 @@ private: BlockParamFloat _crMax; struct pollfd _attPoll; - mission_item_triplet_s _lastMissionCmd; + position_setpoint_triplet_s _lastMissionCmd; enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; uint64_t _timeStamp; public: diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 73df3fb9e..888dd0942 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -299,7 +299,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon, (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); printf("next wp direction: %0.4f\n", (double)psi_track); 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 e49b3c140..17b1028f9 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main() } /* Simple handling of failsafe: deploy parachute if failsafe is on */ - if (_vcontrol_mode.flag_control_flighttermination_enabled) { + if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[1] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { @@ -704,9 +704,9 @@ FixedwingAttitudeControl::task_main() float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz; - speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; - speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; + speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { warnx("Did not get a valid R\n"); } 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 d8dbf9085..a62b53221 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 @@ -68,7 +68,7 @@ #include <uORB/uORB.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> @@ -126,7 +126,7 @@ private: int _control_task; /**< task handle for sensor task */ int _global_pos_sub; - int _mission_item_triplet_sub; + int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ @@ -145,7 +145,7 @@ private: struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -332,10 +332,10 @@ private: * Control position. */ bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, - const struct mission_item_triplet_s &_mission_item_triplet); + const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -367,7 +367,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* subscriptions */ _global_pos_sub(-1), - _mission_item_triplet_sub(-1), + _pos_sp_triplet_sub(-1), _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), @@ -406,7 +406,7 @@ FixedwingPositionControl::FixedwingPositionControl() : airspeed_s _airspeed = {0}; vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; - mission_item_triplet_s _mission_item_triplet = {0}; + position_setpoint_triplet_s _pos_sp_triplet = {0}; sensor_combined_s _sensor_combined = {0}; @@ -653,11 +653,11 @@ void FixedwingPositionControl::vehicle_setpoint_poll() { /* check if there is a new setpoint */ - bool mission_item_triplet_updated; - orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated); + bool pos_sp_triplet_updated; + orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated); - if (mission_item_triplet_updated) { - orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet); + if (pos_sp_triplet_updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); _setpoint_valid = true; } } @@ -700,7 +700,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { if (_global_pos_valid) { @@ -713,12 +713,12 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; float delta_altitude = 0.0f; - if (mission_item_triplet.previous_valid) { - distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon); - delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude; + if (pos_sp_triplet.previous.valid) { + distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt; } else { - distance = get_distance_to_next_waypoint(current_position(0), current_position(1), mission_item_triplet.current.lat, mission_item_triplet.current.lon); - delta_altitude = mission_item_triplet.current.altitude - _global_pos.alt; + distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt; } float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance)); @@ -751,11 +751,11 @@ void FixedwingPositionControl::navigation_capabilities_publish() bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, - const struct mission_item_triplet_s &mission_item_triplet) + const struct position_setpoint_triplet_s &pos_sp_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet); + calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -767,7 +767,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_earth = _R_nb * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt; + float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ float throttle_max = 1.0f; @@ -785,58 +785,56 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ - math::Vector<2> next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* current waypoint (the one currently heading for) */ - math::Vector<2> curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* previous waypoint */ math::Vector<2> prev_wp; - if (mission_item_triplet.previous_valid) { - prev_wp(0) = mission_item_triplet.previous.lat; - prev_wp(1) = mission_item_triplet.previous.lon; + if (pos_sp_triplet.previous.valid) { + prev_wp(0) = pos_sp_triplet.previous.lat; + prev_wp(1) = pos_sp_triplet.previous.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp(0) = mission_item_triplet.current.lat; - prev_wp(1) = mission_item_triplet.current.lon; + prev_wp(0) = pos_sp_triplet.current.lat; + prev_wp(1) = pos_sp_triplet.current.lon; } - if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius, - mission_item_triplet.current.loiter_direction, ground_speed); + _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, + pos_sp_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ @@ -847,7 +845,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* heading hold, along the line connecting this and the last waypoint */ if (!land_noreturn_horizontal) {//set target_bearing in first occurrence - if (mission_item_triplet.previous_valid) { + if (pos_sp_triplet.previous.valid) { target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); } else { target_bearing = _att.yaw; @@ -879,23 +877,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // /* do not go down too early */ // if (wp_distance > 50.0f) { -// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; +// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt; // } /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) + float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1) float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -914,12 +912,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude); + float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) { - flare_curve_alt = mission_item_triplet.current.altitude; + flare_curve_alt = pos_sp_triplet.current.alt; land_stayonground = true; } @@ -977,7 +975,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ // warnx("Launch detection running"); @@ -1011,9 +1009,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)), + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); @@ -1022,7 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1037,14 +1035,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1), - // (double)next_wp(0), (double)next_wp(1), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); + // (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid"); // XXX at this point we always want no loiter hold if a // mission is active _loiter_hold = false; /* reset land state */ - if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) { + if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { land_noreturn_horizontal = false; land_noreturn_vertical = false; land_stayonground = false; @@ -1053,7 +1051,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ - if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { + if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; } @@ -1176,7 +1174,7 @@ FixedwingPositionControl::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1264,14 +1262,14 @@ FixedwingPositionControl::task_main() vehicle_airspeed_poll(); // vehicle_baro_poll(); - math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy); + math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. */ - if (control_position(current_position, ground_speed, _mission_item_triplet)) { + if (control_position(current_position, ground_speed, _pos_sp_triplet)) { _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ @@ -1285,7 +1283,7 @@ FixedwingPositionControl::task_main() } /* XXX check if radius makes sense here */ - float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius); + float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 70229558c..9a50c059b 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -199,8 +199,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } /* arming state */ - if (v_status.arming_state == ARMING_STATE_ARMED - || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + if (armed.armed) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -208,31 +207,40 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; union px4_custom_mode custom_mode; custom_mode.data = 0; - if (v_status.main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (v_status.main_state == MAIN_STATE_SEATBELT) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (v_status.main_state == MAIN_STATE_EASY) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (v_status.main_state == MAIN_STATE_AUTO) { + if (pos_sp_triplet.nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (v_status.main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (v_status.main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (v_status.main_state == MAIN_STATE_AUTO) { + /* this must not happen */ + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (v_status.main_state == MAIN_STATE_OFFBOARD) { + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; + } + } else { + /* use navigation state when navigator is active */ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_READY) { + if (pos_sp_triplet.nav_state == NAV_STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_LOITER) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_MISSION) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (control_mode.nav_state == NAV_STATE_RTL) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } - } else if (v_status.main_state == MAIN_STATE_OFFBOARD) { - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 62597d91a..b75305406 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -664,13 +664,13 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); // global position packet hil_global_pos.timestamp = timestamp; - hil_global_pos.valid = true; + hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - hil_global_pos.vy = hil_state.vy / 100.0f; - hil_global_pos.vz = hil_state.vz / 100.0f; + hil_global_pos.vel_n = hil_state.vx / 100.0f; + hil_global_pos.vel_e = hil_state.vy / 100.0f; + hil_global_pos.vel_d = hil_state.vz / 100.0f; } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 6e177bc4d..69cd820a0 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -67,9 +67,10 @@ extern bool gcs_link; struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; +struct home_position_s home; struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; -struct vehicle_control_mode_s control_mode; +struct position_setpoint_triplet_s pos_sp_triplet; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; @@ -126,7 +127,6 @@ static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); static void l_nav_cap(const struct listener *l); -static void l_control_mode(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -153,7 +153,6 @@ static const struct listener listeners[] = { {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, - {l_control_mode, &mavlink_subs.control_mode_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -247,10 +246,10 @@ l_vehicle_attitude(const struct listener *l) hrt_abstime t = hrt_absolute_time(); if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; - float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d); } /* send quaternion values if it exists */ @@ -314,6 +313,7 @@ l_vehicle_status(const struct listener *l) /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.position_setpoint_triplet_sub, &pos_sp_triplet); /* enable or disable HIL */ if (v_status.hil_state == HIL_STATE_ON) @@ -380,13 +380,13 @@ l_global_position(const struct listener *l) mavlink_msg_global_position_int_send(MAVLINK_COMM_0, global_pos.timestamp / 1000, - global_pos.lat, - global_pos.lon, + global_pos.lat * 1e7, + global_pos.lon * 1e7, global_pos.alt * 1000.0f, - global_pos.relative_alt * 1000.0f, - global_pos.vx * 100.0f, - global_pos.vy * 100.0f, - global_pos.vz * 100.0f, + (global_pos.alt - home.alt) * 1000.0f, + global_pos.vel_n * 100.0f, + global_pos.vel_e * 100.0f, + global_pos.vel_d * 100.0f, _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } @@ -410,23 +410,18 @@ l_local_position(const struct listener *l) void l_global_position_setpoint(const struct listener *l) { - struct mission_item_triplet_s triplet; - orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); + struct position_setpoint_triplet_s triplet; + orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet); - uint8_t coordinate_frame = MAV_FRAME_GLOBAL; - - if (!triplet.current_valid) + if (!triplet.current.valid) return; - if (triplet.current.altitude_is_relative) - coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - if (gcs_link) mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, - coordinate_frame, + MAV_FRAME_GLOBAL, (int32_t)(triplet.current.lat * 1e7d), (int32_t)(triplet.current.lon * 1e7d), - (int32_t)(triplet.current.altitude * 1e3f), + (int32_t)(triplet.current.alt * 1e3f), (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); } @@ -662,11 +657,9 @@ l_optical_flow(const struct listener *l) void l_home(const struct listener *l) { - struct home_position_s home; - orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); } void @@ -688,26 +681,6 @@ l_nav_cap(const struct listener *l) } -void -l_control_mode(const struct listener *l) -{ - orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode); - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); -} - static void * uorb_receive_thread(void *arg) { @@ -783,9 +756,9 @@ uorb_receive_start(void) status_sub = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ - /* --- CONTROL MODE --- */ - mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */ + /* --- POSITION SETPOINT TRIPLET --- */ + mavlink_subs.position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + orb_set_interval(mavlink_subs.position_setpoint_triplet_sub, 0); /* not polled, don't limit */ /* --- RC CHANNELS VALUE --- */ rc_sub = orb_subscribe(ORB_ID(rc_channels)); @@ -804,7 +777,7 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ /* --- GLOBAL SETPOINT VALUE --- */ - mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */ /* --- LOCAL SETPOINT VALUE --- */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 4d428406a..89f647bdc 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -51,9 +51,8 @@ #include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/mission_item_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_control_mode.h> @@ -95,7 +94,7 @@ struct mavlink_subscriptions { int home_sub; int airspeed_sub; int navigation_capabilities_sub; - int control_mode_sub; + int position_setpoint_triplet_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -112,8 +111,8 @@ extern struct navigation_capabilities_s nav_cap; /** Vehicle status */ extern struct vehicle_status_s v_status; -/** Vehicle control mode */ -extern struct vehicle_control_mode_s control_mode; +/** Position setpoint triplet */ +extern struct position_setpoint_triplet_s pos_sp_triplet; /** RC channels */ extern struct rc_channels_s rc; diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index 86bfa26f2..bbc9f6e66 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -50,7 +50,7 @@ #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> 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 245ac024b..44baccefc 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -85,7 +85,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THROTTLE 0.3f -#define YAW_DEADZONE 0.01f +#define YAW_DEADZONE 0.05f #define RATES_I_LIMIT 0.5f class MulticopterAttitudeControl 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 5ce4500cd..fe8377a40 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -63,9 +63,8 @@ #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/parameter_update.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <systemlib/param/param.h> #include <systemlib/err.h> @@ -109,6 +108,7 @@ private: bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ + int _mavlink_fd; /**< mavlink fd */ int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ @@ -116,11 +116,11 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ - int _local_pos_sub; /**< vehicle local position */ - int _mission_items_sub; /**< mission item triplet */ + int _global_pos_sub; /**< vehicle local position */ + int _pos_sp_triplet_sub; /**< position setpoint triplet */ - orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _pos_sp_triplet_pub; /**< position setpoint triplet publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */ struct vehicle_attitude_s _att; /**< vehicle attitude */ @@ -128,9 +128,8 @@ private: struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_local_position_s _local_pos; /**< vehicle local position */ - struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */ - struct mission_item_triplet_s _mission_items; /**< vehicle global position setpoint */ + struct vehicle_global_position_s _global_pos; /**< vehicle global position */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct { @@ -177,9 +176,15 @@ private: math::Vector<3> sp_offs_max; } _params; - math::Vector<3> _pos; + double _lat_sp; + double _lon_sp; + float _alt_sp; + + bool _reset_lat_lon_sp; + bool _reset_alt_sp; + bool _use_global_alt; /**< switch between global (AMSL) and barometric altitudes */ + math::Vector<3> _vel; - math::Vector<3> _pos_sp; math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ @@ -201,6 +206,21 @@ private: static float scale_control(float ctl, float end, float dz); /** + * Reset lat/lon to current position + */ + void reset_lat_lon_sp(); + + /** + * Reset altitude setpoint to current altitude + */ + void reset_alt_sp(); + + /** + * Select between barometric and global (AMSL) altitudes + */ + void select_alt(bool global); + + /** * Shim for calling task_main from task_create. */ static void task_main_trampoline(int argc, char *argv[]); @@ -227,6 +247,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _task_should_exit(false), _control_task(-1), + _mavlink_fd(-1), /* subscriptions */ _att_sub(-1), @@ -235,22 +256,29 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_sub(-1), _manual_sub(-1), _arming_sub(-1), - _local_pos_sub(-1), - _mission_items_sub(-1), + _global_pos_sub(-1), + _pos_sp_triplet_sub(-1), /* publications */ - _local_pos_sp_pub(-1), _att_sp_pub(-1), - _global_vel_sp_pub(-1) + _pos_sp_triplet_pub(-1), + _global_vel_sp_pub(-1), + + _lat_sp(0.0), + _lon_sp(0.0), + _alt_sp(0.0f), + + _reset_lat_lon_sp(true), + _reset_alt_sp(true), + _use_global_alt(false) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); memset(&_arming, 0, sizeof(_arming)); - memset(&_local_pos, 0, sizeof(_local_pos)); - memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); - memset(&_mission_items, 0, sizeof(_mission_items)); + memset(&_global_pos, 0, sizeof(_global_pos)); + memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); _params.pos_p.zero(); @@ -261,9 +289,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params.vel_ff.zero(); _params.sp_offs_max.zero(); - _pos.zero(); _vel.zero(); - _pos_sp.zero(); _vel_sp.zero(); _vel_prev.zero(); @@ -405,10 +431,10 @@ MulticopterPositionControl::poll_subscriptions() if (updated) orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); - orb_check(_mission_items_sub, &updated); + orb_check(_global_pos_sub, &updated); if (updated) - orb_copy(ORB_ID(mission_item_triplet), _mission_items_sub, &_mission_items); + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } float @@ -432,13 +458,48 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) } void +MulticopterPositionControl::reset_lat_lon_sp() +{ + if (_reset_lat_lon_sp) { + _reset_lat_lon_sp = false; + _lat_sp = _global_pos.lat; + _lon_sp = _global_pos.lon; + mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp); + } +} + +void +MulticopterPositionControl::reset_alt_sp() +{ + if (_reset_alt_sp) { + _reset_alt_sp = false; + _alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt; + mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp); + } +} + +void +MulticopterPositionControl::select_alt(bool global) +{ + if (global != _use_global_alt) { + _use_global_alt = global; + if (global) { + /* switch from barometric to global altitude */ + _alt_sp += _global_pos.alt - _global_pos.baro_alt; + } else { + /* switch from global to barometric altitude */ + _alt_sp += _global_pos.baro_alt - _global_pos.alt; + } + } +} + +void MulticopterPositionControl::task_main() { warnx("started"); - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[mpc] started"); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions @@ -449,8 +510,8 @@ MulticopterPositionControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); - _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _mission_items_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); parameters_update(true); @@ -460,8 +521,6 @@ MulticopterPositionControl::task_main() /* get an initial update for all sensor and status data */ poll_subscriptions(); - bool reset_sp_z = true; - bool reset_sp_xy = true; bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; @@ -472,11 +531,6 @@ MulticopterPositionControl::task_main() const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - hrt_abstime ref_timestamp = 0; - int32_t ref_lat = 0.0f; - int32_t ref_lon = 0.0f; - float ref_alt = 0.0f; - math::Vector<3> sp_move_rate; sp_move_rate.zero(); math::Vector<3> thrust_int; @@ -488,7 +542,7 @@ MulticopterPositionControl::task_main() struct pollfd fds[1]; /* Setup of loop */ - fds[0].fd = _local_pos_sub; + fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; while (!_task_should_exit) { @@ -505,8 +559,6 @@ MulticopterPositionControl::task_main() continue; } - orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); - poll_subscriptions(); parameters_update(false); @@ -516,8 +568,8 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ - reset_sp_z = true; - reset_sp_xy = true; + _reset_lat_lon_sp = true; + _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -529,60 +581,34 @@ MulticopterPositionControl::task_main() _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - _pos(0) = _local_pos.x; - _pos(1) = _local_pos.y; - _pos(2) = _local_pos.z; - _vel(0) = _local_pos.vx; - _vel(1) = _local_pos.vy; - _vel(2) = _local_pos.vz; + _vel(0) = _global_pos.vel_n; + _vel(1) = _global_pos.vel_e; + _vel(2) = _global_pos.vel_d; sp_move_rate.zero(); - if (_local_pos.ref_timestamp != ref_timestamp) { - /* initialize local projection with new reference */ - double lat_home = _local_pos.ref_lat * 1e-7; - double lon_home = _local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); - - if (_control_mode.flag_control_manual_enabled && ref_timestamp != 0) { - /* correct setpoint in manual mode to stay in the same point */ - float ref_change_x = 0.0f; - float ref_change_y = 0.0f; - map_projection_project(ref_lat, ref_lon, &ref_change_x, &ref_change_y); - _pos_sp(0) += ref_change_x; - _pos_sp(1) += ref_change_y; - _pos_sp(2) += _local_pos.ref_alt - ref_alt; - } - - ref_timestamp = _local_pos.ref_timestamp; - ref_lat = _local_pos.ref_lat; - ref_lon = _local_pos.ref_lon; - ref_alt = _local_pos.ref_alt; - } + float alt = _global_pos.alt; + /* select control source */ if (_control_mode.flag_control_manual_enabled) { + /* select altitude source and update setpoint */ + select_alt(_global_pos.global_valid); + if (!_use_global_alt) { + alt = _global_pos.baro_alt; + } + /* manual control */ if (_control_mode.flag_control_altitude_enabled) { - /* reset setpoint Z to current altitude if needed */ - if (reset_sp_z) { - reset_sp_z = false; - _pos_sp(2) = _pos(2); - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - _pos_sp(2)); - } + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); /* move altitude setpoint with throttle stick */ sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz); } if (_control_mode.flag_control_position_enabled) { - /* reset setpoint XY to current position if needed */ - if (reset_sp_xy) { - reset_sp_xy = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); - } + /* reset lat/lon setpoint to current position if needed */ + reset_lat_lon_sp(); /* move position setpoint with roll/pitch stick */ sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); @@ -602,353 +628,390 @@ MulticopterPositionControl::task_main() sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); /* move position setpoint */ - _pos_sp += sp_move_rate * dt; + add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp); + _alt_sp -= sp_move_rate(2) * dt; /* check if position setpoint is too far from actual position */ math::Vector<3> pos_sp_offs; pos_sp_offs.zero(); if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]); + pos_sp_offs(0) /= _params.sp_offs_max(0); + pos_sp_offs(1) /= _params.sp_offs_max(1); } if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); + _alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2); } - } else { - /* AUTO */ - if (_mission_items.current_valid) { - struct mission_item_s item = _mission_items.current; + /* fill position setpoint triplet */ + _pos_sp_triplet.previous.valid = true; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.next.valid = true; + + _pos_sp_triplet.nav_state = NAV_STATE_NONE; + _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; + _pos_sp_triplet.current.lat = _lat_sp; + _pos_sp_triplet.current.lon = _lon_sp; + _pos_sp_triplet.current.alt = _alt_sp; + _pos_sp_triplet.current.yaw = _att_sp.yaw_body; + _pos_sp_triplet.current.loiter_radius = 0.0f; + _pos_sp_triplet.current.loiter_direction = 1.0f; + _pos_sp_triplet.current.pitch_min = 0.0f; + + /* publish position setpoint triplet */ + if (_pos_sp_triplet_pub > 0) { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); - // TODO home altitude can be != ref_alt, check home_position topic - _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); + } else { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); + } - map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); + } else if (_control_mode.flag_control_auto_enabled) { + /* always use AMSL altitude for AUTO */ + select_alt(true); - if (isfinite(_mission_items.current.yaw)) { - _att_sp.yaw_body = _mission_items.current.yaw; - } + /* AUTO */ + bool updated; + orb_check(_pos_sp_triplet_sub, &updated); + if (updated) + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + + if (_pos_sp_triplet.current.valid) { /* in case of interrupted mission don't go to waypoint but stay at current position */ - reset_sp_xy = true; - reset_sp_z = true; + _reset_lat_lon_sp = true; + _reset_alt_sp = true; + + _lat_sp = _pos_sp_triplet.current.lat; + _lon_sp = _pos_sp_triplet.current.lon; + _alt_sp = _pos_sp_triplet.current.alt; } else { /* no waypoint, loiter, reset position setpoint if needed */ - if (reset_sp_xy) { - reset_sp_xy = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - } - - if (reset_sp_z) { - reset_sp_z = false; - _pos_sp(2) = _pos(2); - } + reset_lat_lon_sp(); + reset_alt_sp(); } + } else { + /* no control, reset setpoint */ + reset_lat_lon_sp(); + reset_alt_sp(); } - /* copy resulting setpoint to vehicle_local_position_setpoint topic for logging */ - _local_pos_sp.yaw = _att_sp.yaw_body; - _local_pos_sp.x = _pos_sp(0); - _local_pos_sp.y = _pos_sp(1); - _local_pos_sp.z = _pos_sp(2); + if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { + /* idle state, don't run controller and set zero thrust */ + R.identity(); + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; + _att_sp.yaw_body = 0.0f; + _att_sp.thrust = 0.0f; + + _att_sp.timestamp = hrt_absolute_time(); + + /* publish attitude setpoint */ + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - /* publish local position setpoint */ - if (_local_pos_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } } else { - _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); - } + /* run position & altitude controllers, calculate velocity setpoint */ + math::Vector<3> pos_err; + float err_x, err_y; + get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]); + pos_err(2) = -(_alt_sp - alt); - /* run position & altitude controllers, calculate velocity setpoint */ - math::Vector<3> pos_err = _pos_sp - _pos; - _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); - if (!_control_mode.flag_control_altitude_enabled) { - reset_sp_z = true; - _vel_sp(2) = 0.0f; - } + if (!_control_mode.flag_control_altitude_enabled) { + _reset_alt_sp = true; + _vel_sp(2) = 0.0f; + } - if (!_control_mode.flag_control_position_enabled) { - reset_sp_xy = true; - _vel_sp(0) = 0.0f; - _vel_sp(1) = 0.0f; - } + if (!_control_mode.flag_control_position_enabled) { + _reset_lat_lon_sp = true; + _vel_sp(0) = 0.0f; + _vel_sp(1) = 0.0f; + } - if (!_control_mode.flag_control_manual_enabled) { /* use constant descend rate when landing, ignore altitude setpoint */ - if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) { + if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } - /* limit 3D speed only in AUTO mode */ - float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); + if (!_control_mode.flag_control_manual_enabled) { + /* limit 3D speed only in non-manual modes */ + float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); - if (vel_sp_norm > 1.0f) { - _vel_sp /= vel_sp_norm; + if (vel_sp_norm > 1.0f) { + _vel_sp /= vel_sp_norm; + } } - } - _global_vel_sp.vx = _vel_sp(0); - _global_vel_sp.vy = _vel_sp(1); - _global_vel_sp.vz = _vel_sp(2); + _global_vel_sp.vx = _vel_sp(0); + _global_vel_sp.vy = _vel_sp(1); + _global_vel_sp.vz = _vel_sp(2); - /* publish velocity setpoint */ - if (_global_vel_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); + /* publish velocity setpoint */ + if (_global_vel_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); - } else { - _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); - } + } else { + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); + } - if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - /* reset integrals if needed */ - if (_control_mode.flag_control_climb_rate_enabled) { - if (reset_int_z) { - reset_int_z = false; - float i = _params.thr_min; + if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode.flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = _params.thr_min; - if (reset_int_z_manual) { - i = _manual.throttle; + if (reset_int_z_manual) { + i = _manual.throttle; - if (i < _params.thr_min) { - i = _params.thr_min; + if (i < _params.thr_min) { + i = _params.thr_min; - } else if (i > _params.thr_max) { - i = _params.thr_max; + } else if (i > _params.thr_max) { + i = _params.thr_max; + } } + + thrust_int(2) = -i; + mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } - thrust_int(2) = -i; - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + } else { + reset_int_z = true; } - } else { - reset_int_z = true; - } + if (_control_mode.flag_control_velocity_enabled) { + if (reset_int_xy) { + reset_int_xy = false; + thrust_int(0) = 0.0f; + thrust_int(1) = 0.0f; + mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral"); + } - if (_control_mode.flag_control_velocity_enabled) { - if (reset_int_xy) { - reset_int_xy = false; - thrust_int(0) = 0.0f; - thrust_int(1) = 0.0f; - mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral"); + } else { + reset_int_xy = true; } - } else { - reset_int_xy = true; - } - - /* velocity error */ - math::Vector<3> vel_err = _vel_sp - _vel; + /* velocity error */ + math::Vector<3> vel_err = _vel_sp - _vel; - /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; - _vel_prev = _vel; + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; - /* thrust vector in NED frame */ - math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; - if (!_control_mode.flag_control_velocity_enabled) { - thrust_sp(0) = 0.0f; - thrust_sp(1) = 0.0f; - } - - if (!_control_mode.flag_control_climb_rate_enabled) { - thrust_sp(2) = 0.0f; - } + if (!_control_mode.flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } - /* limit thrust vector and check for saturation */ - bool saturation_xy = false; - bool saturation_z = false; + if (!_control_mode.flag_control_climb_rate_enabled) { + thrust_sp(2) = 0.0f; + } - /* limit min lift */ - float thr_min = _params.thr_min; + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; - if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { - /* don't allow downside thrust direction in manual attitude mode */ - thr_min = 0.0f; - } + /* limit min lift */ + float thr_min = _params.thr_min; - if (-thrust_sp(2) < thr_min) { - thrust_sp(2) = -thr_min; - saturation_z = true; - } + if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } - /* limit max tilt */ - float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); - float tilt_max = _params.tilt_max; - if (!_control_mode.flag_control_manual_enabled) { - if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) { - /* limit max tilt and min lift when landing */ - tilt_max = _params.land_tilt_max; - if (thr_min < 0.0f) - thr_min = 0.0f; + if (-thrust_sp(2) < thr_min) { + thrust_sp(2) = -thr_min; + saturation_z = true; } - } - if (_control_mode.flag_control_velocity_enabled) { - if (tilt > tilt_max && thr_min >= 0.0f) { - /* crop horizontal component */ - float k = tanf(tilt_max) / tanf(tilt); - thrust_sp(0) *= k; - thrust_sp(1) *= k; - saturation_xy = true; + /* limit max tilt */ + float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); + float tilt_max = _params.tilt_max; + if (!_control_mode.flag_control_manual_enabled) { + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) + thr_min = 0.0f; + } } - } else { - /* thrust compensation for altitude only control mode */ - float att_comp; - if (_att.R[2][2] > TILT_COS_MAX) { - att_comp = 1.0f / _att.R[2][2]; - } else if (_att.R[2][2] > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; - saturation_z = true; + if (_control_mode.flag_control_velocity_enabled) { + if (tilt > tilt_max && thr_min >= 0.0f) { + /* crop horizontal component */ + float k = tanf(tilt_max) / tanf(tilt); + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } } else { - att_comp = 1.0f; - saturation_z = true; - } + /* thrust compensation for altitude only control mode */ + float att_comp; - thrust_sp(2) *= att_comp; - } + if (_att.R[2][2] > TILT_COS_MAX) { + att_comp = 1.0f / _att.R[2][2]; + } else if (_att.R[2][2] > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + saturation_z = true; + } else { + att_comp = 1.0f; + saturation_z = true; + } - /* limit max thrust */ - float thrust_abs = thrust_sp.length(); + thrust_sp(2) *= att_comp; + } - if (thrust_abs > _params.thr_max) { - if (thrust_sp(2) < 0.0f) { - if (-thrust_sp(2) > _params.thr_max) { - /* thrust Z component is too large, limit it */ - thrust_sp(0) = 0.0f; - thrust_sp(1) = 0.0f; - thrust_sp(2) = -_params.thr_max; - saturation_xy = true; - saturation_z = true; + /* limit max thrust */ + float thrust_abs = thrust_sp.length(); + + if (thrust_abs > _params.thr_max) { + if (thrust_sp(2) < 0.0f) { + if (-thrust_sp(2) > _params.thr_max) { + /* thrust Z component is too large, limit it */ + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + thrust_sp(2) = -_params.thr_max; + saturation_xy = true; + saturation_z = true; + + } else { + /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ + float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } } else { - /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ - float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); - float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); - float k = thrust_xy_max / thrust_xy_abs; - thrust_sp(0) *= k; - thrust_sp(1) *= k; + /* Z component is negative, going down, simply limit thrust vector */ + float k = _params.thr_max / thrust_abs; + thrust_sp *= k; saturation_xy = true; + saturation_z = true; } - } else { - /* Z component is negative, going down, simply limit thrust vector */ - float k = _params.thr_max / thrust_abs; - thrust_sp *= k; - saturation_xy = true; - saturation_z = true; + thrust_abs = _params.thr_max; } - thrust_abs = _params.thr_max; - } + /* update integrals */ + if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { + thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } - /* update integrals */ - if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { - thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; - thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; - } + if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { + thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; - if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { - thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; + /* protection against flipping on ground when landing */ + if (thrust_int(2) > 0.0f) + thrust_int(2) = 0.0f; + } - /* protection against flipping on ground when landing */ - if (thrust_int(2) > 0.0f) - thrust_int(2) = 0.0f; - } + /* calculate attitude setpoint from thrust vector */ + if (_control_mode.flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + math::Vector<3> body_x; + math::Vector<3> body_y; + math::Vector<3> body_z; - /* calculate attitude setpoint from thrust vector */ - if (_control_mode.flag_control_velocity_enabled) { - /* desired body_z axis = -normalize(thrust_vector) */ - math::Vector<3> body_x; - math::Vector<3> body_y; - math::Vector<3> body_z; + if (thrust_abs > SIGMA) { + body_z = -thrust_sp / thrust_abs; - if (thrust_abs > SIGMA) { - body_z = -thrust_sp / thrust_abs; + } else { + /* no thrust, set Z axis to safe value */ + body_z.zero(); + body_z(2) = 1.0f; + } - } else { - /* no thrust, set Z axis to safe value */ - body_z.zero(); - body_z(2) = 1.0f; - } + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); - /* vector of desired yaw direction in XY plane, rotated by PI/2 */ - math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); + if (fabsf(body_z(2)) > SIGMA) { + /* desired body_x axis, orthogonal to body_z */ + body_x = y_C % body_z; - if (fabsf(body_z(2)) > SIGMA) { - /* desired body_x axis, orthogonal to body_z */ - body_x = y_C % body_z; + /* keep nose to front while inverted upside down */ + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + body_x.normalize(); - /* keep nose to front while inverted upside down */ - if (body_z(2) < 0.0f) { - body_x = -body_x; + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x.zero(); + body_x(2) = 1.0f; } - body_x.normalize(); + /* desired body_y axis */ + body_y = body_z % body_x; - } else { - /* desired thrust is in XY plane, set X downside to construct correct matrix, - * but yaw component will not be used actually */ - body_x.zero(); - body_x(2) = 1.0f; - } + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + R(i, 0) = body_x(i); + R(i, 1) = body_y(i); + R(i, 2) = body_z(i); + } - /* desired body_y axis */ - body_y = body_z % body_x; + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; - /* fill rotation matrix */ - for (int i = 0; i < 3; i++) { - R(i, 0) = body_x(i); - R(i, 1) = body_y(i); - R(i, 2) = body_z(i); + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = R.to_euler(); + _att_sp.roll_body = euler(0); + _att_sp.pitch_body = euler(1); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ } - /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); - _att_sp.R_valid = true; - - /* calculate euler angles, for logging only, must not be used for control */ - math::Vector<3> euler = R.to_euler(); - _att_sp.roll_body = euler(0); - _att_sp.pitch_body = euler(1); - /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ - } + _att_sp.thrust = thrust_abs; - _att_sp.thrust = thrust_abs; + _att_sp.timestamp = hrt_absolute_time(); - _att_sp.timestamp = hrt_absolute_time(); + /* publish attitude setpoint */ + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - /* publish attitude setpoint */ - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + reset_int_z = true; } - - } else { - reset_int_z = true; } } else { /* position controller disabled, reset setpoints */ - reset_sp_z = true; - reset_sp_xy = true; + _reset_alt_sp = true; + _reset_lat_lon_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -958,7 +1021,7 @@ MulticopterPositionControl::task_main() } warnx("stopped"); - mavlink_log_info(mavlink_fd, "[mpc] stopped"); + mavlink_log_info(_mavlink_fd, "[mpc] stopped"); _control_task = -1; _exit(0); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 829511053..061baf0c5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -64,7 +64,7 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/home_position.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/mission_result.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_control_mode.h> @@ -72,7 +72,6 @@ #include <uORB/topics/mission.h> #include <uORB/topics/fence.h> #include <uORB/topics/navigation_capabilities.h> -#include <uORB/topics/offboard_control_setpoint.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/state_table.h> @@ -85,6 +84,7 @@ #include <sys/types.h> #include <sys/stat.h> +#include "navigator_state.h" #include "navigator_mission.h" #include "mission_feasibility_checker.h" #include "geofence.h" @@ -150,21 +150,21 @@ private: int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _offboard_mission_sub; /**< notification of offboard mission updates */ - int _onboard_mission_sub; /**< notification of onboard mission updates */ + int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ - int _offboard_sub; /**< offboard control setpoint subscription */ + int _control_mode_sub; /**< vehicle control mode subscription */ - orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ - orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct home_position_s _home_pos; /**< home position for RTL */ - struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ - struct offboard_control_setpoint_s _offboard; /**< offboard control setpoint, to get desired offboard control mode only */ + struct mission_item_s _mission_item; /**< current mission item */ + bool _mission_item_valid; /**< current mission item valid */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -178,6 +178,7 @@ private: class Mission _mission; + bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -189,6 +190,8 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + bool _pos_sp_triplet_updated; + char *nav_states_str[NAV_STATE_MAX]; struct { @@ -199,6 +202,7 @@ private: float takeoff_alt; float land_alt; float rtl_alt; + float rtl_land_delay; } _parameters; /**< local copies of parameters */ struct { @@ -209,6 +213,7 @@ private: param_t takeoff_alt; param_t land_alt; param_t rtl_alt; + param_t rtl_land_delay; } _parameter_handles; /**< handles for parameters */ enum Event { @@ -217,6 +222,7 @@ private: EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, + EVENT_LAND_REQUESTED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, MAX_EVENT @@ -231,8 +237,7 @@ private: RTL_STATE_NONE = 0, RTL_STATE_CLIMB, RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LAND + RTL_STATE_DESCEND }; enum RTLState _rtl_state; @@ -271,12 +276,11 @@ private: * Retrieve vehicle status */ void vehicle_status_update(); - + /** - * Retrieve offboard setpoint + * Retrieve vehicle control mode */ - void offboard_update(); - + void vehicle_control_mode_update(); /** * Shim for calling task_main from task_create. @@ -298,7 +302,8 @@ private: void start_loiter(); void start_mission(); void start_rtl(); - void finish_rtl(); + void start_land(); + void start_land_home(); /** * Guards offboard mission @@ -331,35 +336,19 @@ private: void set_rtl_item(); /** - * Helper function to get a loiter item + * Set position setpoint for mission item */ - void get_loiter_item(mission_item_s *item); + void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item); /** * Helper function to get a takeoff item */ - void get_takeoff_item(mission_item_s *item); + void get_takeoff_setpoint(position_setpoint_s *pos_sp); /** * Publish a new mission item triplet for position controller */ - void publish_mission_item_triplet(); - - /** - * Publish vehicle_control_mode topic for controllers - */ - void publish_control_mode(); - - - /** - * Compare two mission items if they are equivalent - * Two mission items can be considered equivalent for the purpose of the navigator even if some fields differ. - * - * @return true if equivalent, false otherwise - */ - bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b); - - void add_home_pos_to_rtl(struct mission_item_s *new_mission_item); + void publish_position_setpoint_triplet(); }; namespace navigator @@ -387,16 +376,15 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), + _control_mode_sub(-1), _params_sub(-1), _offboard_mission_sub(-1), _onboard_mission_sub(-1), _capabilities_sub(-1), - _offboard_sub(-1), /* publications */ - _triplet_pub(-1), + _pos_sp_triplet_pub(-1), _mission_result_pub(-1), - _control_mode_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -406,13 +394,16 @@ Navigator::Navigator() : _fence_valid(false), _inside_fence(true), _mission(), + _global_pos_valid(false), _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _set_nav_state_timestamp(0), + _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), + _pos_sp_triplet_updated(false), _geofence_violation_warning_sent(false) { _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); @@ -422,22 +413,19 @@ Navigator::Navigator() : _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); + _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; - memset(&_mission_item_triplet.previous, 0, sizeof(struct mission_item_s)); - memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s)); - memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s)); - + memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); memset(&_mission_result, 0, sizeof(struct mission_result_s)); - memset(&_offboard, 0, sizeof(_offboard)); + memset(&_mission_item, 0, sizeof(struct mission_item_s)); + memset(&nav_states_str, 0, sizeof(nav_states_str)); nav_states_str[0] = "NONE"; nav_states_str[1] = "READY"; nav_states_str[2] = "LOITER"; nav_states_str[3] = "MISSION"; nav_states_str[4] = "RTL"; + nav_states_str[5] = "LAND"; /* Initialize state machine */ myState = NAV_STATE_NONE; @@ -483,6 +471,7 @@ Navigator::parameters_update() param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); + param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay)); _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); @@ -492,7 +481,6 @@ Navigator::parameters_update() void Navigator::global_position_update() { - /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } @@ -508,11 +496,6 @@ Navigator::navigation_capabilities_update() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } -void -Navigator::offboard_update() -{ - orb_copy(ORB_ID(offboard_control_setpoint), _offboard_sub, &_offboard); -} void Navigator::offboard_mission_update(bool isrotaryWing) @@ -563,9 +546,19 @@ Navigator::onboard_mission_update() void Navigator::vehicle_status_update() { - /* try to load initial states */ if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { - _vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */ + /* in case the commander is not be running */ + _vstatus.arming_state = ARMING_STATE_STANDBY; + } +} + +void +Navigator::vehicle_control_mode_update() +{ + if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) { + /* in case the commander is not be running */ + _control_mode.flag_control_auto_enabled = false; + _control_mode.flag_armed = false; } } @@ -611,25 +604,24 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - _offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); + vehicle_control_mode_update(); parameters_update(); global_position_update(); home_position_update(); navigation_capabilities_update(); offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); - offboard_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); unsigned prevState = NAV_STATE_NONE; - bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -651,9 +643,8 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _vstatus_sub; fds[6].events = POLLIN; - fds[7].fd = _offboard_sub; + fds[7].fd = _control_mode_sub; fds[7].events = POLLIN; - while (!_task_should_exit) { @@ -679,21 +670,26 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* vehicle control mode updated */ + if (fds[7].revents & POLLIN) { + vehicle_control_mode_update(); + } + /* vehicle status updated */ if (fds[6].revents & POLLIN) { vehicle_status_update(); - pub_control_mode = true; - /* Evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { + /* evaluate state machine from commander and set the navigator mode accordingly */ + if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { bool stick_mode = false; if (!_vstatus.rc_signal_lost) { /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + /* switch to RTL if not already landed after RTL and home position set */ + if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -750,12 +746,20 @@ Navigator::task_main() break; case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } break; + case NAV_STATE_LAND: + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); + } + + break; + default: warnx("ERROR: Requested navigation state not supported"); break; @@ -775,7 +779,7 @@ Navigator::task_main() } } else { - /* not in AUTO */ + /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } } @@ -816,10 +820,22 @@ Navigator::task_main() if (fds[1].revents & POLLIN) { global_position_update(); - /* only check if waypoint has been reached in MISSION and RTL modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { - if (check_mission_item_reached()) { - on_mission_item_reached(); + /* publish position setpoint triplet on each position update if navigator active */ + if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + _pos_sp_triplet_updated = true; + + if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { + /* got global position when landing, update setpoint */ + start_land(); + } + + _global_pos_valid = _global_pos.global_valid; + + /* check if waypoint has been reached in MISSION, RTL and LAND modes */ + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { + if (check_mission_item_reached()) { + on_mission_item_reached(); + } } } @@ -839,21 +855,16 @@ Navigator::task_main() } } - /* offboard setpoint updated */ - if (fds[7].revents & POLLIN) { - offboard_update(); + /* publish position setpoint triplet if updated */ + if (_pos_sp_triplet_updated) { + _pos_sp_triplet_updated = false; + publish_position_setpoint_triplet(); } - + /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s -> %s", nav_states_str[prevState], nav_states_str[myState]); + mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; - pub_control_mode = true; - } - - /* publish control mode if updated */ - if (pub_control_mode) { - publish_control_mode(); } perf_end(_loop_perf); @@ -889,14 +900,14 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); - if (_global_pos.valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); + if (_global_pos.global_valid) { + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)_global_pos.relative_alt); - warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", - (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", + (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } @@ -935,87 +946,108 @@ Navigator::status() StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { - /* STATE_NONE */ + /* NAV_STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, { - /* STATE_READY */ + /* NAV_STATE_READY */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, { - /* STATE_LOITER */ + /* NAV_STATE_LOITER */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, { - /* STATE_MISSION */ + /* NAV_STATE_MISSION */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, { - /* STATE_RTL */ + /* NAV_STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, + { + /* NAV_STATE_LAND */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, + }, }; void Navigator::start_none() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; _rtl_state = RTL_STATE_NONE; - publish_mission_item_triplet(); + _pos_sp_triplet_updated = true; } void Navigator::start_ready() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.next.valid = false; + + _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; + + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; - if (_rtl_state != RTL_STATE_LAND) { - /* allow RTL if landed not at home */ + if (_rtl_state != RTL_STATE_DESCEND) { + /* reset RTL state if landed not at home */ _rtl_state = RTL_STATE_NONE; } - publish_mission_item_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1024,39 +1056,36 @@ Navigator::start_loiter() _do_takeoff = false; /* set loiter position if needed */ - if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { + if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { _reset_loiter_pos = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - _mission_item_triplet.current.altitude_is_relative = false; - float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; + float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { - _mission_item_triplet.current.altitude = min_alt_amsl; + _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { - _mission_item_triplet.current.altitude = _global_pos.alt; + _pos_sp_triplet.current.alt = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } - if (_rtl_state == RTL_STATE_LAND) { - /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ - _rtl_state = RTL_STATE_DESCEND; - } + _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; } - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - get_loiter_item(&_mission_item_triplet.current); + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.loiter_direction = 1; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; - publish_mission_item_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1064,7 +1093,6 @@ Navigator::start_mission() { _need_takeoff = true; - mavlink_log_info(_mavlink_fd, "[navigator] mission started"); set_mission_item(); } @@ -1072,8 +1100,7 @@ void Navigator::set_mission_item() { /* copy current mission to previous item */ - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); _reset_loiter_pos = true; _do_takeoff = false; @@ -1082,37 +1109,34 @@ Navigator::set_mission_item() bool onboard; unsigned index; - ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { - _mission_item_triplet.current_valid = true; - add_home_pos_to_rtl(&_mission_item_triplet.current); + _mission_item_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && + _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { /* don't reset RTL state on RTL or LOITER items */ _rtl_state = RTL_STATE_NONE; } if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( - _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED )) { /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; /* calculate desired takeoff altitude AMSL */ - float takeoff_alt_amsl = _mission_item_triplet.current.altitude; - - if (_mission_item_triplet.current.altitude_is_relative) - takeoff_alt_amsl += _home_pos.altitude; + float takeoff_alt_amsl = _pos_sp_triplet.current.alt; if (_vstatus.condition_landed) { /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ @@ -1120,31 +1144,30 @@ Navigator::set_mission_item() } /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item_triplet.current.acceptance_radius) { + if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) { /* force TAKEOFF if landed or waypoint altitude is more than current */ _do_takeoff = true; - /* move current mission item to next */ - memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.next_valid = true; + /* move current position setpoint to next */ + memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - /* set current item to TAKEOFF */ - get_takeoff_item(&_mission_item_triplet.current); + /* set current setpoint to takeoff */ - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = takeoff_alt_amsl; - _mission_item_triplet.current.altitude_is_relative = false; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.alt = takeoff_alt_amsl; + _pos_sp_triplet.current.yaw = NAN; + _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; } - } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; } } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); } else { if (onboard) { @@ -1157,24 +1180,25 @@ Navigator::set_mission_item() } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ - _mission_item_triplet.current_valid = false; + _mission_item_valid = false; + _pos_sp_triplet.current.valid = false; warnx("ERROR: current WP can't be set"); } if (!_do_takeoff) { - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + mission_item_s item_next; + ret = _mission.get_next_mission_item(&item_next); if (ret == OK) { - add_home_pos_to_rtl(&_mission_item_triplet.next); - _mission_item_triplet.next_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); } else { /* this will fail for the last WP */ - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.next.valid = false; } } - publish_mission_item_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1182,128 +1206,174 @@ Navigator::start_rtl() { _do_takeoff = false; + /* decide if we need climb */ if (_rtl_state == RTL_STATE_NONE) { - if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) { + if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; } else { _rtl_state = RTL_STATE_RETURN; - - if (_reset_loiter_pos) { - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.altitude = _global_pos.alt; - } } } + /* if switching directly to return state, reset altitude setpoint */ + if (_rtl_state == RTL_STATE_RETURN) { + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; + } + _reset_loiter_pos = true; - mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); set_rtl_item(); } void +Navigator::start_land() +{ + /* this state can be requested by commander even if no global position available, + * in his case controller must perform landing without position control */ + _do_takeoff = false; + _reset_loiter_pos = true; + + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; +} + +void +Navigator::start_land_home() +{ + /* land to home position, should be called when hovering above home, from RTL state */ + _do_takeoff = false; + _reset_loiter_pos = true; + + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; +} + +void Navigator::set_rtl_item() { switch (_rtl_state) { case RTL_STATE_CLIMB: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - float climb_alt = _home_pos.altitude + _parameters.rtl_alt; + float climb_alt = _home_pos.alt + _parameters.rtl_alt; - if (_vstatus.condition_landed) + if (_vstatus.condition_landed) { climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + } + + _mission_item_valid = true; + + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = climb_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); break; } case RTL_STATE_RETURN: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - // don't change altitude setpoint - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + // don't change altitude + _mission_item.yaw = NAN; // TODO set heading to home + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } case RTL_STATE_DESCEND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - float descend_alt = _home_pos.altitude + _parameters.land_alt; - - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = descend_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); - break; - } + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt + _parameters.land_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - case RTL_STATE_LAND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1314,18 +1384,56 @@ Navigator::set_rtl_item() } } - publish_mission_item_triplet(); + _pos_sp_triplet_updated = true; +} + +void +Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) +{ + sp->valid = true; + + if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* set home position for RTL item */ + sp->lat = _home_pos.lat; + sp->lon = _home_pos.lon; + sp->alt = _home_pos.alt + _parameters.rtl_alt; + + } else { + sp->lat = item->lat; + sp->lon = item->lon; + sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; + } + + sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; + + if (item->nav_cmd == NAV_CMD_TAKEOFF) { + sp->type = SETPOINT_TYPE_TAKEOFF; + + } else if (item->nav_cmd == NAV_CMD_LAND) { + sp->type = SETPOINT_TYPE_LAND; + + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + sp->type = SETPOINT_TYPE_LOITER; + + } else { + sp->type = SETPOINT_TYPE_NORMAL; + } } bool Navigator::check_mission_item_reached() { /* only check if there is actually a mission item to check */ - if (!_mission_item_triplet.current_valid) { + if (!_mission_item_valid) { return false; } - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + if (_mission_item.nav_cmd == NAV_CMD_LAND) { if (_vstatus.is_rotary_wing) { return _vstatus.condition_landed; @@ -1337,10 +1445,10 @@ Navigator::check_mission_item_reached() } /* XXX TODO count turns */ - if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item_triplet.current.loiter_radius > 0.01f) { + if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item.loiter_radius > 0.01f) { return false; } @@ -1350,8 +1458,8 @@ Navigator::check_mission_item_reached() if (!_waypoint_position_reached) { float acceptance_radius; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item_triplet.current.acceptance_radius; + if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) { + acceptance_radius = _mission_item.acceptance_radius; } else { acceptance_radius = _parameters.acceptance_radius; @@ -1361,14 +1469,14 @@ Navigator::check_mission_item_reached() float dist_xy = -1.0f; float dist_z = -1.0f; - /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ - float wp_alt_amsl = _mission_item_triplet.current.altitude; + /* calculate AMSL altitude for this waypoint */ + float wp_alt_amsl = _mission_item.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - _mission_item_triplet.current.altitude += _home_pos.altitude; + if (_mission_item.altitude_is_relative) + wp_alt_amsl += _home_pos.alt; - dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, &dist_xy, &dist_z); if (_do_takeoff) { @@ -1385,9 +1493,9 @@ Navigator::check_mission_item_reached() } if (!_waypoint_yaw_reached) { - if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) { + if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); + float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; @@ -1403,14 +1511,14 @@ Navigator::check_mission_item_reached() if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; - if (_mission_item_triplet.current.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside); + if (_mission_item.time_inside > 0.01f) { + mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); } } /* check if the MAV was long enough inside the waypoint orbit */ - if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) - || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) + || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; @@ -1453,243 +1561,45 @@ Navigator::on_mission_item_reached() } } - } else { - /* RTL finished */ - if (_rtl_state == RTL_STATE_LAND) { - /* landed at home position */ - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); - dispatch(EVENT_READY_REQUESTED); + } else if (myState == NAV_STATE_RTL) { + /* RTL completed */ + if (_rtl_state == RTL_STATE_DESCEND) { + /* hovering above home position, land if needed or loiter */ + mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); + if (_mission_item.autocontinue) { + dispatch(EVENT_LAND_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } } else { /* next RTL step */ _rtl_state = (RTLState)(_rtl_state + 1); set_rtl_item(); } - } -} -void -Navigator::get_loiter_item(struct mission_item_s *item) -{ - //item->altitude_is_relative - //item->lat - //item->lon - //item->altitude - //item->yaw - item->loiter_radius = _parameters.loiter_radius; - item->loiter_direction = 1; - item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - item->acceptance_radius = _parameters.acceptance_radius; - item->time_inside = 0.0f; - item->pitch_min = 0.0f; - item->autocontinue = false; - item->origin = ORIGIN_ONBOARD; - -} - -void -Navigator::get_takeoff_item(mission_item_s *item) -{ - //item->altitude_is_relative - //item->lat - //item->lon - //item->altitude - item->yaw = NAN; - item->loiter_radius = _parameters.loiter_radius; - item->loiter_direction = 1; - item->nav_cmd = NAV_CMD_TAKEOFF; - item->acceptance_radius = _parameters.acceptance_radius; - item->time_inside = 0.0f; - item->pitch_min = 0.0; - item->autocontinue = true; - item->origin = ORIGIN_ONBOARD; -} - -void -Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) -{ - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* append the home position to RTL item */ - new_mission_item->lat = _home_pos.lat; - new_mission_item->lon = _home_pos.lon; - new_mission_item->altitude = _home_pos.altitude + _parameters.rtl_alt; - new_mission_item->altitude_is_relative = false; - new_mission_item->loiter_radius = _parameters.loiter_radius; - new_mission_item->acceptance_radius = _parameters.acceptance_radius; + } else if (myState == NAV_STATE_LAND) { + /* landing completed */ + mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); + dispatch(EVENT_READY_REQUESTED); } } void -Navigator::publish_mission_item_triplet() +Navigator::publish_position_setpoint_triplet() { - /* lazily publish the mission triplet only once available */ - if (_triplet_pub > 0) { - /* publish the mission triplet */ - orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); + /* update navigation state */ + _pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState); - } else { - /* advertise and publish */ - _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); - } -} - -void -Navigator::publish_control_mode() -{ - /* update vehicle_control_mode topic*/ - _control_mode.main_state = _vstatus.main_state; - _control_mode.nav_state = static_cast<nav_state_t>(myState); - _control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR; - _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; - _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; - - _control_mode.flag_control_flighttermination_enabled = false; - - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_EASY: - _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: - _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_manual_enabled = false; - - if (myState == NAV_STATE_READY) { - /* disable all controllers, armed but idle */ - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - - } else { - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - } - - break; - - case MAIN_STATE_OFFBOARD: - _control_mode.flag_control_offboard_enabled = true; - _control_mode.flag_control_manual_enabled = false; - - switch (_offboard.mode) { - case OFFBOARD_CONTROL_MODE_DIRECT_RATES: - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: - _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; - break; - - case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: - _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 = true; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = true; - break; - - case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - break; - - default: - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - } - - break; - - default: - break; - } - - _control_mode.timestamp = hrt_absolute_time(); - - /* lazily publish the mission triplet only once available */ - if (_control_mode_pub > 0) { - /* publish the mission triplet */ - orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode); + /* lazily publish the position setpoint triplet only once available */ + if (_pos_sp_triplet_pub > 0) { + /* publish the position setpoint triplet */ + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { /* advertise and publish */ - _control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode); - } -} - -bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) -{ - if (a.altitude_is_relative == b.altitude_is_relative && - fabs(a.lat - b.lat) < FLT_EPSILON && - fabs(a.lon - b.lon) < FLT_EPSILON && - fabsf(a.altitude - b.altitude) < FLT_EPSILON && - fabsf(a.yaw - b.yaw) < FLT_EPSILON && - fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && - a.loiter_direction == b.loiter_direction && - a.nav_cmd == b.nav_cmd && - fabsf(a.acceptance_radius - b.acceptance_radius) < FLT_EPSILON && - fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - a.autocontinue == b.autocontinue) { - return true; - - } else { - return false; + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index ff819fca4..af1d9d7d5 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -60,3 +60,4 @@ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, 10.0f); // delay after descend before landing diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h new file mode 100644 index 000000000..6a1475c9b --- /dev/null +++ b/src/modules/navigator/navigator_state.h @@ -0,0 +1,21 @@ +/* + * navigator_state.h + * + * Created on: 27.01.2014 + * Author: ton + */ + +#ifndef NAVIGATOR_STATE_H_ +#define NAVIGATOR_STATE_H_ + +typedef enum { + NAV_STATE_NONE = 0, + NAV_STATE_READY, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_RTL, + NAV_STATE_LAND, + NAV_STATE_MAX +} nav_state_t; + +#endif /* NAVIGATOR_STATE_H_ */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 02fa6a8f2..e045ce4cc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -202,8 +202,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool landed = true; hrt_abstime landed_time = 0; - bool flag_armed = false; - uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -329,6 +327,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; + global_pos.baro_valid = true; } } } @@ -379,17 +378,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - - /* reset ground level on arm */ - if (armed.armed && !flag_armed) { - flag_armed = armed.armed; - baro_offset -= z_est[0]; - corr_baro = 0.0f; - local_pos.ref_alt -= z_est[0]; - local_pos.ref_timestamp = t; - z_est[0] = 0.0f; - alt_avg = 0.0f; - } } /* sensor combined */ @@ -637,6 +625,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; + dt = fmaxf(fminf(0.02, dt), 0.005); t_prev = t; /* use GPS if it's valid and reference position initialized */ @@ -679,7 +668,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; baro_offset += offs_corr; - baro_counter += offs_corr; + corr_baro += offs_corr; } /* accelerometer bias correction */ @@ -835,32 +824,32 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); /* publish global position */ - global_pos.valid = local_pos.xy_global; + global_pos.global_valid = local_pos.xy_global; if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - global_pos.lat = (int32_t)(est_lat * 1e7d); - global_pos.lon = (int32_t)(est_lon * 1e7d); + global_pos.lat = est_lat; + global_pos.lon = est_lon; global_pos.time_gps_usec = gps.time_gps_usec; } /* set valid values even if position is not valid */ if (local_pos.v_xy_valid) { - global_pos.vx = local_pos.vx; - global_pos.vy = local_pos.vy; - } - - if (local_pos.z_valid) { - global_pos.relative_alt = -local_pos.z; + global_pos.vel_n = local_pos.vx; + global_pos.vel_e = local_pos.vy; } if (local_pos.z_global) { global_pos.alt = local_pos.ref_alt - local_pos.z; } + if (local_pos.z_valid) { + global_pos.baro_alt = baro_offset - local_pos.z; + } + if (local_pos.v_z_valid) { - global_pos.vz = local_pos.vz; + global_pos.vel_d = local_pos.vz; } global_pos.yaw = local_pos.yaw; diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index 81566eb2a..2f5908ac5 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -83,6 +83,14 @@ adc_init(void) { adc_perf = perf_alloc(PC_ELAPSED, "adc"); + /* put the ADC into power-down mode */ + rCR2 &= ~ADC_CR2_ADON; + up_udelay(10); + + /* bring the ADC out of power-down mode */ + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + /* do calibration if supported */ #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_RSTCAL; @@ -96,41 +104,25 @@ adc_init(void) if (rCR2 & ADC_CR2_CAL) return -1; - #endif - /* arbitrarily configure all channels for 55 cycle sample time */ - rSMPR1 = 0b00000011011011011011011011011011; + /* + * Configure sampling time. + * + * For electrical protection reasons, we want to be able to have + * 10K in series with ADC inputs that leave the board. At 12MHz this + * means we need 28.5 cycles of sampling time (per table 43 in the + * datasheet). + */ + rSMPR1 = 0b00000000011011011011011011011011; rSMPR2 = 0b00011011011011011011011011011011; - /* XXX for F2/4, might want to select 12-bit mode? */ - rCR1 = 0; - - /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = -#ifdef ADC_CR2_TSVREFE - /* enable the temperature sensor in CR2 */ - ADC_CR2_TSVREFE | -#endif - 0; - -#ifdef ADC_CCR_TSVREFE - /* enable temperature sensor in CCR */ - rCCR = ADC_CCR_TSVREFE; -#endif + rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */ /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ - - /* power-cycle the ADC and turn it on */ - rCR2 &= ~ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); + rSQR3 = 0; /* will be updated with the channel at conversion time */ return 0; } @@ -141,11 +133,12 @@ adc_init(void) uint16_t adc_measure(unsigned channel) { + perf_begin(adc_perf); /* clear any previous EOC */ - if (rSR & ADC_SR_EOC) - rSR &= ~ADC_SR_EOC; + rSR = 0; + (void)rDR; /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ rSQR3 = channel; @@ -158,7 +151,6 @@ adc_measure(unsigned channel) /* never spin forever - this will give a bogus result though */ if (hrt_elapsed_time(&now) > 100) { - debug("adc timeout"); perf_end(adc_perf); return 0xffff; } @@ -166,6 +158,7 @@ adc_measure(unsigned channel) /* read the result and clear EOC */ uint16_t result = rDR; + rSR = 0; perf_end(adc_perf); return result; diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 541eed0e1..5e2c92bf4 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -114,9 +114,20 @@ controls_tick() { perf_begin(c_gather_sbus); bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); + + bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; } + + /* switch S.Bus output pin as needed */ + if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { + #ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); + #endif + } + perf_end(c_gather_sbus); /* diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e55ef784a..2e79f0ac6 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -71,6 +71,7 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; +static volatile bool in_mixer = false; /* selected control values and count for mixing */ enum mixer_source { @@ -95,6 +96,7 @@ static void mixer_set_failsafe(); void mixer_tick(void) { + /* check that we are receiving fresh data from the FMU */ if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { @@ -199,13 +201,17 @@ mixer_tick(void) } - } else if (source != MIX_NONE) { + } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ + + /* poor mans mutex */ + in_mixer = true; mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + in_mixer = false; pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -297,12 +303,17 @@ mixer_callback(uintptr_t handle, static char mixer_text[256]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; -void +int mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while safety off */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { - return; + return 1; + } + + /* abort if we're in the mixer */ + if (in_mixer) { + return 1; } px4io_mixdata *msg = (px4io_mixdata *)buffer; @@ -310,7 +321,7 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) - return; + return 0; unsigned text_length = length - sizeof(px4io_mixdata); @@ -328,13 +339,16 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); + /* disable mixing during the update */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - return; + return 0; } - /* append mixer text and nul-terminate */ + /* append mixer text and nul-terminate, guard against overflow */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; @@ -369,6 +383,8 @@ mixer_handle_text(const void *buffer, size_t length) break; } + + return 0; } static void diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 0b8c4a6a8..d4c25911e 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -196,6 +196,11 @@ user_start(int argc, char *argv[]) POWER_SERVO(true); #endif + /* turn off S.Bus out (if supported) */ +#ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(false); +#endif + /* start the safety switch handler */ safety_init(); @@ -205,6 +210,9 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); + /* set up the ADC */ + adc_init(); + /* start the FMU interface */ interface_init(); @@ -223,24 +231,41 @@ user_start(int argc, char *argv[]) /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); -#if 0 - /* not enough memory, lock down */ - if (minfo.mxordblk < 500) { + /* + * P O L I C E L I G H T S + * + * Not enough memory, lock down. + * + * We might need to allocate mixers later, and this will + * ensure that a developer doing a change will notice + * that he just burned the remaining RAM with static + * allocations. We don't want him to be able to + * get past that point. This needs to be clearly + * documented in the dev guide. + * + */ + if (minfo.mxordblk < 600) { + lowsyslog("ERR: not enough MEM"); bool phase = false; - if (phase) { - LED_AMBER(true); - LED_BLUE(false); - } else { - LED_AMBER(false); - LED_BLUE(true); - } + while (true) { + + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + up_udelay(250000); - phase = !phase; - usleep(300000); + phase = !phase; + } } -#endif + + /* Start the failsafe led init */ + failsafe_led_init(); /* * Run everything in a tight loop. @@ -270,11 +295,12 @@ user_start(int argc, char *argv[]) check_reboot(); -#if 0 - /* check for debug activity */ + /* check for debug activity (default: none) */ show_debug_messages(); - /* post debug state at ~1Hz */ + /* post debug state at ~1Hz - this is via an auxiliary serial port + * DEFAULTS TO OFF! + */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { struct mallinfo minfo = mallinfo(); @@ -287,7 +313,6 @@ user_start(int argc, char *argv[]) (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } -#endif } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a0daa97ea..393e0560e 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -160,6 +160,7 @@ extern pwm_limit_t pwm_limit; # define PX4IO_RELAY_CHANNELS 0 # define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) +# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) # define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) @@ -177,12 +178,13 @@ extern pwm_limit_t pwm_limit; * Mixer */ extern void mixer_tick(void); -extern void mixer_handle_text(const void *buffer, size_t length); +extern int mixer_handle_text(const void *buffer, size_t length); /** * Safety switch/LED. */ extern void safety_init(void); +extern void failsafe_led_init(void); /** * FMU communications diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a59dbe89c..2c437d2c0 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -89,7 +89,9 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_IBATT] = 0, [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, - [PX4IO_P_STATUS_PRSSI] = 0 + [PX4IO_P_STATUS_PRSSI] = 0, + [PX4IO_P_STATUS_NRSSI] = 0, + [PX4IO_P_STATUS_RC_DATA] = 0 }; /** @@ -380,7 +382,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - mixer_handle_text(values, num_values * sizeof(*values)); + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + return mixer_handle_text(values, num_values * sizeof(*values)); + } break; default: @@ -507,8 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_REBOOT_BL: if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { // don't allow reboot while armed break; } @@ -538,8 +542,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * do not allow a RC config change while outputs armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } @@ -599,7 +602,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if (REG_TO_SIGNED(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 83bd3026e..ff2e4af6e 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -83,7 +83,11 @@ safety_init(void) { /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); +} +void +failsafe_led_init(void) +{ /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -164,8 +168,8 @@ failsafe_blink(void *arg) /* indicate that a serious initialisation error occured */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { LED_AMBER(true); - return; - } + return; + } static bool failsafe = false; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 11ccd7356..495447740 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -218,11 +218,33 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { + if ((frame[0] != 0x0f)) { sbus_frame_drops++; return false; } + switch (frame[24]) { + case 0x00: + /* this is S.BUS 1 */ + break; + case 0x03: + /* S.BUS 2 SLOT0: RX battery and external voltage */ + break; + case 0x83: + /* S.BUS 2 SLOT1 */ + break; + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: + break; + default: + /* we expect one of the bits above, but there are some we don't know yet */ + break; + } + /* we have received something we think is a frame */ last_frame_time = frame_time; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e0bb81e0e..3c218e21f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -62,7 +62,6 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> @@ -73,7 +72,7 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> @@ -740,7 +739,6 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; - struct vehicle_control_mode_s control_mode; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -750,7 +748,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; - struct mission_item_triplet_s triplet; + struct position_setpoint_triplet_s triplet; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; @@ -767,7 +765,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; - int control_mode_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -847,12 +844,6 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- VEHICLE CONTROL MODE --- */ - subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - fds[fdsc_count].fd = subs.control_mode_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- SENSORS COMBINED --- */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; @@ -908,7 +899,7 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- GLOBAL POSITION SETPOINT--- */ - subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); fds[fdsc_count].fd = subs.triplet_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -1002,7 +993,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; - /* poll all topics if logging enabled or only management (first 2) if not */ + /* poll all topics if logging enabled or only management (first 3) if not */ int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); /* handle the poll result */ @@ -1064,11 +1055,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS --- */ if (fds[ifds++].revents & POLLIN) { /* don't orb_copy, it's already done few lines above */ - /* copy VEHICLE CONTROL MODE control mode here to construct STAT message */ - orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state; - log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; @@ -1078,7 +1066,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + /* don't orb_copy, it's already done few lines above */ log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; @@ -1094,8 +1082,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPS); } - ifds++; // skip CONTROL MODE, already handled - /* --- SENSOR COMBINED --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); @@ -1252,29 +1238,29 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); log_msg.msg_type = LOG_GPOS_MSG; - log_msg.body.log_GPOS.lat = buf.global_pos.lat; - log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7; + log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7; log_msg.body.log_GPOS.alt = buf.global_pos.alt; - log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; - log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; - log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; + log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; + log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); LOGBUFFER_WRITE_AND_COUNT(GPOS); } /* --- GLOBAL POSITION SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet); + orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative; + log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); - log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude; + log_msg.body.log_GPSP.alt = buf.triplet.current.alt; log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; + log_msg.body.log_GPSP.type = buf.triplet.current.type; log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.acceptance_radius = buf.triplet.current.acceptance_radius; - log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside; log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; LOGBUFFER_WRITE_AND_COUNT(GPSP); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 503dc0afc..db87e3a6a 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -149,7 +149,6 @@ struct log_ATTC_s { #define LOG_STAT_MSG 10 struct log_STAT_s { uint8_t main_state; - uint8_t navigation_state; uint8_t arming_state; float battery_remaining; uint8_t battery_warning; @@ -205,21 +204,21 @@ struct log_GPOS_s { float vel_n; float vel_e; float vel_d; + float baro_alt; + uint8_t flags; }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ #define LOG_GPSP_MSG 17 struct log_GPSP_s { - uint8_t altitude_is_relative; + uint8_t nav_state; int32_t lat; int32_t lon; - float altitude; + float alt; float yaw; - uint8_t nav_cmd; + uint8_t type; float loiter_radius; int8_t loiter_direction; - float acceptance_radius; - float time_inside; float pitch_min; }; @@ -300,14 +299,14 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"), + LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"), + LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), + LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index eb771382d..999eca833 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -381,14 +381,73 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ +/** + * Roll control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading roll inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); + +/** + * Pitch control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading pitch inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); + +/** + * Throttle control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading throttle inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); + +/** + * Yaw control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading yaw inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +/** + * Mode switch channel mapping. + * + * This is the main flight mode selector. + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for deciding about the main mode. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f87f2f282..f99e9d8bb 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -791,7 +791,6 @@ Sensors::accel_init() #endif - warnx("using system accel"); close(fd); } } @@ -831,7 +830,6 @@ Sensors::gyro_init() #endif - warnx("using system gyro"); close(fd); } } @@ -1502,9 +1500,6 @@ void Sensors::task_main() { - /* inform about start */ - warnx("Initializing.."); - /* start individual sensors */ accel_init(); gyro_init(); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 79a820c06..4c84c1f25 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -117,8 +117,8 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi #include "topics/vehicle_bodyframe_speed_setpoint.h" ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); -#include "topics/mission_item_triplet.h" -ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s); +#include "topics/position_setpoint_triplet.h" +ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); #include "topics/vehicle_global_velocity_setpoint.h" ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 3e2fee84e..08d11abae 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -62,7 +62,7 @@ struct home_position_s //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float altitude; /**< Altitude in meters */ + float alt; /**< Altitude in meters */ }; /** diff --git a/src/modules/uORB/topics/mission_item_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index b35eae607..cf1ddfc38 100644 --- a/src/modules/uORB/topics/mission_item_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -45,32 +45,47 @@ #include <stdint.h> #include <stdbool.h> #include "../uORB.h" - -#include "mission.h" +#include <navigator/navigator_state.h> /** * @addtogroup topics * @{ */ +enum SETPOINT_TYPE +{ + SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */ + SETPOINT_TYPE_LOITER, /**< loiter setpoint */ + SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ + SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, vehicle must descend until landing */ + SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ +}; + +struct position_setpoint_s +{ + bool valid; /**< true if setpoint is valid */ + enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ + double lat; /**< latitude, in deg */ + double lon; /**< longitude, in deg */ + float alt; /**< altitude AMSL, in m */ + float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + float loiter_radius; /**< loiter radius (only for fixed wing), in m */ + int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ + float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ +}; + /** * Global position setpoint triplet in WGS84 coordinates. * * This are the three next waypoints (or just the next two or one). */ -struct mission_item_triplet_s +struct position_setpoint_triplet_s { - bool previous_valid; - bool current_valid; /**< flag indicating previous mission item is valid */ - bool next_valid; /**< flag indicating next mission item is valid */ - - struct mission_item_s previous; - struct mission_item_s current; - struct mission_item_s next; + struct position_setpoint_s previous; + struct position_setpoint_s current; + struct position_setpoint_s next; - int previous_index; - int current_index; - int next_index; + nav_state_t nav_state; /**< navigation state */ }; /** @@ -78,6 +93,6 @@ struct mission_item_triplet_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission_item_triplet); +ORB_DECLARE(position_setpoint_triplet); #endif diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 26dcbd985..7cbb37cd8 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,22 +61,10 @@ * Encodes the complete system state and is set by the commander app. */ -typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_MAX -} nav_state_t; - struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - main_state_t main_state; - nav_state_t nav_state; - bool flag_armed; bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ @@ -85,14 +73,14 @@ struct vehicle_control_mode_s bool flag_system_hil_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ - bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 143734e37..ff9e98e1c 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -54,25 +54,28 @@ /** * Fused global position in WGS84. * - * This struct contains the system's believ about its position. It is not the raw GPS + * This struct contains global position estimation. It is not the raw GPS * measurement (@see vehicle_gps_position). This topic is usually published by the position * estimator, which will take more sources of information into account than just GPS, * e.g. control inputs of the vehicle in a Kalman-filter implementation. */ struct vehicle_global_position_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ - bool valid; /**< true if position satisfies validity criteria of estimator */ + uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - float alt; /**< Altitude in meters */ - float relative_alt; /**< Altitude above home position in meters, */ - float vx; /**< Ground X velocity, m/s in NED */ - float vy; /**< Ground Y velocity, m/s in NED */ - float vz; /**< Ground Z velocity, m/s in NED */ - float yaw; /**< Compass heading in radians -PI..+PI. */ + bool global_valid; /**< true if position satisfies validity criteria of estimator */ + bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ + + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + double lat; /**< Latitude in degrees */ + double lon; /**< Longitude in degrees */ + float alt; /**< Altitude AMSL in meters */ + float vel_n; /**< Ground north velocity, m/s */ + float vel_e; /**< Ground east velocity, m/s */ + float vel_d; /**< Ground downside velocity, m/s */ + float yaw; /**< Yaw in radians -PI..+PI. */ + + float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index d74b696bb..4aea1b083 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,6 +54,8 @@ #include <stdbool.h> #include "../uORB.h" +#include <navigator/navigator_state.h> + /** * @addtogroup topics @{ */ @@ -65,6 +67,7 @@ typedef enum { MAIN_STATE_EASY, MAIN_STATE_AUTO, MAIN_STATE_OFFBOARD, + MAIN_STATE_MAX } main_state_t; typedef enum { @@ -74,7 +77,8 @@ typedef enum { ARMING_STATE_ARMED_ERROR, ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE + ARMING_STATE_IN_AIR_RESTORE, + ARMING_STATE_MAX } arming_state_t; typedef enum { @@ -83,9 +87,12 @@ typedef enum { } hil_state_t; typedef enum { - FLIGHTTERMINATION_STATE_OFF = 0, - FLIGHTTERMINATION_STATE_ON -} flighttermination_state_t; + FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ + FAILSAFE_STATE_RTL, /**< Return To Launch */ + FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ + FAILSAFE_STATE_MAX +} failsafe_state_t; typedef enum { MODE_SWITCH_MANUAL = 0, @@ -180,6 +187,7 @@ struct vehicle_status_s uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ + failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -231,8 +239,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - flighttermination_state_t flighttermination_state; }; /** |